From 10ea99f8db509480f3ef63997afa67839dc7ab03 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 27 Feb 2024 09:29:08 +0900 Subject: [PATCH 1/4] feat: enable to change synchronized pointcloud message Signed-off-by: yoshiri --- ...intcloud_based_occupancy_grid_map_node.cpp | 5 +++++ .../concatenate_and_time_sync_nodelet.hpp | 19 +++++++++++++++++++ .../time_synchronizer_nodelet.hpp | 17 +++++++++++++++++ .../concatenate_and_time_sync_nodelet.cpp | 18 ++++++++++++++---- .../concatenate_pointclouds.cpp | 6 ------ .../time_synchronizer_nodelet.cpp | 14 +++++++++++++- 6 files changed, 68 insertions(+), 11 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 63b6715948f93..3c98b77332521 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/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{}; 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..3888125340eb1 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 @@ -85,6 +85,24 @@ #include #include +// 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 not_subscribed_topic_names_; 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..330574eeb2f76 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 @@ -85,6 +85,23 @@ #include #include +// 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; 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..606556f6e64a8 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,7 @@ #include #include -#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( new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); transformed_raw_pc_publisher_map_.insert({topic, publisher}); 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..f88cdfceb25ac 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/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(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,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( new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); transformed_raw_pc_publisher_map_.insert({topic, publisher}); From 96f643fbee5cd801b2a0b5e4b8234243b1e0da4d Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 1 Mar 2024 14:42:49 +0900 Subject: [PATCH 2/4] fix: bug fix in topic name replace function Signed-off-by: yoshiri --- .../concatenate_data/concatenate_and_time_sync_nodelet.hpp | 2 +- .../time_synchronizer/time_synchronizer_nodelet.hpp | 2 +- 2 files changed, 2 insertions(+), 2 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 3888125340eb1..eace74a8d0382 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 @@ -99,7 +99,7 @@ inline std::string replace_topic_name_postfix( return original_topic_name + postfix; } else { // replace the last element with the new postfix - return original_topic_name.substr(0, pos) + postfix; + return original_topic_name.substr(0, pos) + "/" + postfix; } } 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 330574eeb2f76..4b883fca10ad9 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 @@ -99,7 +99,7 @@ inline std::string replace_topic_name_postfix( return original_topic_name + postfix; } else { // replace the last element with the new postfix - return original_topic_name.substr(0, pos) + postfix; + return original_topic_name.substr(0, pos) + '/' + postfix; } } namespace pointcloud_preprocessor From 3c59e1c2ef79f95f32b6e43bbf4be202120f7357 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 1 Mar 2024 16:49:27 +0900 Subject: [PATCH 3/4] revert: revert changes do not related with this PR Signed-off-by: yoshiri --- .../pointcloud_based_occupancy_grid_map_node.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 3c98b77332521..63b6715948f93 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -136,11 +136,6 @@ 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{}; From 6de7344c88c4bea824092ef44b327094ce0d7a80 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Sat, 2 Mar 2024 01:28:12 +0900 Subject: [PATCH 4/4] chore: move topic rename function to class member Signed-off-by: yoshiri --- .../concatenate_and_time_sync_nodelet.hpp | 20 +-------- .../time_synchronizer_nodelet.hpp | 19 +-------- .../concatenate_and_time_sync_nodelet.cpp | 42 ++++++++++++++----- .../time_synchronizer_nodelet.cpp | 41 +++++++++++++----- 4 files changed, 67 insertions(+), 55 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 eace74a8d0382..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 @@ -85,24 +85,6 @@ #include #include -// 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; @@ -198,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 4b883fca10ad9..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 @@ -85,23 +85,6 @@ #include #include -// 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; @@ -194,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 606556f6e64a8..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" // default postfix name for synchronized pointcloud +#define DEFAULT_SYNC_TOPIC_POSTFIX \ + "_synchronized" // default postfix name for synchronized pointcloud ////////////////////////////////////////////////////////////////////////////////////////////// @@ -190,15 +191,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud if (publish_synchronized_pointcloud_) { for (auto & topic : input_topics_) { - 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; - } + 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}); @@ -241,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/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index f88cdfceb25ac..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 @@ -154,15 +154,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // Transformed Raw PointCloud2 Publisher { for (auto & topic : input_topics_) { - 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; - } + 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}); @@ -185,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(