diff --git a/grab2_behavior_tree/CMakeLists.txt b/grab2_behavior_tree/CMakeLists.txt
index 122c35c..3670da1 100644
--- a/grab2_behavior_tree/CMakeLists.txt
+++ b/grab2_behavior_tree/CMakeLists.txt
@@ -22,6 +22,7 @@ find_package(trajectory_msgs REQUIRED)
find_package(grab2_msgs REQUIRED)
find_package(behaviortree_cpp REQUIRED)
find_package(behaviortree_ros2 REQUIRED)
+find_package(sensor_msgs REQUIRED)
set(CMAKE_CXX_STANDARD 17)
@@ -34,6 +35,9 @@ add_library(grab2_compute_plan_to_target_ik_bt_node SHARED plugins/action/comput
add_library(grab2_move_bt_node SHARED plugins/action/move.cpp)
add_library(grab2_grip_bt_node SHARED plugins/action/grip.cpp)
add_library(grab2_reach_object_bt_node SHARED plugins/action/reach_object.cpp)
+add_library(grab2_check_gripper_has_object_bt_node SHARED
+ plugins/condition/check_gripper_has_object.cpp
+)
list(APPEND plugin_libs
grab2_detect_object_bt_node
@@ -44,6 +48,7 @@ list(APPEND plugin_libs
grab2_move_bt_node
grab2_grip_bt_node
grab2_reach_object_bt_node
+ grab2_check_gripper_has_object_bt_node
)
foreach(bt_plugin ${plugin_libs})
@@ -65,6 +70,7 @@ foreach(bt_plugin ${plugin_libs})
ament_index_cpp::ament_index_cpp
${control_msgs_TARGETS}
${grab2_msgs_TARGETS}
+ ${sensor_msgs_TARGETS}
behaviortree_cpp::behaviortree_cpp
behaviortree_ros2::behaviortree_ros2
)
@@ -87,6 +93,7 @@ target_link_libraries(grab2_engine
ament_index_cpp::ament_index_cpp
${control_msgs_TARGETS}
${grab2_msgs_TARGETS}
+ ${sensor_msgs_TARGETS}
behaviortree_cpp::behaviortree_cpp
behaviortree_ros2::behaviortree_ros2
)
diff --git a/grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml b/grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml
new file mode 100644
index 0000000..6f4be59
--- /dev/null
+++ b/grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml
@@ -0,0 +1,63 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/grab2_behavior_tree/behavior_trees/test_tree.xml b/grab2_behavior_tree/behavior_trees/test_tree.xml
new file mode 100644
index 0000000..755d8c5
--- /dev/null
+++ b/grab2_behavior_tree/behavior_trees/test_tree.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
diff --git a/grab2_behavior_tree/include/grab2_behavior_tree/plugins/condition/check_gripper_has_object.hpp b/grab2_behavior_tree/include/grab2_behavior_tree/plugins/condition/check_gripper_has_object.hpp
new file mode 100644
index 0000000..95ebd42
--- /dev/null
+++ b/grab2_behavior_tree/include/grab2_behavior_tree/plugins/condition/check_gripper_has_object.hpp
@@ -0,0 +1,40 @@
+// Copyright 2025, Zaynab Ahmed
+
+#pragma once
+
+#include
+
+#include "behaviortree_cpp/condition_node.h"
+#include "behaviortree_ros2/bt_topic_sub_node.hpp" // For BT::RosNodeParams
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace grab2_behavior_tree
+{
+
+class CheckGripperHasObject : public BT::ConditionNode
+{
+public:
+ CheckGripperHasObject(
+ const std::string & name,
+ const BT::NodeConfig & config,
+ const BT::RosNodeParams & params);
+
+ static BT::PortsList providedPorts();
+ BT::NodeStatus tick() override;
+
+private:
+ void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
+ double getGripperPosition();
+ double getGripperEffort();
+
+ BT::RosNodeParams params_;
+
+ rclcpp::Node::SharedPtr node_;
+ rclcpp::Subscription::SharedPtr joint_state_sub_;
+ sensor_msgs::msg::JointState::SharedPtr latest_joint_state_;
+ double force_threshold_ = 0.5;
+ double position_threshold_ = 0.01;
+};
+
+} // namespace grab2_behavior_tree
diff --git a/grab2_behavior_tree/package.xml b/grab2_behavior_tree/package.xml
index a9e4f45..67eaf89 100644
--- a/grab2_behavior_tree/package.xml
+++ b/grab2_behavior_tree/package.xml
@@ -24,6 +24,7 @@
grab2_msgs
behaviortree_cpp
behaviortree_ros2
+ sensor_msgs
ament_lint_auto
ament_lint_common
diff --git a/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
new file mode 100644
index 0000000..6ddc50e
--- /dev/null
+++ b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
@@ -0,0 +1,104 @@
+// Copyright 2025, Zaynab Ahmed
+
+#include "grab2_behavior_tree/plugins/condition/check_gripper_has_object.hpp"
+#include "behaviortree_cpp/condition_node.h"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+
+namespace grab2_behavior_tree
+{
+
+// Constructor definition
+CheckGripperHasObject::CheckGripperHasObject(
+ const std::string & name,
+ const BT::NodeConfig & config,
+ const BT::RosNodeParams & params)
+: BT::ConditionNode(name, config), params_(params) // Now OK
+{
+ node_ = params.nh.lock();
+ if (!node_) {
+ throw BT::RuntimeError("CheckGripperHasObject: Missing ROS node handle");
+ }
+
+ joint_state_sub_ = node_->create_subscription(
+ "/joint_states", 10,
+ std::bind(&CheckGripperHasObject::jointStateCallback, this, std::placeholders::_1)
+ );
+
+ getInput("force_threshold", force_threshold_);
+ getInput("position_threshold", position_threshold_);
+}
+
+// providedPorts
+BT::PortsList CheckGripperHasObject::providedPorts()
+{
+ return {
+ BT::InputPort("force_threshold", 0.5, "Force threshold to detect object"),
+ BT::InputPort(
+ "position_threshold", 0.01,
+ "Minimum gripper opening when holding object"),
+ BT::OutputPort("has_object", "Whether gripper has object")
+ };
+}
+
+// tick
+BT::NodeStatus CheckGripperHasObject::tick()
+{
+ if (!latest_joint_state_) {
+ RCLCPP_WARN(node_->get_logger(), "[%s] No joint state received yet", name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double gripper_position = getGripperPosition();
+ double gripper_effort = getGripperEffort();
+
+ bool has_object = (gripper_position > position_threshold_) &&
+ (gripper_effort > force_threshold_);
+
+ setOutput("has_object", has_object);
+
+ if (has_object) {
+ RCLCPP_INFO(
+ node_->get_logger(),
+ "[%s] Object detected: pos=%.3f, effort=%.3f",
+ name().c_str(), gripper_position, gripper_effort);
+ return BT::NodeStatus::SUCCESS;
+ } else {
+ RCLCPP_WARN(
+ node_->get_logger(),
+ "[%s] No object: pos=%.3f, effort=%.3f",
+ name().c_str(), gripper_position, gripper_effort);
+ return BT::NodeStatus::FAILURE;
+ }
+}
+
+// Callback
+void CheckGripperHasObject::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
+{
+ latest_joint_state_ = msg;
+}
+
+double CheckGripperHasObject::getGripperPosition()
+{
+ for (size_t i = 0; i < latest_joint_state_->name.size(); ++i) {
+ if (latest_joint_state_->name[i].find("finger") != std::string::npos) {
+ return latest_joint_state_->position[i];
+ }
+ }
+ return 0.0;
+}
+
+double CheckGripperHasObject::getGripperEffort()
+{
+ for (size_t i = 0; i < latest_joint_state_->name.size(); ++i) {
+ if (latest_joint_state_->name[i].find("finger") != std::string::npos) {
+ return std::abs(latest_joint_state_->effort[i]);
+ }
+ }
+ return 0.0;
+}
+
+} // namespace grab2_behavior_tree
+
+#include "behaviortree_ros2/plugins.hpp"
+CreateRosNodePlugin(grab2_behavior_tree::CheckGripperHasObject, "CheckGripperHasObject") // NOLINT
diff --git a/grab2_behavior_tree/src/main.cpp b/grab2_behavior_tree/src/main.cpp
index 8134dcf..4de45a1 100644
--- a/grab2_behavior_tree/src/main.cpp
+++ b/grab2_behavior_tree/src/main.cpp
@@ -48,6 +48,10 @@ int main(int argc, char ** argv)
factory,
loader.getOSName("grab2_detect_object"),
{node, "/eef_camera/bbox_3d"});
+ RegisterRosNode(
+ factory,
+ loader.getOSName("grab2_check_gripper_has_object"),
+ {node});
// SaySomething BT Node
BT::PortsList say_something_ports = {BT::InputPort("message")};
diff --git a/grab2_planner/CMakeLists.txt b/grab2_planner/CMakeLists.txt
index ac85053..29da13b 100644
--- a/grab2_planner/CMakeLists.txt
+++ b/grab2_planner/CMakeLists.txt
@@ -24,7 +24,7 @@ find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
-find_package(moveit_visual_tools REQUIRED)
+# find_package(moveit_visual_tools REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
diff --git a/grab2_planner/package.xml b/grab2_planner/package.xml
index 8d1fb88..a481c0b 100644
--- a/grab2_planner/package.xml
+++ b/grab2_planner/package.xml
@@ -23,7 +23,7 @@
moveit_msgs
moveit_ros_planning
moveit_ros_planning_interface
- moveit_visual_tools
+
moveit_configs_utils
moveit_resources_panda_moveit_config
grab2_ros_common