adding reactivity

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
 )

grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml

@@ -0,0 +1,63 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <SubTree ID="PickAndDrop" name="cube1"/>
+      <SubTree ID="PickAndDrop" name="cube0"/>
+      <SubTree ID="PickAndDrop" name="cube2"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="PickAndDrop">
+    <Sequence>
+      <Script code="retract_pose:='-0.34;-0.3;0.6;1.0;0.0;0.0;0.0' " />
+      <SubTree ID="MoveTo" name="Retract Pose" target="{retract_pose}"/>
+
+      <SubTree ID="Pick"/>
+
+      <CheckGripperHasObject
+          name="Check After Pick"
+          force_threshold="0.5"
+          position_threshold="0.01"
+          has_object="{has_object_after_pick}"/>
+
+      <SubTree ID="MoveTo" name="Retract Pose" target="{retract_pose}"/>
+
+      <CheckGripperHasObject
+          name="Check Before Drop"
+          force_threshold="0.5"
+          position_threshold="0.01"
+          has_object="{has_object_before_drop}"/>
+
+      <SubTree ID="Drop"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="Pick">
+    <Sequence>
+      <GetGraspHardcoded pregrasp="{pregrasp_pose}" grasp="{grasp_pose}"/>
+      <SubTree ID="MoveTo" target="{pregrasp_pose}"/>
+      <Grip pose="0.04"/>
+      <SubTree ID="MoveTo" target="{grasp_pose}"/>
+      <Grip pose="0.02"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="Drop">
+    <Sequence>
+      <SubTree ID="MoveTo" name="Pre-Drop Pose" target="0.34;-0.3;0.6;1.0;0.0;0.0;0.0"/>
+      <SubTree ID="MoveTo" name="Drop Pose" target="0.34;-0.3;0.4;1.0;0.0;0.0;0.0"/>
+      <Grip pose="0.04"/>
+      <SubTree ID="MoveTo" name="Post-Drop Pose" target="0.34;-0.3;0.6;1.0;0.0;0.0;0.0"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="MoveTo">
+    <Sequence>
+      <Plan target_ik="{target}" trajectory="{trajectory_msg}"/>
+      <Move trajectory="{trajectory_msg}"/>
+    </Sequence>
+  </BehaviorTree>
+
+</root>

grab2_behavior_tree/behavior_trees/test_tree.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <CheckGripperHasObject force_threshold="0.5" position_threshold="0.01" has_object="{object_grasped}" />
+  </BehaviorTree>
+</root>

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 <string>
+
+#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<sensor_msgs::msg::JointState>::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

grab2_behavior_tree/package.xml

@@ -24,6 +24,7 @@
   <depend>grab2_msgs</depend>
   <depend>behaviortree_cpp</depend>
   <depend>behaviortree_ros2</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

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<sensor_msgs::msg::JointState>(
+    "/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<double>("force_threshold", 0.5, "Force threshold to detect object"),
+    BT::InputPort<double>(
+      "position_threshold", 0.01,
+      "Minimum gripper opening when holding object"),
+    BT::OutputPort<bool>("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

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<std::string>("message")};

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)
 

grab2_planner/package.xml

@@ -23,7 +23,7 @@
   <depend>moveit_msgs</depend>
   <depend>moveit_ros_planning</depend>
   <depend>moveit_ros_planning_interface</depend>
-  <depend>moveit_visual_tools</depend>
+  <!-- <depend>moveit_visual_tools</depend> -->
   <depend>moveit_configs_utils</depend>
   <depend>moveit_resources_panda_moveit_config</depend>
   <depend>grab2_ros_common</depend>