From 63e039959a47253c5a98c0cd21330010327cfeb7 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] 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