feat(control_evaluator): add a trigger to choice whether to output metrics to log folder

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.

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 <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
@@ -32,10 +34,11 @@
 #include <deque>
 #include <optional>
 #include <string>
+#include <vector>
 
 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<rclcpp::Time> stamps_;
+  // Parameters
+  bool output_metrics_;
+
+  // Metric
+  std::vector<Metric> metrics_ = {
+    // collect all metrics
+    Metric::lateral_deviation,      Metric::yaw_deviation,      Metric::goal_longitudinal_deviation,
+    Metric::goal_lateral_deviation, Metric::goal_yaw_deviation,
+  };
+
+  std::array<Accumulator<double>, static_cast<size_t>(Metric::SIZE)>
+    metric_accumulators_;  // 3(min, max, mean) * metric_size
 
   MetricArrayMsg metrics_msg_;
   autoware::route_handler::RouteHandler route_handler_;

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 <iostream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+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<std::string, Metric> 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, std::string> 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, std::string> 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<size_t>(Metric::SIZE) ||
+      metric_to_str.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_descriptions.size() != static_cast<size_t>(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_

evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -1,20 +1,19 @@
 <launch>
-  <arg name="input/diagnostics" default="/diagnostics"/>
   <arg name="input/odometry" default="/localization/kinematic_state"/>
   <arg name="input/acceleration" default="/localization/acceleration"/>
   <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
-  <arg name="map_topic_name" default="/map/vector_map"/>
-  <arg name="route_topic_name" default="/planning/mission_planning/route"/>
+  <arg name="input/vector_map" default="/map/vector_map"/>
+  <arg name="input/route" default="/planning/mission_planning/route"/>
+
   <!-- control evaluator -->
   <group>
     <node name="control_evaluator" exec="control_evaluator" pkg="autoware_control_evaluator">
       <param from="$(find-pkg-share autoware_control_evaluator)/param/control_evaluator.defaults.yaml"/>
-      <remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
       <remap from="~/input/odometry" to="$(var input/odometry)"/>
       <remap from="~/input/acceleration" to="$(var input/acceleration)"/>
       <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
-      <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
-      <remap from="~/input/route" to="$(var route_topic_name)"/>
+      <remap from="~/input/vector_map" to="$(var input/vector_map)"/>
+      <remap from="~/input/route" to="$(var input/route)"/>
     </node>
   </group>
 </launch>

evaluator/autoware_control_evaluator/package.xml

@@ -22,6 +22,7 @@
   <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>nav_msgs</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml

@@ -1,2 +1,3 @@
 /**:
   ros__parameters:
+    output_metrics: false # if true, metrics are written to `<ros2_logging_directory>/autoware_metrics/<node_name>-<time_stamp>.json`.

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -16,7 +16,11 @@
 
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
+#include <nlohmann/json.hpp>
 
+#include <chrono>
+#include <filesystem>
+#include <fstream>
 #include <limits>
 #include <string>
 #include <vector>
@@ -33,12 +37,63 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti
     this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
   metrics_pub_ = create_publisher<MetricArrayMsg>("~/metrics", 1);
 
+  // Parameters
+  output_metrics_ = declare_parameter<bool>("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<size_t>(metric)].min();
+    j[base_name + "max"] = metric_accumulators_[static_cast<size_t>(metric)].max();
+    j[base_name + "mean"] = metric_accumulators_[static_cast<size_t>(metric)].mean();
+    j[base_name + "count"] = metric_accumulators_[static_cast<size_t>(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<size_t>(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()

evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp

@@ -52,6 +52,8 @@ 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", "--params-file", share_dir + "/param/control_evaluator.defaults.yaml"});
 
     dummy_node = std::make_shared<rclcpp::Node>("control_evaluator_test_node");
     eval_node = std::make_shared<EvalNode>(options);