feat(pointcloud_preprocessor): enable to change synchronized pointclo…

…ud topic name (autowarefoundation#6525) * feat: enable to change synchronized pointcloud message Signed-off-by: yoshiri <[email protected]> * fix: bug fix in topic name replace function Signed-off-by: yoshiri <[email protected]> * revert: revert changes do not related with this PR Signed-off-by: yoshiri <[email protected]> * chore: move topic rename function to class member Signed-off-by: yoshiri <[email protected]> --------- Signed-off-by: yoshiri <[email protected]>
tier4 · Mar 2, 2024 · 44e4be7 · 44e4be7
1 parent 92c478e
commit 44e4be7
Show file tree

Hide file tree

Showing 5 changed files with 76 additions and 12 deletions.
diff --git a/...or/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/...or/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
@@ -126,6 +126,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_;
 
@@ -179,6 +180,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   void timer_callback();
 
   void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  std::string replaceSyncTopicNamePostfix(
+    const std::string & original_topic_name, const std::string & postfix);
 
   /** \brief processing time publisher. **/
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;

diff --git a/...processor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/...processor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp
@@ -177,6 +177,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node
   void timer_callback();
 
   void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  std::string replaceSyncTopicNamePostfix(
+    const std::string & original_topic_name, const std::string & postfix);
 
   /** \brief processing time publisher. **/
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;

diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
@@ -61,7 +61,8 @@
 #include <utility>
 #include <vector>
 
-#define POSTFIX_NAME "_synchronized"
+#define DEFAULT_SYNC_TOPIC_POSTFIX \
+  "_synchronized"  // default postfix name for synchronized pointcloud
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -112,6 +113,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 +188,10 @@ 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 = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix_);
       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});
@@ -231,6 +234,35 @@ void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud(
   }
 }
 
+std::string PointCloudConcatenateDataSynchronizerComponent::replaceSyncTopicNamePostfix(
+  const std::string & original_topic_name, const std::string & postfix)
+{
+  std::string replaced_topic_name;
+  // 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
+    RCLCPP_WARN_STREAM(
+      get_logger(),
+      "The topic name is not namespaced. The postfix will be added to the end of the topic name.");
+    return original_topic_name + postfix;
+  } else {
+    // replace the last element with the new postfix
+    replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix;
+  }
+
+  // if topic name is the same with original topic name, add postfix to the end of the topic name
+  if (replaced_topic_name == original_topic_name) {
+    RCLCPP_WARN_STREAM(
+      get_logger(), "The topic name "
+                      << original_topic_name
+                      << " have the same postfix with synchronized pointcloud. We use the postfix "
+                         "to the end of the topic name.");
+    replaced_topic_name = original_topic_name + DEFAULT_SYNC_TOPIC_POSTFIX;
+  }
+  return replaced_topic_name;
+}
+
 /**
  * @brief compute transform to adjust for old timestamp
  *

diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/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_
   {

diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp
@@ -34,7 +34,7 @@
 #include <vector>
 
 // postfix for output topics
-#define POSTFIX_NAME "_synchronized"
+#define DEFAULT_SYNC_TOPIC_POSTFIX "_synchronized"
 //////////////////////////////////////////////////////////////////////////////////////////////
 
 namespace pointcloud_preprocessor
@@ -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,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
   // Transformed Raw PointCloud2 Publisher
   {
     for (auto & topic : input_topics_) {
-      std::string new_topic = topic + POSTFIX_NAME;
+      std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix);
       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});
@@ -173,6 +177,35 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
   }
 }
 
+std::string PointCloudDataSynchronizerComponent::replaceSyncTopicNamePostfix(
+  const std::string & original_topic_name, const std::string & postfix)
+{
+  std::string replaced_topic_name;
+  // 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
+    RCLCPP_WARN_STREAM(
+      get_logger(),
+      "The topic name is not namespaced. The postfix will be added to the end of the topic name.");
+    return original_topic_name + postfix;
+  } else {
+    // replace the last element with the new postfix
+    replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix;
+  }
+
+  // if topic name is the same with original topic name, add postfix to the end of the topic name
+  if (replaced_topic_name == original_topic_name) {
+    RCLCPP_WARN_STREAM(
+      get_logger(), "The topic name "
+                      << original_topic_name
+                      << " have the same postfix with synchronized pointcloud. We use the postfix "
+                         "to the end of the topic name.");
+    replaced_topic_name = original_topic_name + DEFAULT_SYNC_TOPIC_POSTFIX;
+  }
+  return replaced_topic_name;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 // overloaded functions
 void PointCloudDataSynchronizerComponent::transformPointCloud(