From 8952524ccfb1f4fd0108aed16c4b6208413fbdf4 Mon Sep 17 00:00:00 2001
From: zaynab ahmad <zozaahmad203@gmail.com>
Date: Wed, 19 Nov 2025 06:38:11 +0200
Subject: [PATCH 1/7] adding reactivity

---
 .vscode/c_cpp_properties.json                 | 18 ++++
 .vscode/launch.json                           | 24 +++++
 .vscode/settings.json                         | 60 ++++++++++++
 grab2_behavior_tree/CMakeLists.txt            | 51 +++++-----
 .../behavior_trees/collect_cubes_reactive.xml | 63 ++++++++++++
 .../behavior_trees/test_tree.xml              |  6 ++
 .../check_gripper_has_object.hpp              | 36 +++++++
 grab2_behavior_tree/package.xml               |  1 +
 .../plugins/action/compute_top_down_grasp.cpp |  4 +-
 .../plugins/check_gripper_has_object.cpp      | 98 +++++++++++++++++++
 grab2_behavior_tree/src/main.cpp              |  8 +-
 grab2_planner/CMakeLists.txt                  |  2 +-
 grab2_planner/package.xml                     |  2 +-
 topic_based_ros2_control                      |  1 +
 14 files changed, 345 insertions(+), 29 deletions(-)
 create mode 100644 .vscode/c_cpp_properties.json
 create mode 100644 .vscode/launch.json
 create mode 100644 .vscode/settings.json
 create mode 100644 grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml
 create mode 100644 grab2_behavior_tree/behavior_trees/test_tree.xml
 create mode 100644 grab2_behavior_tree/include/grab2_behavior_tree/check_gripper_has_object.hpp
 create mode 100644 grab2_behavior_tree/plugins/check_gripper_has_object.cpp
 create mode 160000 topic_based_ros2_control

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..c2098a2
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,18 @@
+{
+  "configurations": [
+    {
+      "name": "linux-gcc-x64",
+      "includePath": [
+        "${workspaceFolder}/**"
+      ],
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "${default}",
+      "cppStandard": "${default}",
+      "intelliSenseMode": "linux-gcc-x64",
+      "compilerArgs": [
+        ""
+      ]
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file
diff --git a/.vscode/launch.json b/.vscode/launch.json
new file mode 100644
index 0000000..a2eea67
--- /dev/null
+++ b/.vscode/launch.json
@@ -0,0 +1,24 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "C/C++ Runner: Debug Session",
+      "type": "cppdbg",
+      "request": "launch",
+      "args": [],
+      "stopAtEntry": false,
+      "externalConsole": false,
+      "cwd": "/home/zaynap/Projects/grab/grab2/grab2_behavior_tree/src",
+      "program": "/home/zaynap/Projects/grab/grab2/grab2_behavior_tree/src/build/Debug/outDebug",
+      "MIMode": "gdb",
+      "miDebuggerPath": "gdb",
+      "setupCommands": [
+        {
+          "description": "Enable pretty-printing for gdb",
+          "text": "-enable-pretty-printing",
+          "ignoreFailures": true
+        }
+      ]
+    }
+  ]
+}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..9abfe92
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,60 @@
+{
+  "C_Cpp_Runner.cCompilerPath": "gcc",
+  "C_Cpp_Runner.cppCompilerPath": "g++",
+  "C_Cpp_Runner.debuggerPath": "gdb",
+  "C_Cpp_Runner.cStandard": "",
+  "C_Cpp_Runner.cppStandard": "",
+  "C_Cpp_Runner.msvcBatchPath": "",
+  "C_Cpp_Runner.useMsvc": false,
+  "C_Cpp_Runner.warnings": [
+    "-Wall",
+    "-Wextra",
+    "-Wpedantic",
+    "-Wshadow",
+    "-Wformat=2",
+    "-Wcast-align",
+    "-Wconversion",
+    "-Wsign-conversion",
+    "-Wnull-dereference"
+  ],
+  "C_Cpp_Runner.msvcWarnings": [
+    "/W4",
+    "/permissive-",
+    "/w14242",
+    "/w14287",
+    "/w14296",
+    "/w14311",
+    "/w14826",
+    "/w44062",
+    "/w44242",
+    "/w14905",
+    "/w14906",
+    "/w14263",
+    "/w44265",
+    "/w14928"
+  ],
+  "C_Cpp_Runner.enableWarnings": true,
+  "C_Cpp_Runner.warningsAsError": false,
+  "C_Cpp_Runner.compilerArgs": [],
+  "C_Cpp_Runner.linkerArgs": [],
+  "C_Cpp_Runner.includePaths": [],
+  "C_Cpp_Runner.includeSearch": [
+    "*",
+    "**/*"
+  ],
+  "C_Cpp_Runner.excludeSearch": [
+    "**/build",
+    "**/build/**",
+    "**/.*",
+    "**/.*/**",
+    "**/.vscode",
+    "**/.vscode/**"
+  ],
+  "C_Cpp_Runner.useAddressSanitizer": false,
+  "C_Cpp_Runner.useUndefinedSanitizer": false,
+  "C_Cpp_Runner.useLeakSanitizer": false,
+  "C_Cpp_Runner.showCompilationTime": false,
+  "C_Cpp_Runner.useLinkTimeOptimization": false,
+  "C_Cpp_Runner.msvcSecureNoWarnings": false,
+  "ros.distro": "humble"
+}
\ No newline at end of file
diff --git a/grab2_behavior_tree/CMakeLists.txt b/grab2_behavior_tree/CMakeLists.txt
index 122c35c..e52276a 100644
--- a/grab2_behavior_tree/CMakeLists.txt
+++ b/grab2_behavior_tree/CMakeLists.txt
@@ -22,28 +22,31 @@ 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)
 
 # Libraries
-add_library(grab2_detect_object_bt_node SHARED plugins/action/detect_object.cpp)
-add_library(grab2_get_trajectory_from_yaml_bt_node SHARED plugins/action/get_trajectory_from_yaml.cpp)
-add_library(grab2_get_grasp_hardcoded_bt_node SHARED plugins/action/get_grasp_hardcoded.cpp)
-add_library(grab2_compute_top_down_grasp_bt_node SHARED plugins/action/compute_top_down_grasp.cpp)
-add_library(grab2_compute_plan_to_target_ik_bt_node SHARED plugins/action/compute_plan_to_target_ik.cpp)
-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_get_trajectory_hardcoded SHARED plugins/get_trajectory_hardcoded.cpp)
+add_library(grab2_get_grasp_hardcoded SHARED plugins/get_grasp_hardcoded.cpp)
+add_library(grab2_detect_object SHARED plugins/detect_object.cpp)
+add_library(grab2_get_grasp SHARED plugins/get_grasp.cpp)
+add_library(grab2_plan SHARED plugins/plan.cpp)
+add_library(grab2_move SHARED plugins/move.cpp)
+add_library(grab2_grip SHARED plugins/grip.cpp)
+add_library(grab2_check_gripper_has_object SHARED
+  plugins/check_gripper_has_object.cpp
+)  
 
 list(APPEND plugin_libs
-  grab2_detect_object_bt_node
-  grab2_get_trajectory_from_yaml_bt_node
-  grab2_get_grasp_hardcoded_bt_node
-  grab2_compute_top_down_grasp_bt_node
-  grab2_compute_plan_to_target_ik_bt_node
-  grab2_move_bt_node
-  grab2_grip_bt_node
-  grab2_reach_object_bt_node
+  grab2_get_trajectory_hardcoded
+  grab2_get_grasp_hardcoded
+  grab2_detect_object
+  grab2_get_grasp
+  grab2_plan
+  grab2_move
+  grab2_grip
+  grab2_check_gripper_has_object
 )
 
 foreach(bt_plugin ${plugin_libs})
@@ -91,6 +94,9 @@ target_link_libraries(grab2_engine
   behaviortree_ros2::behaviortree_ros2
 )
 
+
+
+
 # Install Libs & Exec
 install(
   TARGETS
@@ -101,14 +107,11 @@ install(
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-# Install plugins for other packages to load
-install(
-  TARGETS
-  ${plugin_libs}
-  LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins
-  ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins
-  RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins
-)
+# install(TARGETS grab2_check_gripper_has_object
+#   ARCHIVE DESTINATION lib
+#   LIBRARY DESTINATION lib
+#   RUNTIME DESTINATION bin
+# )
 
 # Install directories
 install(
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..797d46c
--- /dev/null
+++ b/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>
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 @@
+<?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>
diff --git a/grab2_behavior_tree/include/grab2_behavior_tree/check_gripper_has_object.hpp b/grab2_behavior_tree/include/grab2_behavior_tree/check_gripper_has_object.hpp
new file mode 100644
index 0000000..db8ecc7
--- /dev/null
+++ b/grab2_behavior_tree/include/grab2_behavior_tree/check_gripper_has_object.hpp
@@ -0,0 +1,36 @@
+#pragma once
+
+#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
\ No newline at end of file
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 @@
   <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>
diff --git a/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp b/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp
index 795ece4..245f0ba 100644
--- a/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp
+++ b/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp
@@ -55,5 +55,5 @@ ComputeTopDownGrasp::tick()
 
 BT_REGISTER_NODES(factory)
 {
-  factory.registerNodeType<grab2_behavior_tree::ComputeTopDownGrasp>("ComputeTopDownGrasp");
-}
+  factory.registerNodeType<grab2_behavior_tree::GetGrasp>("GetGrasp");
+}
\ No newline at end of file
diff --git a/grab2_behavior_tree/plugins/check_gripper_has_object.cpp b/grab2_behavior_tree/plugins/check_gripper_has_object.cpp
new file mode 100644
index 0000000..847dd59
--- /dev/null
+++ b/grab2_behavior_tree/plugins/check_gripper_has_object.cpp
@@ -0,0 +1,98 @@
+#include "grab2_behavior_tree/check_gripper_has_object.hpp"  // Include the header!
+#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")
\ No newline at end of file
diff --git a/grab2_behavior_tree/src/main.cpp b/grab2_behavior_tree/src/main.cpp
index 8134dcf..b1f8029 100644
--- a/grab2_behavior_tree/src/main.cpp
+++ b/grab2_behavior_tree/src/main.cpp
@@ -20,7 +20,9 @@ int main(int argc, char ** argv)
   // Register Native BT Nodes
   factory.registerFromPlugin(loader.getOSName("grab2_get_grasp"));
   factory.registerFromPlugin(loader.getOSName("grab2_get_grasp_hardcoded"));
-  factory.registerFromPlugin(loader.getOSName("grab2_get_trajectory_from_yaml"));
+  factory.registerFromPlugin(loader.getOSName("grab2_get_trajectory_hardcoded"));
+  // factory.registerFromPlugin(loader.getOSName("grab2_check_gripper_has_object"));  // added 
+
 
   // Declare Parameters
   node->declare_parameter("behavior_tree", "");
@@ -48,6 +50,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")};
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 @@
   <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>
diff --git a/topic_based_ros2_control b/topic_based_ros2_control
new file mode 160000
index 0000000..6bd8d55
--- /dev/null
+++ b/topic_based_ros2_control
@@ -0,0 +1 @@
+Subproject commit 6bd8d55e1c4ad3188770fe5c8b93b942bcede4a2

From 7cf4656b8a62c8ec21d7494cb413e1094b873e34 Mon Sep 17 00:00:00 2001
From: zaynab ahmad <zozaahmad203@gmail.com>
Date: Sun, 23 Nov 2025 22:39:22 +0200
Subject: [PATCH 2/7] remove unnecessery files

---
 .vscode/c_cpp_properties.json | 18 -----------
 .vscode/launch.json           | 24 --------------
 .vscode/settings.json         | 60 -----------------------------------
 topic_based_ros2_control      |  1 -
 4 files changed, 103 deletions(-)
 delete mode 100644 .vscode/c_cpp_properties.json
 delete mode 100644 .vscode/launch.json
 delete mode 100644 .vscode/settings.json
 delete mode 160000 topic_based_ros2_control

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
deleted file mode 100644
index c2098a2..0000000
--- a/.vscode/c_cpp_properties.json
+++ /dev/null
@@ -1,18 +0,0 @@
-{
-  "configurations": [
-    {
-      "name": "linux-gcc-x64",
-      "includePath": [
-        "${workspaceFolder}/**"
-      ],
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "${default}",
-      "cppStandard": "${default}",
-      "intelliSenseMode": "linux-gcc-x64",
-      "compilerArgs": [
-        ""
-      ]
-    }
-  ],
-  "version": 4
-}
\ No newline at end of file
diff --git a/.vscode/launch.json b/.vscode/launch.json
deleted file mode 100644
index a2eea67..0000000
--- a/.vscode/launch.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
-  "version": "0.2.0",
-  "configurations": [
-    {
-      "name": "C/C++ Runner: Debug Session",
-      "type": "cppdbg",
-      "request": "launch",
-      "args": [],
-      "stopAtEntry": false,
-      "externalConsole": false,
-      "cwd": "/home/zaynap/Projects/grab/grab2/grab2_behavior_tree/src",
-      "program": "/home/zaynap/Projects/grab/grab2/grab2_behavior_tree/src/build/Debug/outDebug",
-      "MIMode": "gdb",
-      "miDebuggerPath": "gdb",
-      "setupCommands": [
-        {
-          "description": "Enable pretty-printing for gdb",
-          "text": "-enable-pretty-printing",
-          "ignoreFailures": true
-        }
-      ]
-    }
-  ]
-}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
deleted file mode 100644
index 9abfe92..0000000
--- a/.vscode/settings.json
+++ /dev/null
@@ -1,60 +0,0 @@
-{
-  "C_Cpp_Runner.cCompilerPath": "gcc",
-  "C_Cpp_Runner.cppCompilerPath": "g++",
-  "C_Cpp_Runner.debuggerPath": "gdb",
-  "C_Cpp_Runner.cStandard": "",
-  "C_Cpp_Runner.cppStandard": "",
-  "C_Cpp_Runner.msvcBatchPath": "",
-  "C_Cpp_Runner.useMsvc": false,
-  "C_Cpp_Runner.warnings": [
-    "-Wall",
-    "-Wextra",
-    "-Wpedantic",
-    "-Wshadow",
-    "-Wformat=2",
-    "-Wcast-align",
-    "-Wconversion",
-    "-Wsign-conversion",
-    "-Wnull-dereference"
-  ],
-  "C_Cpp_Runner.msvcWarnings": [
-    "/W4",
-    "/permissive-",
-    "/w14242",
-    "/w14287",
-    "/w14296",
-    "/w14311",
-    "/w14826",
-    "/w44062",
-    "/w44242",
-    "/w14905",
-    "/w14906",
-    "/w14263",
-    "/w44265",
-    "/w14928"
-  ],
-  "C_Cpp_Runner.enableWarnings": true,
-  "C_Cpp_Runner.warningsAsError": false,
-  "C_Cpp_Runner.compilerArgs": [],
-  "C_Cpp_Runner.linkerArgs": [],
-  "C_Cpp_Runner.includePaths": [],
-  "C_Cpp_Runner.includeSearch": [
-    "*",
-    "**/*"
-  ],
-  "C_Cpp_Runner.excludeSearch": [
-    "**/build",
-    "**/build/**",
-    "**/.*",
-    "**/.*/**",
-    "**/.vscode",
-    "**/.vscode/**"
-  ],
-  "C_Cpp_Runner.useAddressSanitizer": false,
-  "C_Cpp_Runner.useUndefinedSanitizer": false,
-  "C_Cpp_Runner.useLeakSanitizer": false,
-  "C_Cpp_Runner.showCompilationTime": false,
-  "C_Cpp_Runner.useLinkTimeOptimization": false,
-  "C_Cpp_Runner.msvcSecureNoWarnings": false,
-  "ros.distro": "humble"
-}
\ No newline at end of file
diff --git a/topic_based_ros2_control b/topic_based_ros2_control
deleted file mode 160000
index 6bd8d55..0000000
--- a/topic_based_ros2_control
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 6bd8d55e1c4ad3188770fe5c8b93b942bcede4a2

From ee3e223c95117d02f58435ab4c4c201afef9a83e Mon Sep 17 00:00:00 2001
From: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
Date: Thu, 27 Nov 2025 14:12:49 +0200
Subject: [PATCH 3/7] Rebase CMakeLists

---
 grab2_behavior_tree/CMakeLists.txt            | 52 ++++++++++---------
 .../plugins/action/compute_top_down_grasp.cpp |  4 +-
 grab2_behavior_tree/src/main.cpp              |  4 +-
 3 files changed, 31 insertions(+), 29 deletions(-)

diff --git a/grab2_behavior_tree/CMakeLists.txt b/grab2_behavior_tree/CMakeLists.txt
index e52276a..0f00f89 100644
--- a/grab2_behavior_tree/CMakeLists.txt
+++ b/grab2_behavior_tree/CMakeLists.txt
@@ -27,26 +27,28 @@ find_package(sensor_msgs REQUIRED)
 set(CMAKE_CXX_STANDARD 17)
 
 # Libraries
-add_library(grab2_get_trajectory_hardcoded SHARED plugins/get_trajectory_hardcoded.cpp)
-add_library(grab2_get_grasp_hardcoded SHARED plugins/get_grasp_hardcoded.cpp)
-add_library(grab2_detect_object SHARED plugins/detect_object.cpp)
-add_library(grab2_get_grasp SHARED plugins/get_grasp.cpp)
-add_library(grab2_plan SHARED plugins/plan.cpp)
-add_library(grab2_move SHARED plugins/move.cpp)
-add_library(grab2_grip SHARED plugins/grip.cpp)
-add_library(grab2_check_gripper_has_object SHARED
+add_library(grab2_detect_object_bt_node SHARED plugins/action/detect_object.cpp)
+add_library(grab2_get_trajectory_from_yaml_bt_node SHARED plugins/action/get_trajectory_from_yaml.cpp)
+add_library(grab2_get_grasp_hardcoded_bt_node SHARED plugins/action/get_grasp_hardcoded.cpp)
+add_library(grab2_compute_top_down_grasp_bt_node SHARED plugins/action/compute_top_down_grasp.cpp)
+add_library(grab2_compute_plan_to_target_ik_bt_node SHARED plugins/action/compute_plan_to_target_ik.cpp)
+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/check_gripper_has_object.cpp
 )  
 
 list(APPEND plugin_libs
-  grab2_get_trajectory_hardcoded
-  grab2_get_grasp_hardcoded
-  grab2_detect_object
-  grab2_get_grasp
-  grab2_plan
-  grab2_move
-  grab2_grip
-  grab2_check_gripper_has_object
+  grab2_detect_object_bt_node
+  grab2_get_trajectory_from_yaml_bt_node
+  grab2_get_grasp_hardcoded_bt_node
+  grab2_compute_top_down_grasp_bt_node
+  grab2_compute_plan_to_target_ik_bt_node
+  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})
@@ -68,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
   )
@@ -90,13 +93,11 @@ 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
 )
 
-
-
-
 # Install Libs & Exec
 install(
   TARGETS
@@ -107,11 +108,14 @@ install(
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-# install(TARGETS grab2_check_gripper_has_object
-#   ARCHIVE DESTINATION lib
-#   LIBRARY DESTINATION lib
-#   RUNTIME DESTINATION bin
-# )
+# Install plugins for other packages to load
+install(
+  TARGETS
+  ${plugin_libs}
+  LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins
+  ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins
+  RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins
+)
 
 # Install directories
 install(
diff --git a/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp b/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp
index 245f0ba..795ece4 100644
--- a/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp
+++ b/grab2_behavior_tree/plugins/action/compute_top_down_grasp.cpp
@@ -55,5 +55,5 @@ ComputeTopDownGrasp::tick()
 
 BT_REGISTER_NODES(factory)
 {
-  factory.registerNodeType<grab2_behavior_tree::GetGrasp>("GetGrasp");
-}
\ No newline at end of file
+  factory.registerNodeType<grab2_behavior_tree::ComputeTopDownGrasp>("ComputeTopDownGrasp");
+}
diff --git a/grab2_behavior_tree/src/main.cpp b/grab2_behavior_tree/src/main.cpp
index b1f8029..4de45a1 100644
--- a/grab2_behavior_tree/src/main.cpp
+++ b/grab2_behavior_tree/src/main.cpp
@@ -20,9 +20,7 @@ int main(int argc, char ** argv)
   // Register Native BT Nodes
   factory.registerFromPlugin(loader.getOSName("grab2_get_grasp"));
   factory.registerFromPlugin(loader.getOSName("grab2_get_grasp_hardcoded"));
-  factory.registerFromPlugin(loader.getOSName("grab2_get_trajectory_hardcoded"));
-  // factory.registerFromPlugin(loader.getOSName("grab2_check_gripper_has_object"));  // added 
-
+  factory.registerFromPlugin(loader.getOSName("grab2_get_trajectory_from_yaml"));
 
   // Declare Parameters
   node->declare_parameter("behavior_tree", "");

From f080e0a8aabeb7f7f139a9085f23cdf468244247 Mon Sep 17 00:00:00 2001
From: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
Date: Thu, 27 Nov 2025 14:16:50 +0200
Subject: [PATCH 4/7] Restructure condition plugin

---
 grab2_behavior_tree/CMakeLists.txt                              | 2 +-
 .../{ => plugins/condition}/check_gripper_has_object.hpp        | 0
 .../plugins/{ => condition}/check_gripper_has_object.cpp        | 2 +-
 3 files changed, 2 insertions(+), 2 deletions(-)
 rename grab2_behavior_tree/include/grab2_behavior_tree/{ => plugins/condition}/check_gripper_has_object.hpp (100%)
 rename grab2_behavior_tree/plugins/{ => condition}/check_gripper_has_object.cpp (96%)

diff --git a/grab2_behavior_tree/CMakeLists.txt b/grab2_behavior_tree/CMakeLists.txt
index 0f00f89..89fb731 100644
--- a/grab2_behavior_tree/CMakeLists.txt
+++ b/grab2_behavior_tree/CMakeLists.txt
@@ -36,7 +36,7 @@ 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/check_gripper_has_object.cpp
+  plugins/condition/check_gripper_has_object.cpp
 )  
 
 list(APPEND plugin_libs
diff --git a/grab2_behavior_tree/include/grab2_behavior_tree/check_gripper_has_object.hpp b/grab2_behavior_tree/include/grab2_behavior_tree/plugins/condition/check_gripper_has_object.hpp
similarity index 100%
rename from grab2_behavior_tree/include/grab2_behavior_tree/check_gripper_has_object.hpp
rename to grab2_behavior_tree/include/grab2_behavior_tree/plugins/condition/check_gripper_has_object.hpp
diff --git a/grab2_behavior_tree/plugins/check_gripper_has_object.cpp b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
similarity index 96%
rename from grab2_behavior_tree/plugins/check_gripper_has_object.cpp
rename to grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
index 847dd59..90c61c2 100644
--- a/grab2_behavior_tree/plugins/check_gripper_has_object.cpp
+++ b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
@@ -1,4 +1,4 @@
-#include "grab2_behavior_tree/check_gripper_has_object.hpp"  // Include the header!
+#include "grab2_behavior_tree/condition/check_gripper_has_object.hpp"  // Include the header!
 #include "behaviortree_cpp/condition_node.h"
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"

From 616bb50bd441303e965ceed2b02835d92a49cbe6 Mon Sep 17 00:00:00 2001
From: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
Date: Thu, 27 Nov 2025 14:21:31 +0200
Subject: [PATCH 5/7] make linters happy

---
 grab2_behavior_tree/CMakeLists.txt            |  2 +-
 .../behavior_trees/collect_cubes_reactive.xml |  4 +--
 .../condition/check_gripper_has_object.hpp    | 14 +++++----
 .../condition/check_gripper_has_object.cpp    | 29 ++++++++++---------
 4 files changed, 28 insertions(+), 21 deletions(-)

diff --git a/grab2_behavior_tree/CMakeLists.txt b/grab2_behavior_tree/CMakeLists.txt
index 89fb731..3670da1 100644
--- a/grab2_behavior_tree/CMakeLists.txt
+++ b/grab2_behavior_tree/CMakeLists.txt
@@ -37,7 +37,7 @@ 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
diff --git a/grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml b/grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml
index 797d46c..6f4be59 100644
--- a/grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml
+++ b/grab2_behavior_tree/behavior_trees/collect_cubes_reactive.xml
@@ -16,7 +16,7 @@
 
       <SubTree ID="Pick"/>
 
-      <CheckGripperHasObject 
+      <CheckGripperHasObject
           name="Check After Pick"
           force_threshold="0.5"
           position_threshold="0.01"
@@ -24,7 +24,7 @@
 
       <SubTree ID="MoveTo" name="Retract Pose" target="{retract_pose}"/>
 
-      <CheckGripperHasObject 
+      <CheckGripperHasObject
           name="Check Before Drop"
           force_threshold="0.5"
           position_threshold="0.01"
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
index db8ecc7..95ebd42 100644
--- 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
@@ -1,5 +1,9 @@
+// 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"
@@ -12,9 +16,9 @@ class CheckGripperHasObject : public BT::ConditionNode
 {
 public:
   CheckGripperHasObject(
-    const std::string& name,
-    const BT::NodeConfig& config,
-    const BT::RosNodeParams& params);
+    const std::string & name,
+    const BT::NodeConfig & config,
+    const BT::RosNodeParams & params);
 
   static BT::PortsList providedPorts();
   BT::NodeStatus tick() override;
@@ -24,7 +28,7 @@ class CheckGripperHasObject : public BT::ConditionNode
   double getGripperPosition();
   double getGripperEffort();
 
-  BT::RosNodeParams params_; 
+  BT::RosNodeParams params_;
 
   rclcpp::Node::SharedPtr node_;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
@@ -33,4 +37,4 @@ class CheckGripperHasObject : public BT::ConditionNode
   double position_threshold_ = 0.01;
 };
 
-}  // namespace grab2_behavior_tree
\ No newline at end of file
+}  // namespace grab2_behavior_tree
diff --git a/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
index 90c61c2..441db7c 100644
--- a/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
+++ b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
@@ -1,3 +1,5 @@
+// Copyright 2025, Zaynab Ahmed
+
 #include "grab2_behavior_tree/condition/check_gripper_has_object.hpp"  // Include the header!
 #include "behaviortree_cpp/condition_node.h"
 #include "rclcpp/rclcpp.hpp"
@@ -8,10 +10,10 @@ 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
+  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_) {
@@ -32,7 +34,8 @@ 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::InputPort<double>("position_threshold", 0.01,
+        "Minimum gripper opening when holding object"),
     BT::OutputPort<bool>("has_object", "Whether gripper has object")
   };
 }
@@ -48,19 +51,19 @@ BT::NodeStatus CheckGripperHasObject::tick()
   double gripper_position = getGripperPosition();
   double gripper_effort = getGripperEffort();
 
-  bool has_object = (gripper_position > position_threshold_) && 
-                    (gripper_effort > force_threshold_);
+  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", 
+    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", 
+    RCLCPP_WARN(node_->get_logger(),
+                "[%s] No object: pos=%.3f, effort=%.3f",
                 name().c_str(), gripper_position, gripper_effort);
     return BT::NodeStatus::FAILURE;
   }
@@ -92,7 +95,7 @@ double CheckGripperHasObject::getGripperEffort()
   return 0.0;
 }
 
-} // namespace grab2_behavior_tree
+}  // namespace grab2_behavior_tree
 
 #include "behaviortree_ros2/plugins.hpp"
-CreateRosNodePlugin(grab2_behavior_tree::CheckGripperHasObject, "CheckGripperHasObject")
\ No newline at end of file
+CreateRosNodePlugin(grab2_behavior_tree::CheckGripperHasObject, "CheckGripperHasObject")  // NOLINT

From 1a6dddb3fc1f4e080d7b22325704f712e3ca8b0a Mon Sep 17 00:00:00 2001
From: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
Date: Thu, 27 Nov 2025 14:30:53 +0200
Subject: [PATCH 6/7] fix include path for CheckGripperHasObject header

---
 .../plugins/condition/check_gripper_has_object.cpp              | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
index 441db7c..c6a4f7c 100644
--- a/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
+++ b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
@@ -1,6 +1,6 @@
 // Copyright 2025, Zaynab Ahmed
 
-#include "grab2_behavior_tree/condition/check_gripper_has_object.hpp"  // Include the header!
+#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"

From eeb59d410eb51475e4540bdcf5a318e73df8a7d6 Mon Sep 17 00:00:00 2001
From: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
Date: Thu, 27 Nov 2025 14:42:25 +0200
Subject: [PATCH 7/7] Refactor logging and formatting in CheckGripperHasObject
 class

---
 .../condition/check_gripper_has_object.cpp    | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
index c6a4f7c..6ddc50e 100644
--- a/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
+++ b/grab2_behavior_tree/plugins/condition/check_gripper_has_object.cpp
@@ -34,8 +34,9 @@ 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::InputPort<double>(
+      "position_threshold", 0.01,
+      "Minimum gripper opening when holding object"),
     BT::OutputPort<bool>("has_object", "Whether gripper has object")
   };
 }
@@ -57,14 +58,16 @@ BT::NodeStatus CheckGripperHasObject::tick()
   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);
+    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);
+    RCLCPP_WARN(
+      node_->get_logger(),
+      "[%s] No object: pos=%.3f, effort=%.3f",
+      name().c_str(), gripper_position, gripper_effort);
     return BT::NodeStatus::FAILURE;
   }
 }