From 6b13edc0f409d2d37452168e6ff010e7c914a03f Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 24 Jun 2024 10:12:00 +0900 Subject: [PATCH 1/4] feat(motion_velocity_planner): publish processing times (#7633) Signed-off-by: Maxime CLEMENT --- .../src/dynamic_obstacle_stop_module.cpp | 9 +++++++++ .../src/obstacle_velocity_limiter_module.cpp | 9 +++++++++ .../src/out_of_lane_module.cpp | 17 +++++++++++++++-- .../plugin_module_interface.hpp | 2 ++ .../src/node.cpp | 11 +++++++++++ .../src/node.hpp | 1 + 6 files changed, 47 insertions(+), 2 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index fde54a7041672..bf9dc30145cd3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -48,6 +49,8 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_time_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms"); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -179,6 +182,12 @@ 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 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_time_publisher_->publish(processing_times); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 2b78a2f1495bf..5a4f03f6f20a6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -34,6 +34,7 @@ #include #include +#include namespace autoware::motion_velocity_planner { @@ -52,6 +53,8 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_time_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms"); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -218,6 +221,12 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( safe_footprint_polygons, obstacle_masks, planner_data->current_odometry.pose.pose.position.z)); } + std::map 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_time_publisher_->publish(processing_times); return result; } } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index f6d81f6f036d0..18a37e74df9aa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -33,6 +33,7 @@ #include +#include #include #include #include @@ -56,6 +57,8 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_time_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms"); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -207,7 +210,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 +291,22 @@ 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 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_time_publisher_->publish(processing_times); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 535510453b8ba..bf0be64a0880d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -19,6 +19,7 @@ #include "velocity_planning_result.hpp" #include +#include #include #include @@ -44,6 +45,7 @@ class PluginModuleInterface rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + std::shared_ptr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; }; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 714b6fd948a32..da997dcfbc6e4 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include #include +#include #include #include @@ -252,9 +254,14 @@ void MotionVelocityPlannerNode::on_trajectory( { std::unique_lock lk(mutex_); + autoware::universe_utils::StopWatch stop_watch; + std::map 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,6 +271,7 @@ void MotionVelocityPlannerNode::on_trajectory( if (has_received_map_) { planner_data_.route_handler = std::make_shared(*map_ptr_); has_received_map_ = false; + processing_times["make_RouteHandler"] = stop_watch.toc(true); } autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ @@ -271,12 +279,15 @@ void MotionVelocityPlannerNode::on_trajectory( 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( trajectory_pub_, output_trajectory_msg.header.stamp); + processing_times["Total"] = stop_watch.toc("Total"); + processing_time_publisher_.publish(processing_times); } void MotionVelocityPlannerNode::insert_stop( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 7b771d0b04442..04e3e6b02c7b0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -96,6 +96,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; + autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; // parameters rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; From 9e557401069345ae8f9b1d45e62a91da49e51767 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 25 Jun 2024 19:57:39 +0900 Subject: [PATCH 2/4] feat(motion_velocity_planner, lane_departure_checker): add processing time Float64 publishers (#7683) Signed-off-by: Maxime CLEMENT --- .../lane_departure_checker_node.hpp | 4 +++- .../lane_departure_checker_node.cpp | 8 +++++++- .../src/dynamic_obstacle_stop_module.cpp | 12 +++++++++--- .../src/obstacle_velocity_limiter_module.cpp | 12 +++++++++--- .../src/out_of_lane_module.cpp | 12 +++++++++--- .../plugin_module_interface.hpp | 4 +++- .../package.xml | 1 + .../package.xml | 1 + .../src/node.cpp | 12 ++++++++---- .../src/node.hpp | 7 ++++--- 10 files changed, 54 insertions(+), 19 deletions(-) diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index e6421b2af84bb..876797a58df25 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/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::SharedPtr processing_time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 427cf0898470e..dde37b03ffecc 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/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("~/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( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index bf9dc30145cd3..12cb59ac46195 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -49,8 +49,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_time_publisher_ = std::make_shared( - &node, "~/debug/" + ns_ + "/processing_time_ms"); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -187,7 +189,11 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( processing_times["footprints"] = footprints_duration_us / 1000; processing_times["collisions"] = collisions_duration_us / 1000; processing_times["Total"] = total_time_us / 1000; - processing_time_publisher_->publish(processing_times); + 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; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 5a4f03f6f20a6..eb8a33ede2c72 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -53,8 +53,10 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_time_publisher_ = std::make_shared( - &node, "~/debug/" + ns_ + "/processing_time_ms"); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -226,7 +228,11 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( processing_times["obstacles"] = obstacles_us / 1000; processing_times["slowdowns"] = slowdowns_us / 1000; processing_times["Total"] = total_us / 1000; - processing_time_publisher_->publish(processing_times); + 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 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 18a37e74df9aa..8397d0c116090 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -57,8 +57,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_time_publisher_ = std::make_shared( - &node, "~/debug/" + ns_ + "/processing_time_ms"); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -306,7 +308,11 @@ VelocityPlanningResult OutOfLaneModule::plan( 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_time_publisher_->publish(processing_times); + 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; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index bf0be64a0880d..9bd662af697ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -45,7 +46,8 @@ class PluginModuleInterface rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; - std::shared_ptr processing_time_publisher_; + std::shared_ptr processing_diag_publisher_; + rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; }; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index d2a3e305180d5..e428719f6e32f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -27,6 +27,7 @@ lanelet2_extension libboost-dev rclcpp + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index c00e0dd856200..aad25d5fb20a1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -38,6 +38,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index da997dcfbc6e4..6578c5cfaca65 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -82,6 +82,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); + processing_time_publisher_ = this->create_publisher( + "~/debug/total_time/processing_time_ms", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); @@ -105,8 +107,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); } void MotionVelocityPlannerNode::on_load_plugin( @@ -284,10 +284,14 @@ void MotionVelocityPlannerNode::on_trajectory( 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_time_publisher_.publish(processing_times); + 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( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 04e3e6b02c7b0..8debb9cbedf00 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -96,7 +97,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; - autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + rclcpp::Publisher::SharedPtr processing_time_publisher_; + autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; // parameters rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; @@ -140,8 +143,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_velocity_planner From e403943d3fe9a05aca938b3538335578298e977d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 28 Jun 2024 14:14:48 +0900 Subject: [PATCH 3/4] perf(dynamic_obstacle_stop): create rtree with packing algorithm (#7730) Signed-off-by: Maxime CLEMENT --- .../src/footprint.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 9695146a2ad38..73189e732b312 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/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 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( 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 From 91da5749330e2085323ca319aab4bf9a9d881512 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 1 Jul 2024 09:57:19 +0900 Subject: [PATCH 4/4] perf(motion_velocity_planner): resample trajectory after vel smoothing (#7732) * perf(dynamic_obstacle_stop): create rtree with packing algorithm Signed-off-by: Maxime CLEMENT * Revert "perf(out_of_lane): downsample the trajectory to improve performance (#7691)" This reverts commit 8444a9eb29b32f500be3724dd5662013b9b81060. * perf(motion_velocity_planner): resample trajectory after vel smoothing Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_planner_node/src/node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 6578c5cfaca65..2bc820d74693b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -14,6 +14,7 @@ #include "node.hpp" +#include #include #include #include @@ -380,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(planner_data_)); + resampled_trajectory.points, std::make_shared(planner_data_)); autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; velocity_factors.header.frame_id = "map";