From 241dddbf2211e6d8db0da71f34f7094d3bff1de0 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Mon, 2 Sep 2024 19:30:06 +0900 Subject: [PATCH 01/21] first commit Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../CMakeLists.txt | 22 +++++++++++++++++++ .../msg/Metric.msg | 8 +++++++ .../msg/MetricList.msg | 5 +++++ .../autoware_planning_evaluator/package.xml | 5 +++++ 4 files changed, 40 insertions(+) create mode 100644 evaluator/autoware_planning_evaluator/msg/Metric.msg create mode 100644 evaluator/autoware_planning_evaluator/msg/MetricList.msg diff --git a/evaluator/autoware_planning_evaluator/CMakeLists.txt b/evaluator/autoware_planning_evaluator/CMakeLists.txt index 371182bd2e388..792803e93226a 100644 --- a/evaluator/autoware_planning_evaluator/CMakeLists.txt +++ b/evaluator/autoware_planning_evaluator/CMakeLists.txt @@ -10,6 +10,28 @@ ament_auto_add_library(planning_evaluator_node SHARED DIRECTORY src ) + +# ------------------------------------------------------------------------------ +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/Metric.msg" + "msg/MetricList.msg" + DEPENDENCIES builtin_interfaces +) + +# For using message definitions from the same package +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(planning_evaluator_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(planning_evaluator_node "${cpp_typesupport_target}") +endif() +# ------------------------------------------------------------------------------ + rclcpp_components_register_node(planning_evaluator_node PLUGIN "planning_diagnostics::PlanningEvaluatorNode" EXECUTABLE planning_evaluator diff --git a/evaluator/autoware_planning_evaluator/msg/Metric.msg b/evaluator/autoware_planning_evaluator/msg/Metric.msg new file mode 100644 index 0000000000000..02ea0122d2078 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/msg/Metric.msg @@ -0,0 +1,8 @@ +# name of the metric +string name + +# unit (or type) of the metric +string unit + +# value of the metric +string value \ No newline at end of file diff --git a/evaluator/autoware_planning_evaluator/msg/MetricList.msg b/evaluator/autoware_planning_evaluator/msg/MetricList.msg new file mode 100644 index 0000000000000..b27c357b1fce7 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/msg/MetricList.msg @@ -0,0 +1,5 @@ +# time stamp +builtin_interface/Time stamp + +# metrics list +Mtric[] metric_list diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 38d83d46e915d..a71791a5e1a64 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -6,12 +6,15 @@ ROS 2 node for evaluating planners Maxime CLEMENT Kyoichi Sugahara + Temkei Kem Apache License 2.0 Maxime CLEMENT ament_cmake_auto autoware_cmake + rosidl_default_generators + ament_cmake_python autoware_evaluator_utils autoware_motion_utils @@ -28,6 +31,8 @@ rclcpp_components tf2 tf2_ros + + rosidl_interface_packages ament_cmake_ros ament_lint_auto From 735f082512447b28bcb81fc934ffb5850b0aad5e Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Mon, 2 Sep 2024 19:57:05 +0900 Subject: [PATCH 02/21] fix building errs. Signed-off-by: xtk8532704 <1041084556@qq.com> --- evaluator/autoware_planning_evaluator/CMakeLists.txt | 9 +++------ evaluator/autoware_planning_evaluator/msg/MetricList.msg | 4 ++-- evaluator/autoware_planning_evaluator/package.xml | 2 +- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/CMakeLists.txt b/evaluator/autoware_planning_evaluator/CMakeLists.txt index 792803e93226a..836409f753241 100644 --- a/evaluator/autoware_planning_evaluator/CMakeLists.txt +++ b/evaluator/autoware_planning_evaluator/CMakeLists.txt @@ -1,19 +1,17 @@ -cmake_minimum_required(VERSION 3.14) +cmake_minimum_required(VERSION 3.22) project(autoware_planning_evaluator) find_package(autoware_cmake REQUIRED) autoware_package() find_package(pluginlib REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) ament_auto_add_library(planning_evaluator_node SHARED DIRECTORY src ) - -# ------------------------------------------------------------------------------ -find_package(builtin_interfaces REQUIRED) -find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces( ${PROJECT_NAME} "msg/Metric.msg" @@ -30,7 +28,6 @@ else() cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") target_link_libraries(planning_evaluator_node "${cpp_typesupport_target}") endif() -# ------------------------------------------------------------------------------ rclcpp_components_register_node(planning_evaluator_node PLUGIN "planning_diagnostics::PlanningEvaluatorNode" diff --git a/evaluator/autoware_planning_evaluator/msg/MetricList.msg b/evaluator/autoware_planning_evaluator/msg/MetricList.msg index b27c357b1fce7..3964dfba47153 100644 --- a/evaluator/autoware_planning_evaluator/msg/MetricList.msg +++ b/evaluator/autoware_planning_evaluator/msg/MetricList.msg @@ -1,5 +1,5 @@ # time stamp -builtin_interface/Time stamp +builtin_interfaces/Time stamp # metrics list -Mtric[] metric_list +Metric[] metric_list diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index a71791a5e1a64..61312ec710d06 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -14,7 +14,6 @@ ament_cmake_auto autoware_cmake rosidl_default_generators - ament_cmake_python autoware_evaluator_utils autoware_motion_utils @@ -31,6 +30,7 @@ rclcpp_components tf2 tf2_ros + builtin_interfaces rosidl_interface_packages From 2250f599d7152991c6267f331a3ce6f8db5d6ac3 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 3 Sep 2024 17:41:22 +0900 Subject: [PATCH 03/21] change diagnostic messages to metric messages for publishing decision. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../launch/motion_velocity_planner.launch.xml | 1 + .../package.xml | 2 +- .../src/node.cpp | 13 ++--- .../src/node.hpp | 5 +- .../src/planner_manager.cpp | 55 ++++++++----------- .../src/planner_manager.hpp | 18 +++--- 6 files changed, 43 insertions(+), 51 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml index 3732d86ef380a..be748ec7c7ef2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -27,6 +27,7 @@ + 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 bf4e8a242d576..f8f3ed9d13d95 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 @@ -25,7 +25,7 @@ autoware_planning_msgs autoware_universe_utils autoware_velocity_smoother - diagnostic_msgs + tier4_metric_msgs eigen geometry_msgs libboost-dev 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 eaa3b8c17402e..fb52720b5c176 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 @@ -26,7 +26,6 @@ #include #include -#include #include #include @@ -89,7 +88,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & this->create_publisher("~/debug/processing_time_ms", 1); debug_viz_pub_ = this->create_publisher("~/debug/markers", 1); - diagnostics_pub_ = this->create_publisher("/diagnostics", 10); + metrics_pub_ = this->create_publisher("/metrics", 10); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); @@ -307,12 +306,12 @@ void MotionVelocityPlannerNode::on_trajectory( processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); - std::shared_ptr diagnostics = - planner_manager_.get_diagnostics(get_clock()->now()); - if (!diagnostics->status.empty()) { - diagnostics_pub_->publish(*diagnostics); + std::shared_ptr metrics = + planner_manager_.get_metrics(get_clock()->now()); + if (!metrics->metric_array.empty()) { + metrics_pub_->publish(*metrics); } - planner_manager_.clear_diagnostics(); + planner_manager_.clear_metrics(); } 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 8283396d17414..47253ca658751 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 @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -44,7 +45,7 @@ #include #include -using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; +using MetricArray = tier4_metric_msgs::msg::MetricArray; namespace autoware::motion_velocity_planner { @@ -104,7 +105,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; - rclcpp::Publisher::SharedPtr diagnostics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; // parameters rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index 04641e0cea6bb..b4ae24e319529 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -71,41 +71,32 @@ void MotionVelocityPlannerManager::update_module_parameters( for (auto & plugin : loaded_plugins_) plugin->update_parameters(parameters); } -std::shared_ptr MotionVelocityPlannerManager::make_diagnostic( + +std::shared_ptr MotionVelocityPlannerManager::make_decision_metric( const std::string & module_name, const std::string & reason, const bool is_decided) { - auto status = std::make_shared(); - status->level = status->OK; - status->name = module_name + '.' + reason; - diagnostic_msgs::msg::KeyValue key_value; - { - // Decision - key_value.key = "decision"; - if (is_decided) - key_value.value = reason; - else - key_value.value = "none"; - status->values.push_back(key_value); - } - // Add other information to the status if necessary in the future. - return status; + auto metric = std::make_shared(); + metric->name = module_name + '/' + 'decision'; + metric->unit = "string"; + if (is_decided) + metric->value = reason; + else + metric->value = "none"; + return metric; } -std::shared_ptr MotionVelocityPlannerManager::get_diagnostics( +std::shared_ptr MotionVelocityPlannerManager::get_metrics( const rclcpp::Time & current_time) const { - auto diagnostics = std::make_shared(); + auto metrics = std::make_shared(); - for (const auto & ds_ptr : diagnostics_) { - if ( - !ds_ptr->values.empty() && ds_ptr->values[0].key == "decision" && - ds_ptr->values[0].value != "none") { - diagnostics->status.push_back(*ds_ptr); + for (const auto & mtr_ptr : metrics_) { + if (mtr_ptr.value != "none") { + metrics->metric_array.push_back(*mtr_ptr); } } - diagnostics->header.stamp = current_time; - diagnostics->header.frame_id = "map"; - return diagnostics; + metrics->stamp = current_time; + return metrics; } std::vector MotionVelocityPlannerManager::plan_velocities( @@ -117,13 +108,13 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); - const auto stop_reason_diag = - make_diagnostic(plugin->get_module_name(), "stop", res.stop_points.size() > 0); - diagnostics_.push_back(stop_reason_diag); + const auto stop_decition_metric = + make_decision_metric(plugin->get_module_name(), "stop", res.stop_points.size() > 0); + metrics_.push_back(stop_decition_metric); - const auto slow_down_reason_diag = - make_diagnostic(plugin->get_module_name(), "slow_down", res.slowdown_intervals.size() > 0); - diagnostics_.push_back(slow_down_reason_diag); + const auto slow_down_decition_metric = + make_decision_metric(plugin->get_module_name(), "slow_down", res.slowdown_intervals.size() > 0); + metrics_.push_back(slow_down_decition_metric); } return results; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp index ac2e421d30cb6..753843b8908f7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -23,8 +23,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -36,8 +36,8 @@ #include #include -using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; -using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; namespace autoware::motion_velocity_planner { @@ -52,14 +52,14 @@ class MotionVelocityPlannerManager const std::vector & ego_trajectory_points, const std::shared_ptr planner_data); - // Diagnostic - std::shared_ptr make_diagnostic( + // Metrics + std::shared_ptr make_metric( const std::string & module_name, const std::string & reason, const bool is_decided = true); - std::shared_ptr get_diagnostics(const rclcpp::Time & current_time) const; - void clear_diagnostics() { diagnostics_.clear(); } + std::shared_ptr get_metrics(const rclcpp::Time & current_time) const; + void clear_metrics() { metrics_.clear(); } private: - std::vector> diagnostics_; + std::vector> metrics_; pluginlib::ClassLoader plugin_loader_; std::vector> loaded_plugins_; }; From b957f8bf3aac4a2ad2a36da46a85d5c107323b9b Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 4 Sep 2024 00:26:08 +0900 Subject: [PATCH 04/21] fix bug about motion_velocity_planner Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware_motion_velocity_planner_node/package.xml | 2 +- .../autoware_motion_velocity_planner_node/src/node.hpp | 3 --- .../src/planner_manager.cpp | 6 +++--- .../src/planner_manager.hpp | 6 +++--- 4 files changed, 7 insertions(+), 10 deletions(-) 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 f8f3ed9d13d95..ae7e80e927988 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 @@ -25,7 +25,6 @@ autoware_planning_msgs autoware_universe_utils autoware_velocity_smoother - tier4_metric_msgs eigen geometry_msgs libboost-dev @@ -40,6 +39,7 @@ tf2_ros tier4_debug_msgs tier4_planning_msgs + tier4_metric_msgs visualization_msgs rosidl_default_runtime 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 47253ca658751..757be518e018a 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 @@ -33,7 +33,6 @@ #include #include #include -#include #include #include @@ -45,8 +44,6 @@ #include #include -using MetricArray = tier4_metric_msgs::msg::MetricArray; - namespace autoware::motion_velocity_planner { using autoware_map_msgs::msg::LaneletMapBin; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index b4ae24e319529..af816f6f77679 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -76,7 +76,7 @@ std::shared_ptr MotionVelocityPlannerManager::make_decision_metric( const std::string & module_name, const std::string & reason, const bool is_decided) { auto metric = std::make_shared(); - metric->name = module_name + '/' + 'decision'; + metric->name = module_name + "/decision"; metric->unit = "string"; if (is_decided) metric->value = reason; @@ -89,13 +89,13 @@ std::shared_ptr MotionVelocityPlannerManager::get_metrics( const rclcpp::Time & current_time) const { auto metrics = std::make_shared(); + metrics->stamp = current_time; for (const auto & mtr_ptr : metrics_) { - if (mtr_ptr.value != "none") { + if (mtr_ptr->value != "none") { metrics->metric_array.push_back(*mtr_ptr); } } - metrics->stamp = current_time; return metrics; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp index 753843b8908f7..47180f9bc5a46 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -23,8 +23,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -53,7 +53,7 @@ class MotionVelocityPlannerManager const std::shared_ptr planner_data); // Metrics - std::shared_ptr make_metric( + std::shared_ptr make_decision_metric( const std::string & module_name, const std::string & reason, const bool is_decided = true); std::shared_ptr get_metrics(const rclcpp::Time & current_time) const; void clear_metrics() { metrics_.clear(); } From 9d9a5bc34ebeb0e14be5400148bc4e39012ed5e9 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 4 Sep 2024 01:20:47 +0900 Subject: [PATCH 05/21] change the diagnostic msg to metric msg in autoware_obstacle_cruise_planner. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../common_structs.hpp | 12 +- .../planner_interface.hpp | 19 +-- .../package.xml | 1 + .../src/node.cpp | 2 +- .../optimization_based_planner.cpp | 4 +- .../pid_based_planner/pid_based_planner.cpp | 2 +- .../src/planner_interface.cpp | 120 +++++++++--------- 7 files changed, 84 insertions(+), 76 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index 0f35960c350a8..7bd4d37ba5797 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -25,12 +25,16 @@ #include -#include +#include +#include #include #include #include +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; + struct PlannerData { rclcpp::Time current_time; @@ -293,9 +297,9 @@ struct DebugData MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; std::vector detection_polygons; - std::optional stop_reason_diag{std::nullopt}; - std::optional slow_down_reason_diag{std::nullopt}; - std::optional cruise_reason_diag{std::nullopt}; + std::optional> stop_metrics{std::nullopt}; + std::optional> slow_down_metrics{std::nullopt}; + std::optional> cruise_metrics{std::nullopt}; }; struct EgoNearestParam diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 099d80306bb4e..7b17f7b003976 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -23,7 +23,8 @@ #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include +#include +#include #include #include @@ -35,8 +36,9 @@ #include #include -using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; -using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; + class PlannerInterface { public: @@ -56,7 +58,7 @@ class PlannerInterface node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); - diagnostics_pub_ = node.create_publisher("/diagnostics", 10); + metrics_pub_ = node.create_publisher("/metrics", 10); moving_object_speed_threshold = node.declare_parameter("slow_down.moving_object_speed_threshold"); @@ -95,13 +97,12 @@ class PlannerInterface const std::vector & slow_down_obstacles, std::optional & vel_limit); - DiagnosticStatus makeEmptyDiagnostic(const std::string & reason); - DiagnosticStatus makeDiagnostic( + std::vector makeDicisionMetrics( const std::string & reason, const std::optional & planner_data = std::nullopt, const std::optional & stop_pose = std::nullopt, const std::optional & stop_obstacle = std::nullopt); - void publishDiagnostics(const rclcpp::Time & current_time); - void clearDiagnostics(); + void publishMetrics(const rclcpp::Time & current_time); + void clearMetrics(); void onParam(const std::vector & parameters) { @@ -148,7 +149,7 @@ class PlannerInterface rclcpp::Publisher::SharedPtr stop_reasons_pub_; rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; // Vehicle Parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index d73f86d27a781..58420072b2ac8 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -39,6 +39,7 @@ tf2_ros tier4_debug_msgs tier4_planning_msgs + tier4_metric_msgs visualization_msgs ament_cmake_ros diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 37b82ecb66e74..a6a029530b1a5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -719,7 +719,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); - planner_ptr_->publishDiagnostics(now()); + planner_ptr_->publishMetrics(now()); publishDebugMarker(); publishDebugInfo(); diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 3a24cfa7dcfe0..9711fbc4c4fee 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -199,7 +199,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( output.at(i).longitudinal_velocity_mps = 0.0; } prev_output_ = output; - debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeDicisionMetrics("cruise", planner_data); return output; } else if (opt_position.size() == 1) { RCLCPP_DEBUG( @@ -256,7 +256,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Insert Closest Stop Point autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); - debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeDicisionMetrics("cruise", planner_data); prev_output_ = output; return output; } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index fefe7fd06007e..cca7a5137c8c5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -325,7 +325,7 @@ std::vector PIDBasedPlanner::planCruise( // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); - debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeDicisionMetrics("cruise", planner_data); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index ba93efc1f10c4..f1cd5efb12e5e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -405,8 +405,8 @@ std::vector PlannerInterface::generateStopTrajectory( stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); // Store stop reason debug data - debug_data_ptr_->stop_reason_diag = - makeDiagnostic("stop", planner_data, stop_pose, *determined_stop_obstacle); + debug_data_ptr_->stop_metrics = + makeDicisionMetrics("stop", planner_data, stop_pose, *determined_stop_obstacle); // Publish if ego vehicle will over run against the stop point with a limit acceleration const bool will_over_run = determined_zero_vel_dist.value() > @@ -677,8 +677,8 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // Add debug data debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); - if (!debug_data_ptr_->slow_down_reason_diag.has_value()) { - debug_data_ptr_->slow_down_reason_diag = makeDiagnostic("slow_down", planner_data); + if (!debug_data_ptr_->stop_metrics.has_value()) { + debug_data_ptr_->slow_down_metrics = makeDicisionMetrics("slow_down", planner_data); } // update prev_slow_down_output_ @@ -837,89 +837,91 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); } -DiagnosticStatus PlannerInterface::makeEmptyDiagnostic(const std::string & reason) -{ - // Create status - DiagnosticStatus status; - status.level = status.OK; - status.name = "obstacle_cruise_planner_" + reason; - diagnostic_msgs::msg::KeyValue key_value; - { - // Decision - key_value.key = "decision"; - key_value.value = "none"; - status.values.push_back(key_value); - } - - return status; -} - -DiagnosticStatus PlannerInterface::makeDiagnostic( +std::vector PlannerInterface::makeDicisionMetrics( const std::string & reason, const std::optional & planner_data, const std::optional & stop_pose, const std::optional & stop_obstacle) { + + auto metrics = std::vector(); + // Create status - DiagnosticStatus status; - status.level = status.OK; - status.name = "obstacle_cruise_planner_" + reason; - diagnostic_msgs::msg::KeyValue key_value; + std::string metrics_name = "obstacle_cruise_planner/" + reason; { // Decision - key_value.key = "decision"; - key_value.value = reason; - status.values.push_back(key_value); + Metric decision_metric; + decision_metric.name = metrics_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics.push_back(decision_metric); } if (stop_pose.has_value() && planner_data.has_value()) { // Stop info - key_value.key = "stop_position"; + Metric stop_posision_metric; + stop_posision_metric.name = metrics_name + "/stop_position"; + stop_posision_metric.unit = "string"; const auto & p = stop_pose.value().position; - key_value.value = + stop_posision_metric.value = "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; - status.values.push_back(key_value); - key_value.key = "stop_orientation"; + metrics.push_back(stop_posision_metric); + + Metric stop_orientation_metric; + stop_orientation_metric.name = metrics_name + "/stop_orientation"; + stop_orientation_metric.unit = "string"; const auto & o = stop_pose.value().orientation; - key_value.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + - std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; - status.values.push_back(key_value); + stop_orientation_metric.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + + std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; + metrics.push_back(stop_orientation_metric); + const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( planner_data.value().traj_points, planner_data.value().ego_pose.position, stop_pose.value().position); - key_value.key = "distance_to_stop_pose"; - key_value.value = std::to_string(dist_to_stop_pose); - status.values.push_back(key_value); + + Metric dist_to_stop_pose_metric; + dist_to_stop_pose_metric.name = metrics_name + "/distance_to_stop_pose"; + dist_to_stop_pose_metric.unit = "double"; + dist_to_stop_pose_metric.value = std::to_string(dist_to_stop_pose); + metrics.push_back(dist_to_stop_pose_metric); } if (stop_obstacle.has_value()) { // Obstacle info + Metric collision_point_metric; const auto & p = stop_obstacle.value().collision_point; - key_value.key = "collision_point"; - key_value.value = + collision_point_metric.name = metrics_name + "/collision_point"; + collision_point_metric.unit = "string"; + collision_point_metric.value = "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; - status.values.push_back(key_value); + metrics.push_back(collision_point_metric); } - - return status; + return metrics; } -void PlannerInterface::publishDiagnostics(const rclcpp::Time & current_time) +void PlannerInterface::publishMetrics(const rclcpp::Time & current_time) { // create array - DiagnosticArray diagnostics; - diagnostics.header.stamp = current_time; - diagnostics.header.frame_id = "map"; - const auto & d = debug_data_ptr_; - diagnostics.status = { - (d->stop_reason_diag) ? d->stop_reason_diag.value() : makeEmptyDiagnostic("stop"), - (d->slow_down_reason_diag) ? *(d->slow_down_reason_diag) : makeEmptyDiagnostic("slow_down"), - (d->cruise_reason_diag) ? d->cruise_reason_diag.value() : makeEmptyDiagnostic("cruise")}; - diagnostics_pub_->publish(diagnostics); - clearDiagnostics(); + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + + auto addMetrics = [&metrics_msg](std::optional>& opt_metrics) { + if (opt_metrics) { + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), + opt_metrics->begin(), + opt_metrics->end() + ); + } + }; + addMetrics(debug_data_ptr_->stop_metrics); + addMetrics(debug_data_ptr_->slow_down_metrics); + addMetrics(debug_data_ptr_->cruise_metrics); + metrics_pub_->publish(metrics_msg); + clearMetrics(); } -void PlannerInterface::clearDiagnostics() +void PlannerInterface::clearMetrics() { - debug_data_ptr_->stop_reason_diag = std::nullopt; - debug_data_ptr_->slow_down_reason_diag = std::nullopt; - debug_data_ptr_->cruise_reason_diag = std::nullopt; + debug_data_ptr_->stop_metrics = std::nullopt; + debug_data_ptr_->slow_down_metrics = std::nullopt; + debug_data_ptr_->cruise_metrics = std::nullopt; } From 5b1788455663d8f20698a8597785f7f6b66fb18f Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 4 Sep 2024 01:21:11 +0900 Subject: [PATCH 06/21] tmp save for planning_evaluator Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../CMakeLists.txt | 19 --------------- .../planning_evaluator_node.hpp | 24 +++++++++---------- .../msg/Metric.msg | 8 ------- .../msg/MetricList.msg | 5 ---- .../autoware_planning_evaluator/package.xml | 5 +--- .../src/planning_evaluator_node.cpp | 8 +++---- 6 files changed, 17 insertions(+), 52 deletions(-) delete mode 100644 evaluator/autoware_planning_evaluator/msg/Metric.msg delete mode 100644 evaluator/autoware_planning_evaluator/msg/MetricList.msg diff --git a/evaluator/autoware_planning_evaluator/CMakeLists.txt b/evaluator/autoware_planning_evaluator/CMakeLists.txt index 836409f753241..23959cefdcca4 100644 --- a/evaluator/autoware_planning_evaluator/CMakeLists.txt +++ b/evaluator/autoware_planning_evaluator/CMakeLists.txt @@ -5,30 +5,11 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(pluginlib REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(rosidl_default_generators REQUIRED) ament_auto_add_library(planning_evaluator_node SHARED DIRECTORY src ) -rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/Metric.msg" - "msg/MetricList.msg" - DEPENDENCIES builtin_interfaces -) - -# For using message definitions from the same package -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(planning_evaluator_node - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(planning_evaluator_node "${cpp_typesupport_target}") -endif() - rclcpp_components_register_node(planning_evaluator_node PLUGIN "planning_diagnostics::PlanningEvaluatorNode" EXECUTABLE planning_evaluator diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 4a84bdd2ffcd5..c80c3c78e714a 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -17,6 +17,8 @@ #include "autoware/planning_evaluator/metrics_calculator.hpp" #include "autoware/planning_evaluator/stat.hpp" +#include "tier4_metric_msgs/msg/Metric.hpp" +#include "tier4_metric_msgs/msg/MetricList.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -28,11 +30,9 @@ #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" -#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include -#include #include #include @@ -46,8 +46,8 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; +using autoware::planning_evaluator::msg::Metric; +using autoware::planning_evaluator::msg::MetricList; using nav_msgs::msg::Odometry; using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; @@ -97,28 +97,28 @@ class PlanningEvaluatorNode : public rclcpp::Node /** * @brief obtain diagnostics information */ - void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); + void onDiagnostics(const MetricList::ConstSharedPtr diag_msg); /** * @brief publish the given metric statistic */ - DiagnosticStatus generateDiagnosticStatus( + Metric generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const; /** * @brief publish current ego lane info */ - DiagnosticStatus generateDiagnosticEvaluationStatus(const DiagnosticStatus & diag); + Metric generateDiagnosticEvaluationStatus(const Metric & diag); /** * @brief publish current ego lane info */ - DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); + Metric generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief publish current ego kinematic state */ - DiagnosticStatus generateKinematicStateDiagnosticStatus( + Metric generateKinematicStateDiagnosticStatus( const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); private: @@ -163,12 +163,12 @@ class PlanningEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; - rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; autoware::route_handler::RouteHandler route_handler_; - DiagnosticArray metrics_msg_; + MetricList metrics_msg_; // Parameters std::string output_file_str_; std::string ego_frame_str_; @@ -182,7 +182,7 @@ class PlanningEvaluatorNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // queue for diagnostics and time stamp - std::deque> diag_queue_; + std::deque> diag_queue_; const std::vector target_functions_ = { "obstacle_cruise_planner_stop", "obstacle_cruise_planner_slow_down", diff --git a/evaluator/autoware_planning_evaluator/msg/Metric.msg b/evaluator/autoware_planning_evaluator/msg/Metric.msg deleted file mode 100644 index 02ea0122d2078..0000000000000 --- a/evaluator/autoware_planning_evaluator/msg/Metric.msg +++ /dev/null @@ -1,8 +0,0 @@ -# name of the metric -string name - -# unit (or type) of the metric -string unit - -# value of the metric -string value \ No newline at end of file diff --git a/evaluator/autoware_planning_evaluator/msg/MetricList.msg b/evaluator/autoware_planning_evaluator/msg/MetricList.msg deleted file mode 100644 index 3964dfba47153..0000000000000 --- a/evaluator/autoware_planning_evaluator/msg/MetricList.msg +++ /dev/null @@ -1,5 +0,0 @@ -# time stamp -builtin_interfaces/Time stamp - -# metrics list -Metric[] metric_list diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 61312ec710d06..c2f9f43b91130 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -13,7 +13,6 @@ ament_cmake_auto autoware_cmake - rosidl_default_generators autoware_evaluator_utils autoware_motion_utils @@ -21,6 +20,7 @@ autoware_planning_msgs autoware_route_handler autoware_universe_utils + tier4_metric_msgs diagnostic_msgs eigen geometry_msgs @@ -30,9 +30,6 @@ rclcpp_components tf2 tf2_ros - builtin_interfaces - - rosidl_interface_packages ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index d378d55dd2a46..1e90d7af111f2 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -62,7 +62,7 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op "~/input/diagnostics", 1, std::bind(&PlanningEvaluatorNode::onDiagnostics, this, _1)); // List of metrics to calculate and publish - metrics_pub_ = create_publisher("~/metrics", 1); + metrics_pub_ = create_publisher("~/metrics", 1); for (const std::string & selected_metric : declare_parameter>("selected_metrics")) { Metric metric = str_to_metric.at(selected_metric); @@ -111,8 +111,8 @@ void PlanningEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr } } -DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticEvaluationStatus( - const DiagnosticStatus & diag) +Metric PlanningEvaluatorNode::generateDiagnosticEvaluationStatus( + const Metric & diag) { DiagnosticStatus status; status.name = diag.name; @@ -232,7 +232,7 @@ DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( return status; } -DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( +Metric PlanningEvaluatorNode::generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const { DiagnosticStatus status; From ed9fe723655a85fd2f67fd2509d15fab305e7f62 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 4 Sep 2024 01:40:34 +0900 Subject: [PATCH 07/21] change the topic to which metrics published to. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware/obstacle_cruise_planner/planner_interface.hpp | 2 +- .../launch/obstacle_cruise_planner.launch.xml | 1 + .../launch/motion_velocity_planner.launch.xml | 2 +- .../autoware_motion_velocity_planner_node/src/node.cpp | 2 +- 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 7b17f7b003976..f5c719a8f4867 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -58,7 +58,7 @@ class PlannerInterface node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); - metrics_pub_ = node.create_publisher("/metrics", 10); + metrics_pub_ = node.create_publisher("~/metrics", 10); moving_object_speed_threshold = node.declare_parameter("slow_down.moving_object_speed_threshold"); diff --git a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml index aaf6db3e36613..018d120879ff2 100644 --- a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml +++ b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml @@ -32,5 +32,6 @@ + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml index be748ec7c7ef2..7fb3b3a1bd5f8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -27,7 +27,7 @@ - + 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 fb52720b5c176..13d767ff9ea6b 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 @@ -88,7 +88,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & this->create_publisher("~/debug/processing_time_ms", 1); debug_viz_pub_ = this->create_publisher("~/debug/markers", 1); - metrics_pub_ = this->create_publisher("/metrics", 10); + metrics_pub_ = this->create_publisher("~/metrics", 10); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); From 0ae67d3699aec7f9f0de64c48b14ed203d0939cf Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 4 Sep 2024 01:51:51 +0900 Subject: [PATCH 08/21] fix typo. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../src/planner_manager.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index af816f6f77679..30a1e7bd34bb3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -108,13 +108,13 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); - const auto stop_decition_metric = + const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop", res.stop_points.size() > 0); - metrics_.push_back(stop_decition_metric); + metrics_.push_back(stop_decision_metric); - const auto slow_down_decition_metric = + const auto slow_down_decision_metric = make_decision_metric(plugin->get_module_name(), "slow_down", res.slowdown_intervals.size() > 0); - metrics_.push_back(slow_down_decition_metric); + metrics_.push_back(slow_down_decision_metric); } return results; } From 254f1a401f856204a01678b47e42c246099801ba Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 4 Sep 2024 03:05:14 +0900 Subject: [PATCH 09/21] remove unnesessary publishing of metrics. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../planner_interface.hpp | 5 ++- .../optimization_based_planner.cpp | 4 +- .../pid_based_planner/pid_based_planner.cpp | 2 +- .../src/planner_interface.cpp | 41 +++++++++---------- .../src/planner_manager.cpp | 28 ++++++------- .../src/planner_manager.hpp | 4 +- 6 files changed, 40 insertions(+), 44 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index f5c719a8f4867..a2265ea6affa0 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -97,8 +97,9 @@ class PlannerInterface const std::vector & slow_down_obstacles, std::optional & vel_limit); - std::vector makeDicisionMetrics( - const std::string & reason, const std::optional & planner_data = std::nullopt, + std::vector makeMetrics( + const std::string & module_name, const std::string & reason, + const std::optional & planner_data = std::nullopt, const std::optional & stop_pose = std::nullopt, const std::optional & stop_obstacle = std::nullopt); void publishMetrics(const rclcpp::Time & current_time); diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 9711fbc4c4fee..5c1d60d200b1a 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -199,7 +199,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( output.at(i).longitudinal_velocity_mps = 0.0; } prev_output_ = output; - debug_data_ptr_->cruise_metrics = makeDicisionMetrics("cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeMetrics("OptimizationBasedPlanner","cruise", planner_data); return output; } else if (opt_position.size() == 1) { RCLCPP_DEBUG( @@ -256,7 +256,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Insert Closest Stop Point autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); - debug_data_ptr_->cruise_metrics = makeDicisionMetrics("cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeMetrics("OptimizationBasedPlanner","cruise", planner_data); prev_output_ = output; return output; } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index cca7a5137c8c5..761e2b05b548b 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -325,7 +325,7 @@ std::vector PIDBasedPlanner::planCruise( // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); - debug_data_ptr_->cruise_metrics = makeDicisionMetrics("cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner","cruise", planner_data); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index f1cd5efb12e5e..612726f5cd23e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -406,7 +406,7 @@ std::vector PlannerInterface::generateStopTrajectory( velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); // Store stop reason debug data debug_data_ptr_->stop_metrics = - makeDicisionMetrics("stop", planner_data, stop_pose, *determined_stop_obstacle); + makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); // Publish if ego vehicle will over run against the stop point with a limit acceleration const bool will_over_run = determined_zero_vel_dist.value() > @@ -678,7 +678,8 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // Add debug data debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); if (!debug_data_ptr_->stop_metrics.has_value()) { - debug_data_ptr_->slow_down_metrics = makeDicisionMetrics("slow_down", planner_data); + debug_data_ptr_->slow_down_metrics = + makeMetrics("PlannerInterface", "slow_down", planner_data); } // update prev_slow_down_output_ @@ -837,20 +838,19 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); } -std::vector PlannerInterface::makeDicisionMetrics( - const std::string & reason, const std::optional & planner_data, +std::vector PlannerInterface::makeMetrics( + const std::string & module_name, const std::string & reason, + const std::optional & planner_data, const std::optional & stop_pose, const std::optional & stop_obstacle) { - auto metrics = std::vector(); // Create status - std::string metrics_name = "obstacle_cruise_planner/" + reason; { // Decision Metric decision_metric; - decision_metric.name = metrics_name + "/decision"; + decision_metric.name = module_name + "/decision"; decision_metric.unit = "string"; decision_metric.value = reason; metrics.push_back(decision_metric); @@ -858,7 +858,7 @@ std::vector PlannerInterface::makeDicisionMetrics( if (stop_pose.has_value() && planner_data.has_value()) { // Stop info Metric stop_posision_metric; - stop_posision_metric.name = metrics_name + "/stop_position"; + stop_posision_metric.name = module_name + "/stop_position"; stop_posision_metric.unit = "string"; const auto & p = stop_pose.value().position; stop_posision_metric.value = @@ -866,7 +866,7 @@ std::vector PlannerInterface::makeDicisionMetrics( metrics.push_back(stop_posision_metric); Metric stop_orientation_metric; - stop_orientation_metric.name = metrics_name + "/stop_orientation"; + stop_orientation_metric.name = module_name + "/stop_orientation"; stop_orientation_metric.unit = "string"; const auto & o = stop_pose.value().orientation; stop_orientation_metric.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + @@ -878,7 +878,7 @@ std::vector PlannerInterface::makeDicisionMetrics( stop_pose.value().position); Metric dist_to_stop_pose_metric; - dist_to_stop_pose_metric.name = metrics_name + "/distance_to_stop_pose"; + dist_to_stop_pose_metric.name = module_name + "/distance_to_stop_pose"; dist_to_stop_pose_metric.unit = "double"; dist_to_stop_pose_metric.value = std::to_string(dist_to_stop_pose); metrics.push_back(dist_to_stop_pose_metric); @@ -888,7 +888,7 @@ std::vector PlannerInterface::makeDicisionMetrics( // Obstacle info Metric collision_point_metric; const auto & p = stop_obstacle.value().collision_point; - collision_point_metric.name = metrics_name + "/collision_point"; + collision_point_metric.name = module_name + "/collision_point"; collision_point_metric.unit = "string"; collision_point_metric.value = "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; @@ -902,20 +902,19 @@ void PlannerInterface::publishMetrics(const rclcpp::Time & current_time) // create array MetricArray metrics_msg; metrics_msg.stamp = current_time; - - auto addMetrics = [&metrics_msg](std::optional>& opt_metrics) { - if (opt_metrics) { - metrics_msg.metric_array.insert( - metrics_msg.metric_array.end(), - opt_metrics->begin(), - opt_metrics->end() - ); - } + + auto addMetrics = [&metrics_msg](std::optional> & opt_metrics) { + if (opt_metrics) { + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), opt_metrics->begin(), opt_metrics->end()); + } }; addMetrics(debug_data_ptr_->stop_metrics); addMetrics(debug_data_ptr_->slow_down_metrics); addMetrics(debug_data_ptr_->cruise_metrics); - metrics_pub_->publish(metrics_msg); + if (!metrics_msg.metric_array.empty()) { + metrics_pub_->publish(metrics_msg); + } clearMetrics(); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index 30a1e7bd34bb3..0cba5b16c19b5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -71,17 +71,13 @@ void MotionVelocityPlannerManager::update_module_parameters( for (auto & plugin : loaded_plugins_) plugin->update_parameters(parameters); } - std::shared_ptr MotionVelocityPlannerManager::make_decision_metric( - const std::string & module_name, const std::string & reason, const bool is_decided) + const std::string & module_name, const std::string & reason) { auto metric = std::make_shared(); metric->name = module_name + "/decision"; metric->unit = "string"; - if (is_decided) - metric->value = reason; - else - metric->value = "none"; + metric->value = reason; return metric; } @@ -92,9 +88,7 @@ std::shared_ptr MotionVelocityPlannerManager::get_metrics( metrics->stamp = current_time; for (const auto & mtr_ptr : metrics_) { - if (mtr_ptr->value != "none") { - metrics->metric_array.push_back(*mtr_ptr); - } + metrics->metric_array.push_back(*mtr_ptr); } return metrics; } @@ -108,13 +102,15 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); - const auto stop_decision_metric = - make_decision_metric(plugin->get_module_name(), "stop", res.stop_points.size() > 0); - metrics_.push_back(stop_decision_metric); - - const auto slow_down_decision_metric = - make_decision_metric(plugin->get_module_name(), "slow_down", res.slowdown_intervals.size() > 0); - metrics_.push_back(slow_down_decision_metric); + if (res.stop_points.size() > 0) { + const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop"); + metrics_.push_back(stop_decision_metric); + } + if (res.slowdown_intervals.size() > 0) { + const auto slow_down_decision_metric = + make_decision_metric(plugin->get_module_name(), "slow_down"); + metrics_.push_back(slow_down_decision_metric); + } } return results; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp index 47180f9bc5a46..0e3bd77180f6e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -23,9 +23,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -54,7 +54,7 @@ class MotionVelocityPlannerManager // Metrics std::shared_ptr make_decision_metric( - const std::string & module_name, const std::string & reason, const bool is_decided = true); + const std::string & module_name, const std::string & reason); std::shared_ptr get_metrics(const rclcpp::Time & current_time) const; void clear_metrics() { metrics_.clear(); } From bf45efcb33f1d8c24abeff4ebb0f863da3890b01 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 25 Sep 2024 18:45:37 +0900 Subject: [PATCH 10/21] mke planning_evaluator publish msg of MetricArray instead of Diags. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware_planning_evaluator/README.md | 2 +- .../planning_evaluator_node.hpp | 55 ++---- .../launch/planning_evaluator.launch.xml | 2 - .../src/planning_evaluator_node.cpp | 182 +++++++----------- 4 files changed, 83 insertions(+), 158 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index d03603e51351a..cadf8e24c4c1f 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -60,7 +60,7 @@ Each metric is published on a topic named after the metric name. | Name | Type | Description | | ----------- | --------------------------------------- | ------------------------------------------------------- | -| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | DiagnosticArray with a DiagnosticStatus for each metric | +| `~/metrics` | `tier4_metric_msgs::msg::MetricArray` | MetricArray with many metrics of `tier4_metric_msgs::msg::Metric`| When shut down, the evaluation node writes the values of the metrics measured during its lifetime to a file as specified by the `output_file` parameter. diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index c80c3c78e714a..371370ee54813 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -17,8 +17,6 @@ #include "autoware/planning_evaluator/metrics_calculator.hpp" #include "autoware/planning_evaluator/stat.hpp" -#include "tier4_metric_msgs/msg/Metric.hpp" -#include "tier4_metric_msgs/msg/MetricList.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -33,12 +31,13 @@ #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include +#include +#include #include #include #include #include -#include #include namespace planning_diagnostics { @@ -46,8 +45,8 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware::planning_evaluator::msg::Metric; -using autoware::planning_evaluator::msg::MetricList; +using MetricMsg = tier4_metric_msgs::msg::Metric; +using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using nav_msgs::msg::Odometry; using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; @@ -94,31 +93,21 @@ class PlanningEvaluatorNode : public rclcpp::Node const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, const Odometry::ConstSharedPtr ego_state_ptr); - /** - * @brief obtain diagnostics information - */ - void onDiagnostics(const MetricList::ConstSharedPtr diag_msg); - /** * @brief publish the given metric statistic */ - Metric generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const; + void AddMetricMsg( + const Metric & metric, const Stat & metric_stat); /** * @brief publish current ego lane info */ - Metric generateDiagnosticEvaluationStatus(const Metric & diag); - - /** - * @brief publish current ego lane info - */ - Metric generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); + void AddLaneletMetricMsg(const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief publish current ego kinematic state */ - Metric generateKinematicStateDiagnosticStatus( + void AddKinematicStateMetricMsg( const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); private: @@ -134,15 +123,6 @@ class PlanningEvaluatorNode : public rclcpp::Node */ void onTimer(); - /** - * @brief fetch topic data - */ - void fetchData(); - // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately. - // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with - // onDiagnostics(). - rclcpp::Subscription::SharedPtr planning_diag_sub_; - // ROS autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; @@ -163,12 +143,14 @@ class PlanningEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; - rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; autoware::route_handler::RouteHandler route_handler_; - MetricList metrics_msg_; + // Message to publish + MetricArrayMsg metrics_msg_; + // Parameters std::string output_file_str_; std::string ego_frame_str_; @@ -181,19 +163,6 @@ class PlanningEvaluatorNode : public rclcpp::Node std::array>, static_cast(Metric::SIZE)> metric_stats_; rclcpp::TimerBase::SharedPtr timer_; - // queue for diagnostics and time stamp - std::deque> diag_queue_; - const std::vector target_functions_ = { - "obstacle_cruise_planner_stop", - "obstacle_cruise_planner_slow_down", - "obstacle_cruise_planner_cruise", - "autoware::motion_velocity_planner::OutOfLaneModule.stop", - "autoware::motion_velocity_planner::OutOfLaneModule.slow_down", - "autoware::motion_velocity_planner::ObstacleVelocityLimiterModule.stop", - "autoware::motion_velocity_planner::ObstacleVelocityLimiterModule.slow_down", - "autoware::motion_velocity_planner::DynamicObstacleStopModule.stop", - "autoware::motion_velocity_planner::DynamicObstacleStopModule.slow_down", - }; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index e20cda378f188..41767a927fe96 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -7,7 +7,6 @@ - @@ -16,7 +15,6 @@ - diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 1e90d7af111f2..c9b2fe63a05a2 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -14,8 +14,6 @@ #include "autoware/planning_evaluator/planning_evaluator_node.hpp" -#include "autoware/evaluator_utils/evaluator_utils.hpp" - #include #include @@ -58,11 +56,8 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op output_file_str_ = declare_parameter("output_file"); ego_frame_str_ = declare_parameter("ego_frame"); - planning_diag_sub_ = create_subscription( - "~/input/diagnostics", 1, std::bind(&PlanningEvaluatorNode::onDiagnostics, this, _1)); - // List of metrics to calculate and publish - metrics_pub_ = create_publisher("~/metrics", 1); + metrics_pub_ = create_publisher("~/metrics", 1); for (const std::string & selected_metric : declare_parameter>("selected_metrics")) { Metric metric = str_to_metric.at(selected_metric); @@ -103,43 +98,6 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() } } -void PlanningEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) -{ - // add target diagnostics to the queue and remove old ones - for (const auto & function : target_functions_) { - autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); - } -} - -Metric PlanningEvaluatorNode::generateDiagnosticEvaluationStatus( - const Metric & diag) -{ - DiagnosticStatus status; - status.name = diag.name; - - const auto it = std::find_if(diag.values.begin(), diag.values.end(), [](const auto & key_value) { - return key_value.key.find("decision") != std::string::npos; - }); - const bool found = it != diag.values.end(); - status.level = (found) ? status.OK : status.ERROR; - - auto get_key_value = [&]() { - if (!found) { - return diagnostic_msgs::msg::KeyValue{}; - } - if (it->value.find("none") != std::string::npos) { - return *it; - } - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "decision"; - key_value.value = "deceleration"; - return key_value; - }; - - status.values.push_back(get_key_value()); - return status; -} - void PlanningEvaluatorNode::getRouteData() { // route @@ -163,7 +121,7 @@ void PlanningEvaluatorNode::getRouteData() } } -DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus( +void PlanningEvaluatorNode::AddLaneletMetricMsg( const Odometry::ConstSharedPtr ego_state_ptr) { const auto & ego_pose = ego_state_ptr->pose.pose; @@ -180,37 +138,45 @@ DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); - DiagnosticStatus status; - status.name = "ego_lane_info"; - status.level = status.OK; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "lane_id"; - key_value.value = std::to_string(current_lane.id()); - status.values.push_back(key_value); - key_value.key = "s"; - key_value.value = std::to_string(arc_coordinates.length); - status.values.push_back(key_value); - key_value.key = "t"; - key_value.value = std::to_string(arc_coordinates.distance); - status.values.push_back(key_value); - return status; + // push_back lanelet info to MetricArrayMsg + const std::string base_name = "ego_lane_info/"; + MetricMsg metric_msg; + + { + metric_msg.name = base_name+ "lane_id"; + metric_msg.value = std::to_string(current_lane.id()); + metrics_msg_.metric_array.push_back(metric_msg); + } + + { + metric_msg.name = base_name + "s"; + metric_msg.value = std::to_string(arc_coordinates.length); + metrics_msg_.metric_array.push_back(metric_msg); + } + + { + metric_msg.name = base_name + "t"; + metric_msg.value = std::to_string(arc_coordinates.distance); + metrics_msg_.metric_array.push_back(metric_msg); + } + return; } -DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( +void PlanningEvaluatorNode::AddKinematicStateMetricMsg( const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr) { - DiagnosticStatus status; - status.name = "kinematic_state"; - status.level = status.OK; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "vel"; - key_value.value = std::to_string(ego_state_ptr->twist.twist.linear.x); - status.values.push_back(key_value); - key_value.key = "acc"; + const std::string base_name = "ego_lane_info/"; + MetricMsg metric_msg; + + metric_msg.name = base_name + "vel"; + metric_msg.value = std::to_string(ego_state_ptr->twist.twist.linear.x); + metrics_msg_.metric_array.push_back(metric_msg); + + metric_msg.name = base_name + "acc"; const auto & acc = accel_stamped.accel.accel.linear.x; - key_value.value = std::to_string(acc); - status.values.push_back(key_value); - key_value.key = "jerk"; + metric_msg.value = std::to_string(acc); + metrics_msg_.metric_array.push_back(metric_msg); + const auto jerk = [&]() { if (!prev_acc_stamped_.has_value()) { prev_acc_stamped_ = accel_stamped; @@ -227,33 +193,41 @@ DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( prev_acc_stamped_ = accel_stamped; return (acc - prev_acc) / dt; }(); - key_value.value = std::to_string(jerk); - status.values.push_back(key_value); - return status; + metric_msg.name = base_name + "jerk"; + metric_msg.value = std::to_string(jerk); + metrics_msg_.metric_array.push_back(metric_msg); + return; } -Metric PlanningEvaluatorNode::generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const +void PlanningEvaluatorNode::AddMetricMsg( + const Metric & metric, const Stat & metric_stat) { - DiagnosticStatus status; - status.level = status.OK; - status.name = metric_to_str.at(metric); - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "min"; - key_value.value = boost::lexical_cast(metric_stat.min()); - status.values.push_back(key_value); - key_value.key = "max"; - key_value.value = boost::lexical_cast(metric_stat.max()); - status.values.push_back(key_value); - key_value.key = "mean"; - key_value.value = boost::lexical_cast(metric_stat.mean()); - status.values.push_back(key_value); - return status; + const std::string base_name = metric_to_str.at(metric) + "/"; + MetricMsg metric_msg; + { + metric_msg.name = base_name + "min"; + metric_msg.value = boost::lexical_cast(metric_stat.min()); + metrics_msg_.metric_array.push_back(metric_msg); + } + + { + metric_msg.name = base_name + "max"; + metric_msg.value = boost::lexical_cast(metric_stat.max()); + metrics_msg_.metric_array.push_back(metric_msg); + } + + { + metric_msg.name = base_name + "mean"; + metric_msg.value = boost::lexical_cast(metric_stat.mean()); + metrics_msg_.metric_array.push_back(metric_msg); + } + + return; } void PlanningEvaluatorNode::onTimer() { - metrics_msg_.header.stamp = now(); + metrics_msg_.stamp = now(); const auto ego_state_ptr = odometry_sub_.takeData(); onOdometry(ego_state_ptr); @@ -276,26 +250,10 @@ void PlanningEvaluatorNode::onTimer() onModifiedGoal(modified_goal_msg, ego_state_ptr); } - { - // generate decision diagnostics from input diagnostics - for (const auto & function : target_functions_) { - const auto it = std::find_if( - diag_queue_.begin(), diag_queue_.end(), - [&function](const std::pair & p) { - return p.first.name.find(function) != std::string::npos; - }); - if (it == diag_queue_.end()) { - continue; - } - // generate each decision diagnostics - metrics_msg_.status.push_back(generateDiagnosticEvaluationStatus(it->first)); - } - } - - if (!metrics_msg_.status.empty()) { + if (!metrics_msg_.metric_array.empty()) { metrics_pub_->publish(metrics_msg_); } - metrics_msg_ = DiagnosticArray{}; + metrics_msg_ = MetricArrayMsg{}; } void PlanningEvaluatorNode::onTrajectory( @@ -321,7 +279,7 @@ void PlanningEvaluatorNode::onTrajectory( } if (metric_stat->count() > 0) { - metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + AddMetricMsg(metric, *metric_stat); } } @@ -347,7 +305,7 @@ void PlanningEvaluatorNode::onModifiedGoal( } metric_stats_[static_cast(metric)].push_back(*metric_stat); if (metric_stat->count() > 0) { - metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + AddMetricMsg(metric, *metric_stat); } } auto runtime = (now() - start).seconds(); @@ -363,12 +321,12 @@ void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_m { getRouteData(); if (route_handler_.isHandlerReady() && odometry_msg) { - metrics_msg_.status.push_back(generateLaneletDiagnosticStatus(odometry_msg)); + AddLaneletMetricMsg(odometry_msg); } const auto acc_msg = accel_sub_.takeData(); if (acc_msg && odometry_msg) { - metrics_msg_.status.push_back(generateKinematicStateDiagnosticStatus(*acc_msg, odometry_msg)); + AddKinematicStateMetricMsg(*acc_msg, odometry_msg); } } } From b1e56dc9592bbb36af723c8fe591ca8d440089b9 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Thu, 3 Oct 2024 13:27:51 +0900 Subject: [PATCH 11/21] update aeb with metric type for decision. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware/autonomous_emergency_braking/node.hpp | 5 +++++ .../autoware_autonomous_emergency_braking/package.xml | 1 + .../src/node.cpp | 11 +++++++++++ .../launch/planning_evaluator.launch.xml | 1 - .../launch/obstacle_cruise_planner.launch.xml | 1 - .../launch/motion_velocity_planner.launch.xml | 1 - .../src/node.cpp | 2 +- .../src/planner_manager.cpp | 1 - 8 files changed, 18 insertions(+), 5 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 32bedd15c5c53..1bb72af2892bf 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -36,6 +36,8 @@ #include #include #include +#include +#include #include @@ -77,6 +79,8 @@ using Vector3 = geometry_msgs::msg::Vector3; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using colorTuple = std::tuple; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; /** * @brief Struct to store object data @@ -345,6 +349,7 @@ class AEB : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; + rclcpp::Publisher::SharedPtr metrics_pub_; // timer rclcpp::TimerBase::SharedPtr timer_; mutable std::shared_ptr time_keeper_{nullptr}; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 2fc87ee0a8acd..053c447ac0c5b 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -43,6 +43,7 @@ tf2_ros tier4_debug_msgs visualization_msgs + tier4_metric_msgs ament_cmake diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index c785ab661060d..0e1ce4bf3c77d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -142,6 +142,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) virtual_wall_publisher_ = this->create_publisher("~/virtual_wall", 1); debug_rss_distance_publisher_ = this->create_publisher("~/debug/rss_distance", 1); + metrics_pub_ = this->create_publisher("~/metrics", 1); } // Diagnostics { @@ -416,6 +417,16 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) } } addVirtualStopWallMarker(virtual_wall_marker); + + // publish metrics + auto metric = Metric(); + metric.name = "decision"; + metric.value = "brake"; + auto metrics = MetricArray(); + metrics.stamp = get_clock()->now(); + metrics.metric_array.push_back(metric); + metrics_pub_->publish(metrics); + } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 41767a927fe96..90c205dd55f1f 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -20,7 +20,6 @@ - diff --git a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml index 018d120879ff2..aaf6db3e36613 100644 --- a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml +++ b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml @@ -32,6 +32,5 @@ - diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml index 7fb3b3a1bd5f8..3732d86ef380a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -27,7 +27,6 @@ - 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 13d767ff9ea6b..6f6ff89c1efe1 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 @@ -88,7 +88,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & this->create_publisher("~/debug/processing_time_ms", 1); debug_viz_pub_ = this->create_publisher("~/debug/markers", 1); - metrics_pub_ = this->create_publisher("~/metrics", 10); + metrics_pub_ = this->create_publisher("~/metrics", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index 0cba5b16c19b5..416ad215d5e25 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -76,7 +76,6 @@ std::shared_ptr MotionVelocityPlannerManager::make_decision_metric( { auto metric = std::make_shared(); metric->name = module_name + "/decision"; - metric->unit = "string"; metric->value = reason; return metric; } From d8609f1b3c74d28a9c20c55619564cbdeeb72832 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Thu, 3 Oct 2024 19:21:41 +0900 Subject: [PATCH 12/21] fix some bug Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../control_evaluator_node.hpp | 35 ++- .../launch/control_evaluator.launch.xml | 1 - .../autoware_control_evaluator/package.xml | 2 +- .../src/control_evaluator_node.cpp | 216 +++++++----------- .../src/planning_evaluator_node.cpp | 7 +- 5 files changed, 100 insertions(+), 161 deletions(-) diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index d1c459d1462e9..c7cbd014e2a9a 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -23,8 +23,9 @@ #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include -#include #include +#include +#include #include #include @@ -36,14 +37,14 @@ namespace control_diagnostics { using autoware_planning_msgs::msg::Trajectory; -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; +using MetricMsg = tier4_metric_msgs::msg::Metric; +using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; /** * @brief Node for control evaluation @@ -52,28 +53,21 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); - DiagnosticStatus generateLateralDeviationDiagnosticStatus( + void AddLateralDeviationMetricMsg( const Trajectory & traj, const Point & ego_point); - DiagnosticStatus generateYawDeviationDiagnosticStatus( + void AddYawDeviationMetricMsg( const Trajectory & traj, const Pose & ego_pose); - DiagnosticStatus generateGoalLongitudinalDeviationDiagnosticStatus(const Pose & ego_pose); - DiagnosticStatus generateGoalLateralDeviationDiagnosticStatus(const Pose & ego_pose); - DiagnosticStatus generateGoalYawDeviationDiagnosticStatus(const Pose & ego_pose); + void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose); + void AddGoalLateralDeviationMetricMsg(const Pose & ego_pose); + void AddGoalYawDeviationMetricMsg(const Pose & ego_pose); - DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); - DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; - DiagnosticStatus generateKinematicStateDiagnosticStatus( + void AddLaneletMetricMsg(const Pose & ego_pose); + void AddKinematicStateMetricMsg( const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); - void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); void onTimer(); private: - // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately. - // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with - // onDiagnostics(). - rclcpp::Subscription::SharedPtr control_diag_sub_; - autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ @@ -87,7 +81,7 @@ class ControlEvaluatorNode : public rclcpp::Node LaneletMapBin, autoware::universe_utils::polling_policy::Newest> vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; // update Route Handler void getRouteData(); @@ -96,10 +90,7 @@ class ControlEvaluatorNode : public rclcpp::Node // Metrics std::deque stamps_; - // queue for diagnostics and time stamp - std::deque> diag_queue_; - const std::vector target_functions_ = {"autonomous_emergency_braking"}; - + MetricArrayMsg metrics_msg_; autoware::route_handler::RouteHandler route_handler_; rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index 0070c07fe4aa7..e9fee7ffadbf2 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -13,7 +13,6 @@ - diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 0e2b1727a465b..3b020297afc3a 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -19,10 +19,10 @@ autoware_evaluator_utils autoware_motion_utils autoware_planning_msgs + tier4_metric_msgs autoware_route_handler autoware_test_utils autoware_universe_utils - diagnostic_msgs nav_msgs pluginlib rclcpp diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 9fb9d4bd080f7..d605e6aaaaf06 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -14,15 +14,11 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" -#include "autoware/evaluator_utils/evaluator_utils.hpp" - #include #include -#include #include #include -#include #include namespace control_diagnostics @@ -31,11 +27,9 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti : Node("control_evaluator", node_options) { using std::placeholders::_1; - control_diag_sub_ = create_subscription( - "~/input/diagnostics", 1, std::bind(&ControlEvaluatorNode::onDiagnostics, this, _1)); // Publisher - metrics_pub_ = create_publisher("~/metrics", 1); + metrics_pub_ = create_publisher("~/metrics", 1); // Timer callback to publish evaluator diagnostics using namespace std::literals::chrono_literals; @@ -66,28 +60,7 @@ void ControlEvaluatorNode::getRouteData() } } -void ControlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) -{ - // add target diagnostics to the queue and remove old ones - for (const auto & function : target_functions_) { - autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); - } -} - -DiagnosticStatus ControlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) -{ - DiagnosticStatus status; - status.level = status.OK; - status.name = diag.name; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "decision"; - const bool is_emergency_brake = (diag.level == DiagnosticStatus::ERROR); - key_value.value = (is_emergency_brake) ? "deceleration" : "none"; - status.values.push_back(key_value); - return status; -} - -DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const +void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) { const auto current_lanelets = [&]() { lanelet::ConstLanelet closest_route_lanelet; @@ -102,37 +75,44 @@ DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); - DiagnosticStatus status; - status.name = "ego_lane_info"; - status.level = status.OK; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "lane_id"; - key_value.value = std::to_string(current_lane.id()); - status.values.push_back(key_value); - key_value.key = "s"; - key_value.value = std::to_string(arc_coordinates.length); - status.values.push_back(key_value); - key_value.key = "t"; - key_value.value = std::to_string(arc_coordinates.distance); - status.values.push_back(key_value); - return status; + const std::string base_name = "ego_lane_info/"; + MetricMsg metric_msg; + + { + metric_msg.name = base_name+ "lane_id"; + metric_msg.value = std::to_string(current_lane.id()); + metrics_msg_.metric_array.push_back(metric_msg); + } + + { + metric_msg.name = base_name + "s"; + metric_msg.value = std::to_string(arc_coordinates.length); + metrics_msg_.metric_array.push_back(metric_msg); + } + + { + metric_msg.name = base_name + "t"; + metric_msg.value = std::to_string(arc_coordinates.distance); + metrics_msg_.metric_array.push_back(metric_msg); + } + return; } -DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( +void ControlEvaluatorNode::AddKinematicStateMetricMsg( const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) { - DiagnosticStatus status; - status.name = "kinematic_state"; - status.level = status.OK; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "vel"; - key_value.value = std::to_string(odom.twist.twist.linear.x); - status.values.push_back(key_value); - key_value.key = "acc"; + const std::string base_name = "kinematic_state/"; + MetricMsg metric_msg; + + metric_msg.name = base_name + "vel"; + metric_msg.value = std::to_string(odom.twist.twist.linear.x); + metrics_msg_.metric_array.push_back(metric_msg); + + metric_msg.name = base_name + "acc"; const auto & acc = accel_stamped.accel.accel.linear.x; - key_value.value = std::to_string(acc); - status.values.push_back(key_value); - key_value.key = "jerk"; + metric_msg.value = std::to_string(acc); + metrics_msg_.metric_array.push_back(metric_msg); + const auto jerk = [&]() { if (!prev_acc_stamped_.has_value()) { prev_acc_stamped_ = accel_stamped; @@ -149,138 +129,108 @@ DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( prev_acc_stamped_ = accel_stamped; return (acc - prev_acc) / dt; }(); - key_value.value = std::to_string(jerk); - status.values.push_back(key_value); - return status; + + metric_msg.name = base_name + "jerk"; + metric_msg.value = std::to_string(jerk); + metrics_msg_.metric_array.push_back(metric_msg); + return; } -DiagnosticStatus ControlEvaluatorNode::generateLateralDeviationDiagnosticStatus( +void ControlEvaluatorNode::AddLateralDeviationMetricMsg( const Trajectory & traj, const Point & ego_point) { const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); - DiagnosticStatus status; - status.level = status.OK; - status.name = "lateral_deviation"; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metric_value"; - key_value.value = std::to_string(lateral_deviation); - status.values.push_back(key_value); - - return status; + MetricMsg metric_msg; + metric_msg.name = "lateral_deviation"; + metric_msg.value = std::to_string(lateral_deviation); + metrics_msg_.metric_array.push_back(metric_msg); + return; } -DiagnosticStatus ControlEvaluatorNode::generateYawDeviationDiagnosticStatus( +void ControlEvaluatorNode::AddYawDeviationMetricMsg( const Trajectory & traj, const Pose & ego_pose) { const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); - DiagnosticStatus status; - status.level = status.OK; - status.name = "yaw_deviation"; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metric_value"; - key_value.value = std::to_string(yaw_deviation); - status.values.push_back(key_value); - - return status; + MetricMsg metric_msg; + metric_msg.name = "yaw_deviation"; + metric_msg.value = std::to_string(yaw_deviation); + metrics_msg_.metric_array.push_back(metric_msg); + return; } -DiagnosticStatus ControlEvaluatorNode::generateGoalLongitudinalDeviationDiagnosticStatus( +void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg( const Pose & ego_pose) { - DiagnosticStatus status; const double longitudinal_deviation = metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position); - status.level = status.OK; - status.name = "goal_longitudinal_deviation"; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metrics_value"; - key_value.value = std::to_string(longitudinal_deviation); - status.values.push_back(key_value); - return status; + MetricMsg metric_msg; + metric_msg.name = "goal_longitudinal_deviation"; + metric_msg.value = std::to_string(longitudinal_deviation); + metrics_msg_.metric_array.push_back(metric_msg); + return; } -DiagnosticStatus ControlEvaluatorNode::generateGoalLateralDeviationDiagnosticStatus( +void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg( const Pose & ego_pose) { - DiagnosticStatus status; const double lateral_deviation = metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position); - status.level = status.OK; - status.name = "goal_lateral_deviation"; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metrics_value"; - key_value.value = std::to_string(lateral_deviation); - status.values.push_back(key_value); - return status; + MetricMsg metric_msg; + metric_msg.name = "goal_lateral_deviation"; + metric_msg.value = std::to_string(lateral_deviation); + metrics_msg_.metric_array.push_back(metric_msg); + return; } -DiagnosticStatus ControlEvaluatorNode::generateGoalYawDeviationDiagnosticStatus( +void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg( const Pose & ego_pose) { - DiagnosticStatus status; const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); - status.level = status.OK; - status.name = "goal_yaw_deviation"; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metrics_value"; - key_value.value = std::to_string(yaw_deviation); - status.values.push_back(key_value); - return status; + MetricMsg metric_msg; + metric_msg.name = "goal_yaw_deviation"; + metric_msg.value = std::to_string(yaw_deviation); + metrics_msg_.metric_array.push_back(metric_msg); + return; } void ControlEvaluatorNode::onTimer() { - DiagnosticArray metrics_msg; const auto traj = traj_sub_.takeData(); const auto odom = odometry_sub_.takeData(); const auto acc = accel_sub_.takeData(); - // generate decision diagnostics from input diagnostics - for (const auto & function : target_functions_) { - const auto it = std::find_if( - diag_queue_.begin(), diag_queue_.end(), - [&function](const std::pair & p) { - return p.first.name.find(function) != std::string::npos; - }); - if (it == diag_queue_.end()) { - continue; - } - // generate each decision diagnostics - // - AEB decision - if (it->first.name.find("autonomous_emergency_braking") != std::string::npos) { - metrics_msg.status.push_back(generateAEBDiagnosticStatus(it->first)); - } - } - // calculate deviation metrics if (odom && traj && !traj->points.empty()) { const Pose ego_pose = odom->pose.pose; - metrics_msg.status.push_back( - generateLateralDeviationDiagnosticStatus(*traj, ego_pose.position)); - metrics_msg.status.push_back(generateYawDeviationDiagnosticStatus(*traj, ego_pose)); + AddLateralDeviationMetricMsg(*traj, ego_pose.position); + AddYawDeviationMetricMsg(*traj, ego_pose); } getRouteData(); if (odom && route_handler_.isHandlerReady()) { const Pose ego_pose = odom->pose.pose; - metrics_msg.status.push_back(generateLaneletDiagnosticStatus(ego_pose)); + AddLaneletMetricMsg(ego_pose); - metrics_msg.status.push_back(generateGoalLongitudinalDeviationDiagnosticStatus(ego_pose)); - metrics_msg.status.push_back(generateGoalLateralDeviationDiagnosticStatus(ego_pose)); - metrics_msg.status.push_back(generateGoalYawDeviationDiagnosticStatus(ego_pose)); + AddGoalLongitudinalDeviationMetricMsg(ego_pose); + AddGoalLateralDeviationMetricMsg(ego_pose); + AddGoalYawDeviationMetricMsg(ego_pose); } if (odom && acc) { - metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*odom, *acc)); + AddKinematicStateMetricMsg(*odom, *acc); + } + + if (!metrics_msg_.metric_array.empty()) { + metrics_msg_.stamp = now(); + metrics_pub_->publish(metrics_msg_); + metrics_msg_ = MetricArrayMsg{}; } - metrics_msg.header.stamp = now(); - metrics_pub_->publish(metrics_msg); } } // namespace control_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index c9b2fe63a05a2..45f1a1176472d 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -165,7 +165,7 @@ void PlanningEvaluatorNode::AddLaneletMetricMsg( void PlanningEvaluatorNode::AddKinematicStateMetricMsg( const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr) { - const std::string base_name = "ego_lane_info/"; + const std::string base_name = "kinematic_state/"; MetricMsg metric_msg; metric_msg.name = base_name + "vel"; @@ -227,8 +227,6 @@ void PlanningEvaluatorNode::AddMetricMsg( void PlanningEvaluatorNode::onTimer() { - metrics_msg_.stamp = now(); - const auto ego_state_ptr = odometry_sub_.takeData(); onOdometry(ego_state_ptr); { @@ -251,9 +249,10 @@ void PlanningEvaluatorNode::onTimer() } if (!metrics_msg_.metric_array.empty()) { + metrics_msg_.stamp = now(); metrics_pub_->publish(metrics_msg_); + metrics_msg_ = MetricArrayMsg{}; } - metrics_msg_ = MetricArrayMsg{}; } void PlanningEvaluatorNode::onTrajectory( From a8d494c2a3e4310bee47801de211367f14c4a42f Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Thu, 3 Oct 2024 19:27:54 +0900 Subject: [PATCH 13/21] remove autoware_evaluator_utils package. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../control_evaluator_node.hpp | 2 - .../autoware_control_evaluator/package.xml | 1 - .../autoware_evaluator_utils/CHANGELOG.rst | 17 ----- .../autoware_evaluator_utils/CMakeLists.txt | 13 ---- evaluator/autoware_evaluator_utils/README.md | 5 -- .../evaluator_utils/evaluator_utils.hpp | 45 ------------- .../autoware_evaluator_utils/package.xml | 24 ------- .../src/evaluator_utils.cpp | 66 ------------------- .../autoware_planning_evaluator/package.xml | 1 - .../src/planning_evaluator_node.cpp | 3 - 10 files changed, 177 deletions(-) delete mode 100644 evaluator/autoware_evaluator_utils/CHANGELOG.rst delete mode 100644 evaluator/autoware_evaluator_utils/CMakeLists.txt delete mode 100644 evaluator/autoware_evaluator_utils/README.md delete mode 100644 evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp delete mode 100644 evaluator/autoware_evaluator_utils/package.xml delete mode 100644 evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index c7cbd014e2a9a..32ee424b81ce1 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -30,8 +30,6 @@ #include #include #include -#include -#include namespace control_diagnostics { diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 3b020297afc3a..42b441e0620b8 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -16,7 +16,6 @@ ament_cmake_auto autoware_cmake - autoware_evaluator_utils autoware_motion_utils autoware_planning_msgs tier4_metric_msgs diff --git a/evaluator/autoware_evaluator_utils/CHANGELOG.rst b/evaluator/autoware_evaluator_utils/CHANGELOG.rst deleted file mode 100644 index 473f358114507..0000000000000 --- a/evaluator/autoware_evaluator_utils/CHANGELOG.rst +++ /dev/null @@ -1,17 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_evaluator_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* feat(planning_evaluator,control_evaluator, evaluator utils): add diagnostics subscriber to planning eval (`#7849 `_) - * add utils and diagnostics subscription to planning_evaluator - * add diagnostics eval - * fix input diag in launch - --------- - Co-authored-by: kosuke55 -* Contributors: Yutaka Kondo, danielsanchezaran - -0.26.0 (2024-04-03) -------------------- diff --git a/evaluator/autoware_evaluator_utils/CMakeLists.txt b/evaluator/autoware_evaluator_utils/CMakeLists.txt deleted file mode 100644 index d75ed43a4e3cb..0000000000000 --- a/evaluator/autoware_evaluator_utils/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_evaluator_utils) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(evaluator_utils SHARED - src/evaluator_utils.cpp -) - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/evaluator/autoware_evaluator_utils/README.md b/evaluator/autoware_evaluator_utils/README.md deleted file mode 100644 index b0db86f86b5c0..0000000000000 --- a/evaluator/autoware_evaluator_utils/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# Evaluator Utils - -## Purpose - -This package provides utils functions for other evaluator packages diff --git a/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp b/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp deleted file mode 100644 index 3b91c9d7605e0..0000000000000 --- a/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ -#define AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ - -#include - -#include - -#include -#include -#include -#include -#include - -namespace autoware::evaluator_utils -{ - -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; -using DiagnosticQueue = std::deque>; - -void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); -void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue); -void addDiagnostic( - const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); -void updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); - -} // namespace autoware::evaluator_utils - -#endif // AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ diff --git a/evaluator/autoware_evaluator_utils/package.xml b/evaluator/autoware_evaluator_utils/package.xml deleted file mode 100644 index 7176abdc7be0a..0000000000000 --- a/evaluator/autoware_evaluator_utils/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - autoware_evaluator_utils - 0.38.0 - ROS 2 node for evaluating control - Daniel SANCHEZ - Takayuki MUROOKA - Apache License 2.0 - - Daniel SANCHEZ - Takayuki MUROOKA - - ament_cmake_auto - autoware_cmake - - diagnostic_msgs - rclcpp - rclcpp_components - - - ament_cmake - - diff --git a/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp b/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp deleted file mode 100644 index 293db21f50e7f..0000000000000 --- a/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/evaluator_utils/evaluator_utils.hpp" - -#include - -namespace autoware::evaluator_utils -{ -void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) -{ - constexpr double KEEP_TIME = 1.0; - diag_queue.erase( - std::remove_if( - diag_queue.begin(), diag_queue.end(), - [stamp](const std::pair & p) { - return (stamp - p.second).seconds() > KEEP_TIME; - }), - diag_queue.end()); -} - -void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue) -{ - diag_queue.erase( - std::remove_if( - diag_queue.begin(), diag_queue.end(), - [&name](const std::pair & p) { - return p.first.name.find(name) != std::string::npos; - }), - diag_queue.end()); -} - -void addDiagnostic( - const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) -{ - diag_queue.push_back(std::make_pair(diag, stamp)); -} - -void updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) -{ - const auto it = std::find_if( - input_diagnostics.status.begin(), input_diagnostics.status.end(), - [&function](const DiagnosticStatus & diag) { - return diag.name.find(function) != std::string::npos; - }); - if (it != input_diagnostics.status.end()) { - removeDiagnosticsByName(it->name, diag_queue); - addDiagnostic(*it, input_diagnostics.header.stamp, diag_queue); - } - - removeOldDiagnostics(stamp, diag_queue); -} -} // namespace autoware::evaluator_utils diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index c2f9f43b91130..368e585cea9e6 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -14,7 +14,6 @@ ament_cmake_auto autoware_cmake - autoware_evaluator_utils autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 45f1a1176472d..ba973c161f8ba 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -21,13 +21,10 @@ #include "boost/lexical_cast.hpp" -#include #include #include -#include #include #include -#include #include namespace planning_diagnostics From 09f0ea45bd1f844153e22717708561276b620774 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Mon, 7 Oct 2024 18:41:51 +0900 Subject: [PATCH 14/21] remove diagnostic_msgs dependency of planning_evaluator Signed-off-by: xtk8532704 <1041084556@qq.com> --- evaluator/autoware_planning_evaluator/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 368e585cea9e6..e789740bc3abe 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -20,7 +20,6 @@ autoware_route_handler autoware_universe_utils tier4_metric_msgs - diagnostic_msgs eigen geometry_msgs nav_msgs From e6d03434a976631df744f944f9e3c8760a2e0680 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Fri, 25 Oct 2024 14:21:13 +0900 Subject: [PATCH 15/21] use metric_msgs for autoware_processing_time_checker. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../README.md | 2 +- .../package.xml | 2 +- .../src/processing_time_checker.cpp | 28 ++++++++----------- .../src/processing_time_checker.hpp | 11 ++++---- 4 files changed, 19 insertions(+), 24 deletions(-) diff --git a/system/autoware_processing_time_checker/README.md b/system/autoware_processing_time_checker/README.md index db2dd8d041e61..ab550994ae229 100644 --- a/system/autoware_processing_time_checker/README.md +++ b/system/autoware_processing_time_checker/README.md @@ -25,7 +25,7 @@ ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml | Name | Type | Description | | ----------------------------------------- | --------------------------------- | ---------------------------------- | -| `/system/processing_time_checker/metrics` | `diagnostic_msgs/DiagnosticArray` | processing time of all the modules | +| `/system/processing_time_checker/metrics` | `tier4_metric_msgs::msg::MetricArray` | processing time of all the modules | ## Parameters diff --git a/system/autoware_processing_time_checker/package.xml b/system/autoware_processing_time_checker/package.xml index 579f7de0bdb4e..73a0b43e44c50 100644 --- a/system/autoware_processing_time_checker/package.xml +++ b/system/autoware_processing_time_checker/package.xml @@ -12,10 +12,10 @@ ament_cmake autoware_cmake - diagnostic_updater rclcpp rclcpp_components tier4_debug_msgs + tier4_metric_msgs ament_lint_auto ament_lint_common diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/system/autoware_processing_time_checker/src/processing_time_checker.cpp index fec7a16134915..c4f688b0900df 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.cpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.cpp @@ -16,7 +16,6 @@ #include -#include #include #include @@ -84,7 +83,7 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op // clang-format on } - diag_pub_ = create_publisher("~/metrics", 1); + metrics_pub_ = create_publisher("~/metrics", 1); const auto period_ns = rclcpp::Rate(update_rate).period(); timer_ = rclcpp::create_timer( @@ -93,28 +92,23 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op void ProcessingTimeChecker::on_timer() { - // create diagnostic status - DiagnosticStatus status; - status.level = status.OK; - status.name = "processing_time"; + // create MetricArrayMsg + MetricArrayMsg metrics_msg; for (const auto & processing_time_iterator : processing_time_map_) { const auto processing_time_topic_name = processing_time_iterator.first; const double processing_time = processing_time_iterator.second; - // generate diagnostic status - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = processing_time_topic_name; - key_value.value = std::to_string(processing_time); - status.values.push_back(key_value); + // generate MetricMsg + MetricMsg metric; + metric.name = processing_time_topic_name + "/processing_time"; + metric.value = std::to_string(processing_time); + metric.unit = "millisecond"; + metrics_msg.metric_array.push_back(metric); } - // create diagnostic array - DiagnosticArray diag_msg; - diag_msg.header.stamp = now(); - diag_msg.status.push_back(status); - // publish - diag_pub_->publish(diag_msg); + metrics_msg.stamp = now(); + metrics_pub_->publish(metrics_msg); } } // namespace autoware::processing_time_checker diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/system/autoware_processing_time_checker/src/processing_time_checker.hpp index eba6f2c0642e6..199410623f8b1 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.hpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.hpp @@ -17,8 +17,9 @@ #include -#include "diagnostic_msgs/msg/diagnostic_array.hpp" -#include "tier4_debug_msgs/msg/float64_stamped.hpp" +#include +#include +#include #include #include @@ -26,8 +27,8 @@ namespace autoware::processing_time_checker { -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; +using MetricMsg = tier4_metric_msgs::msg::Metric; +using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using tier4_debug_msgs::msg::Float64Stamped; class ProcessingTimeChecker : public rclcpp::Node @@ -40,7 +41,7 @@ class ProcessingTimeChecker : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr diag_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; std::vector::SharedPtr> processing_time_subscribers_; // topic name - module name From d835a0692448ecb80154fc989f5f093ffb33a942 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Fri, 25 Oct 2024 17:26:26 +0900 Subject: [PATCH 16/21] rewrite diagnostic_convertor to scenario_simulator_v2_adapter, supporting metric_msgs. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .github/CODEOWNERS | 2 +- evaluator/diagnostic_converter/README.md | 53 -------- .../src/converter_node.cpp | 101 -------------- .../test/test_converter_node.cpp | 128 ------------------ .../CHANGELOG.rst | 0 .../CMakeLists.txt | 4 +- .../scenario_simulator_v2_adapter/README.md | 42 ++++++ .../scenario_simulator_v2_adapter.param.yaml | 7 + .../converter_node.hpp | 38 +++--- .../package.xml | 10 +- .../scenario_simulator_v2_adapter.schema.json | 45 ++++++ .../src/converter_node.cpp | 85 ++++++++++++ .../test/test_converter_node.cpp | 112 +++++++++++++++ .../launch/simulator.launch.xml | 10 +- 14 files changed, 323 insertions(+), 314 deletions(-) delete mode 100644 evaluator/diagnostic_converter/README.md delete mode 100644 evaluator/diagnostic_converter/src/converter_node.cpp delete mode 100644 evaluator/diagnostic_converter/test/test_converter_node.cpp rename evaluator/{diagnostic_converter => scenario_simulator_v2_adapter}/CHANGELOG.rst (100%) rename evaluator/{diagnostic_converter => scenario_simulator_v2_adapter}/CMakeLists.txt (89%) create mode 100644 evaluator/scenario_simulator_v2_adapter/README.md create mode 100644 evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml rename evaluator/{diagnostic_converter/include/diagnostic_converter => scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter}/converter_node.hpp (53%) rename evaluator/{diagnostic_converter => scenario_simulator_v2_adapter}/package.xml (76%) create mode 100644 evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json create mode 100644 evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp create mode 100644 evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 8adb801617337..565aeb3cbecd8 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -64,7 +64,7 @@ control/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp temkei.kem@tier4.jp evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp +evaluator/scenario_simulator_v2_adapter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp diff --git a/evaluator/diagnostic_converter/README.md b/evaluator/diagnostic_converter/README.md deleted file mode 100644 index adc84c3967d25..0000000000000 --- a/evaluator/diagnostic_converter/README.md +++ /dev/null @@ -1,53 +0,0 @@ -# Planning Evaluator - -## Purpose - -This package provides a node to convert `diagnostic_msgs::msg::DiagnosticArray` messages -into `tier4_simulation_msgs::msg::UserDefinedValue` messages. - -## Inner-workings / Algorithms - -The node subscribes to all topics listed in the parameters and assumes they publish -`DiagnosticArray` messages. -Each time such message is received, -it is converted into as many `UserDefinedValue` messages as the number of `KeyValue` objects. -The format of the output topic is detailed in the _output_ section. - -## Inputs / Outputs - -### Inputs - -The node listens to `DiagnosticArray` messages on the topics specified in the parameters. - -### Outputs - -The node outputs `UserDefinedValue` messages that are converted from the received `DiagnosticArray`. - -The name of the output topics are generated from the corresponding input topic, the name of the diagnostic status, and the key of the diagnostic. -For example, we might listen to topic `/diagnostic_topic` and receive a `DiagnosticArray` with 2 status: - -- Status with `name: "x"`. - - Key: `a`. - - Key: `b`. -- Status with `name: "y"`. - - Key: `a`. - - Key: `c`. - -The resulting topics to publish the `UserDefinedValue` are as follows: - -- `/metrics_x_a`. -- `/metrics_x_b`. -- `/metrics_y_a`. -- `/metrics_y_c`. - -## Parameters - -| Name | Type | Description | -| :------------------ | :--------------- | :------------------------------------------------------------ | -| `diagnostic_topics` | list of `string` | list of DiagnosticArray topics to convert to UserDefinedValue | - -## Assumptions / Known limits - -Values in the `KeyValue` objects of a `DiagnosticStatus` are assumed to be of type `double`. - -## Future extensions / Unimplemented parts diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp deleted file mode 100644 index 2a2574732694c..0000000000000 --- a/evaluator/diagnostic_converter/src/converter_node.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "diagnostic_converter/converter_node.hpp" - -#include - -namespace -{ -std::string removeInvalidTopicString(const std::string & input_string) -{ - std::regex pattern{R"([a-zA-Z0-9/_]+)"}; - - std::string result; - for (std::sregex_iterator itr(std::begin(input_string), std::end(input_string), pattern), end; - itr != end; ++itr) { - result += itr->str(); - } - return result; -} - -std::string removeUnitString(const std::string & input_string) -{ - for (size_t i = 0; i < input_string.size(); ++i) { - if (input_string.at(i) == '[') { - if (i != 0 && input_string.at(i - 1) == ' ') { - // Blank is also removed - return std::string{input_string.begin(), input_string.begin() + i - 1}; - } - return std::string{input_string.begin(), input_string.begin() + i}; - } - } - return input_string; -} -} // namespace - -namespace diagnostic_converter -{ -DiagnosticConverter::DiagnosticConverter(const rclcpp::NodeOptions & node_options) -: Node("diagnostic_converter", node_options) -{ - using std::placeholders::_1; - - size_t sub_counter = 0; - std::vector diagnostic_topics; - declare_parameter>("diagnostic_topics", std::vector()); - get_parameter>("diagnostic_topics", diagnostic_topics); - for (const std::string & diagnostic_topic : diagnostic_topics) { - // std::function required with multiple arguments https://answers.ros.org/question/289207 - const std::function fn = - std::bind(&DiagnosticConverter::onDiagnostic, this, _1, sub_counter++, diagnostic_topic); - diagnostics_sub_.push_back(create_subscription(diagnostic_topic, 1, fn)); - } - params_pub_.resize(diagnostics_sub_.size()); -} - -void DiagnosticConverter::onDiagnostic( - const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx, - const std::string & base_topic) -{ - for (const auto & status : diag_msg->status) { - std::string status_topic = base_topic + (status.name.empty() ? "" : "_" + status.name); - for (const auto & key_value : status.values) { - const auto valid_topic_name = removeInvalidTopicString(status_topic + "_" + key_value.key); - getPublisher(valid_topic_name, diag_idx)->publish(createUserDefinedValue(key_value)); - } - } -} - -UserDefinedValue DiagnosticConverter::createUserDefinedValue(const KeyValue & key_value) const -{ - UserDefinedValue param_msg; - param_msg.type.data = UserDefinedValueType::DOUBLE; - param_msg.value = removeUnitString(key_value.value); - return param_msg; -} - -rclcpp::Publisher::SharedPtr DiagnosticConverter::getPublisher( - const std::string & topic, const size_t pub_idx) -{ - auto & pubs = params_pub_[pub_idx]; - if (pubs.count(topic) == 0) { - pubs[topic] = create_publisher(topic, 1); - } - return pubs.at(topic); -} -} // namespace diagnostic_converter - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_converter::DiagnosticConverter) diff --git a/evaluator/diagnostic_converter/test/test_converter_node.cpp b/evaluator/diagnostic_converter/test/test_converter_node.cpp deleted file mode 100644 index 167421f0777df..0000000000000 --- a/evaluator/diagnostic_converter/test/test_converter_node.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "diagnostic_converter/converter_node.hpp" - -#include - -#include "diagnostic_msgs/msg/diagnostic_array.hpp" -#include "tier4_simulation_msgs/msg/user_defined_value.hpp" -#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" - -#include - -#include -#include -#include -#include - -using ConverterNode = diagnostic_converter::DiagnosticConverter; -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; -using diagnostic_msgs::msg::KeyValue; -using tier4_simulation_msgs::msg::UserDefinedValue; - -void waitForMsg( - bool & flag, const rclcpp::Node::SharedPtr node1, const rclcpp::Node::SharedPtr node2) -{ - while (!flag) { - rclcpp::spin_some(node1); - rclcpp::spin_some(node2); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - flag = false; -} - -std::function generateCallback( - bool & flag, UserDefinedValue & msg) -{ - return [&](UserDefinedValue::ConstSharedPtr received_msg) { - flag = true; - msg = *received_msg; - }; -} - -TEST(ConverterNode, ConvertDiagnostics) -{ - const std::vector input_topics = {"/test1/diag", "/test2/diag"}; - - rclcpp::init(0, nullptr); - rclcpp::Node::SharedPtr dummy_node = std::make_shared("converter_test_node"); - - rclcpp::NodeOptions options; - options.append_parameter_override("diagnostic_topics", rclcpp::ParameterValue(input_topics)); - auto node = std::make_shared(options); - - { // Simple case with 1 resulting UserDefinedValue - bool msg_received = false; - UserDefinedValue param; - // DiagnosticArray publishers - const auto diag_pub = dummy_node->create_publisher(input_topics[0], 1); - // UserDefinedValue subscribers - const auto param_sub_a = dummy_node->create_subscription( - input_topics[0] + "_a", 1, generateCallback(msg_received, param)); - DiagnosticArray diag; - DiagnosticStatus status; - status.name = ""; - KeyValue key_value = KeyValue().set__key("a").set__value("1"); - status.values.push_back(key_value); - diag.status.push_back(status); - diag_pub->publish(diag); - waitForMsg(msg_received, node, dummy_node); - EXPECT_EQ(param.value, "1"); - } - { // Case with multiple UserDefinedValue converted from one DiagnosticArray - bool msg_received_xa = false; - bool msg_received_xb = false; - bool msg_received_ya = false; - bool msg_received_yb = false; - UserDefinedValue param_xa; - UserDefinedValue param_xb; - UserDefinedValue param_ya; - UserDefinedValue param_yb; - // DiagnosticArray publishers - const auto diag_pub = dummy_node->create_publisher(input_topics[1], 1); - // UserDefinedValue subscribers - const auto param_sub_xa = dummy_node->create_subscription( - input_topics[1] + "_x_a", 1, generateCallback(msg_received_xa, param_xa)); - const auto param_sub_xb = dummy_node->create_subscription( - input_topics[1] + "_x_b", 1, generateCallback(msg_received_xb, param_xb)); - const auto param_sub_ya = dummy_node->create_subscription( - input_topics[1] + "_y_a", 1, generateCallback(msg_received_ya, param_ya)); - const auto param_sub_yb = dummy_node->create_subscription( - input_topics[1] + "_y_b", 1, generateCallback(msg_received_yb, param_yb)); - DiagnosticArray diag; - DiagnosticStatus status_x; - status_x.name = "x"; - status_x.values.push_back(KeyValue().set__key("a").set__value("1")); - status_x.values.push_back(KeyValue().set__key("b").set__value("10")); - diag.status.push_back(status_x); - DiagnosticStatus status_y; - status_y.name = "y"; - status_y.values.push_back(KeyValue().set__key("a").set__value("9")); - status_y.values.push_back(KeyValue().set__key("b").set__value("6")); - diag.status.push_back(status_y); - diag_pub->publish(diag); - waitForMsg(msg_received_xa, node, dummy_node); - EXPECT_EQ(param_xa.value, "1"); - waitForMsg(msg_received_xb, node, dummy_node); - EXPECT_EQ(param_xb.value, "10"); - waitForMsg(msg_received_ya, node, dummy_node); - EXPECT_EQ(param_ya.value, "9"); - waitForMsg(msg_received_yb, node, dummy_node); - EXPECT_EQ(param_yb.value, "6"); - } - - rclcpp::shutdown(); -} diff --git a/evaluator/diagnostic_converter/CHANGELOG.rst b/evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst similarity index 100% rename from evaluator/diagnostic_converter/CHANGELOG.rst rename to evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst diff --git a/evaluator/diagnostic_converter/CMakeLists.txt b/evaluator/scenario_simulator_v2_adapter/CMakeLists.txt similarity index 89% rename from evaluator/diagnostic_converter/CMakeLists.txt rename to evaluator/scenario_simulator_v2_adapter/CMakeLists.txt index 3f47eb0386a1c..576bd9682725a 100644 --- a/evaluator/diagnostic_converter/CMakeLists.txt +++ b/evaluator/scenario_simulator_v2_adapter/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version -project(diagnostic_converter) +project(scenario_simulator_v2_adapter) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -21,7 +21,7 @@ ament_auto_add_library(${PROJECT_NAME}_node SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "diagnostic_converter::DiagnosticConverter" + PLUGIN "scenario_simulator_v2_adapter::MetricConverter" EXECUTABLE ${PROJECT_NAME} ) diff --git a/evaluator/scenario_simulator_v2_adapter/README.md b/evaluator/scenario_simulator_v2_adapter/README.md new file mode 100644 index 0000000000000..640680d97cf44 --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/README.md @@ -0,0 +1,42 @@ +# scenario_simulator_v2 Adapter + +## Purpose + +This package provides a node to convert various messages from the Autoware into `tier4_simulation_msgs::msg::UserDefinedValue` messages for the scenario_simulator_v2. +Currently, this node supports conversion of: +- `tier4_metric_msgs::msg::MetricArray` for metric topics + +## Inner-workings / Algorithms + +- For `tier4_metric_msgs::msg::MetricArray`, +The node subscribes to all topics listed in the parameter `metric_topic_list`. +Each time such message is received, it is converted into as many `UserDefinedValue` messages as the number of `Metric` objects. +The format of the output topic is detailed in the _output_ section. + +## Inputs / Outputs + +### Inputs + +The node listens to `MetricArray` messages on the topics specified in `metric_topic_list`. + +### Outputs + +The node outputs `UserDefinedValue` messages that are converted from the received messages. + +- For `MetricArray` messages, +The name of the output topics are generated from the corresponding input topic, the name of the metric. + - For example, we might listen to topic `/planning/planning_evaluator/metrics` and receive a `MetricArray` with 2 metrics: + - metric with `name: "metricA/x"` + - metric with `name: "metricA/y"` + - The resulting topics to publish the `UserDefinedValue` are as follows: + - `/planning/planning_evaluator/metrics/metricA/x` + - `/planning/planning_evaluator/metrics/metricA/y` + +## Parameters +{{ json_to_markdown("src/autoware/universe/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json") }} + +## Assumptions / Known limitsmetrics_x + +Values in the `Metric` objects of a `MetricArray` are assumed to be of type `double`. + +## Future extensions / Unimplemented parts diff --git a/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml b/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml new file mode 100644 index 0000000000000..20a745e353de2 --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + update_rate: 10.0 + metric_topic_list: + - /planning/planning_evaluator/metrics + - /control/control_evaluator/metrics + - /system/processing_time_checker/metrics \ No newline at end of file diff --git a/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp b/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp similarity index 53% rename from evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp rename to evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp index 59bb02ebf301f..040b75850b798 100644 --- a/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp +++ b/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ -#define DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ +#ifndef SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ +#define SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ #include -#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include +#include #include "tier4_simulation_msgs/msg/user_defined_value.hpp" #include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" @@ -26,41 +27,40 @@ #include #include -namespace diagnostic_converter +namespace scenario_simulator_v2_adapter { -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; -using diagnostic_msgs::msg::KeyValue; +using tier4_metric_msgs::msg::Metric; +using tier4_metric_msgs::msg::MetricArray; using tier4_simulation_msgs::msg::UserDefinedValue; using tier4_simulation_msgs::msg::UserDefinedValueType; /** - * @brief Node for converting from DiagnosticArray to UserDefinedValue + * @brief Node for converting Autoware's messages to UserDefinedValue */ -class DiagnosticConverter : public rclcpp::Node +class MetricConverter : public rclcpp::Node { public: - explicit DiagnosticConverter(const rclcpp::NodeOptions & node_options); + explicit MetricConverter(const rclcpp::NodeOptions & node_options); /** - * @brief callback for DiagnosticArray msgs that publishes equivalent UserDefinedValue msgs - * @param [in] diag_msg received diagnostic message + * @brief callback for MetricArray msgs that publishes equivalent UserDefinedValue msgs + * @param [in] metrics_msg received metrics message */ - void onDiagnostic( - const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx, + void onMetrics( + const MetricArray::ConstSharedPtr metrics_msg, const size_t topic_idx, const std::string & topic); - UserDefinedValue createUserDefinedValue(const KeyValue & key_value) const; + UserDefinedValue createUserDefinedValue(const Metric & metric) const; rclcpp::Publisher::SharedPtr getPublisher( - const std::string & topic, const size_t pub_idx); + const std::string & topic, const size_t topic_idx); private: // ROS - std::vector::SharedPtr> diagnostics_sub_; + std::vector::SharedPtr> metrics_sub_; std::vector::SharedPtr>> params_pub_; }; -} // namespace diagnostic_converter +} // namespace scenario_simulator_v2_adapter -#endif // DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ +#endif // SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ diff --git a/evaluator/diagnostic_converter/package.xml b/evaluator/scenario_simulator_v2_adapter/package.xml similarity index 76% rename from evaluator/diagnostic_converter/package.xml rename to evaluator/scenario_simulator_v2_adapter/package.xml index 91b9a2e2ede56..4557e1e144bef 100644 --- a/evaluator/diagnostic_converter/package.xml +++ b/evaluator/scenario_simulator_v2_adapter/package.xml @@ -1,18 +1,18 @@ - diagnostic_converter - 0.38.0 - Node for converting diagnostic messages into ParameterDeclaration messages + scenario_simulator_v2_adapter + 0.5.6 + Node for converting autoware's messages into UserDefinedValue messages Kyoichi Sugahara Maxime CLEMENT Takamasa Horibe + Takamasa Horibe Apache License 2.0 ament_cmake_auto autoware_cmake - - diagnostic_msgs + tier4_metric_msgs pluginlib rclcpp rclcpp_components diff --git a/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json b/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json new file mode 100644 index 0000000000000..274faa280937b --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json @@ -0,0 +1,45 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for scenario_simulator_v2 Adapter", + "type": "object", + "definitions": { + "scenario_simulator_v2_adapter": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + }, + "processing_time_topic_list": { + "type": "array", + "items": { + "type": "string" + }, + "description": "The topic name list of the processing time." + }, + "metric_topic_list": { + "type": "array", + "items": { + "type": "string" + }, + "description": "The topic name list of the metrics." + } + }, + "required": ["update_rate", "processing_time_topic_list", "metric_topic_list"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/scenario_simulator_v2_adapter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} \ No newline at end of file diff --git a/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp b/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp new file mode 100644 index 0000000000000..98d36327793fe --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp @@ -0,0 +1,85 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scenario_simulator_v2_adapter/converter_node.hpp" + +#include + +namespace +{ +std::string removeInvalidTopicString(const std::string & input_string) +{ + std::regex pattern{R"([a-zA-Z0-9/_]+)"}; + + std::string result; + for (std::sregex_iterator itr(std::begin(input_string), std::end(input_string), pattern), end; + itr != end; ++itr) { + result += itr->str(); + } + return result; +} +} // namespace + +namespace scenario_simulator_v2_adapter +{ +MetricConverter::MetricConverter(const rclcpp::NodeOptions & node_options) +: Node("scenario_simulator_v2_adapter", node_options) +{ + using std::placeholders::_1; + + size_t sub_counter = 0; + std::vector metric_topic_list; + declare_parameter>("metric_topic_list", std::vector()); + get_parameter>("metric_topic_list", metric_topic_list); + for (const std::string & metric_topic : metric_topic_list) { + // std::function required with multiple arguments https://answers.ros.org/question/289207 + const std::function fn = + std::bind(&MetricConverter::onMetrics, this, _1, sub_counter++, metric_topic); + metrics_sub_.push_back(create_subscription(metric_topic, 1, fn)); + } + params_pub_.resize(metrics_sub_.size()); +} + +void MetricConverter::onMetrics( + const MetricArray::ConstSharedPtr metrics_msg, const size_t topic_idx, + const std::string & base_topic_name) +{ + for (const auto & metric : metrics_msg->metric_array) { + std::string metric_name = base_topic_name + (metric.name.empty() ? "" : "/" + metric.name); + const auto valid_topic_name = removeInvalidTopicString(metric_name); + getPublisher(valid_topic_name, topic_idx)->publish(createUserDefinedValue(metric)); + } +} + +UserDefinedValue MetricConverter::createUserDefinedValue(const Metric & metric) const +{ + UserDefinedValue param_msg; + param_msg.type.data = UserDefinedValueType::DOUBLE; + param_msg.value = metric.value; + return param_msg; +} + +rclcpp::Publisher::SharedPtr MetricConverter::getPublisher( + const std::string & topic_name, const size_t topic_idx) +{ + auto & pubs = params_pub_[topic_idx]; + if (pubs.count(topic_name) == 0) { + pubs[topic_name] = create_publisher(topic_name, 1); + } + return pubs.at(topic_name); +} +} // namespace scenario_simulator_v2_adapter + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(scenario_simulator_v2_adapter::MetricConverter) diff --git a/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp b/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp new file mode 100644 index 0000000000000..3acdee3bd4e5e --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp @@ -0,0 +1,112 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scenario_simulator_v2_adapter/converter_node.hpp" + +#include + +#include +#include +#include "tier4_simulation_msgs/msg/user_defined_value.hpp" +#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" + +#include + +#include +#include +#include +#include + +using ConverterNode = scenario_simulator_v2_adapter::MetricConverter; +using tier4_metric_msgs::msg::Metric; +using tier4_metric_msgs::msg::MetricArray; +using tier4_simulation_msgs::msg::UserDefinedValue; + +void waitForMsg( + bool & flag, const rclcpp::Node::SharedPtr node1, const rclcpp::Node::SharedPtr node2) +{ + while (!flag) { + rclcpp::spin_some(node1); + rclcpp::spin_some(node2); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + flag = false; +} + +std::function generateCallback( + bool & flag, UserDefinedValue & msg) +{ + return [&](UserDefinedValue::ConstSharedPtr received_msg) { + flag = true; + msg = *received_msg; + }; +} + +TEST(ConverterNode, ConvertMetrics) +{ + const std::vector input_topics = {"/test1/metrics", "/test2/metrics"}; + + rclcpp::init(0, nullptr); + rclcpp::Node::SharedPtr dummy_node = std::make_shared("converter_test_node"); + + rclcpp::NodeOptions options; + options.append_parameter_override("metric_topic_list", rclcpp::ParameterValue(input_topics)); + auto node = std::make_shared(options); + + { // Simple case with 1 resulting UserDefinedValue + bool msg_received = false; + UserDefinedValue param; + // MetricArray publishers + const auto metric_pub = dummy_node->create_publisher(input_topics[0], 1); + // UserDefinedValue subscribers + const auto param_sub_a = dummy_node->create_subscription( + input_topics[0] + "/a", 1, generateCallback(msg_received, param)); + MetricArray metrics; + Metric metric; + metric.name = "a"; + metric.value = "1"; + metrics.metric_array.push_back(metric); + metric_pub->publish(metrics); + waitForMsg(msg_received, node, dummy_node); + EXPECT_EQ(param.value, "1"); + } + { // Case with multiple UserDefinedValue converted from one MetricArray + bool msg_received_x = false; + bool msg_received_y = false; + UserDefinedValue param_x; + UserDefinedValue param_y; + // MetricArray publishers + const auto metric_pub = dummy_node->create_publisher(input_topics[1], 1); + // UserDefinedValue subscribers + const auto param_sub_x = dummy_node->create_subscription( + input_topics[1] + "/x", 1, generateCallback(msg_received_x, param_x)); + const auto param_sub_y = dummy_node->create_subscription( + input_topics[1] + "/y", 1, generateCallback(msg_received_y, param_y)); + MetricArray metrics; + Metric metric; + metric.name = "x"; + metric.value = "2"; + metrics.metric_array.push_back(metric); + metric.name = "y"; + metric.value = "3.33333"; + metrics.metric_array.push_back(metric); + metric_pub->publish(metrics); + waitForMsg(msg_received_x, node, dummy_node); + EXPECT_EQ(param_x.value, "2"); + waitForMsg(msg_received_y, node, dummy_node); + EXPECT_EQ(param_y.value, "3.33333"); + } + + rclcpp::shutdown(); +} diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 7be992e2e4764..e1226ee63f5e2 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -20,7 +20,7 @@ description="Select localization mode. Options are 'none', 'api' or 'pose_twist_estimator'. 'pose_twist_estimator' starts most of the localization modules except for the ndt_scan_matcher. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process." /> - + @@ -203,10 +203,10 @@ - - - - + + + + From 40ee2ef3cc22a352c6b846ee8e7e3498eaf11288 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 30 Oct 2024 18:10:46 +0900 Subject: [PATCH 17/21] pre-commit and fix typo Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autonomous_emergency_braking/node.hpp | 4 +- .../package.xml | 2 +- .../control_evaluator_node.hpp | 6 +-- .../autoware_control_evaluator/package.xml | 2 +- .../src/control_evaluator_node.cpp | 35 +++++++-------- .../autoware_planning_evaluator/README.md | 6 +-- .../planning_evaluator_node.hpp | 3 +- .../autoware_planning_evaluator/package.xml | 2 +- .../src/planning_evaluator_node.cpp | 44 +++++++++---------- .../scenario_simulator_v2_adapter/README.md | 14 +++--- .../scenario_simulator_v2_adapter.param.yaml | 2 +- .../converter_node.hpp | 4 +- .../scenario_simulator_v2_adapter/package.xml | 2 +- .../scenario_simulator_v2_adapter.schema.json | 13 ++---- .../test/test_converter_node.cpp | 4 +- .../package.xml | 2 +- .../optimization_based_planner.cpp | 5 ++- .../pid_based_planner/pid_based_planner.cpp | 2 +- .../src/planner_interface.cpp | 10 ++--- .../package.xml | 2 +- .../src/node.cpp | 3 +- .../README.md | 4 +- 22 files changed, 78 insertions(+), 93 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 1bb72af2892bf..d18185c77335c 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -34,10 +34,10 @@ #include #include #include -#include -#include #include #include +#include +#include #include diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 053c447ac0c5b..135986f592663 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -42,8 +42,8 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - visualization_msgs tier4_metric_msgs + visualization_msgs ament_cmake diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 32ee424b81ce1..eed263db49bbc 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -51,10 +51,8 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); - void AddLateralDeviationMetricMsg( - const Trajectory & traj, const Point & ego_point); - void AddYawDeviationMetricMsg( - const Trajectory & traj, const Pose & ego_pose); + void AddLateralDeviationMetricMsg(const Trajectory & traj, const Point & ego_point); + void AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose); void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose); void AddGoalLateralDeviationMetricMsg(const Pose & ego_pose); void AddGoalYawDeviationMetricMsg(const Pose & ego_pose); diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 42b441e0620b8..8ab2c73cb619a 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -18,7 +18,6 @@ autoware_motion_utils autoware_planning_msgs - tier4_metric_msgs autoware_route_handler autoware_test_utils autoware_universe_utils @@ -28,6 +27,7 @@ rclcpp_components tf2 tf2_ros + tier4_metric_msgs ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index d605e6aaaaf06..6d893b8f79ded 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -79,21 +79,21 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) MetricMsg metric_msg; { - metric_msg.name = base_name+ "lane_id"; - metric_msg.value = std::to_string(current_lane.id()); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "lane_id"; + metric_msg.value = std::to_string(current_lane.id()); + metrics_msg_.metric_array.push_back(metric_msg); } { - metric_msg.name = base_name + "s"; - metric_msg.value = std::to_string(arc_coordinates.length); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "s"; + metric_msg.value = std::to_string(arc_coordinates.length); + metrics_msg_.metric_array.push_back(metric_msg); } - + { - metric_msg.name = base_name + "t"; - metric_msg.value = std::to_string(arc_coordinates.distance); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "t"; + metric_msg.value = std::to_string(arc_coordinates.distance); + metrics_msg_.metric_array.push_back(metric_msg); } return; } @@ -103,7 +103,7 @@ void ControlEvaluatorNode::AddKinematicStateMetricMsg( { const std::string base_name = "kinematic_state/"; MetricMsg metric_msg; - + metric_msg.name = base_name + "vel"; metric_msg.value = std::to_string(odom.twist.twist.linear.x); metrics_msg_.metric_array.push_back(metric_msg); @@ -148,8 +148,7 @@ void ControlEvaluatorNode::AddLateralDeviationMetricMsg( return; } -void ControlEvaluatorNode::AddYawDeviationMetricMsg( - const Trajectory & traj, const Pose & ego_pose) +void ControlEvaluatorNode::AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose) { const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); @@ -160,8 +159,7 @@ void ControlEvaluatorNode::AddYawDeviationMetricMsg( return; } -void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg( - const Pose & ego_pose) +void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose) { const double longitudinal_deviation = metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position); @@ -173,8 +171,7 @@ void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg( return; } -void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg( - const Pose & ego_pose) +void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(const Pose & ego_pose) { const double lateral_deviation = metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position); @@ -186,8 +183,7 @@ void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg( return; } -void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg( - const Pose & ego_pose) +void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose) { const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); @@ -230,7 +226,6 @@ void ControlEvaluatorNode::onTimer() metrics_pub_->publish(metrics_msg_); metrics_msg_ = MetricArrayMsg{}; } - } } // namespace control_diagnostics diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index cadf8e24c4c1f..d09949bd69b0e 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -58,9 +58,9 @@ Adding a new metric `M` requires the following steps: Each metric is published on a topic named after the metric name. -| Name | Type | Description | -| ----------- | --------------------------------------- | ------------------------------------------------------- | -| `~/metrics` | `tier4_metric_msgs::msg::MetricArray` | MetricArray with many metrics of `tier4_metric_msgs::msg::Metric`| +| Name | Type | Description | +| ----------- | ------------------------------------- | ----------------------------------------------------------------- | +| `~/metrics` | `tier4_metric_msgs::msg::MetricArray` | MetricArray with many metrics of `tier4_metric_msgs::msg::Metric` | When shut down, the evaluation node writes the values of the metrics measured during its lifetime to a file as specified by the `output_file` parameter. diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 371370ee54813..81787741a7583 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -96,8 +96,7 @@ class PlanningEvaluatorNode : public rclcpp::Node /** * @brief publish the given metric statistic */ - void AddMetricMsg( - const Metric & metric, const Stat & metric_stat); + void AddMetricMsg(const Metric & metric, const Stat & metric_stat); /** * @brief publish current ego lane info diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index e789740bc3abe..572c99d8ac3a5 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -19,7 +19,6 @@ autoware_planning_msgs autoware_route_handler autoware_universe_utils - tier4_metric_msgs eigen geometry_msgs nav_msgs @@ -28,6 +27,7 @@ rclcpp_components tf2 tf2_ros + tier4_metric_msgs ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index ba973c161f8ba..a1e77e751d7e1 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -118,8 +118,7 @@ void PlanningEvaluatorNode::getRouteData() } } -void PlanningEvaluatorNode::AddLaneletMetricMsg( - const Odometry::ConstSharedPtr ego_state_ptr) +void PlanningEvaluatorNode::AddLaneletMetricMsg(const Odometry::ConstSharedPtr ego_state_ptr) { const auto & ego_pose = ego_state_ptr->pose.pose; const auto current_lanelets = [&]() { @@ -140,21 +139,21 @@ void PlanningEvaluatorNode::AddLaneletMetricMsg( MetricMsg metric_msg; { - metric_msg.name = base_name+ "lane_id"; - metric_msg.value = std::to_string(current_lane.id()); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "lane_id"; + metric_msg.value = std::to_string(current_lane.id()); + metrics_msg_.metric_array.push_back(metric_msg); } { - metric_msg.name = base_name + "s"; - metric_msg.value = std::to_string(arc_coordinates.length); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "s"; + metric_msg.value = std::to_string(arc_coordinates.length); + metrics_msg_.metric_array.push_back(metric_msg); } - + { - metric_msg.name = base_name + "t"; - metric_msg.value = std::to_string(arc_coordinates.distance); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "t"; + metric_msg.value = std::to_string(arc_coordinates.distance); + metrics_msg_.metric_array.push_back(metric_msg); } return; } @@ -196,27 +195,26 @@ void PlanningEvaluatorNode::AddKinematicStateMetricMsg( return; } -void PlanningEvaluatorNode::AddMetricMsg( - const Metric & metric, const Stat & metric_stat) +void PlanningEvaluatorNode::AddMetricMsg(const Metric & metric, const Stat & metric_stat) { const std::string base_name = metric_to_str.at(metric) + "/"; MetricMsg metric_msg; { - metric_msg.name = base_name + "min"; - metric_msg.value = boost::lexical_cast(metric_stat.min()); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "min"; + metric_msg.value = boost::lexical_cast(metric_stat.min()); + metrics_msg_.metric_array.push_back(metric_msg); } { - metric_msg.name = base_name + "max"; - metric_msg.value = boost::lexical_cast(metric_stat.max()); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "max"; + metric_msg.value = boost::lexical_cast(metric_stat.max()); + metrics_msg_.metric_array.push_back(metric_msg); } { - metric_msg.name = base_name + "mean"; - metric_msg.value = boost::lexical_cast(metric_stat.mean()); - metrics_msg_.metric_array.push_back(metric_msg); + metric_msg.name = base_name + "mean"; + metric_msg.value = boost::lexical_cast(metric_stat.mean()); + metrics_msg_.metric_array.push_back(metric_msg); } return; diff --git a/evaluator/scenario_simulator_v2_adapter/README.md b/evaluator/scenario_simulator_v2_adapter/README.md index 640680d97cf44..03786f3033521 100644 --- a/evaluator/scenario_simulator_v2_adapter/README.md +++ b/evaluator/scenario_simulator_v2_adapter/README.md @@ -4,14 +4,15 @@ This package provides a node to convert various messages from the Autoware into `tier4_simulation_msgs::msg::UserDefinedValue` messages for the scenario_simulator_v2. Currently, this node supports conversion of: + - `tier4_metric_msgs::msg::MetricArray` for metric topics ## Inner-workings / Algorithms -- For `tier4_metric_msgs::msg::MetricArray`, -The node subscribes to all topics listed in the parameter `metric_topic_list`. -Each time such message is received, it is converted into as many `UserDefinedValue` messages as the number of `Metric` objects. -The format of the output topic is detailed in the _output_ section. +- For `tier4_metric_msgs::msg::MetricArray`, + The node subscribes to all topics listed in the parameter `metric_topic_list`. + Each time such message is received, it is converted into as many `UserDefinedValue` messages as the number of `Metric` objects. + The format of the output topic is detailed in the _output_ section. ## Inputs / Outputs @@ -24,7 +25,7 @@ The node listens to `MetricArray` messages on the topics specified in `metric_to The node outputs `UserDefinedValue` messages that are converted from the received messages. - For `MetricArray` messages, -The name of the output topics are generated from the corresponding input topic, the name of the metric. + The name of the output topics are generated from the corresponding input topic, the name of the metric. - For example, we might listen to topic `/planning/planning_evaluator/metrics` and receive a `MetricArray` with 2 metrics: - metric with `name: "metricA/x"` - metric with `name: "metricA/y"` @@ -33,9 +34,10 @@ The name of the output topics are generated from the corresponding input topic, - `/planning/planning_evaluator/metrics/metricA/y` ## Parameters + {{ json_to_markdown("src/autoware/universe/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json") }} -## Assumptions / Known limitsmetrics_x +## Assumptions / Known limits Values in the `Metric` objects of a `MetricArray` are assumed to be of type `double`. diff --git a/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml b/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml index 20a745e353de2..6c27d440c3942 100644 --- a/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml +++ b/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml @@ -4,4 +4,4 @@ metric_topic_list: - /planning/planning_evaluator/metrics - /control/control_evaluator/metrics - - /system/processing_time_checker/metrics \ No newline at end of file + - /system/processing_time_checker/metrics diff --git a/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp b/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp index 040b75850b798..1ac4585035277 100644 --- a/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp +++ b/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp @@ -17,10 +17,10 @@ #include -#include -#include #include "tier4_simulation_msgs/msg/user_defined_value.hpp" #include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" +#include +#include #include #include diff --git a/evaluator/scenario_simulator_v2_adapter/package.xml b/evaluator/scenario_simulator_v2_adapter/package.xml index 4557e1e144bef..ae6b49671a774 100644 --- a/evaluator/scenario_simulator_v2_adapter/package.xml +++ b/evaluator/scenario_simulator_v2_adapter/package.xml @@ -12,10 +12,10 @@ ament_cmake_auto autoware_cmake - tier4_metric_msgs pluginlib rclcpp rclcpp_components + tier4_metric_msgs tier4_simulation_msgs ament_cmake_gtest diff --git a/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json b/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json index 274faa280937b..e353aadccc111 100644 --- a/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json +++ b/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json @@ -12,22 +12,15 @@ "exclusiveMinimum": 2, "description": "The scanning and update frequency of the checker." }, - "processing_time_topic_list": { - "type": "array", - "items": { - "type": "string" - }, - "description": "The topic name list of the processing time." - }, "metric_topic_list": { "type": "array", "items": { "type": "string" }, - "description": "The topic name list of the metrics." + "description": "The topic name list of the processing time." } }, - "required": ["update_rate", "processing_time_topic_list", "metric_topic_list"] + "required": ["update_rate", "metric_topic_list"] } }, "properties": { @@ -42,4 +35,4 @@ } }, "required": ["/**"] -} \ No newline at end of file +} diff --git a/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp b/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp index 3acdee3bd4e5e..de2a12ac2caaa 100644 --- a/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp +++ b/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp @@ -16,10 +16,10 @@ #include -#include -#include #include "tier4_simulation_msgs/msg/user_defined_value.hpp" #include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" +#include +#include #include diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 58420072b2ac8..d8bf161ef866f 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -38,8 +38,8 @@ tf2 tf2_ros tier4_debug_msgs - tier4_planning_msgs tier4_metric_msgs + tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 5c1d60d200b1a..7bf0f67129a20 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -199,7 +199,8 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( output.at(i).longitudinal_velocity_mps = 0.0; } prev_output_ = output; - debug_data_ptr_->cruise_metrics = makeMetrics("OptimizationBasedPlanner","cruise", planner_data); + debug_data_ptr_->cruise_metrics = + makeMetrics("OptimizationBasedPlanner", "cruise", planner_data); return output; } else if (opt_position.size() == 1) { RCLCPP_DEBUG( @@ -256,7 +257,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Insert Closest Stop Point autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); - debug_data_ptr_->cruise_metrics = makeMetrics("OptimizationBasedPlanner","cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeMetrics("OptimizationBasedPlanner", "cruise", planner_data); prev_output_ = output; return output; } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 761e2b05b548b..80fb85d4e30c9 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -325,7 +325,7 @@ std::vector PIDBasedPlanner::planCruise( // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); - debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner","cruise", planner_data); + debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 612726f5cd23e..389fa30b4a1e8 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -857,13 +857,13 @@ std::vector PlannerInterface::makeMetrics( } if (stop_pose.has_value() && planner_data.has_value()) { // Stop info - Metric stop_posision_metric; - stop_posision_metric.name = module_name + "/stop_position"; - stop_posision_metric.unit = "string"; + Metric stop_position_metric; + stop_position_metric.name = module_name + "/stop_position"; + stop_position_metric.unit = "string"; const auto & p = stop_pose.value().position; - stop_posision_metric.value = + stop_position_metric.value = "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; - metrics.push_back(stop_posision_metric); + metrics.push_back(stop_position_metric); Metric stop_orientation_metric; stop_orientation_metric.name = module_name + "/stop_orientation"; 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 ae7e80e927988..3e68d8262aae9 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,8 +38,8 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - tier4_planning_msgs tier4_metric_msgs + tier4_planning_msgs visualization_msgs rosidl_default_runtime 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 6f6ff89c1efe1..65adcb2683ecc 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 @@ -306,8 +306,7 @@ void MotionVelocityPlannerNode::on_trajectory( processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); - std::shared_ptr metrics = - planner_manager_.get_metrics(get_clock()->now()); + std::shared_ptr metrics = planner_manager_.get_metrics(get_clock()->now()); if (!metrics->metric_array.empty()) { metrics_pub_->publish(*metrics); } diff --git a/system/autoware_processing_time_checker/README.md b/system/autoware_processing_time_checker/README.md index ab550994ae229..5745ec52086cc 100644 --- a/system/autoware_processing_time_checker/README.md +++ b/system/autoware_processing_time_checker/README.md @@ -23,8 +23,8 @@ ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml ### Output -| Name | Type | Description | -| ----------------------------------------- | --------------------------------- | ---------------------------------- | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------- | ---------------------------------- | | `/system/processing_time_checker/metrics` | `tier4_metric_msgs::msg::MetricArray` | processing time of all the modules | ## Parameters From 785cd34bd944390eaac2deba4ac56e5b79d5a673 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 5 Nov 2024 13:07:21 +0900 Subject: [PATCH 18/21] publish metrics even if there is no metric in the MetricArray. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../src/node.cpp | 18 ++++++++++-------- .../src/control_evaluator_node.cpp | 9 ++++----- .../src/planning_evaluator_node.cpp | 9 ++++----- .../src/planner_interface.cpp | 4 +--- .../src/node.cpp | 4 +--- 5 files changed, 20 insertions(+), 24 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 0e1ce4bf3c77d..846a7654d7313 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -401,6 +401,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; MarkerArray virtual_wall_marker; + auto metrics = MetricArray(); checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -418,14 +419,12 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) } addVirtualStopWallMarker(virtual_wall_marker); - // publish metrics - auto metric = Metric(); - metric.name = "decision"; - metric.value = "brake"; - auto metrics = MetricArray(); - metrics.stamp = get_clock()->now(); - metrics.metric_array.push_back(metric); - metrics_pub_->publish(metrics); + { + auto metric = Metric(); + metric.name = "decision"; + metric.value = "brake"; + metrics.metric_array.push_back(metric); + } } else { const std::string error_msg = "[AEB]: No Collision"; @@ -436,6 +435,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) // publish debug markers debug_marker_publisher_->publish(debug_markers); virtual_wall_publisher_->publish(virtual_wall_marker); + // publish metrics + metrics.stamp = get_clock()->now(); + metrics_pub_->publish(metrics); } bool AEB::checkCollision(MarkerArray & debug_markers) diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 6d893b8f79ded..28034eed5f89c 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -221,11 +221,10 @@ void ControlEvaluatorNode::onTimer() AddKinematicStateMetricMsg(*odom, *acc); } - if (!metrics_msg_.metric_array.empty()) { - metrics_msg_.stamp = now(); - metrics_pub_->publish(metrics_msg_); - metrics_msg_ = MetricArrayMsg{}; - } + // Publish metrics + metrics_msg_.stamp = now(); + metrics_pub_->publish(metrics_msg_); + metrics_msg_ = MetricArrayMsg{}; } } // namespace control_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index a1e77e751d7e1..a1d7c75891f4a 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -243,11 +243,10 @@ void PlanningEvaluatorNode::onTimer() onModifiedGoal(modified_goal_msg, ego_state_ptr); } - if (!metrics_msg_.metric_array.empty()) { - metrics_msg_.stamp = now(); - metrics_pub_->publish(metrics_msg_); - metrics_msg_ = MetricArrayMsg{}; - } + // Publish metrics + metrics_msg_.stamp = now(); + metrics_pub_->publish(metrics_msg_); + metrics_msg_ = MetricArrayMsg{}; } void PlanningEvaluatorNode::onTrajectory( diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 389fa30b4a1e8..140396a09e042 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -912,9 +912,7 @@ void PlannerInterface::publishMetrics(const rclcpp::Time & current_time) addMetrics(debug_data_ptr_->stop_metrics); addMetrics(debug_data_ptr_->slow_down_metrics); addMetrics(debug_data_ptr_->cruise_metrics); - if (!metrics_msg.metric_array.empty()) { - metrics_pub_->publish(metrics_msg); - } + metrics_pub_->publish(metrics_msg); clearMetrics(); } 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 65adcb2683ecc..0e8a36977fec8 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 @@ -307,9 +307,7 @@ void MotionVelocityPlannerNode::on_trajectory( processing_time_publisher_->publish(processing_time_msg); std::shared_ptr metrics = planner_manager_.get_metrics(get_clock()->now()); - if (!metrics->metric_array.empty()) { - metrics_pub_->publish(*metrics); - } + metrics_pub_->publish(*metrics); planner_manager_.clear_metrics(); } From 00be49adcd938097b6f8b90e3bfda57531c10fa4 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 5 Nov 2024 15:20:06 +0900 Subject: [PATCH 19/21] modify the metric name of processing_time. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../src/processing_time_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/system/autoware_processing_time_checker/src/processing_time_checker.cpp index c4f688b0900df..3ab96ab0f9711 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.cpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.cpp @@ -100,7 +100,7 @@ void ProcessingTimeChecker::on_timer() // generate MetricMsg MetricMsg metric; - metric.name = processing_time_topic_name + "/processing_time"; + metric.name = "processing_time/" + processing_time_topic_name; metric.value = std::to_string(processing_time); metric.unit = "millisecond"; metrics_msg.metric_array.push_back(metric); From abe647736b7e991aaeeb67e807f334a3032229a1 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 12 Nov 2024 15:59:14 +0900 Subject: [PATCH 20/21] update unit test for test_planning/control_evaluator Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../test/test_control_evaluator_node.cpp | 21 ++++++++-------- .../test/test_planning_evaluator_node.cpp | 24 +++++++++---------- 2 files changed, 22 insertions(+), 23 deletions(-) diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp index ca51721fa1932..8e1960e11c0f2 100644 --- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -19,9 +19,8 @@ #include "tf2_ros/transform_broadcaster.h" #include - +#include #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "boost/lexical_cast.hpp" @@ -36,7 +35,7 @@ using EvalNode = control_diagnostics::ControlEvaluatorNode; using Trajectory = autoware_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; -using diagnostic_msgs::msg::DiagnosticArray; +using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using nav_msgs::msg::Odometry; constexpr double epsilon = 1e-6; @@ -77,14 +76,14 @@ class EvalTest : public ::testing::Test void setTargetMetric(const std::string & metric_str) { - const auto is_target_metric = [metric_str](const auto & status) { - return status.name == metric_str; + const auto is_target_metric = [metric_str](const auto & metric) { + return metric.name == metric_str; }; - metric_sub_ = rclcpp::create_subscription( - dummy_node, "/control_evaluator/metrics", 1, [=](const DiagnosticArray::ConstSharedPtr msg) { - const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); - if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[0].value); + metric_sub_ = rclcpp::create_subscription( + dummy_node, "/control_evaluator/metrics", 1, [=](const MetricArrayMsg::ConstSharedPtr msg) { + const auto it = std::find_if(msg->metric_array.begin(), msg->metric_array.end(), is_target_metric); + if (it != msg->metric_array.end()) { + metric_value_ = boost::lexical_cast(it->value); metric_updated_ = true; } }); @@ -153,7 +152,7 @@ class EvalTest : public ::testing::Test // Trajectory publishers rclcpp::Publisher::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Subscription::SharedPtr metric_sub_; + rclcpp::Subscription::SharedPtr metric_sub_; // TF broadcaster std::unique_ptr tf_broadcaster_; }; diff --git a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp index 9f797500bc5f0..b50313e3cc724 100644 --- a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,10 +19,10 @@ #include "tf2_ros/transform_broadcaster.h" #include +#include #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "boost/lexical_cast.hpp" @@ -37,7 +37,7 @@ using Trajectory = autoware_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; using Objects = autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; -using diagnostic_msgs::msg::DiagnosticArray; +using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using nav_msgs::msg::Odometry; constexpr double epsilon = 1e-6; @@ -86,17 +86,17 @@ class EvalTest : public ::testing::Test ~EvalTest() override { rclcpp::shutdown(); } - void setTargetMetric(planning_diagnostics::Metric metric) + void setTargetMetric(planning_diagnostics::Metric metric, const std::string & postfix = "/mean") { const auto metric_str = planning_diagnostics::metric_to_str.at(metric); - const auto is_target_metric = [metric_str](const auto & status) { - return status.name == metric_str; + const auto is_target_metric = [metric_str, postfix](const auto & metric) { + return metric.name == metric_str + postfix; }; - metric_sub_ = rclcpp::create_subscription( - dummy_node, "/planning_evaluator/metrics", 1, [=](const DiagnosticArray::ConstSharedPtr msg) { - const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); - if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[2].value); + metric_sub_ = rclcpp::create_subscription( + dummy_node, "/planning_evaluator/metrics", 1, [=](const MetricArrayMsg::ConstSharedPtr msg) { + const auto it = std::find_if(msg->metric_array.begin(), msg->metric_array.end(), is_target_metric); + if (it != msg->metric_array.end()) { + metric_value_ = boost::lexical_cast(it->value); metric_updated_ = true; } }); @@ -207,7 +207,7 @@ class EvalTest : public ::testing::Test rclcpp::Publisher::SharedPtr objects_pub_; rclcpp::Publisher::SharedPtr modified_goal_pub_; rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Subscription::SharedPtr metric_sub_; + rclcpp::Subscription::SharedPtr metric_sub_; // TF broadcaster std::unique_ptr tf_broadcaster_; }; From f5329183bc378be56d59e9bcd1773bb6f0e7a39a Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 12 Nov 2024 16:08:12 +0900 Subject: [PATCH 21/21] manual pre-commit Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../test/test_control_evaluator_node.cpp | 6 ++++-- .../test/test_planning_evaluator_node.cpp | 5 +++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp index 8e1960e11c0f2..030080213bfe5 100644 --- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -19,9 +19,10 @@ #include "tf2_ros/transform_broadcaster.h" #include -#include + #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" +#include #include "boost/lexical_cast.hpp" @@ -81,7 +82,8 @@ class EvalTest : public ::testing::Test }; metric_sub_ = rclcpp::create_subscription( dummy_node, "/control_evaluator/metrics", 1, [=](const MetricArrayMsg::ConstSharedPtr msg) { - const auto it = std::find_if(msg->metric_array.begin(), msg->metric_array.end(), is_target_metric); + const auto it = + std::find_if(msg->metric_array.begin(), msg->metric_array.end(), is_target_metric); if (it != msg->metric_array.end()) { metric_value_ = boost::lexical_cast(it->value); metric_updated_ = true; diff --git a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp index b50313e3cc724..ac99d164708ba 100644 --- a/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp @@ -19,11 +19,11 @@ #include "tf2_ros/transform_broadcaster.h" #include -#include #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" +#include #include "boost/lexical_cast.hpp" @@ -94,7 +94,8 @@ class EvalTest : public ::testing::Test }; metric_sub_ = rclcpp::create_subscription( dummy_node, "/planning_evaluator/metrics", 1, [=](const MetricArrayMsg::ConstSharedPtr msg) { - const auto it = std::find_if(msg->metric_array.begin(), msg->metric_array.end(), is_target_metric); + const auto it = + std::find_if(msg->metric_array.begin(), msg->metric_array.end(), is_target_metric); if (it != msg->metric_array.end()) { metric_value_ = boost::lexical_cast(it->value); metric_updated_ = true;