feat(control_evaluator): add control_evaluator for v0.3.19

control/autoware_autonomous_emergency_braking/package.xml

@@ -16,6 +16,8 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
   <test_depend>autoware_lint_common</test_depend>
   <test_depend>autoware_test_utils</test_depend>
 

control/autoware_control_evaluator/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_control_evaluator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(pluginlib REQUIRED)
+
+ament_auto_add_library(control_evaluator_node SHARED
+  src/control_evaluator_node.cpp
+  src/metrics/deviation_metrics.cpp
+)
+
+rclcpp_components_register_node(control_evaluator_node
+  PLUGIN "control_diagnostics::ControlEvaluatorNode"
+  EXECUTABLE control_evaluator
+)
+
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)

control/autoware_control_evaluator/README.md

@@ -0,0 +1,5 @@
+# Planning Evaluator
+
+## Purpose
+
+This package provides nodes that generate metrics to evaluate the quality of control.

control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

@@ -0,0 +1,110 @@
+// 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.
+
+#ifndef AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
+#define AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
+
+#include "autoware/control_evaluator/metrics/deviation_metrics.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <route_handler/route_handler.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include <autoware_auto_planning_msgs/msg/route.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+
+#include <deque>
+#include <optional>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace control_diagnostics
+{
+
+using autoware_auto_planning_msgs::msg::Trajectory;
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using nav_msgs::msg::Odometry;
+using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
+using LaneletRoute = autoware_auto_planning_msgs::msg::HADMapRoute;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
+using sensor_msgs::msg::Imu;
+
+/**
+ * @brief Node for control evaluation
+ */
+class ControlEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options);
+  DiagnosticStatus generateLateralDeviationDiagnosticStatus(
+    const Trajectory & traj, const Point & ego_point);
+  DiagnosticStatus generateYawDeviationDiagnosticStatus(
+    const Trajectory & traj, const Pose & ego_pose);
+  std::optional<DiagnosticStatus> generateStopDiagnosticStatus(
+    const DiagnosticArray & diag, const std::string & function_name);
+
+  DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
+  DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;
+  DiagnosticStatus generateKinematicStateDiagnosticStatus(const Odometry & odom, const Imu & imu);
+
+  void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
+  void onTimer();
+
+private:
+  // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately.
+  // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with
+  // onDiagnostics().
+  rclcpp::Subscription<DiagnosticArray>::SharedPtr control_diag_sub_;
+
+  autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
+    this, "~/input/odometry"};
+  // autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
+  //   this, "~/input/acceleration"};
+  autoware::universe_utils::InterProcessPollingSubscriber<Imu> imu_sub_{this, "~/input/imu"};
+  autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
+    this, "~/input/trajectory"};
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    LaneletRoute, autoware::universe_utils::polling_policy::Newest>
+    route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()};
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    LaneletMapBin, autoware::universe_utils::polling_policy::Newest>
+    vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
+
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
+
+  // update Route Handler
+  void getRouteData();
+
+  // Calculator
+  // Metrics
+  std::deque<rclcpp::Time> stamps_;
+
+  // queue for diagnostics and time stamp
+  std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
+  const std::vector<std::string> target_functions_ = {"autonomous_emergency_braking"};
+
+  route_handler::RouteHandler route_handler_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::optional<Imu> prev_imu_{std::nullopt};
+};
+}  // namespace control_diagnostics
+
+#endif  // AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_

control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp

@@ -0,0 +1,48 @@
+// 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.
+
+#ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+#define AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+namespace control_diagnostics
+{
+namespace metrics
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+
+/**
+ * @brief calculate lateral deviation of the given trajectory from the reference trajectory
+ * @param [in] ref reference trajectory
+ * @param [in] point input point
+ * @return lateral deviation
+ */
+double calcLateralDeviation(const Trajectory & traj, const Point & point);
+
+/**
+ * @brief calculate yaw deviation of the given trajectory from the reference trajectory
+ * @param [in] traj input trajectory
+ * @param [in] pose input pose
+ * @return yaw deviation
+ */
+double calcYawDeviation(const Trajectory & traj, const Pose & pose);
+
+}  // namespace metrics
+}  // namespace control_diagnostics
+
+#endif  // AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_

control/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -0,0 +1,21 @@
+<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"/>
+  <!-- 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="~/metrics" to="/control/control_evaluator/metrics"/>
+      <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
+      <remap from="~/input/route" to="$(var route_topic_name)"/>
+    </node>
+  </group>
+</launch>

control/autoware_control_evaluator/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_control_evaluator</name>
+  <version>0.1.0</version>
+  <description>ROS 2 node for evaluating control</description>
+  <maintainer email="[email protected]">Daniel SANCHEZ</maintainer>
+  <maintainer email="[email protected]">takayuki MUROOKA</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Daniel SANCHEZ</author>
+  <author email="[email protected]">takayuki MUROOKA</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_evaluator_utils</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>route_handler</depend>
+  <depend>tier4_autoware_utils</depend>
+  <!-- <depend>nav_msgs</depend> -->
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

control/autoware_control_evaluator/param/control_evaluator.defaults.yaml

@@ -0,0 +1,2 @@
+/**:
+  ros__parameters: