feat(motion_velocity_planner): use polling subscriber to efficiently get messages (#7223)

* feat(motion_velocity_planner): use polling subscriber for odometry topic

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* use polling subscribers for more topics

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* remove blocking mutex lock when processing traffic lights

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* fix assign after return

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -103,8 +103,8 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
   const PlannerParam & params)
 {
   autoware_perception_msgs::msg::PredictedObjects filtered_objects;
-  filtered_objects.header = planner_data->predicted_objects->header;
-  for (const auto & object : planner_data->predicted_objects->objects) {
+  filtered_objects.header = planner_data->predicted_objects.header;
+  for (const auto & object : planner_data->predicted_objects.objects) {
     const auto is_pedestrian =
       std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
         return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN;

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -154,11 +154,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
   tier4_autoware_utils::StopWatch<std::chrono::microseconds> stopwatch;
   stopwatch.tic();
   out_of_lane::EgoData ego_data;
-  ego_data.pose = planner_data->current_odometry->pose;
+  ego_data.pose = planner_data->current_odometry.pose.pose;
   ego_data.trajectory_points = ego_trajectory_points;
   ego_data.first_trajectory_idx =
     motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
-  ego_data.velocity = planner_data->current_velocity->twist.linear.x;
+  ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x;
   ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel();
   stopwatch.tic("calculate_trajectory_footprints");
   const auto current_ego_footprint =

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp

@@ -24,9 +24,8 @@
 #include <autoware_perception_msgs/msg/traffic_light_group.hpp>
 #include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/header.hpp>
 #include <tier4_api_msgs/msg/crosswalk_status.hpp>
@@ -60,12 +59,11 @@ struct PlannerData
   }
 
   // msgs from callbacks that are used for data-ready
-  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry;
-  geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity;
-  geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration;
-  autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects;
-  pcl::PointCloud<pcl::PointXYZ>::ConstPtr no_ground_pointcloud;
-  nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid;
+  nav_msgs::msg::Odometry current_odometry{};
+  geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration{};
+  autoware_perception_msgs::msg::PredictedObjects predicted_objects{};
+  pcl::PointCloud<pcl::PointXYZ> no_ground_pointcloud{};
+  nav_msgs::msg::OccupancyGrid occupancy_grid{};
   std::shared_ptr<route_handler::RouteHandler> route_handler;
 
   // nearest search
@@ -78,7 +76,7 @@ struct PlannerData
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
   std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
-  tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
+  tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states;
 
   // velocity smoother
   std::shared_ptr<autoware::velocity_smoother::SmootherBase> velocity_smoother_{};

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -63,38 +63,10 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
   sub_trajectory_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>(
     "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1),
     create_subscription_options(this));
-  sub_predicted_objects_ =
-    this->create_subscription<autoware_perception_msgs::msg::PredictedObjects>(
-      "~/input/dynamic_objects", 1,
-      std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1),
-      create_subscription_options(this));
-  sub_no_ground_pointcloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-    "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(),
-    std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1),
-    create_subscription_options(this));
-  sub_vehicle_odometry_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    "~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1),
-    create_subscription_options(this));
-  sub_acceleration_ = this->create_subscription<geometry_msgs::msg::AccelWithCovarianceStamped>(
-    "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1),
-    create_subscription_options(this));
   sub_lanelet_map_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
     "~/input/vector_map", rclcpp::QoS(10).transient_local(),
     std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1),
     create_subscription_options(this));
-  sub_traffic_signals_ =
-    this->create_subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>(
-      "~/input/traffic_signals", 1,
-      std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1),
-      create_subscription_options(this));
-  sub_virtual_traffic_light_states_ =
-    this->create_subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>(
-      "~/input/virtual_traffic_light_states", 1,
-      std::bind(&MotionVelocityPlannerNode::on_virtual_traffic_light_states, this, _1),
-      create_subscription_options(this));
-  sub_occupancy_grid_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
-    "~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1),
-    create_subscription_options(this));
 
   srv_load_plugin_ = create_service<LoadPlugin>(
     "~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2));
@@ -151,45 +123,59 @@ void MotionVelocityPlannerNode::on_unload_plugin(
 }
 
 // NOTE: argument planner_data must not be referenced for multithreading
-bool MotionVelocityPlannerNode::is_data_ready() const
+bool MotionVelocityPlannerNode::update_planner_data()
 {
-  const auto & d = planner_data_;
   auto clock = *get_clock();
-  const auto check_with_msg = [&](const auto ptr, const auto & msg) {
+  auto is_ready = true;
+  const auto check_with_log = [&](const auto ptr, const auto & log) {
     constexpr auto throttle_duration = 3000;  // [ms]
     if (!ptr) {
-      RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, msg);
+      RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log);
+      is_ready = false;
       return false;
     }
     return true;
   };
 
-  return check_with_msg(d.current_odometry, "Waiting for current odometry") &&
-         check_with_msg(d.current_velocity, "Waiting for current velocity") &&
-         check_with_msg(d.current_acceleration, "Waiting for current acceleration") &&
-         check_with_msg(d.predicted_objects, "Waiting for predicted objects") &&
-         check_with_msg(d.no_ground_pointcloud, "Waiting for pointcloud") &&
-         check_with_msg(map_ptr_, "Waiting for the map") &&
-         check_with_msg(
-           d.velocity_smoother_, "Waiting for the initialization of the velocity smoother") &&
-         check_with_msg(d.occupancy_grid, "Waiting for the occupancy grid");
-}
+  const auto ego_state_ptr = sub_vehicle_odometry_.takeData();
+  if (check_with_log(ego_state_ptr, "Waiting for current odometry"))
+    planner_data_.current_odometry = *ego_state_ptr;
 
-void MotionVelocityPlannerNode::on_occupancy_grid(
-  const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  planner_data_.occupancy_grid = msg;
-}
+  const auto ego_accel_ptr = sub_acceleration_.takeData();
+  if (check_with_log(ego_accel_ptr, "Waiting for current acceleration"))
+    planner_data_.current_acceleration = *ego_accel_ptr;
 
-void MotionVelocityPlannerNode::on_predicted_objects(
-  const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  planner_data_.predicted_objects = msg;
+  const auto predicted_objects_ptr = sub_predicted_objects_.takeData();
+  if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects"))
+    planner_data_.predicted_objects = *predicted_objects_ptr;
+
+  const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData();
+  if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) {
+    const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr);
+    if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud;
+  }
+
+  const auto occupancy_grid_ptr = sub_occupancy_grid_.takeData();
+  if (check_with_log(occupancy_grid_ptr, "Waiting for the occupancy grid"))
+    planner_data_.occupancy_grid = *occupancy_grid_ptr;
+
+  // here we use bitwise operator to not short-circuit the logging messages
+  is_ready &= check_with_log(map_ptr_, "Waiting for the map");
+  is_ready &= check_with_log(
+    planner_data_.velocity_smoother_, "Waiting for the initialization of the velocity smoother");
+
+  // optional data
+  const auto traffic_signals_ptr = sub_traffic_signals_.takeData();
+  if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr);
+  const auto virtual_traffic_light_states_ptr = sub_virtual_traffic_light_states_.takeData();
+  if (virtual_traffic_light_states_ptr)
+    planner_data_.virtual_traffic_light_states = *virtual_traffic_light_states_ptr;
+
+  return is_ready;
 }
 
-void MotionVelocityPlannerNode::on_no_ground_pointcloud(
+std::optional<pcl::PointCloud<pcl::PointXYZ>>
+MotionVelocityPlannerNode::process_no_ground_pointcloud(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
 {
   geometry_msgs::msg::TransformStamped transform;
@@ -198,44 +184,16 @@ void MotionVelocityPlannerNode::on_no_ground_pointcloud(
       "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1));
   } catch (tf2::TransformException & e) {
     RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what());
-    return;
+    return {};
   }
 
   pcl::PointCloud<pcl::PointXYZ> pc;
   pcl::fromROSMsg(*msg, pc);
 
   Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast<float>();
   pcl::PointCloud<pcl::PointXYZ>::Ptr pc_transformed(new pcl::PointCloud<pcl::PointXYZ>);
-  if (!pc.empty()) {
-    tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine);
-  }
-
-  {
-    std::lock_guard<std::mutex> lock(mutex_);
-    planner_data_.no_ground_pointcloud = pc_transformed;
-  }
-}
-
-void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-
-  auto current_odometry = std::make_shared<geometry_msgs::msg::PoseStamped>();
-  current_odometry->header = msg->header;
-  current_odometry->pose = msg->pose.pose;
-  planner_data_.current_odometry = current_odometry;
-
-  auto current_velocity = std::make_shared<geometry_msgs::msg::TwistStamped>();
-  current_velocity->header = msg->header;
-  current_velocity->twist = msg->twist.twist;
-  planner_data_.current_velocity = current_velocity;
-}
-
-void MotionVelocityPlannerNode::on_acceleration(
-  const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  planner_data_.current_acceleration = msg;
+  if (!pc.empty()) tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine);
+  return *pc_transformed;
 }
 
 void MotionVelocityPlannerNode::set_velocity_smoother_params()
@@ -253,11 +211,9 @@ void MotionVelocityPlannerNode::on_lanelet_map(
   has_received_map_ = true;
 }
 
-void MotionVelocityPlannerNode::on_traffic_signals(
+void MotionVelocityPlannerNode::process_traffic_signals(
   const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
-
   // clear previous observation
   planner_data_.traffic_light_id_map_raw_.clear();
   const auto traffic_light_id_map_last_observed_old =
@@ -290,19 +246,12 @@ void MotionVelocityPlannerNode::on_traffic_signals(
   }
 }
 
-void MotionVelocityPlannerNode::on_virtual_traffic_light_states(
-  const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  planner_data_.virtual_traffic_light_states = msg;
-}
-
 void MotionVelocityPlannerNode::on_trajectory(
   const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg)
 {
   std::unique_lock<std::mutex> lk(mutex_);
 
-  if (!is_data_ready()) {
+  if (!update_planner_data()) {
     return;
   }
 
@@ -367,9 +316,9 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
   const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points,
   const autoware::motion_velocity_planner::PlannerData & planner_data) const
 {
-  const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry->pose;
-  const double v0 = planner_data.current_velocity->twist.linear.x;
-  const double a0 = planner_data.current_acceleration->accel.accel.linear.x;
+  const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry.pose.pose;
+  const double v0 = planner_data.current_odometry.twist.twist.linear.x;
+  const double a0 = planner_data.current_acceleration.accel.accel.linear.x;
   const auto & external_v_limit = planner_data.external_velocity_limit;
   const auto & smoother = planner_data.velocity_smoother_;
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp

@@ -22,10 +22,12 @@
 #include <autoware_motion_velocity_planner_node/srv/unload_plugin.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/logger_level_configure.hpp>
+#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 #include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -60,31 +62,33 @@ class MotionVelocityPlannerNode : public rclcpp::Node
 
   // subscriber
   rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_trajectory_;
-  rclcpp::Subscription<autoware_perception_msgs::msg::PredictedObjects>::SharedPtr
-    sub_predicted_objects_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_no_ground_pointcloud_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_vehicle_odometry_;
-  rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_acceleration_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<
+    autoware_perception_msgs::msg::PredictedObjects>
+    sub_predicted_objects_{this, "~/input/dynamic_objects"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<sensor_msgs::msg::PointCloud2>
+    sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
+    sub_vehicle_odometry_{this, "~/input/vehicle_odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<
+    geometry_msgs::msg::AccelWithCovarianceStamped>
+    sub_acceleration_{this, "~/input/accel"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::OccupancyGrid>
+    sub_occupancy_grid_{this, "~/input/occupancy_grid"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<
+    autoware_perception_msgs::msg::TrafficLightGroupArray>
+    sub_traffic_signals_{this, "~/input/traffic_signals"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<
+    tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>
+    sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"};
   rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr sub_lanelet_map_;
-  rclcpp::Subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>::SharedPtr
-    sub_traffic_signals_;
-  rclcpp::Subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
-    sub_virtual_traffic_light_states_;
-  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_occupancy_grid_;
 
   void on_trajectory(
     const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg);
-  void on_predicted_objects(
-    const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
-  void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
-  void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
-  void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg);
+  std::optional<pcl::PointCloud<pcl::PointXYZ>> process_no_ground_pointcloud(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
   void on_lanelet_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg);
-  void on_traffic_signals(
+  void process_traffic_signals(
     const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg);
-  void on_virtual_traffic_light_states(
-    const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg);
-  void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg);
 
   // publishers
   rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr trajectory_pub_;
@@ -118,7 +122,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node
   std::mutex mutex_;
 
   // function
-  bool is_data_ready() const;
+  /// @brief update the PlannerData instance with the latest messages received
+  /// @return false if some data is not available
+  bool update_planner_data();
   void insert_stop(
     autoware_planning_msgs::msg::Trajectory & trajectory,
     const geometry_msgs::msg::Point & stop_point) const;