diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt index e01f1d2bba1f8..66885fdb5d7e1 100644 --- a/evaluator/autoware_control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -27,6 +27,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE - param launch + config ) diff --git a/evaluator/autoware_control_evaluator/config/control_evaluator.param.yaml b/evaluator/autoware_control_evaluator/config/control_evaluator.param.yaml new file mode 100644 index 0000000000000..6ebed026315b7 --- /dev/null +++ b/evaluator/autoware_control_evaluator/config/control_evaluator.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + planning_factor_metrics: + topic_prefix: /planning/planning_factors/ + stop_deviation: + module_list: # list of modules to be checked for stop deviation. Here we assume that the `module` inside the planning_factors should be the same as the topic name. + - blind_spot + - crosswalk + - detection_area + - intersection + - merge_from_private + - no_drivable_lane + - no_stopping_area + - stop_line + - traffic_light + - virtual_traffic_light + - walkway 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 6646ce5fd30c9..4a309d7bf4ad1 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,6 +30,8 @@ #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include #include +#include +#include #include #include #include @@ -38,6 +40,8 @@ #include #include #include +#include +#include #include namespace control_diagnostics @@ -57,6 +61,8 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::PlanningFactorArray; /** * @brief Node for control evaluation @@ -79,7 +85,8 @@ class ControlEvaluatorNode : public rclcpp::Node void AddKinematicStateMetricMsg( const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); void AddSteeringMetricMsg(const SteeringReport & steering_report); - + void AddStopDeviationMetricMsg( + const PlanningFactorArray::ConstSharedPtr & planning_factors, const std::string & module_name); void onTimer(); private: @@ -98,6 +105,11 @@ class ControlEvaluatorNode : public rclcpp::Node autoware_utils::InterProcessPollingSubscriber steering_sub_{ this, "~/input/steering_status"}; + std::unordered_map< + std::string, autoware_utils::InterProcessPollingSubscriber> + planning_factors_sub_; + std::unordered_set stop_deviation_modules_; + rclcpp::Publisher::SharedPtr processing_time_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp index 39ef6fa26dfa8..19b1ea7538403 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp @@ -36,6 +36,7 @@ enum class Metric { steering_angle, steering_rate, steering_acceleration, + stop_deviation, SIZE, }; @@ -50,6 +51,7 @@ static const std::unordered_map str_to_metric = { {"steering_angle", Metric::steering_angle}, {"steering_rate", Metric::steering_rate}, {"steering_acceleration", Metric::steering_acceleration}, + {"stop_deviation", Metric::stop_deviation}, }; static const std::unordered_map metric_to_str = { @@ -63,6 +65,7 @@ static const std::unordered_map metric_to_str = { {Metric::steering_angle, "steering_angle"}, {Metric::steering_rate, "steering_rate"}, {Metric::steering_acceleration, "steering_acceleration"}, + {Metric::stop_deviation, "stop_deviation"}, }; // Metrics descriptions @@ -77,6 +80,7 @@ static const std::unordered_map metric_descriptions = { {Metric::steering_angle, "Steering angle[rad]"}, {Metric::steering_rate, "Steering angle rate[rad/s]"}, {Metric::steering_acceleration, "Steering angle acceleration[rad/s^2]"}, + {Metric::stop_deviation, "Deviation to the stop line when the ego stop by a module[m]"}, }; namespace details diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index f1415410c13e4..2306caf1a59cd 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -11,6 +11,7 @@ + diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index bac17e5bda1af..4545963f6f258 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -16,7 +16,9 @@ ament_cmake_auto autoware_cmake + autoware_internal_planning_msgs autoware_motion_utils + autoware_planning_factor_interface autoware_planning_msgs autoware_route_handler autoware_test_utils @@ -31,7 +33,6 @@ tf2 tf2_ros tier4_metric_msgs - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml b/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml deleted file mode 100644 index a20dbd7ffd3d9..0000000000000 --- a/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml +++ /dev/null @@ -1,2 +0,0 @@ -/**: - ros__parameters: diff --git a/evaluator/autoware_control_evaluator/schema/autoware_control_evaluator.schema.json b/evaluator/autoware_control_evaluator/schema/autoware_control_evaluator.schema.json new file mode 100644 index 0000000000000..b2cd29d7ce859 --- /dev/null +++ b/evaluator/autoware_control_evaluator/schema/autoware_control_evaluator.schema.json @@ -0,0 +1,62 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Autoware Control Evaluator Nodes", + "type": "object", + "definitions": { + "autoware_control_evaluator": { + "type": "object", + "properties": { + "planning_factor_metrics": { + "description": "metrics' parameters for planning_factor evaluation", + "type": "object", + "properties": { + "topic_prefix": { + "description": "prefix of the topics to subscribe to", + "type": "string", + "default": "/planning/planning_factors/" + }, + "stop_deviation": { + "description": "stop_deviation metrics' parameters", + "type": "object", + "properties": { + "module_list": { + "description": "list of modules' name to evaluate", + "type": "array", + "items": { + "type": "string" + }, + "default": [ + "blind_spot", + "crosswalk", + "detection_area", + "intersection", + "merge_from_private", + "no_drivable_lane", + "no_stopping_area", + "stop_line", + "traffic_light", + "virtual_traffic_light", + "walkway" + ] + } + } + } + } + } + }, + "required": ["planning_factor_metrics"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/autoware_control_evaluator" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 2d58a1eb02b37..cd922bca9eb88 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -24,9 +24,12 @@ #include #include #include +#include #include #include +#include #include +#include #include namespace control_diagnostics @@ -37,6 +40,21 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti { using std::placeholders::_1; + // planning_factor subscribers + std::vector stop_deviation_modules_list = + declare_parameter>( + "planning_factor_metrics.stop_deviation.module_list"); + stop_deviation_modules_ = std::unordered_set( + stop_deviation_modules_list.begin(), stop_deviation_modules_list.end()); + + const std::string topic_prefix = + declare_parameter("planning_factor_metrics.topic_prefix"); + for (const auto & module_name : stop_deviation_modules_) { + planning_factors_sub_.emplace( + module_name, autoware_utils::InterProcessPollingSubscriber( + this, topic_prefix + module_name)); + } + // Publisher processing_time_pub_ = this->create_publisher( "~/debug/processing_time_ms", 1); @@ -328,6 +346,29 @@ void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose) AddMetricMsg(metric, metric_value); } +void ControlEvaluatorNode::AddStopDeviationMetricMsg( + const PlanningFactorArray::ConstSharedPtr & planning_factors, const std::string & module_name) +{ + std::optional min_distance = std::nullopt; + for (const auto & factor : planning_factors->factors) { + if (factor.behavior == PlanningFactor::STOP) { + for (const auto & control_point : factor.control_points) { + if (!min_distance || std::abs(control_point.distance) < std::abs(*min_distance)) { + min_distance = control_point.distance; + } + } + } + } + if (!min_distance) { + return; + } + + MetricMsg metric_msg; + metric_msg.name = "stop_deviation/" + module_name; + metric_msg.value = std::to_string(*min_distance); + metrics_msg_.metric_array.push_back(metric_msg); +} + void ControlEvaluatorNode::onTimer() { autoware_utils::StopWatch stop_watch; @@ -337,13 +378,14 @@ void ControlEvaluatorNode::onTimer() const auto behavior_path = behavior_path_subscriber_.take_data(); const auto steering_status = steering_sub_.take_data(); - // calculate deviation metrics + // add deviation metrics if (odom && traj && !traj->points.empty()) { const Pose ego_pose = odom->pose.pose; AddLateralDeviationMetricMsg(*traj, ego_pose.position); AddYawDeviationMetricMsg(*traj, ego_pose); } + // add goal deviation metrics getRouteData(); if (odom && route_handler_.isHandlerReady()) { const Pose ego_pose = odom->pose.pose; @@ -353,15 +395,30 @@ void ControlEvaluatorNode::onTimer() AddGoalYawDeviationMetricMsg(ego_pose); } + // add planning_factor related metrics + for (auto & [module_name, planning_factor_sub_] : planning_factors_sub_) { + const auto planning_factors = planning_factor_sub_.take_data(); + if (!planning_factors || planning_factors->factors.empty()) { + continue; + } + + if (stop_deviation_modules_.count(module_name) > 0) { + AddStopDeviationMetricMsg(planning_factors, module_name); + } + } + + // add kinematic info if (odom && acc) { AddKinematicStateMetricMsg(*odom, *acc); } + // add boundary distance metrics if (odom && behavior_path) { const Pose ego_pose = odom->pose.pose; AddBoundaryDistanceMetricMsg(*behavior_path, ego_pose); } + // add steering metrics if (steering_status) { AddSteeringMetricMsg(*steering_status); } 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 b2dcc93855c0f..6b84a1fee8400 100644 --- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -19,6 +19,7 @@ #include "tf2_ros/transform_broadcaster.h" #include +#include #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,7 +39,10 @@ using EvalNode = control_diagnostics::ControlEvaluatorNode; using Trajectory = autoware_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; constexpr double epsilon = 1e-6; @@ -51,10 +55,13 @@ class EvalTest : public ::testing::Test rclcpp::init(0, nullptr); rclcpp::NodeOptions options; + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_control_evaluator"); const auto autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); options.arguments( - {"--ros-args", "-p", "output_metrics:=false", "--params-file", + {"--ros-args", "-p", "output_metrics:=true", "--params-file", + share_dir + "/config/control_evaluator.param.yaml", "--params-file", autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml"}); dummy_node = std::make_shared("control_evaluator_test_node", options); @@ -76,6 +83,9 @@ class EvalTest : public ::testing::Test rclcpp::create_publisher(dummy_node, "/control_evaluator/input/odometry", 1); acc_pub_ = rclcpp::create_publisher( dummy_node, "/control_evaluator/input/acceleration", 1); + planning_factor_interface_ = + std::make_unique( + dummy_node.get(), "stop_line"); tf_broadcaster_ = std::make_unique(dummy_node); publishEgoPose(0.0, 0.0, 0.0); } @@ -169,6 +179,23 @@ class EvalTest : public ::testing::Test } return metric_value_; } + + double publishPlanningFactorAndGetStopDeviationMetric( + const double distance, const double stop_point_x, const double stop_point_y) + { + Pose stop_point = Pose(); + stop_point.position.x = stop_point_x; + stop_point.position.y = stop_point_y; + + planning_factor_interface_->add( + distance, stop_point, PlanningFactor::STOP, SafetyFactorArray({})); + planning_factor_interface_->publish(); + while (!metric_updated_) { + spin_some(); + } + return metric_value_; + } + void spin_some() { rclcpp::spin_some(eval_node); @@ -186,6 +213,9 @@ class EvalTest : public ::testing::Test rclcpp::Publisher::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr acc_pub_; + std::unique_ptr + planning_factor_interface_; + rclcpp::Subscription::SharedPtr metric_sub_; // TF broadcaster std::unique_ptr tf_broadcaster_; @@ -248,3 +278,9 @@ TEST_F(EvalTest, TestKinematicStateJerk) publishEgoAcc(0.0); EXPECT_NEAR(publishEgoAccAndGetMetric(1), 10.0, 0.5); } + +TEST_F(EvalTest, TestStopDeviation) +{ + setTargetMetric("stop_deviation/stop_line"); + EXPECT_NEAR(publishPlanningFactorAndGetStopDeviationMetric(5.0, 4.0, 3.0), 5.0, epsilon); +} diff --git a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json index 506cbe4c57471..f98a17b4a5eb3 100644 --- a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json +++ b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json @@ -6,11 +6,6 @@ "autoware_planning_evaluator": { "type": "object", "properties": { - "output_metrics": { - "description": "If true, the evaluation node writes the metrics' statics to `/autoware_metrics/-.json` when the node shut down,", - "type": "boolean", - "default": "false" - }, "ego_frame": { "description": "reference frame of ego", "type": "string",