Merge pull request autowarefoundation#889 from kminoda/fix/pointcloud…

…_preprocessor/organize_twist_in_concatenate_filter

fix(pointcloud_preprocessor): organize input twist topic (autowarefoundation#5125)

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")}],

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]<br>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. <br> 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 `<original_msg_name>_synchronized`.                                                                                       |
+| `input_twist_topic_type`          | std::string      | twist         | Topic type for twist. Currently support `twist` or `odom`.                                                                                                                                                                                                 |
 
 ## Actual Usage
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

@@ -71,6 +71,7 @@
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <tier4_debug_msgs/msg/int32_stamped.hpp>
@@ -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<rclcpp::Subscription<PointCloud2>::SharedPtr> filters_;
 
   rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr sub_twist_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::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);

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",
                 }
             ],
         )

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<std::string>("input_twist_topic_type", "twist"))
 {
   // initialize debug tool
   {
@@ -164,10 +165,24 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
       filters_[d] = this->create_subscription<sensor_msgs::msg::PointCloud2>(
         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<geometry_msgs::msg::TwistWithCovarianceStamped>(
-      "~/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<geometry_msgs::msg::TwistWithCovarianceStamped>(
+        "~/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<nav_msgs::msg::Odometry>(
+        "~/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<geometry_msgs::msg::TwistStamped>();
+  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)
 {