init commit

Signed-off-by: ismetatabay <ismet@leodrive.ai>
autowarefoundation · Jul 11, 2023 · e9ed75f · e9ed75f
1 parent 093f840
commit e9ed75f
Show file tree

Hide file tree

Showing 5 changed files with 60 additions and 35 deletions.
diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp
@@ -29,6 +29,7 @@
 
 #include <memory>
 #include <vector>
+#include <string>
 
 namespace image_projection_based_fusion
 {
@@ -39,7 +40,8 @@ class Debugger
 {
 public:
   explicit Debugger(
-    rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size);
+    rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size,
+    std::vector<std::string> input_camera_topics);
 
   void publishImage(const std::size_t image_id, const rclcpp::Time & stamp);
 
@@ -55,6 +57,7 @@ class Debugger
 
   rclcpp::Node * node_ptr_;
   std::shared_ptr<image_transport::ImageTransport> image_transport_;
+  std::vector<std::string> input_camera_topics_;
   std::vector<image_transport::Subscriber> image_subs_;
   std::vector<image_transport::Publisher> image_pubs_;
   std::vector<boost::circular_buffer<sensor_msgs::msg::Image::ConstSharedPtr>> image_buffers_;

diff --git a/...ption/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/...ption/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp
@@ -100,13 +100,15 @@ class FusionNode : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_;
   double timeout_ms_{};
   double match_threshold_ms_{};
+  std::vector<std::string> input_rois_topics_;
+  std::vector<std::string> input_camera_info_topics_;
+  std::vector<std::string> input_camera_topics_;
 
   /** \brief A vector of subscriber. */
   typename rclcpp::Subscription<Msg>::SharedPtr sub_;
   std::vector<rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr> rois_subs_;
 
-  /** \brief Input point cloud topics. */
-  std::vector<std::string> input_topics_;
+  // offsets between cameras and the lidars
   std::vector<double> input_offset_ms_;
 
   // cache for fusion

diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
@@ -44,32 +44,6 @@
   <arg name="input/image6" default="/image_raw6"/>
   <arg name="input/image7" default="/image_raw7"/>
   <group>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/rois0" to="$(var input/rois0)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/camera_info0" to="$(var input/camera_info0)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/rois2" to="$(var input/rois2)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/camera_info2" to="$(var input/camera_info2)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/rois3" to="$(var input/rois3)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/camera_info3" to="$(var input/camera_info3)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/rois4" to="$(var input/rois4)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/camera_info4" to="$(var input/camera_info4)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/rois5" to="$(var input/rois5)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/camera_info5" to="$(var input/camera_info5)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/rois6" to="$(var input/rois6)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/camera_info6" to="$(var input/camera_info6)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/rois7" to="$(var input/rois7)"/>
-    <set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/camera_info7" to="$(var input/camera_info7)"/>
-
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '1' &quot;)" from="input/image_raw0" to="$(var input/image0)"/>
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '2' &quot;)" from="input/image_raw1" to="$(var input/image1)"/>
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '3' &quot;)" from="input/image_raw2" to="$(var input/image2)"/>
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '4' &quot;)" from="input/image_raw3" to="$(var input/image3)"/>
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '5' &quot;)" from="input/image_raw4" to="$(var input/image4)"/>
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '6' &quot;)" from="input/image_raw5" to="$(var input/image5)"/>
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '7' &quot;)" from="input/image_raw6" to="$(var input/image6)"/>
-    <set_remap if="$(eval &quot; '$(var input_rois_number)' >= '8' &quot;)" from="input/image_raw7" to="$(var input/image7)"/>
-
     <node pkg="image_projection_based_fusion" exec="roi_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
       <param name="use_iou" value="true"/>
       <param name="use_iou_x" value="false"/>
@@ -84,6 +58,32 @@
       <remap from="input" to="$(var input/clusters)"/>
       <remap from="output" to="$(var output/clusters)"/>
 
+      <!-- rois, camera and info -->
+      <param name="input/rois0" value="$(var input/rois0)"/>
+      <param name="input/camera_info0" value="$(var input/camera_info0)"/>
+      <param name="input/image0" value="$(var input/image0)"/>
+      <param name="input/rois1" value="$(var input/rois1)"/>
+      <param name="input/camera_info1" value="$(var input/camera_info1)"/>
+      <param name="input/image1" value="$(var input/image1)"/>
+      <param name="input/rois2" value="$(var input/rois2)"/>
+      <param name="input/camera_info2" value="$(var input/camera_info2)"/>
+      <param name="input/image2" value="$(var input/image2)"/>
+      <param name="input/rois3" value="$(var input/rois3)"/>
+      <param name="input/camera_info3" value="$(var input/camera_info3)"/>
+      <param name="input/image3" value="$(var input/image3)"/>
+      <param name="input/rois4" value="$(var input/rois4)"/>
+      <param name="input/camera_info4" value="$(var input/camera_info4)"/>
+      <param name="input/image4" value="$(var input/image4)"/>
+      <param name="input/rois5" value="$(var input/rois5)"/>
+      <param name="input/camera_info5" value="$(var input/camera_info5)"/>
+      <param name="input/image5" value="$(var input/image5)"/>
+      <param name="input/rois6" value="$(var input/rois6)"/>
+      <param name="input/camera_info6" value="$(var input/camera_info6)"/>
+      <param name="input/image6" value="$(var input/image6)"/>
+      <param name="input/rois7" value="$(var input/rois7)"/>
+      <param name="input/camera_info7" value="$(var input/camera_info7)"/>
+      <param name="input/image7" value="$(var input/image7)"/>
+
       <!-- debug -->
       <param name="debug_mode" value="$(var debug_mode)"/>
       <param name="filter_scope_minx" value="$(var filter_scope_minx)"/>

diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp
@@ -39,14 +39,15 @@ void drawRoiOnImage(
 namespace image_projection_based_fusion
 {
 Debugger::Debugger(
-  rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size)
-: node_ptr_(node_ptr)
+  rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size,
+  std::vector<std::string> input_camera_topics)
+: node_ptr_(node_ptr), input_camera_topics_{input_camera_topics}
 {
   image_buffers_.resize(image_num);
   image_buffer_size_ = image_buffer_size;
   for (std::size_t img_i = 0; img_i < image_num; ++img_i) {
     auto sub = image_transport::create_subscription(
-      node_ptr, "input/image_raw" + std::to_string(img_i),
+      node_ptr, input_camera_topics.at(img_i),
       std::bind(&Debugger::imageCallback, this, std::placeholders::_1, img_i), "raw",
       rmw_qos_profile_sensor_data);
     image_subs_.push_back(sub);

diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp
@@ -60,6 +60,24 @@ FusionNode<Msg, ObjType>::FusionNode(
   match_threshold_ms_ = declare_parameter<double>("match_threshold_ms");
   timeout_ms_ = declare_parameter<double>("timeout_ms");
 
+  input_rois_topics_.resize(rois_number_);
+  input_camera_topics_.resize(rois_number_);
+  input_camera_info_topics_.resize(rois_number_);
+
+  for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) {
+    input_rois_topics_.at(roi_i) = declare_parameter<std::string>(
+      "input/rois" + std::to_string(roi_i),
+      "/perception/object_recognition/detection/rois" + std::to_string(roi_i));
+
+    input_camera_info_topics_.at(roi_i) = declare_parameter<std::string>(
+      "input/camera_info" + std::to_string(roi_i),
+      "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info");
+
+    input_camera_topics_.at(roi_i) = declare_parameter<std::string>(
+      "input/image" + std::to_string(roi_i),
+      "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color");
+  }
+
   input_offset_ms_ = declare_parameter("input_offset_ms", std::vector<double>{});
   if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) {
     throw std::runtime_error("The number of offsets does not match the number of topics.");
@@ -71,7 +89,7 @@ FusionNode<Msg, ObjType>::FusionNode(
     std::function<void(const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg)> fnc =
       std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i);
     camera_info_subs_.at(roi_i) = this->create_subscription<sensor_msgs::msg::CameraInfo>(
-      "input/camera_info" + std::to_string(roi_i), rclcpp::QoS{1}.best_effort(), fnc);
+      input_camera_info_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc);
   }
 
   // sub rois
@@ -82,7 +100,7 @@ FusionNode<Msg, ObjType>::FusionNode(
     std::function<void(const DetectedObjectsWithFeature::ConstSharedPtr msg)> roi_callback =
       std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i);
     rois_subs_.at(roi_i) = this->create_subscription<DetectedObjectsWithFeature>(
-      "input/rois" + std::to_string(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback);
+      input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback);
   }
 
   // subscribers
@@ -103,7 +121,8 @@ FusionNode<Msg, ObjType>::FusionNode(
   if (declare_parameter("debug_mode", false)) {
     std::size_t image_buffer_size =
       static_cast<std::size_t>(declare_parameter("image_buffer_size", 15));
-    debugger_ = std::make_shared<Debugger>(this, rois_number_, image_buffer_size);
+    debugger_ =
+      std::make_shared<Debugger>(this, rois_number_, image_buffer_size, input_camera_topics_);
   }
 
   // initialize debug tool