fix(pointcloud_preprocessor): organize input twist topic

launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

@@ -122,12 +122,16 @@
                 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 @@
             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 @@
             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/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 @@
             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

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -69,105 +69,116 @@
 {
 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
   {
     using tier4_autoware_utils::DebugPublisher;
     using tier4_autoware_utils::StopWatch;
     stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
     debug_publisher_ = std::make_unique<DebugPublisher>(this, "concatenate_data_synchronizer");
     stop_watch_ptr_->tic("cyclic_time");
     stop_watch_ptr_->tic("processing_time");
   }
 
   // Set parameters
   {
     output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
     if (output_frame_.empty()) {
       RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!");
       return;
     }
     declare_parameter("input_topics", std::vector<std::string>());
     input_topics_ = get_parameter("input_topics").as_string_array();
     if (input_topics_.empty()) {
       RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!");
       return;
     }
     if (input_topics_.size() == 1) {
       RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue.");
       return;
     }
 
     // Optional parameters
     maximum_queue_size_ = static_cast<int>(declare_parameter("max_queue_size", 5));
     timeout_sec_ = static_cast<double>(declare_parameter("timeout_sec", 0.1));
 
     input_offset_ = declare_parameter("input_offset", std::vector<double>{});
     if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) {
       RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets.");
       return;
     }
 
     // Check if publish synchronized pointcloud
     publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false);
   }
 
   // Initialize not_subscribed_topic_names_
   {
     for (const std::string & e : input_topics_) {
       not_subscribed_topic_names_.insert(e);
     }
   }
 
   // Initialize offset map
   {
     for (size_t i = 0; i < input_offset_.size(); ++i) {
       offset_map_[input_topics_[i]] = input_offset_[i];
     }
   }
 
   // tf2 listener
   {
     tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
     tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
   }
 
   // Output Publishers
   {
     pub_output_ = this->create_publisher<PointCloud2>(
       "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_));
   }
 
   // Subscribers
   {
     RCLCPP_INFO_STREAM(
       get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:");
     for (auto & input_topic : input_topics_) {
       RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic);
     }
 
     // Subscribe to the filters
     filters_.resize(input_topics_.size());
 
     // First input_topics_.size () filters are valid
     for (size_t d = 0; d < input_topics_.size(); ++d) {
       cloud_stdmap_.insert(std::make_pair(input_topics_[d], nullptr));
       cloud_stdmap_tmp_ = cloud_stdmap_;
 
       // CAN'T use auto type here.
       std::function<void(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)> cb = std::bind(
         &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this,
         std::placeholders::_1, input_topics_[d]);
 
       filters_[d].reset();
       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);
+    }
   }
 
   // Transformed Raw PointCloud2 Publisher
@@ -555,9 +566,35 @@
   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::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)
 {