feat: enable to change synchronized pointcloud message

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

@@ -136,6 +136,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
   if (stop_watch_ptr_) {
     stop_watch_ptr_->toc("processing_time", true);
   }
+  // if scan_origin_frame_ is "", replace it with input_raw_msg->header.frame_id
+  if (scan_origin_frame_.empty()) {
+    scan_origin_frame_ = input_raw_msg->header.frame_id;
+  }
+
   // Apply height filter
   PointCloud2 cropped_obstacle_pc{};
   PointCloud2 cropped_raw_pc{};

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

@@ -85,6 +85,24 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+// replace topic name postfix
+inline std::string replace_topic_name_postfix(
+  const std::string & original_topic_name, const std::string & postfix)
+{
+  // separate the topic name by '/' and replace the last element with the new postfix
+  size_t pos = original_topic_name.find_last_of("/");
+  if (pos == std::string::npos) {
+    // not found '/': this is not a namespaced topic
+    std::cerr
+      << "The topic name is not namespaced. The postfix will be added to the end of the topic name."
+      << std::endl;
+    return original_topic_name + postfix;
+  } else {
+    // replace the last element with the new postfix
+    return original_topic_name.substr(0, pos) + postfix;
+  }
+}
+
 namespace pointcloud_preprocessor
 {
 using autoware_point_types::PointXYZI;
@@ -126,6 +144,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   double timeout_sec_ = 0.1;
 
   bool publish_synchronized_pointcloud_;
+  std::string synchronized_pointcloud_postfix_;
 
   std::set<std::string> not_subscribed_topic_names_;
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp

@@ -85,6 +85,23 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+// replace topic name postfix
+inline std::string replace_topic_name_postfix(
+  const std::string & original_topic_name, const std::string & postfix)
+{
+  // separate the topic name by '/' and replace the last element with the new postfix
+  size_t pos = original_topic_name.find_last_of("/");
+  if (pos == std::string::npos) {
+    // not found '/': this is not a namespaced topic
+    std::cerr
+      << "The topic name is not namespaced. The postfix will be added to the end of the topic name."
+      << std::endl;
+    return original_topic_name + postfix;
+  } else {
+    // replace the last element with the new postfix
+    return original_topic_name.substr(0, pos) + postfix;
+  }
+}
 namespace pointcloud_preprocessor
 {
 using autoware_point_types::PointXYZI;

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

@@ -61,7 +61,7 @@
 #include <utility>
 #include <vector>
 
-#define POSTFIX_NAME "_synchronized"
+#define POSTFIX_NAME "_synchronized"  // default postfix name for synchronized pointcloud
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -112,6 +112,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
 
     // Check if publish synchronized pointcloud
     publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false);
+    synchronized_pointcloud_postfix_ =
+      declare_parameter("synchronized_pointcloud_postfix", "pointcloud");
   }
 
   // Initialize not_subscribed_topic_names_
@@ -185,10 +187,18 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
     }
   }
 
-  // Transformed Raw PointCloud2 Publisher
-  {
+  // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud
+  if (publish_synchronized_pointcloud_) {
     for (auto & topic : input_topics_) {
-      std::string new_topic = topic + POSTFIX_NAME;
+      std::string new_topic = replace_topic_name_postfix(topic, synchronized_pointcloud_postfix_);
+      if (new_topic == topic) {
+        RCLCPP_WARN_STREAM(
+          get_logger(),
+          "The topic name "
+            << topic
+            << " does not have a postfix. The postfix will be added to the end of the topic name.");
+        new_topic = topic + POSTFIX_NAME;
+      }
       auto publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(
         new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_));
       transformed_raw_pc_publisher_map_.insert({topic, publisher});

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp

@@ -74,12 +74,6 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent(
       return;
     }
   }
-  // add postfix to topic names
-  {
-    for (auto & topic : input_topics_) {
-      topic = topic + POSTFIX_NAME;
-    }
-  }
 
   // Initialize not_subscribed_topic_names_
   {

sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp

@@ -55,6 +55,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
   }
 
   // Set parameters
+  std::string synchronized_pointcloud_postfix;
   {
     output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
     if (output_frame_.empty()) {
@@ -71,6 +72,9 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
       RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue.");
       return;
     }
+    // output topic name postfix
+    synchronized_pointcloud_postfix =
+      declare_parameter("synchronized_pointcloud_postfix", "pointcloud");
 
     // Optional parameters
     maximum_queue_size_ = static_cast<int>(declare_parameter("max_queue_size", 5));
@@ -150,7 +154,15 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
   // Transformed Raw PointCloud2 Publisher
   {
     for (auto & topic : input_topics_) {
-      std::string new_topic = topic + POSTFIX_NAME;
+      std::string new_topic = replace_topic_name_postfix(topic, synchronized_pointcloud_postfix);
+      if (new_topic == topic) {
+        RCLCPP_WARN_STREAM(
+          get_logger(),
+          "The topic name "
+            << topic
+            << " does not have a postfix. The postfix will be added to the end of the topic name.");
+        new_topic = topic + POSTFIX_NAME;
+      }
       auto publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(
         new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_));
       transformed_raw_pc_publisher_map_.insert({topic, publisher});