isDetected updated

bt_nodes/perception/include/perception/IsDetected.hpp

@@ -34,6 +34,8 @@
 #include "perception_system_interfaces/msg/detection_array.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "cv_bridge/cv_bridge.h"
 
 namespace perception
 {
@@ -58,12 +60,14 @@ class IsDetected : public BT::ConditionNode
         BT::InputPort<std::string>("color", "unknown", "color"),
         BT::InputPort<std::string>("gesture", "unknown", "gesture"),
         BT::InputPort<std::string>("pose", "unknown", "pose"),
+        BT::InputPort<bool>("pub_bb_img"),
 
         BT::OutputPort<std::vector<std::string>>("frames"),
         BT::OutputPort<std::string>("best_detection")});
   }
 
 private:
+  void image_callback(const sensor_msg::msg::Image::SharedPtr msg);
   std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
 
@@ -83,6 +87,12 @@ class IsDetected : public BT::ConditionNode
   std::map<std::string, cv::Scalar> colors_;
   std::map<std::string, std::vector<int>> gestures_;
   std::map<int, std::string> pose_names_;
+
+  bool pub_bb_img_{false};
+  rclcpp::Publisher<sensor_msg::msg::Image>::SharedPtr bb_img_pub_;
+  rclcpp::Subscription<sensor_msg::msg::Image>::SharedPtr img_sub_;
+
+  cv::Mat last_image_;
 };
 
 }  // namespace perception

bt_nodes/perception/src/perception/IsDetected.cpp

@@ -66,6 +66,19 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura
   getInput("order", order_);
   getInput("max_depth", max_depth_);
   getInput("person_id", person_id_);
+  getInput("pub_bb_img", pub_bb_img_);
+
+  if (pub_bb_img_) {
+    pub_bb_img_ = node_->create_publisher<sensor_msgs::msg::Image>(
+      "/bb_img_best_detection", 10);
+    img_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
+      "/camera/color/image_raw", 10,
+      std::bind(&IsDetected::image_callback, this, _1));
+  } else {
+    pub_bb_img_ = nullptr;
+    img_sub_ = nullptr;
+  }
+
 }
 
 BT::NodeStatus IsDetected::tick()
@@ -96,7 +109,7 @@ BT::NodeStatus IsDetected::tick()
     return BT::NodeStatus::FAILURE;
   }
 
-  RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Processing %d detections...", detections.size());
+  RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Processing %ld detections...", detections.size());
 
   if (order_ == "color") {
     // sorted by the distance to the color person we should sort it by distance and also by left to right or right to left
@@ -220,12 +233,26 @@ BT::NodeStatus IsDetected::tick()
     RCLCPP_INFO(node_->get_logger(), "[IsDetected] %d detections after filter", frames_.size());
   }
 
-  // auto pub = node_->create_publisher<sensor_msgs::msg::Image>(
-  //   "/object_detected", 10);
+
 
   // pub->publish(detections[0].image);
 
   setOutput("best_detection", detections[0].class_name);
+
+  if (pub_bb_img_) {
+    cv::Point center2d(
+      detections[0].center2d.x, detections[0].center2d.y);
+
+    // cv::circle(last_image_, center2d, 5, cv::Scalar(0, 0, 255), -1);
+
+    cv::putText(
+      last_image_, "X", center2d, cv::FONT_HERSHEY_SIMPLEX, 1,
+      cv::Scalar(0, 0, 255), 2);
+
+    auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", last_image_).toImageMsg();
+    pub_bb_img_->publish(*msg);
+  }
+
   RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections sorted");
   // implement more sorting methods
 
@@ -238,6 +265,17 @@ BT::NodeStatus IsDetected::tick()
   return BT::NodeStatus::SUCCESS;
 }
 
+void IsDetected::image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
+{
+  cv_bridge::CvImagePtr image_rgb_ptr;
+  try {
+    image_rgb_ptr = cv_bridge::toCvCopy(msg->source_img, sensor_msgs::image_encodings::BGR8);
+  } catch (cv_bridge::Exception & e) {
+    RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what());
+    return;
+  }
+  last_image_ = image_rgb_ptr->image;
+
 }  // namespace perception
 
 BT_REGISTER_NODES(factory) {

bt_nodes/perception/src/perception/count_people.cpp

@@ -89,7 +89,7 @@ BT::NodeStatus CountPeople::tick()
     return BT::NodeStatus::SUCCESS;
   }
 
-  RCLCPP_INFO(node_->get_logger(), "[CountPeople] Processing %d detections...", detections.size());
+  RCLCPP_INFO(node_->get_logger(), "[CountPeople] Processing %ld detections...", detections.size());
   auto entity_counter = 0;
   for (auto it = detections.begin(); it != detections.end() && entity_counter < max_entities_; ) {
     auto const & detection = *it;