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

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp → common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021-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.
@@ -15,17 +15,17 @@
 #include <iostream>
 #include <limits>
 
-#ifndef AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_
-#define AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_
+#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_
 
-namespace planning_diagnostics
+namespace autoware::universe_utils
 {
 /**
- * @brief class to incrementally build statistics
+ * @brief class to accumulate statistical data, supporting min, max and mean.
  * @typedef T type of the values (default to double)
  */
 template <typename T = double>
-class Stat
+class Accumulator
 {
 public:
   /**
@@ -65,11 +65,11 @@ class Stat
   unsigned int count() const { return count_; }
 
   template <typename U>
-  friend std::ostream & operator<<(std::ostream & os, const Stat<U> & stat);
+  friend std::ostream & operator<<(std::ostream & os, const Accumulator<U> & accumulator);
 
 private:
   T min_ = std::numeric_limits<T>::max();
-  T max_ = std::numeric_limits<T>::min();
+  T max_ = std::numeric_limits<T>::lowest();
   long double mean_ = 0.0;
   unsigned int count_ = 0;
 };
@@ -78,16 +78,16 @@ class Stat
  * @brief overload << operator for easy print to output stream
  */
 template <typename T>
-std::ostream & operator<<(std::ostream & os, const Stat<T> & stat)
+std::ostream & operator<<(std::ostream & os, const Accumulator<T> & accumulator)
 {
-  if (stat.count() == 0) {
+  if (accumulator.count() == 0) {
     os << "None None None";
   } else {
-    os << stat.min() << " " << stat.max() << " " << stat.mean();
+    os << accumulator.min() << " " << accumulator.max() << " " << accumulator.mean();
   }
   return os;
 }
 
-}  // namespace planning_diagnostics
+}  // namespace autoware::universe_utils
 
-#endif  // AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_
+#endif  // AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_

common/autoware_universe_utils/test/src/math/test_accumulator.cpp

@@ -0,0 +1,64 @@
+// 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/universe_utils/math/accumulator.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(accumulator, empty)
+{
+  autoware::universe_utils::Accumulator<double> acc;
+
+  EXPECT_DOUBLE_EQ(acc.mean(), 0.0);
+  EXPECT_DOUBLE_EQ(acc.min(), std::numeric_limits<double>::max());
+  EXPECT_DOUBLE_EQ(acc.max(), std::numeric_limits<double>::lowest());
+  EXPECT_EQ(acc.count(), 0);
+}
+
+TEST(accumulator, addValues)
+{
+  autoware::universe_utils::Accumulator<double> acc;
+  acc.add(100.0);
+
+  EXPECT_DOUBLE_EQ(acc.mean(), 100.0);
+  EXPECT_DOUBLE_EQ(acc.min(), 100.0);
+  EXPECT_DOUBLE_EQ(acc.max(), 100.0);
+  EXPECT_EQ(acc.count(), 1);
+}
+
+TEST(accumulator, positiveValues)
+{
+  autoware::universe_utils::Accumulator<double> acc;
+  acc.add(10.0);
+  acc.add(40.0);
+  acc.add(10.0);
+
+  EXPECT_DOUBLE_EQ(acc.mean(), 20.0);
+  EXPECT_DOUBLE_EQ(acc.min(), 10.0);
+  EXPECT_DOUBLE_EQ(acc.max(), 40.0);
+  EXPECT_EQ(acc.count(), 3);
+}
+
+TEST(accumulator, negativeValues)
+{
+  autoware::universe_utils::Accumulator<double> acc;
+  acc.add(-10.0);
+  acc.add(-40.0);
+  acc.add(-10.0);
+
+  EXPECT_DOUBLE_EQ(acc.mean(), -20.0);
+  EXPECT_DOUBLE_EQ(acc.min(), -40.0);
+  EXPECT_DOUBLE_EQ(acc.max(), -10.0);
+  EXPECT_EQ(acc.count(), 3);
+}

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`.