diff --git a/evaluator/autoware_control_evaluator/README.md b/evaluator/autoware_control_evaluator/README.md index 59c09015bd7b5..b7da418764959 100644 --- a/evaluator/autoware_control_evaluator/README.md +++ b/evaluator/autoware_control_evaluator/README.md @@ -4,14 +4,14 @@ This package provides nodes that generate metrics to evaluate the quality of control. -It publishes diagnostic information about control modules' outputs as well as the ego vehicle's current kinematics and position. +It publishes metric information about control modules' outputs as well as the ego vehicle's current kinematics and position. ## Evaluated metrics -The control evaluator uses the metrics defined in `include/autoware/control_evaluator/metrics/deviation_metrics.hpp`to calculate deviations in yaw and lateral distance from the ego's set-point. The control_evaluator can also be customized to offer metrics/evaluation about other control modules. Currently, the control_evaluator offers a simple diagnostic output based on the autonomous_emergency_braking node's output, but this functionality can be extended to evaluate other control modules' performance. +The control evaluator uses the metrics defined in `include/autoware/control_evaluator/metrics/metric.hpp`to calculate deviations in yaw and lateral distance from the ego's set-point. The control_evaluator can also be customized to offer metrics/evaluation about other control modules. Currently, the control_evaluator offers a simple metric output based on the autonomous_emergency_braking node's output, but this functionality can be extended to evaluate other control modules' performance. ## Kinematics output -The control evaluator module also constantly publishes information regarding the ego vehicle's kinematics and position. It publishes the current ego lane id with the longitudinal `s` and lateral `t` arc coordinates. It also publishes the current ego speed, acceleration and jerk in its diagnostic messages. +The control evaluator module also constantly publishes information regarding the ego vehicle's kinematics and position. It publishes the current ego lane id with the longitudinal `s` and lateral `t` arc coordinates. It also publishes the current ego speed, acceleration and jerk in its metric messages. This information can be used by other nodes to establish automated evaluation using rosbags: by crosschecking the ego position and kinematics with the evaluated control module's output, it is possible to judge if the evaluated control modules reacted in a satisfactory way at certain interesting points of the rosbag reproduction. 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 6e4a04964a72c..c510b2ea46779 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 @@ -16,6 +16,8 @@ #define AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/control_evaluator/metrics/metric.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include #include @@ -32,10 +34,11 @@ #include #include #include +#include namespace control_diagnostics { - +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -53,6 +56,9 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~ControlEvaluatorNode() override; + + void AddMetricMsg(const Metric & metric, const double & metric_value); void AddLateralDeviationMetricMsg(const Trajectory & traj, const Point & ego_point); void AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose); void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose); @@ -85,9 +91,18 @@ class ControlEvaluatorNode : public rclcpp::Node // update Route Handler void getRouteData(); - // Calculator - // Metrics - std::deque stamps_; + // Parameters + bool output_metrics_; + + // Metric + const std::vector metrics_ = { + // collect all metrics + Metric::lateral_deviation, Metric::yaw_deviation, Metric::goal_longitudinal_deviation, + Metric::goal_lateral_deviation, Metric::goal_yaw_deviation, + }; + + std::array, static_cast(Metric::SIZE)> + metric_accumulators_; // 3(min, max, mean) * metric_size MetricArrayMsg metrics_msg_; autoware::route_handler::RouteHandler route_handler_; 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 new file mode 100644 index 0000000000000..be2f3135d7f7e --- /dev/null +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp @@ -0,0 +1,81 @@ +// Copyright 2021 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__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ + +#include +#include +#include +#include + +namespace control_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +enum class Metric { + lateral_deviation, + yaw_deviation, + goal_longitudinal_deviation, + goal_lateral_deviation, + goal_yaw_deviation, + SIZE, +}; + +static const std::unordered_map str_to_metric = { + {"lateral_deviation", Metric::lateral_deviation}, + {"yaw_deviation", Metric::yaw_deviation}, + {"goal_longitudinal_deviation", Metric::goal_longitudinal_deviation}, + {"goal_lateral_deviation", Metric::goal_lateral_deviation}, + {"goal_yaw_deviation", Metric::goal_yaw_deviation}, +}; + +static const std::unordered_map metric_to_str = { + {Metric::lateral_deviation, "lateral_deviation"}, + {Metric::yaw_deviation, "yaw_deviation"}, + {Metric::goal_longitudinal_deviation, "goal_longitudinal_deviation"}, + {Metric::goal_lateral_deviation, "goal_lateral_deviation"}, + {Metric::goal_yaw_deviation, "goal_yaw_deviation"}, +}; + +// Metrics descriptions +static const std::unordered_map metric_descriptions = { + {Metric::lateral_deviation, "Lateral deviation from the reference trajectory[m]"}, + {Metric::yaw_deviation, "Yaw deviation from the reference trajectory[rad]"}, + {Metric::goal_longitudinal_deviation, "Longitudinal deviation from the goal point[m]"}, + {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"}, + {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}}; + +namespace details +{ +static struct CheckCorrectMaps +{ + CheckCorrectMaps() + { + if ( + str_to_metric.size() != static_cast(Metric::SIZE) || + metric_to_str.size() != static_cast(Metric::SIZE) || + metric_descriptions.size() != static_cast(Metric::SIZE)) { + std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: "; + std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " " + << metric_descriptions.size() << std::endl; + } + } +} check; + +} // namespace details +} // namespace control_diagnostics + +#endif // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index e9fee7ffadbf2..4cf795afb5a7d 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -1,20 +1,20 @@ - + - - + + + - - + - - + + diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 8ab2c73cb619a..1c724fc7bd0ec 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -22,6 +22,7 @@ autoware_test_utils autoware_universe_utils nav_msgs + nlohmann-json-dev pluginlib rclcpp rclcpp_components diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index c24dfee571852..2a2c07b2db319 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -16,7 +16,11 @@ #include #include +#include +#include +#include +#include #include #include #include @@ -33,12 +37,63 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti this->create_publisher("~/debug/processing_time_ms", 1); metrics_pub_ = create_publisher("~/metrics", 1); + // Parameters + output_metrics_ = declare_parameter("output_metrics"); + // Timer callback to publish evaluator diagnostics using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this)); } +ControlEvaluatorNode::~ControlEvaluatorNode() +{ + if (!output_metrics_) { + return; + } + + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)].count(); + j[base_name + "description"] = metric_descriptions.at(metric); + } + + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } + } + + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } +} + void ControlEvaluatorNode::getRouteData() { // route @@ -62,6 +117,18 @@ void ControlEvaluatorNode::getRouteData() } } +void ControlEvaluatorNode::AddMetricMsg(const Metric & metric, const double & metric_value) +{ + MetricMsg metric_msg; + metric_msg.name = metric_to_str.at(metric); + metric_msg.value = std::to_string(metric_value); + metrics_msg_.metric_array.push_back(metric_msg); + + if (output_metrics_) { + metric_accumulators_[static_cast(metric)].add(metric_value); + } +} + void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) { const auto current_lanelets = [&]() { @@ -97,7 +164,6 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) metric_msg.value = std::to_string(arc_coordinates.distance); metrics_msg_.metric_array.push_back(metric_msg); } - return; } void ControlEvaluatorNode::AddKinematicStateMetricMsg( @@ -141,59 +207,44 @@ void ControlEvaluatorNode::AddKinematicStateMetricMsg( void ControlEvaluatorNode::AddLateralDeviationMetricMsg( const Trajectory & traj, const Point & ego_point) { - const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); + const Metric metric = Metric::lateral_deviation; + const double metric_value = metrics::calcLateralDeviation(traj, ego_point); - 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; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose) { - const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); + const Metric metric = Metric::yaw_deviation; + const double metric_value = metrics::calcYawDeviation(traj, ego_pose); - 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; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose) { - const double longitudinal_deviation = + const Metric metric = Metric::goal_longitudinal_deviation; + const double metric_value = metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position); - 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; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(const Pose & ego_pose) { - const double lateral_deviation = + const Metric metric = Metric::lateral_deviation; + const double metric_value = metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position); - 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; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose) { - const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); + const Metric metric = Metric::goal_yaw_deviation; + const double metric_value = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); - 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; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::onTimer() 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 0a2f777e49b61..728cef03901a8 100644 --- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -52,6 +52,7 @@ class EvalTest : public ::testing::Test rclcpp::NodeOptions options; const auto share_dir = ament_index_cpp::get_package_share_directory("autoware_control_evaluator"); + options.arguments({"--ros-args", "-p", "output_metrics:=false"}); dummy_node = std::make_shared("control_evaluator_test_node"); eval_node = std::make_shared(options); diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index 899db1448dbf8..387b82526f624 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -269,21 +269,11 @@ + - - - - - - - - - - - - - - + + +