2d detection plugin addded

grab2_behavior_tree/CMakeLists.txt

@@ -34,6 +34,7 @@ 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_detection_2d_marker_bt_node SHARED plugins/action/detection_2d_marker.cpp)
 
 list(APPEND plugin_libs
   grab2_detect_object_bt_node
@@ -44,6 +45,7 @@ list(APPEND plugin_libs
   grab2_move_bt_node
   grab2_grip_bt_node
   grab2_reach_object_bt_node
+  grab2_detection_2d_marker_bt_node
 )
 
 foreach(bt_plugin ${plugin_libs})

grab2_behavior_tree/include/grab2_behavior_tree/plugins/action/detection_2d_marker.hpp

@@ -0,0 +1,39 @@
+// Copyright (c) 2026, Zaynap Ahmad
+
+#ifndef GRAB2_BEHAVIOR_TREE__PLUGINS__ACTION__DETECTION_2D_MARKER_HPP_
+#define GRAB2_BEHAVIOR_TREE__PLUGINS__ACTION__DETECTION_2D_MARKER_HPP_
+
+#include <string>
+#include <memory>
+#include "behaviortree_ros2/bt_topic_sub_node.hpp"
+#include "vision_msgs/msg/detection2_d_array.hpp"
+#include "geometry_msgs/msg/point.hpp"
+
+namespace grab2_behavior_tree
+{
+
+class Detect2DMarker
+  : public BT::RosTopicSubNode<vision_msgs::msg::Detection2DArray>
+{
+public:
+  Detect2DMarker(
+    const std::string & name,
+    const BT::NodeConfig & conf,
+    const BT::RosNodeParams & params);
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::OutputPort<std::string>("marker_id"),
+      BT::OutputPort<geometry_msgs::msg::Point>("center"),
+      BT::OutputPort<std::string>("frame_id")
+    };
+  }
+
+  BT::NodeStatus onTick(
+    const std::shared_ptr<vision_msgs::msg::Detection2DArray> & last_msg) override;
+};
+
+}  // namespace grab2_behavior_tree
+
+#endif  // GRAB2_BEHAVIOR_TREE__PLUGINS__ACTION__DETECTION_2D_MARKER_HPP_

grab2_behavior_tree/plugins/action/detection_2d_marker.cpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2026, Zaynap Ahmad
+#include <cmath>
+#include <limits>
+
+#include "grab2_behavior_tree/plugins/action/detection_2d_marker.hpp"
+
+namespace grab2_behavior_tree
+{
+
+Detect2DMarker::Detect2DMarker(
+  const std::string & name,
+  const BT::NodeConfig & conf,
+  const BT::RosNodeParams & params)
+: BT::RosTopicSubNode<vision_msgs::msg::Detection2DArray>(name, conf, params) {}
+BT::NodeStatus Detect2DMarker::onTick(
+  const std::shared_ptr<vision_msgs::msg::Detection2DArray> & last_msg)
+{
+  if (!last_msg || last_msg->detections.empty()) {
+    RCLCPP_WARN(logger(), "[%s] No 2D markers detected", name().c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  double min_dist = std::numeric_limits<double>::max();
+  vision_msgs::msg::Detection2D nearest;
+
+  for (const auto & det : last_msg->detections) {
+    double x = det.bbox.center.position.x;
+    double y = det.bbox.center.position.y;
+
+    double dist = std::sqrt(x * x + y * y);
+
+    if (dist < min_dist) {
+      min_dist = dist;
+      nearest = det;
+    }
+  }
+
+  geometry_msgs::msg::Point center;
+  center.x = nearest.bbox.center.position.x;
+  center.y = nearest.bbox.center.position.y;
+  center.z = 0.0;
+  std::string marker_id = nearest.results[0].hypothesis.class_id;
+
+  setOutput("marker_id", marker_id);
+  setOutput("center", center);
+  setOutput("frame_id", last_msg->header.frame_id);
+
+  RCLCPP_INFO(
+    logger(), "[%s] Nearest marker id: %s",
+    name().c_str(), marker_id.c_str());
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace grab2_behavior_tree
+
+
+#include "behaviortree_ros2/plugins.hpp"
+CreateRosNodePlugin(grab2_behavior_tree::Detect2DMarker, "Detect2DMarker") // NOLINT

grab2_behavior_tree/src/main.cpp

@@ -48,6 +48,11 @@ int main(int argc, char ** argv)
     factory,
     loader.getOSName("grab2_detect_object"),
     {node, "/eef_camera/bbox_3d"});
+  RegisterRosNode(
+    factory,
+    loader.getOSName("grab2_detect_2d_marker"),
+    {node, "/detection_2d_array"});
+
 
   // SaySomething BT Node
   BT::PortsList say_something_ports = {BT::InputPort<std::string>("message")};