Merge pull request #1361 from tier4/cherry-pick/perf-out_of_lane

perf(motion_velocity_planner): resample trajectory and build rtree with packing algorithm

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp

@@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node
 
   // Publisher
   autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"};
-  autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
+  autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
+    this, "~/debug/processing_time_ms_diag"};
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;

control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   lane_departure_checker_->setParam(param_, vehicle_info);
 
   // Publisher
+  processing_time_publisher_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
   // Nothing
 
   // Diagnostic Updater
@@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer()
   }
 
   processing_time_map["Total"] = stop_watch.toc("Total");
-  processing_time_publisher_.publish(processing_time_map);
+  processing_diag_publisher_.publish(processing_time_map);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = processing_time_map["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
 }
 
 rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

@@ -29,6 +29,7 @@
 #include <autoware/universe_utils/system/stop_watch.hpp>
 
 #include <algorithm>
+#include <map>
 #include <memory>
 #include <string>
 #include <utility>
@@ -48,6 +49,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
+  processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms_diag");
+  processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/" + ns_ + "/processing_time_ms", 1);
 
   using autoware::universe_utils::getOrDeclareParameter;
   auto & p = params_;
@@ -179,6 +184,16 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
   debug_data_.ego_footprints = ego_data.trajectory_footprints;
   debug_data_.obstacle_footprints = obstacle_forward_footprints;
   debug_data_.z = ego_data.pose.position.z;
+  std::map<std::string, double> processing_times;
+  processing_times["preprocessing"] = preprocessing_duration_us / 1000;
+  processing_times["footprints"] = footprints_duration_us / 1000;
+  processing_times["collisions"] = collisions_duration_us / 1000;
+  processing_times["Total"] = total_time_us / 1000;
+  processing_diag_publisher_->publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = clock_->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
   return result;
 }
 

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp

@@ -74,11 +74,14 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params)
   for (const auto & p : ego_data.trajectory)
     ego_data.trajectory_footprints.push_back(autoware::universe_utils::toFootprint(
       p.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0));
+  std::vector<BoxIndexPair> rtree_nodes;
+  rtree_nodes.reserve(ego_data.trajectory_footprints.size());
   for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) {
     const auto box = boost::geometry::return_envelope<autoware::universe_utils::Box2d>(
       ego_data.trajectory_footprints[i]);
-    ego_data.rtree.insert(std::make_pair(box, i));
+    rtree_nodes.push_back(std::make_pair(box, i));
   }
+  ego_data.rtree = Rtree(rtree_nodes);
 }
 
 }  // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

@@ -34,6 +34,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <map>
 
 namespace autoware::motion_velocity_planner
 {
@@ -52,6 +53,10 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
+  processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms_diag");
+  processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/" + ns_ + "/processing_time_ms", 1);
 
   const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
   vehicle_lateral_offset_ = static_cast<double>(vehicle_info.max_lateral_offset_m);
@@ -218,6 +223,16 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
       safe_footprint_polygons, obstacle_masks,
       planner_data->current_odometry.pose.pose.position.z));
   }
+  std::map<std::string, double> processing_times;
+  processing_times["preprocessing"] = preprocessing_us / 1000;
+  processing_times["obstacles"] = obstacles_us / 1000;
+  processing_times["slowdowns"] = slowdowns_us / 1000;
+  processing_times["Total"] = total_us / 1000;
+  processing_diag_publisher_->publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = clock_->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
   return result;
 }
 }  // namespace autoware::motion_velocity_planner

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -33,6 +33,7 @@
 
 #include <lanelet2_core/geometry/LaneletMap.h>
 
+#include <map>
 #include <memory>
 #include <string>
 #include <utility>
@@ -56,6 +57,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
+  processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms_diag");
+  processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/" + ns_ + "/processing_time_ms", 1);
 }
 void OutOfLaneModule::init_parameters(rclcpp::Node & node)
 {
@@ -207,7 +212,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
   inputs.ego_data = ego_data;
   stopwatch.tic("filter_predicted_objects");
   inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_);
-  const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects");
+  const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects");
   inputs.route_handler = planner_data->route_handler;
   inputs.lanelets = other_lanelets;
   stopwatch.tic("calculate_decisions");
@@ -288,12 +293,26 @@ VelocityPlanningResult OutOfLaneModule::plan(
     "\tcalc_slowdown_points = %2.0fus\n"
     "\tinsert_slowdown_points = %2.0fus\n",
     total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
-    calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us,
+    calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us,
     calc_slowdown_points_us, insert_slowdown_points_us);
   debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_));
   virtual_wall_marker_creator.add_virtual_walls(
     out_of_lane::debug::create_virtual_walls(debug_data_, params_));
   virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
+  std::map<std::string, double> processing_times;
+  processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000;
+  processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000;
+  processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000;
+  processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000;
+  processing_times["calculate_decision"] = calculate_decisions_us / 1000;
+  processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000;
+  processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000;
+  processing_times["Total"] = total_time_us / 1000;
+  processing_diag_publisher_->publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = clock_->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
   return result;
 }
 

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

@@ -19,9 +19,11 @@
 #include "velocity_planning_result.hpp"
 
 #include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
+#include <autoware/universe_utils/ros/processing_time_publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
 #include <memory>
 #include <string>
@@ -44,6 +46,8 @@ class PluginModuleInterface
   rclcpp::Logger logger_ = rclcpp::get_logger("");
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;
+  std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> processing_diag_publisher_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
   autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
 };
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml

@@ -27,6 +27,7 @@
   <depend>lanelet2_extension</depend>
   <depend>libboost-dev</depend>
   <depend>rclcpp</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml

@@ -38,6 +38,7 @@
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -14,9 +14,11 @@
 
 #include "node.hpp"
 
+#include <autoware/motion_utils/resample/resample.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/ros/wait_for_param.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware/universe_utils/transform/transforms.hpp>
 #include <autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp>
 #include <autoware/velocity_smoother/trajectory_utils.hpp>
@@ -31,6 +33,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <functional>
+#include <map>
 #include <memory>
 #include <vector>
 
@@ -80,6 +83,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
   velocity_factor_publisher_ =
     this->create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
       "~/output/velocity_factors", 1);
+  processing_time_publisher_ = this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
+    "~/debug/total_time/processing_time_ms", 1);
 
   // Parameters
   smooth_velocity_before_planning_ = declare_parameter<bool>("smooth_velocity_before_planning");
@@ -103,8 +108,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
     std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1));
 
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
-  published_time_publisher_ =
-    std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
 }
 
 void MotionVelocityPlannerNode::on_load_plugin(
@@ -252,9 +255,14 @@ void MotionVelocityPlannerNode::on_trajectory(
 {
   std::unique_lock<std::mutex> lk(mutex_);
 
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+  std::map<std::string, double> processing_times;
+  stop_watch.tic("Total");
+
   if (!update_planner_data()) {
     return;
   }
+  processing_times["update_planner_data"] = stop_watch.toc(true);
 
   if (input_trajectory_msg->points.empty()) {
     RCLCPP_WARN(get_logger(), "Input trajectory message is empty");
@@ -264,19 +272,27 @@ void MotionVelocityPlannerNode::on_trajectory(
   if (has_received_map_) {
     planner_data_.route_handler = std::make_shared<route_handler::RouteHandler>(*map_ptr_);
     has_received_map_ = false;
+    processing_times["make_RouteHandler"] = stop_watch.toc(true);
   }
 
   autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{
     input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()};
 
   auto output_trajectory_msg = generate_trajectory(input_trajectory_points);
   output_trajectory_msg.header = input_trajectory_msg->header;
+  processing_times["generate_trajectory"] = stop_watch.toc(true);
 
   lk.unlock();
 
   trajectory_pub_->publish(output_trajectory_msg);
-  published_time_publisher_->publish_if_subscribed(
+  published_time_publisher_.publish_if_subscribed(
     trajectory_pub_, output_trajectory_msg.header.stamp);
+  processing_times["Total"] = stop_watch.toc("Total");
+  processing_diag_publisher_.publish(processing_times);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = processing_times["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
 }
 
 void MotionVelocityPlannerNode::insert_stop(
@@ -365,9 +381,11 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj
   output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()};
   if (smooth_velocity_before_planning_)
     input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_);
+  const auto resampled_trajectory =
+    autoware::motion_utils::resampleTrajectory(output_trajectory_msg, 0.5);
 
   const auto planning_results = planner_manager_.plan_velocities(
-    input_trajectory_points, std::make_shared<const PlannerData>(planner_data_));
+    resampled_trajectory.points, std::make_shared<const PlannerData>(planner_data_));
 
   autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors;
   velocity_factors.header.frame_id = "map";

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp

@@ -32,6 +32,7 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <tf2_ros/buffer.h>
@@ -96,6 +97,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
   rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
     velocity_factor_publisher_;
+  autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this};
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
+  autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this};
 
   //  parameters
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_;
@@ -139,8 +143,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node
     autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points);
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
-
-  std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
 };
 }  // namespace autoware::motion_velocity_planner