Skip to content

Commit 38aa207

Browse files
maxime-clemKhalilSelyan
authored and
KhalilSelyan
committed
feat(motion_velocity_planner): publish processing times (#7633)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent ebee6d8 commit 38aa207

File tree

6 files changed

+47
-2
lines changed

6 files changed

+47
-2
lines changed

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

+9
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include <autoware/universe_utils/system/stop_watch.hpp>
3030

3131
#include <algorithm>
32+
#include <map>
3233
#include <memory>
3334
#include <string>
3435
#include <utility>
@@ -48,6 +49,8 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo
4849
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
4950
virtual_wall_publisher_ =
5051
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
52+
processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
53+
&node, "~/debug/" + ns_ + "/processing_time_ms");
5154

5255
using autoware::universe_utils::getOrDeclareParameter;
5356
auto & p = params_;
@@ -179,6 +182,12 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
179182
debug_data_.ego_footprints = ego_data.trajectory_footprints;
180183
debug_data_.obstacle_footprints = obstacle_forward_footprints;
181184
debug_data_.z = ego_data.pose.position.z;
185+
std::map<std::string, double> processing_times;
186+
processing_times["preprocessing"] = preprocessing_duration_us / 1000;
187+
processing_times["footprints"] = footprints_duration_us / 1000;
188+
processing_times["collisions"] = collisions_duration_us / 1000;
189+
processing_times["Total"] = total_time_us / 1000;
190+
processing_time_publisher_->publish(processing_times);
182191
return result;
183192
}
184193

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

+9
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434

3535
#include <algorithm>
3636
#include <chrono>
37+
#include <map>
3738

3839
namespace autoware::motion_velocity_planner
3940
{
@@ -52,6 +53,8 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string
5253
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
5354
virtual_wall_publisher_ =
5455
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
56+
processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
57+
&node, "~/debug/" + ns_ + "/processing_time_ms");
5558

5659
const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
5760
vehicle_lateral_offset_ = static_cast<double>(vehicle_info.max_lateral_offset_m);
@@ -218,6 +221,12 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
218221
safe_footprint_polygons, obstacle_masks,
219222
planner_data->current_odometry.pose.pose.position.z));
220223
}
224+
std::map<std::string, double> processing_times;
225+
processing_times["preprocessing"] = preprocessing_us / 1000;
226+
processing_times["obstacles"] = obstacles_us / 1000;
227+
processing_times["slowdowns"] = slowdowns_us / 1000;
228+
processing_times["Total"] = total_us / 1000;
229+
processing_time_publisher_->publish(processing_times);
221230
return result;
222231
}
223232
} // namespace autoware::motion_velocity_planner

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

+15-2
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333

3434
#include <lanelet2_core/geometry/LaneletMap.h>
3535

36+
#include <map>
3637
#include <memory>
3738
#include <string>
3839
#include <utility>
@@ -56,6 +57,8 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
5657
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
5758
virtual_wall_publisher_ =
5859
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
60+
processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
61+
&node, "~/debug/" + ns_ + "/processing_time_ms");
5962
}
6063
void OutOfLaneModule::init_parameters(rclcpp::Node & node)
6164
{
@@ -207,7 +210,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
207210
inputs.ego_data = ego_data;
208211
stopwatch.tic("filter_predicted_objects");
209212
inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_);
210-
const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects");
213+
const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects");
211214
inputs.route_handler = planner_data->route_handler;
212215
inputs.lanelets = other_lanelets;
213216
stopwatch.tic("calculate_decisions");
@@ -288,12 +291,22 @@ VelocityPlanningResult OutOfLaneModule::plan(
288291
"\tcalc_slowdown_points = %2.0fus\n"
289292
"\tinsert_slowdown_points = %2.0fus\n",
290293
total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
291-
calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us,
294+
calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us,
292295
calc_slowdown_points_us, insert_slowdown_points_us);
293296
debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_));
294297
virtual_wall_marker_creator.add_virtual_walls(
295298
out_of_lane::debug::create_virtual_walls(debug_data_, params_));
296299
virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
300+
std::map<std::string, double> processing_times;
301+
processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000;
302+
processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000;
303+
processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000;
304+
processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000;
305+
processing_times["calculate_decision"] = calculate_decisions_us / 1000;
306+
processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000;
307+
processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000;
308+
processing_times["Total"] = total_time_us / 1000;
309+
processing_time_publisher_->publish(processing_times);
297310
return result;
298311
}
299312

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

+2
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include "velocity_planning_result.hpp"
2020

2121
#include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
22+
#include <autoware/universe_utils/ros/processing_time_publisher.hpp>
2223
#include <rclcpp/rclcpp.hpp>
2324

2425
#include <autoware_planning_msgs/msg/trajectory_point.hpp>
@@ -44,6 +45,7 @@ class PluginModuleInterface
4445
rclcpp::Logger logger_ = rclcpp::get_logger("");
4546
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
4647
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;
48+
std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> processing_time_publisher_;
4749
autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
4850
};
4951

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

+11
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#include <autoware/motion_utils/trajectory/trajectory.hpp>
1818
#include <autoware/universe_utils/ros/update_param.hpp>
1919
#include <autoware/universe_utils/ros/wait_for_param.hpp>
20+
#include <autoware/universe_utils/system/stop_watch.hpp>
2021
#include <autoware/universe_utils/transform/transforms.hpp>
2122
#include <autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp>
2223
#include <autoware/velocity_smoother/trajectory_utils.hpp>
@@ -31,6 +32,7 @@
3132
#include <pcl_conversions/pcl_conversions.h>
3233

3334
#include <functional>
35+
#include <map>
3436
#include <memory>
3537
#include <vector>
3638

@@ -252,9 +254,14 @@ void MotionVelocityPlannerNode::on_trajectory(
252254
{
253255
std::unique_lock<std::mutex> lk(mutex_);
254256

257+
autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
258+
std::map<std::string, double> processing_times;
259+
stop_watch.tic("Total");
260+
255261
if (!update_planner_data()) {
256262
return;
257263
}
264+
processing_times["update_planner_data"] = stop_watch.toc(true);
258265

259266
if (input_trajectory_msg->points.empty()) {
260267
RCLCPP_WARN(get_logger(), "Input trajectory message is empty");
@@ -264,19 +271,23 @@ void MotionVelocityPlannerNode::on_trajectory(
264271
if (has_received_map_) {
265272
planner_data_.route_handler = std::make_shared<route_handler::RouteHandler>(*map_ptr_);
266273
has_received_map_ = false;
274+
processing_times["make_RouteHandler"] = stop_watch.toc(true);
267275
}
268276

269277
autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{
270278
input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()};
271279

272280
auto output_trajectory_msg = generate_trajectory(input_trajectory_points);
273281
output_trajectory_msg.header = input_trajectory_msg->header;
282+
processing_times["generate_trajectory"] = stop_watch.toc(true);
274283

275284
lk.unlock();
276285

277286
trajectory_pub_->publish(output_trajectory_msg);
278287
published_time_publisher_->publish_if_subscribed(
279288
trajectory_pub_, output_trajectory_msg.header.stamp);
289+
processing_times["Total"] = stop_watch.toc("Total");
290+
processing_time_publisher_.publish(processing_times);
280291
}
281292

282293
void MotionVelocityPlannerNode::insert_stop(

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node
9696
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
9797
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
9898
velocity_factor_publisher_;
99+
autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
99100

100101
// parameters
101102
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_;

0 commit comments

Comments
 (0)