feat(time_synchronizer): organize twist input (#5160)

* feat(time_synchronizer): organize twist input Signed-off-by: kminoda <[email protected]> * fix Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Sep 28, 2023 · 32c2e67 · 32c2e67
1 parent c552c3d
commit 32c2e67
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 13 deletions.
diff --git a/...processor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/...processor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp
@@ -68,9 +68,10 @@
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
-#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
 #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>
@@ -127,11 +128,14 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node
   /** \brief A vector of subscriber. */
   std::vector<rclcpp::Subscription<PointCloud2>::SharedPtr> filters_;
 
-  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr sub_twist_;
+  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_;
 
@@ -168,7 +172,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node
   void cloud_callback(
     const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr,
     const std::string & topic_name);
-  void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input);
+  void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input);
+  void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input);
   void timer_callback();
 
   void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);

diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py
@@ -58,7 +58,10 @@ def launch_setup(context, *args, **kwargs):
             package=pkg,
             plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent",
             name="synchronizer_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"),

diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp
@@ -41,7 +41,8 @@ namespace pointcloud_preprocessor
 {
 PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
   const rclcpp::NodeOptions & node_options)
-: Node("point_cloud_time_synchronizer_component", node_options)
+: Node("point_cloud_time_synchronizer_component", node_options),
+  input_twist_topic_type_(declare_parameter<std::string>("input_twist_topic_type", "twist"))
 {
   // initialize debug tool
   {
@@ -129,10 +130,21 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
     }
 
     // Subscribe to the twist
-    auto twist_cb =
-      std::bind(&PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1);
-    sub_twist_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
-      "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb);
+    if (input_twist_topic_type_ == "twist") {
+      auto twist_cb = std::bind(
+        &PointCloudDataSynchronizerComponent::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(&PointCloudDataSynchronizerComponent::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
@@ -474,7 +486,33 @@ void PointCloudDataSynchronizerComponent::timer_callback()
 }
 
 void PointCloudDataSynchronizerComponent::twist_callback(
-  const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input)
+  const geometry_msgs::msg::TwistWithCovarianceStamped::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.stamp = input->header.stamp;
+  twist_ptr->twist = input->twist.twist;
+  twist_ptr_queue_.push_back(twist_ptr);
+}
+
+void PointCloudDataSynchronizerComponent::odom_callback(
+  const nav_msgs::msg::Odometry::ConstSharedPtr input)
 {
   // if rosbag restart, clear buffer
   if (!twist_ptr_queue_.empty()) {
@@ -495,9 +533,7 @@ void PointCloudDataSynchronizerComponent::twist_callback(
 
   auto twist_ptr = std::make_shared<geometry_msgs::msg::TwistStamped>();
   twist_ptr->header.stamp = input->header.stamp;
-  twist_ptr->twist.linear.x = input->longitudinal_velocity;
-  twist_ptr->twist.linear.y = input->lateral_velocity;
-  twist_ptr->twist.angular.z = input->heading_rate;
+  twist_ptr->twist = input->twist.twist;
   twist_ptr_queue_.push_back(twist_ptr);
 }