feat(autoware_motion_velocity_planner)!: only wait for the required subscriptions (#505)

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: mitsudome-r

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp

@@ -60,6 +60,14 @@ class ObstacleStopModule : public PluginModuleInterface
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data) override;
 
+  RequiredSubscriptionInfo getRequiredSubscriptions() const override
+  {
+    RequiredSubscriptionInfo required_subscription_info;
+    required_subscription_info.predicted_objects = true;
+    required_subscription_info.no_ground_pointcloud = true;
+    return required_subscription_info;
+  }
+
 private:
   std::string module_name_{};
   rclcpp::Clock::SharedPtr clock_{};

planning/motion_velocity_planner/autoware_motion_velocity_planner/src/node.cpp

@@ -137,7 +137,11 @@ bool MotionVelocityPlannerNode::update_planner_data(
 {
   auto clock = *get_clock();
   auto is_ready = true;
-  const auto check_with_log = [&](const auto ptr, const auto & log) {
+  const auto check_with_log = [&](const auto ptr, const auto & log, const bool is_required = true) {
+    if (!is_required) {
+      return false;
+    }
+
     constexpr auto throttle_duration = 3000;  // [ms]
     if (!ptr) {
       RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "%s", log);
@@ -147,6 +151,8 @@ bool MotionVelocityPlannerNode::update_planner_data(
     return true;
   };
 
+  const auto required_subscriptions = planner_manager_.getRequiredSubscriptions();
+
   autoware_utils_system::StopWatch<std::chrono::milliseconds> sw;
   const auto ego_state_ptr = sub_vehicle_odometry_.take_data();
   if (check_with_log(ego_state_ptr, "Waiting for current odometry"))
@@ -159,12 +165,16 @@ bool MotionVelocityPlannerNode::update_planner_data(
   processing_times["update_planner_data.accel"] = sw.toc(true);
 
   const auto predicted_objects_ptr = sub_predicted_objects_.take_data();
-  if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects"))
+  if (check_with_log(
+        predicted_objects_ptr, "Waiting for predicted objects",
+        required_subscriptions.predicted_objects))
     planner_data_.process_predicted_objects(*predicted_objects_ptr);
   processing_times["update_planner_data.pred_obj"] = sw.toc(true);
 
   const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.take_data();
-  if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) {
+  if (check_with_log(
+        no_ground_pointcloud_ptr, "Waiting for pointcloud",
+        required_subscriptions.no_ground_pointcloud)) {
     auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr);
     processing_times["update_planner_data.pcl.process_no_ground_pointcloud"] = sw.toc(true);
     if (no_ground_pointcloud) {
@@ -173,7 +183,9 @@ bool MotionVelocityPlannerNode::update_planner_data(
   }
 
   const auto occupancy_grid_ptr = sub_occupancy_grid_.take_data();
-  if (check_with_log(occupancy_grid_ptr, "Waiting for the occupancy grid"))
+  if (check_with_log(
+        occupancy_grid_ptr, "Waiting for the occupancy grid",
+        required_subscriptions.occupancy_grid_map))
     planner_data_.occupancy_grid = *occupancy_grid_ptr;
   processing_times["update_planner_data.occ_grid"] = sw.toc(true);
 
@@ -194,6 +206,7 @@ bool MotionVelocityPlannerNode::update_planner_data(
 
   // optional data
   const auto traffic_signals_ptr = sub_traffic_signals_.take_data();
+  // NOTE: required_subscriptions.traffic_signals is not used since is_ready is not updated here.
   if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr);
   processing_times["update_planner_data.traffic_lights"] = sw.toc(true);
 

planning/motion_velocity_planner/autoware_motion_velocity_planner/src/planner_manager.cpp

@@ -43,6 +43,10 @@ void MotionVelocityPlannerManager::load_module_plugin(rclcpp::Node & node, const
     // register
     loaded_plugins_.push_back(plugin);
     RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded.");
+
+    // update the subscription
+    const auto required_subscriptions = plugin->getRequiredSubscriptions();
+    required_subscriptions_.update(required_subscriptions);
   } else {
     RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available.");
   }

planning/motion_velocity_planner/autoware_motion_velocity_planner/src/planner_manager.hpp

@@ -48,9 +48,12 @@ class MotionVelocityPlannerManager
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data);
 
+  RequiredSubscriptionInfo getRequiredSubscriptions() const { return required_subscriptions_; }
+
 private:
   pluginlib::ClassLoader<PluginModuleInterface> plugin_loader_;
   std::vector<std::shared_ptr<PluginModuleInterface>> loaded_plugins_;
+  RequiredSubscriptionInfo required_subscriptions_;
 };
 }  // namespace autoware::motion_velocity_planner
 

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

@@ -243,6 +243,21 @@ struct PlannerData
       traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
   }
 };
+struct RequiredSubscriptionInfo
+{
+  bool traffic_signals{false};
+  bool predicted_objects{false};
+  bool occupancy_grid_map{false};
+  bool no_ground_pointcloud{false};
+
+  void update(const RequiredSubscriptionInfo & required_subscriptions)
+  {
+    traffic_signals |= required_subscriptions.traffic_signals;
+    predicted_objects |= required_subscriptions.predicted_objects;
+    occupancy_grid_map |= required_subscriptions.occupancy_grid_map;
+    no_ground_pointcloud |= required_subscriptions.no_ground_pointcloud;
+  }
+};
 }  // namespace autoware::motion_velocity_planner
 
 #endif  // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp

@@ -41,6 +41,7 @@ class PluginModuleInterface
 public:
   virtual ~PluginModuleInterface() = default;
   virtual void init(rclcpp::Node & node, const std::string & module_name) = 0;
+  virtual RequiredSubscriptionInfo getRequiredSubscriptions() const = 0;
   virtual void update_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
   virtual VelocityPlanningResult plan(
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,