From 03196c8fbe6a7a2d26c4e81065a07d3ccdd1937b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 26 Sep 2023 20:16:43 +0900 Subject: [PATCH] fix(pointcloud_preprocessor): organize input twist topic (#5125) * fix(pointcloud_preprocessor): organize input twist topic (#25) * fix(pointcloud_preprocessor): organize input twist topic Signed-off-by: kminoda * style(pre-commit): autofix * fix build bug Signed-off-by: kminoda * fix format error Signed-off-by: kminoda * style(pre-commit): autofix * fix Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * minor fixes Signed-off-by: kminoda * style(pre-commit): autofix * add warning Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ground_segmentation.launch.py | 18 ++++- .../docs/concatenate-data.md | 1 + .../concatenate_and_time_sync_nodelet.hpp | 6 ++ .../launch/preprocessor.launch.py | 6 +- .../concatenate_and_time_sync_nodelet.cpp | 66 +++++++++++++++++-- 5 files changed, 86 insertions(+), 11 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..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 @@ -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_topic_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_topic_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_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index a3b5268815f24..74b4f3df17615 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -42,6 +42,7 @@ The figure below represents the reception time of each sensor data and how it is | `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | | `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). | | `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. | +| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. | ## Actual Usage 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..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,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,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 @@ -131,10 +133,13 @@ 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}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -170,6 +175,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..0d65b45b1b3de 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_topic_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 d88665a4c4c98..f3d69b766aa7a 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,24 @@ 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); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); + throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); + } } // Transformed Raw PointCloud2 Publisher @@ -227,8 +242,19 @@ Eigen::Matrix4f PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) { - // return identity if no twist is available or old_stamp is newer than new_stamp - if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + // return identity if no twist is available + if (twist_ptr_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "No twist is available. Please confirm twist topic and timestamp"); + return Eigen::Matrix4f::Identity(); + } + + // return identity if old_stamp is newer than new_stamp + if (old_stamp > new_stamp) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "old_stamp is newer than new_stamp,"); return Eigen::Matrix4f::Identity(); } @@ -558,6 +584,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) {