From 44e4be783019e6c6dc839e6436ce3dd1ab483782 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Sat, 2 Mar 2024 20:59:58 +0900 Subject: [PATCH] feat(pointcloud_preprocessor): enable to change synchronized pointcloud topic name (#6525) * feat: enable to change synchronized pointcloud message Signed-off-by: yoshiri * fix: bug fix in topic name replace function Signed-off-by: yoshiri * revert: revert changes do not related with this PR Signed-off-by: yoshiri * chore: move topic rename function to class member Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../concatenate_and_time_sync_nodelet.hpp | 3 ++ .../time_synchronizer_nodelet.hpp | 2 + .../concatenate_and_time_sync_nodelet.cpp | 40 +++++++++++++++++-- .../concatenate_pointclouds.cpp | 6 --- .../time_synchronizer_nodelet.cpp | 37 ++++++++++++++++- 5 files changed, 76 insertions(+), 12 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index be96aa94382dd..d1560cccd0b15 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/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 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> stop_watch_ptr_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 8c3cb98ba266e..4a36c32980b0e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/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> 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 index f0b04bacf26dd..f6407c532b5f8 100644 --- 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 #include -#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( 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 index 4e26adb19bda5..d0a4fc892e940 100644 --- 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 index 8796477f3d9ae..8f02721a67cfb 100644 --- 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 // 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(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(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( 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(