From a137f149576018cf6753ad58a248756c351e0f52 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 26 Sep 2023 08:52:36 +0900 Subject: [PATCH 1/6] fix(pointcloud_preprocessor): organize input twist topic Signed-off-by: kminoda --- .../ground_segmentation.launch.py | 18 ++++++-- .../concatenate_and_time_sync_nodelet.hpp | 4 ++ .../launch/preprocessor.launch.py | 6 ++- .../concatenate_and_time_sync_nodelet.cpp | 44 ++++++++++++++++--- 4 files changed, 63 insertions(+), 9 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 7ed5860612601..ccbbe959f31fb 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -122,12 +122,16 @@ def create_ransac_pipeline(self): plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", namespace="plane_fitting", - remappings=[("output", "concatenated/pointcloud")], + remappings=[ + ("~/input/odom", "/localization/kinematic_state") + ("output", "concatenated/pointcloud"), + ], parameters=[ { "input_topics": self.ground_segmentation_param["ransac_input_topics"], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, + "input_twist_type": "odom", } ], extra_arguments=[ @@ -432,11 +436,15 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic): package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic) + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -448,11 +456,15 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_no_ground_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic) + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], 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 f5d55a2ebcbf7..0e075e8acfcfd 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 @@ -88,6 +88,7 @@ namespace pointcloud_preprocessor { using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; + /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single @@ -135,6 +136,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -170,6 +173,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name); void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 6b1fc0f5bd976..54f7f0cbd63f4 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -38,13 +38,17 @@ def launch_setup(context, *args, **kwargs): package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="sync_and_concatenate_filter", - remappings=[("output", "points_raw/concatenated")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "points_raw/concatenated") + ], parameters=[ { "input_topics": LaunchConfiguration("input_points_raw_list"), "output_frame": LaunchConfiguration("tf_output_frame"), "approximate_sync": True, "publish_synchronized_pointcloud": False, + "input_twist_type": "twist", } ], ) 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 8ae0bdacb983a..3004687a7e31c 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 @@ -69,7 +69,8 @@ namespace pointcloud_preprocessor { PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) -: Node("point_cloud_concatenator_component", node_options) +: Node("point_cloud_concatenator_component", node_options), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")), { // initialize debug tool { @@ -164,10 +165,17 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro filters_[d] = this->create_subscription( input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); } - auto twist_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); - sub_twist_ = this->create_subscription( - "~/input/twist", rclcpp::QoS{100}, twist_cb); + + if (input_twist_topic_type_ == "twist") { + auto twist_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + sub_twist_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, twist_cb); + } else if (input_twist_topic_type_ == "odom") { + auto odom_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, std::placeholders::_1); + sub_odom_ = this->create_subscription("~/input/odom", rclcpp::QoS{100}, odom_cb); + } } // Transformed Raw PointCloud2 Publisher @@ -558,6 +566,32 @@ void PointCloudConcatenateDataSynchronizerComponent::twist_callback( twist_ptr_queue_.push_back(twist_ptr); } +void PointCloudConcatenateDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header = input->header; + twist_ptr->twist = input->twist.twist; + twist_ptr_queue_.push_back(twist_ptr); +} + void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( diagnostic_updater::DiagnosticStatusWrapper & stat) { From fe25bf78a251541db32bd3f52d011e9f6f7b5ebe Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 25 Sep 2023 23:55:59 +0000 Subject: [PATCH 2/6] style(pre-commit): autofix --- .../ground_segmentation/ground_segmentation.launch.py | 9 +++++---- .../launch/preprocessor.launch.py | 2 +- .../concatenate_and_time_sync_nodelet.cpp | 9 ++++++--- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index ccbbe959f31fb..1b78bd19a9314 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -123,8 +123,9 @@ def create_ransac_pipeline(self): name="concatenate_data", namespace="plane_fitting", remappings=[ - ("~/input/odom", "/localization/kinematic_state") - ("output", "concatenated/pointcloud"), + ("~/input/odom", "/localization/kinematic_state")( + "output", "concatenated/pointcloud" + ), ], parameters=[ { @@ -438,7 +439,7 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic): name="concatenate_data", remappings=[ ("~/input/odom", "/localization/kinematic_state"), - ("output", output_topic) + ("output", output_topic), ], parameters=[ { @@ -458,7 +459,7 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, name="concatenate_no_ground_data", remappings=[ ("~/input/odom", "/localization/kinematic_state"), - ("output", output_topic) + ("output", output_topic), ], parameters=[ { diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 54f7f0cbd63f4..bcfb9b47d1ef1 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -40,7 +40,7 @@ def launch_setup(context, *args, **kwargs): name="sync_and_concatenate_filter", remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("output", "points_raw/concatenated") + ("output", "points_raw/concatenated"), ], parameters=[ { 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 3004687a7e31c..e9b3fc2cfe742 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 @@ -168,13 +168,16 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro if (input_twist_topic_type_ == "twist") { auto twist_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, + std::placeholders::_1); sub_twist_ = this->create_subscription( "~/input/twist", rclcpp::QoS{100}, twist_cb); } else if (input_twist_topic_type_ == "odom") { auto odom_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, std::placeholders::_1); - sub_odom_ = this->create_subscription("~/input/odom", rclcpp::QoS{100}, odom_cb); + &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, + std::placeholders::_1); + sub_odom_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, odom_cb); } } From 071c2bfda9964ebbf5835538db522d4714b3ba58 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 26 Sep 2023 09:16:26 +0900 Subject: [PATCH 3/6] fix build bug Signed-off-by: kminoda --- .../concatenate_data/concatenate_and_time_sync_nodelet.hpp | 2 ++ .../src/concatenate_data/concatenate_and_time_sync_nodelet.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) 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 0e075e8acfcfd..5dc47d986295c 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 @@ -75,6 +75,7 @@ #include #include #include +#include #include #include @@ -132,6 +133,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::vector::SharedPtr> filters_; rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; 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 e9b3fc2cfe742..df98d970bad75 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 @@ -70,7 +70,7 @@ namespace pointcloud_preprocessor PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) : Node("point_cloud_concatenator_component", node_options), - input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) { // initialize debug tool { From 2f1fbc556ab248b62161744cec5cc973052a2e93 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 26 Sep 2023 09:17:09 +0900 Subject: [PATCH 4/6] fix format error Signed-off-by: kminoda --- .../ground_segmentation/ground_segmentation.launch.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 1b78bd19a9314..72310e54263e6 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -123,9 +123,8 @@ def create_ransac_pipeline(self): name="concatenate_data", namespace="plane_fitting", remappings=[ - ("~/input/odom", "/localization/kinematic_state")( - "output", "concatenated/pointcloud" - ), + ("~/input/odom", "/localization/kinematic_state"), + ("output", "concatenated/pointcloud"), ], parameters=[ { From 98a65f68526f1ce15748473bce996a744cd42853 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 26 Sep 2023 00:21:00 +0000 Subject: [PATCH 5/6] style(pre-commit): autofix --- .../concatenate_data/concatenate_and_time_sync_nodelet.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 5dc47d986295c..be96aa94382dd 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 @@ -71,11 +71,11 @@ #include #include #include +#include #include #include #include #include -#include #include #include From 5aaa746d5aa985a979b00cd1668e40f1139173a1 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 26 Sep 2023 09:28:11 +0900 Subject: [PATCH 6/6] fix Signed-off-by: kminoda --- .../ground_segmentation/ground_segmentation.launch.py | 6 +++--- .../pointcloud_preprocessor/launch/preprocessor.launch.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 72310e54263e6..12190fb659235 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -131,7 +131,7 @@ def create_ransac_pipeline(self): "input_topics": self.ground_segmentation_param["ransac_input_topics"], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, - "input_twist_type": "odom", + "input_twist_topic_type": "odom", } ], extra_arguments=[ @@ -444,7 +444,7 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic): { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), - "input_twist_type": "odom", + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -464,7 +464,7 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), - "input_twist_type": "odom", + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index bcfb9b47d1ef1..0d65b45b1b3de 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -48,7 +48,7 @@ def launch_setup(context, *args, **kwargs): "output_frame": LaunchConfiguration("tf_output_frame"), "approximate_sync": True, "publish_synchronized_pointcloud": False, - "input_twist_type": "twist", + "input_twist_topic_type": "twist", } ], )