feat(planning_evaluator): add planning evaluator

planning/planning_evaluator/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.5)
+project(planning_evaluator)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/metrics_calculator.cpp
+  src/${PROJECT_NAME}_node.cpp
+  src/motion_evaluator_node.cpp
+  src/metrics/deviation_metrics.cpp
+  src/metrics/metrics_utils.cpp
+  src/metrics/obstacle_metrics.cpp
+  src/metrics/stability_metrics.cpp
+  src/metrics/trajectory_metrics.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "planning_diagnostics::PlanningEvaluatorNode"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "planning_diagnostics::MotionEvaluatorNode"
+  EXECUTABLE motion_evaluator
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_planning_evaluator_node.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)

planning/planning_evaluator/README.md

@@ -0,0 +1,96 @@
+# Planning Evaluator
+
+## Purpose
+
+This package provides nodes that generate metrics to evaluate the quality of planning and control.
+
+## Inner-workings / Algorithms
+
+The evaluation node calculates metrics each time it receives a trajectory `T(0)`.
+Metrics are calculated using the following information:
+
+- the trajectory `T(0)` itself.
+- the previous trajectory `T(-1)`.
+- the _reference_ trajectory assumed to be used as the reference to plan `T(0)`.
+- the current ego pose.
+- the set of objects in the environment.
+
+These information are maintained by an instance of class `MetricsCalculator`
+which is also responsible for calculating metrics.
+
+### Stat
+
+Each metric is calculated using a `Stat` instance which contains
+the minimum, maximum, and mean values calculated for the metric
+as well as the number of values measured.
+
+### Metric calculation and adding more metrics
+
+All possible metrics are defined in the `Metric` enumeration defined
+`include/planning_evaluator/metrics/metric.hpp`.
+This file also defines conversions from/to string as well as human readable descriptions
+to be used as header of the output file.
+
+The `MetricsCalculator` is responsible for calculating metric statistics
+through calls to function:
+
+```C++
+Stat<double> MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const;
+```
+
+Adding a new metric `M` requires the following steps:
+
+- `metrics/metric.hpp`: add `M` to the `enum`, to the from/to string conversion maps, and to the description map.
+- `metrics_calculator.cpp`: add `M` to the `switch/case` statement of the `calculate` function.
+- Add `M` to the `selected_metrics` parameters.
+
+## Inputs / Outputs
+
+### Inputs
+
+| Name                           | Type                                                   | Description                                       |
+| ------------------------------ | ------------------------------------------------------ | ------------------------------------------------- |
+| `~/input/trajectory`           | `autoware_auto_planning_msgs::msg::Trajectory`         | Main trajectory to evaluate                       |
+| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory`         | Reference trajectory to use for deviation metrics |
+| `~/input/objects`              | `autoware_auto_perception_msgs::msg::PredictedObjects` | Obstacles                                         |
+
+### Outputs
+
+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 |
+
+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.
+
+## Parameters
+
+| Name                              | Type     | Description                                                                 |
+| :-------------------------------- | :------- | :-------------------------------------------------------------------------- |
+| `output_file`                     | `string` | file used to write metrics                                                  |
+| `ego_frame`                       | `string` | frame used for the ego pose                                                 |
+| `selected_metrics`                | List     | metrics to measure and publish                                              |
+| `trajectory.min_point_dist_m`     | `double` | minimum distance between two successive points to use for angle calculation |
+| `trajectory.lookahead.max_dist_m` | `double` | maximum distance from ego along the trajectory to use for calculation       |
+| `trajectory.lookahead.max_time_m` | `double` | maximum time ahead of ego along the trajectory to use for calculation       |
+| `obstacle.dist_thr_m`             | `double` | distance between ego and the obstacle below which a collision is considered |
+
+## Assumptions / Known limits
+
+There is a strong assumption that when receiving a trajectory `T(0)`,
+it has been generated using the last received reference trajectory and objects.
+This can be wrong if a new reference trajectory or objects are published while `T(0)` is being calculated.
+
+Precision is currently limited by the resolution of the trajectories.
+It is possible to interpolate the trajectory and reference trajectory to increase precision but would make computation significantly more expensive.
+
+## Future extensions / Unimplemented parts
+
+- Use `Route` or `Path` messages as reference trajectory.
+- RSS metrics (done in another node <https://tier4.atlassian.net/browse/AJD-263>).
+- Add option to publish the `min` and `max` metric values. For now only the `mean` value is published.
+- `motion_evaluator_node`.
+  - Node which constructs a trajectory over time from the real motion of ego.
+  - Only a proof of concept is currently implemented.

planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp

@@ -0,0 +1,57 @@
+// 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 PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+#define PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+
+#include "planning_evaluator/stat.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+namespace planning_diagnostics
+{
+namespace metrics
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+
+/**
+ * @brief calculate lateral deviation of the given trajectory from the reference trajectory
+ * @param [in] ref reference trajectory
+ * @param [in] traj input trajectory
+ * @return calculated statistics
+ */
+Stat<double> calcLateralDeviation(const Trajectory & ref, const Trajectory & traj);
+
+/**
+ * @brief calculate yaw deviation of the given trajectory from the reference trajectory
+ * @param [in] ref reference trajectory
+ * @param [in] traj input trajectory
+ * @return calculated statistics
+ */
+Stat<double> calcYawDeviation(const Trajectory & ref, const Trajectory & traj);
+
+/**
+ * @brief calculate velocity deviation of the given trajectory from the reference trajectory
+ * @param [in] ref reference trajectory
+ * @param [in] traj input trajectory
+ * @return calculated statistics
+ */
+Stat<double> calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj);
+
+}  // namespace metrics
+}  // namespace planning_diagnostics
+
+#endif  // PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_

planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp

@@ -0,0 +1,122 @@
+// 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 PLANNING_EVALUATOR__METRICS__METRIC_HPP_
+#define PLANNING_EVALUATOR__METRICS__METRIC_HPP_
+
+#include <iostream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace planning_diagnostics
+{
+/**
+ * @brief Enumeration of trajectory metrics
+ */
+enum class Metric {
+  curvature,
+  point_interval,
+  relative_angle,
+  length,
+  duration,
+  velocity,
+  acceleration,
+  jerk,
+  lateral_deviation,
+  yaw_deviation,
+  velocity_deviation,
+  stability,
+  stability_frechet,
+  obstacle_distance,
+  obstacle_ttc,
+  SIZE,
+};
+
+/** TODO(Maxime CLEMENT):
+ * make the addition of metrics simpler, e.g. with some macro ADD_METRIC(metric, metric_description)
+ */
+static const std::unordered_map<std::string, Metric> str_to_metric = {
+  {"curvature", Metric::curvature},
+  {"point_interval", Metric::point_interval},
+  {"relative_angle", Metric::relative_angle},
+  {"length", Metric::length},
+  {"duration", Metric::duration},
+  {"velocity", Metric::velocity},
+  {"acceleration", Metric::acceleration},
+  {"jerk", Metric::jerk},
+  {"lateral_deviation", Metric::lateral_deviation},
+  {"yaw_deviation", Metric::yaw_deviation},
+  {"velocity_deviation", Metric::velocity_deviation},
+  {"stability", Metric::stability},
+  {"stability_frechet", Metric::stability_frechet},
+  {"obstacle_distance", Metric::obstacle_distance},
+  {"obstacle_ttc", Metric::obstacle_ttc},
+};
+static const std::unordered_map<Metric, std::string> metric_to_str = {
+  {Metric::curvature, "curvature"},
+  {Metric::point_interval, "point_interval"},
+  {Metric::relative_angle, "relative_angle"},
+  {Metric::length, "length"},
+  {Metric::duration, "duration"},
+  {Metric::velocity, "velocity"},
+  {Metric::acceleration, "acceleration"},
+  {Metric::jerk, "jerk"},
+  {Metric::lateral_deviation, "lateral_deviation"},
+  {Metric::yaw_deviation, "yaw_deviation"},
+  {Metric::velocity_deviation, "velocity_deviation"},
+  {Metric::stability, "stability"},
+  {Metric::stability_frechet, "stability_frechet"},
+  {Metric::obstacle_distance, "obstacle_distance"},
+  {Metric::obstacle_ttc, "obstacle_ttc"}};
+
+// Metrics descriptions
+static const std::unordered_map<Metric, std::string> metric_descriptions = {
+  {Metric::curvature, "Curvature[1/rad]"},
+  {Metric::point_interval, "Interval_between_points[m]"},
+  {Metric::relative_angle, "Relative_angle[rad]"},
+  {Metric::length, "Trajectory_length[m]"},
+  {Metric::duration, "Trajectory_duration[s]"},
+  {Metric::velocity, "Trajectory_velocity[m/s]"},
+  {Metric::acceleration, "Trajectory_acceleration[m/s²]"},
+  {Metric::jerk, "Trajectory_jerk[m/s³]"},
+  {Metric::lateral_deviation, "Lateral_deviation[m]"},
+  {Metric::yaw_deviation, "Yaw_deviation[rad]"},
+  {Metric::velocity_deviation, "Velocity_deviation[m/s]"},
+  {Metric::stability, "Stability[m]"},
+  {Metric::stability_frechet, "StabilityFrechet[m]"},
+  {Metric::obstacle_distance, "Obstacle_distance[m]"},
+  {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}};
+
+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 planning_diagnostics
+
+#endif  // PLANNING_EVALUATOR__METRICS__METRIC_HPP_

planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp

@@ -0,0 +1,43 @@
+// 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 PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_
+#define PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_
+
+#include "planning_evaluator/stat.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+namespace planning_diagnostics
+{
+namespace metrics
+{
+namespace utils
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+
+/**
+ * @brief find the index in the trajectory at the given distance of the given index
+ * @param [in] traj input trajectory
+ * @param [in] curr_id index
+ * @param [in] distance distance
+ * @return index of the trajectory point at distance ahead of traj[curr_id]
+ */
+size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance);
+
+}  // namespace utils
+}  // namespace metrics
+}  // namespace planning_diagnostics
+#endif  // PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_