Merge pull request #1644 from tier4/cherry-pick/control-evaluator-to-…

…v4.0.0

feat(control_evaluator): cherry pick/control evaluator to v4.0.0

.github/CODEOWNERS

@@ -62,7 +62,9 @@ control/autoware_trajectory_follower_base/** [email protected] takayuki.m
 control/pure_pursuit/** [email protected] [email protected]
 control/shift_decider/** [email protected] [email protected]
 control/autoware_trajectory_follower_node/** [email protected] [email protected]
-evaluator/autoware_control_evaluator/** [email protected] [email protected]
+evaluator/autoware_control_evaluator/** [email protected] [email protected] [email protected]
+evaluator/autoware_evaluator_utils/** [email protected] [email protected]
+evaluator/autoware_planning_evaluator/** [email protected] [email protected]
 evaluator/diagnostic_converter/** [email protected] [email protected] [email protected]
 evaluator/kinematic_evaluator/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 evaluator/localization_evaluator/** [email protected] [email protected]

evaluator/autoware_control_evaluator/CMakeLists.txt

@@ -12,10 +12,19 @@ ament_auto_add_library(control_evaluator_node SHARED
 )
 
 rclcpp_components_register_node(control_evaluator_node
-  PLUGIN "control_diagnostics::controlEvaluatorNode"
+  PLUGIN "control_diagnostics::ControlEvaluatorNode"
   EXECUTABLE control_evaluator
 )
 
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_control_evaluator
+    test/test_control_evaluator_node.cpp
+  )
+  target_link_libraries(test_control_evaluator
+    control_evaluator_node
+  )
+endif()
+
 
 ament_auto_package(
   INSTALL_TO_SHARE

evaluator/autoware_control_evaluator/README.md

@@ -1,5 +1,17 @@
-# Planning Evaluator
+# Control Evaluator
 
 ## Purpose
 
 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.
+
+## 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.
+
+## 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.
+
+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

@@ -17,15 +17,17 @@
 
 #include "autoware/control_evaluator/metrics/deviation_metrics.hpp"
 
+#include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
-#include <array>
 #include <deque>
-#include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -39,29 +41,29 @@ using diagnostic_msgs::msg::DiagnosticStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
+using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
+using autoware_planning_msgs::msg::LaneletRoute;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 
 /**
  * @brief Node for control evaluation
  */
-class controlEvaluatorNode : public rclcpp::Node
+class ControlEvaluatorNode : public rclcpp::Node
 {
 public:
-  explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options);
-  void removeOldDiagnostics(const rclcpp::Time & stamp);
-  void removeDiagnosticsByName(const std::string & name);
-  void addDiagnostic(const DiagnosticStatus & diag, const rclcpp::Time & stamp);
-  void updateDiagnosticQueue(
-    const DiagnosticArray & input_diagnostics, const std::string & function,
-    const rclcpp::Time & stamp);
-
+  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 generateGoalLongitudinalDeviationDiagnosticStatus(const Pose & ego_pose);
+  DiagnosticStatus generateGoalLateralDeviationDiagnosticStatus(const Pose & ego_pose);
+  DiagnosticStatus generateGoalYawDeviationDiagnosticStatus(const Pose & ego_pose);
 
   DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
+  DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;
+  DiagnosticStatus generateKinematicStateDiagnosticStatus(
+    const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
 
   void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
   void onTimer();
@@ -74,11 +76,20 @@ class controlEvaluatorNode : public rclcpp::Node
 
   autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
     this, "~/input/odometry"};
+  autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
+    this, "~/input/acceleration"};
   autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
     this, "~/input/trajectory"};
+  autoware::universe_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
+    this, "~/input/route", rclcpp::QoS{1}.transient_local()};
+  autoware::universe_utils::InterProcessPollingSubscriber<LaneletMapBin> 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_;
@@ -87,7 +98,9 @@ class controlEvaluatorNode : public rclcpp::Node
   std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
   const std::vector<std::string> target_functions_ = {"autonomous_emergency_braking"};
 
+  autoware::route_handler::RouteHandler route_handler_;
   rclcpp::TimerBase::SharedPtr timer_;
+  std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
 };
 }  // namespace control_diagnostics
 

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

@@ -42,6 +42,30 @@ double calcLateralDeviation(const Trajectory & traj, const Point & point);
  */
 double calcYawDeviation(const Trajectory & traj, const Pose & pose);
 
+/**
+ * @brief calculate longitudinal deviation from target_point to base_pose
+ * @param [in] pose input base_pose
+ * @param [in] point input target_point
+ * @return longitudinal deviation from base_pose to target_point
+ */
+double calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate lateral deviation from target_point to base_pose
+ * @param [in] pose input base_pose
+ * @param [in] point input target_point
+ * @return lateral deviation from base_pose to target_point
+ */
+double calcLateralDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate yaw deviation from base_pose to target_pose
+ * @param [in] pose input base_pose
+ * @param [in] pose input target_pose
+ * @return yaw deviation from base_pose to target_pose
+ */
+double calcYawDeviation(const Pose & base_pose, const Pose & target_pose);
+
 }  // namespace metrics
 }  // namespace control_diagnostics
 

evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -1,15 +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>

evaluator/autoware_control_evaluator/package.xml

@@ -6,6 +6,8 @@
   <description>ROS 2 node for evaluating control</description>
   <maintainer email="[email protected]">Daniel SANCHEZ</maintainer>
   <maintainer email="[email protected]">takayuki MUROOKA</maintainer>
+  <maintainer email="[email protected]">kosuke TAKEUCHI</maintainer>
+  <maintainer email="[email protected]">Temkei Kem</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Daniel SANCHEZ</author>
@@ -14,14 +16,19 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_evaluator_utils</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_route_handler</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-  <!-- <depend>nav_msgs</depend> -->
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>