feat(control_evaluator): add deviation metrics and queue for diagnostics (autowarefoundation#7484)

Author: kosuke55

evaluator/control_evaluator/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(pluginlib REQUIRED)
 
 ament_auto_add_library(${PROJECT_NAME}_node SHARED
   src/${PROJECT_NAME}_node.cpp
+  src/metrics/deviation_metrics.cpp
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}_node

evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp

@@ -15,21 +15,30 @@
 #ifndef CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
 #define CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
 
-#include "rclcpp/rclcpp.hpp"
+#include "control_evaluator/metrics/deviation_metrics.hpp"
 
-#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 
 #include <array>
 #include <deque>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace control_diagnostics
 {
 
+using autoware_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;
 
 /**
  * @brief Node for control evaluation
@@ -38,22 +47,46 @@ 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);
+
+  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);
 
-  /**
-   * @brief publish the given metric statistic
-   */
-  DiagnosticStatus generateDiagnosticStatus(const bool is_emergency_brake) const;
   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_;
+
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
+    this, "~/input/odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
+    this, "~/input/trajectory"};
+
   rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
 
   // Calculator
   // Metrics
   std::deque<rclcpp::Time> stamps_;
-  DiagnosticArray metrics_msg_;
+
+  // 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"};
+
   rclcpp::TimerBase::SharedPtr timer_;
 };
 }  // namespace control_diagnostics

evaluator/control_evaluator/include/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 CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+#define CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+
+namespace control_diagnostics
+{
+namespace metrics
+{
+using autoware_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  // CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_

evaluator/control_evaluator/launch/control_evaluator.launch.xml

@@ -1,11 +1,14 @@
 <launch>
   <arg name="input/diagnostics" default="/diagnostics"/>
-
+  <arg name="input/odometry" default="/localization/kinematic_state"/>
+  <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
   <!-- control evaluator -->
   <group>
     <node name="control_evaluator" exec="control_evaluator" pkg="control_evaluator">
       <param from="$(find-pkg-share 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/trajectory" to="$(var input/trajectory)"/>
       <remap from="~/metrics" to="/diagnostic/control_evaluator/metrics"/>
     </node>
   </group>

evaluator/control_evaluator/package.xml

@@ -14,10 +14,14 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_planning_msgs</depend>
   <depend>diagnostic_msgs</depend>
+  <depend>motion_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
+  <!-- <depend>nav_msgs</depend> -->
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

evaluator/control_evaluator/src/control_evaluator_node.cpp

@@ -28,7 +28,6 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti
 : Node("control_evaluator", node_options)
 {
   using std::placeholders::_1;
-
   control_diag_sub_ = create_subscription<DiagnosticArray>(
     "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1));
 
@@ -41,45 +40,140 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti
     rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this));
 }
 
-DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus(const bool is_emergency_brake) const
+void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp)
+{
+  constexpr double KEEP_TIME = 1.0;
+  diag_queue_.erase(
+    std::remove_if(
+      diag_queue_.begin(), diag_queue_.end(),
+      [stamp](const std::pair<diagnostic_msgs::msg::DiagnosticStatus, rclcpp::Time> & p) {
+        return (stamp - p.second).seconds() > KEEP_TIME;
+      }),
+    diag_queue_.end());
+}
+
+void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name)
+{
+  diag_queue_.erase(
+    std::remove_if(
+      diag_queue_.begin(), diag_queue_.end(),
+      [&name](const std::pair<diagnostic_msgs::msg::DiagnosticStatus, rclcpp::Time> & p) {
+        return p.first.name.find(name) != std::string::npos;
+      }),
+    diag_queue_.end());
+}
+
+void controlEvaluatorNode::addDiagnostic(
+  const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp)
+{
+  diag_queue_.push_back(std::make_pair(diag, stamp));
+}
+
+void controlEvaluatorNode::updateDiagnosticQueue(
+  const DiagnosticArray & input_diagnostics, const std::string & function,
+  const rclcpp::Time & stamp)
+{
+  const auto it = std::find_if(
+    input_diagnostics.status.begin(), input_diagnostics.status.end(),
+    [&function](const diagnostic_msgs::msg::DiagnosticStatus & diag) {
+      return diag.name.find(function) != std::string::npos;
+    });
+  if (it != input_diagnostics.status.end()) {
+    removeDiagnosticsByName(it->name);
+    addDiagnostic(*it, input_diagnostics.header.stamp);
+  }
+
+  removeOldDiagnostics(stamp);
+}
+
+void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg)
+{
+  // add target diagnostics to the queue and remove old ones
+  for (const auto & function : target_functions_) {
+    updateDiagnosticQueue(*diag_msg, function, now());
+  }
+}
+
+DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag)
 {
   DiagnosticStatus status;
   status.level = status.OK;
-  status.name = "autonomous_emergency_braking";
+  status.name = diag.name;
   diagnostic_msgs::msg::KeyValue key_value;
   key_value.key = "decision";
+  const bool is_emergency_brake = (diag.level == DiagnosticStatus::ERROR);
   key_value.value = (is_emergency_brake) ? "stop" : "none";
   status.values.push_back(key_value);
+
   return status;
 }
 
-void controlEvaluatorNode::onTimer()
+DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus(
+  const Trajectory & traj, const Point & ego_point)
 {
-  if (!metrics_msg_.status.empty()) {
-    metrics_pub_->publish(metrics_msg_);
-    metrics_msg_.status.clear();
-  }
+  const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point);
+
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = "lateral_deviation";
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "metric_value";
+  key_value.value = std::to_string(lateral_deviation);
+  status.values.push_back(key_value);
+
+  return status;
 }
 
-void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg)
+DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus(
+  const Trajectory & traj, const Pose & ego_pose)
 {
-  const auto start = now();
-  const auto aeb_status =
-    std::find_if(diag_msg->status.begin(), diag_msg->status.end(), [](const auto & status) {
-      const bool aeb_found = status.name.find("autonomous_emergency_braking") != std::string::npos;
-      return aeb_found;
-    });
+  const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose);
 
-  if (aeb_status == diag_msg->status.end()) return;
-
-  const bool is_emergency_brake = (aeb_status->level == DiagnosticStatus::ERROR);
-  metrics_msg_.header.stamp = now();
-  metrics_msg_.status.emplace_back(generateDiagnosticStatus(is_emergency_brake));
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = "yaw_deviation";
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "metric_value";
+  key_value.value = std::to_string(yaw_deviation);
+  status.values.push_back(key_value);
 
-  const auto runtime = (now() - start).seconds();
-  RCLCPP_DEBUG(get_logger(), "control evaluation calculation time: %2.2f ms", runtime * 1e3);
+  return status;
 }
 
+void controlEvaluatorNode::onTimer()
+{
+  DiagnosticArray metrics_msg;
+  const auto traj = traj_sub_.takeData();
+  const auto odom = odometry_sub_.takeData();
+
+  // generate decision diagnostics from input diagnostics
+  for (const auto & function : target_functions_) {
+    const auto it = std::find_if(
+      diag_queue_.begin(), diag_queue_.end(),
+      [&function](const std::pair<diagnostic_msgs::msg::DiagnosticStatus, rclcpp::Time> & p) {
+        return p.first.name.find(function) != std::string::npos;
+      });
+    if (it == diag_queue_.end()) {
+      continue;
+    }
+    // generate each decision diagnostics
+    // - AEB decision
+    if (it->first.name.find("autonomous_emergency_braking") != std::string::npos) {
+      metrics_msg.status.push_back(generateAEBDiagnosticStatus(it->first));
+    }
+  }
+
+  // calculate deviation metrics
+  if (odom && traj && !traj->points.empty()) {
+    const Pose ego_pose = odom->pose.pose;
+    metrics_msg.status.push_back(
+      generateLateralDeviationDiagnosticStatus(*traj, ego_pose.position));
+    metrics_msg.status.push_back(generateYawDeviationDiagnosticStatus(*traj, ego_pose));
+  }
+
+  metrics_msg.header.stamp = now();
+  metrics_pub_->publish(metrics_msg);
+}
 }  // namespace control_diagnostics
 
 #include "rclcpp_components/register_node_macro.hpp"

evaluator/control_evaluator/src/metrics/deviation_metrics.cpp

@@ -0,0 +1,41 @@
+// 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 "control_evaluator/metrics/deviation_metrics.hpp"
+
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/geometry/pose_deviation.hpp"
+
+namespace control_diagnostics
+{
+namespace metrics
+{
+using autoware_planning_msgs::msg::Trajectory;
+
+double calcLateralDeviation(const Trajectory & traj, const Point & point)
+{
+  const size_t nearest_index = motion_utils::findNearestIndex(traj.points, point);
+  return std::abs(
+    tier4_autoware_utils::calcLateralDeviation(traj.points[nearest_index].pose, point));
+}
+
+double calcYawDeviation(const Trajectory & traj, const Pose & pose)
+{
+  const size_t nearest_index = motion_utils::findNearestIndex(traj.points, pose.position);
+  return std::abs(tier4_autoware_utils::calcYawDeviation(traj.points[nearest_index].pose, pose));
+}
+
+}  // namespace metrics
+}  // namespace control_diagnostics

launch/tier4_control_launch/launch/control.launch.py

@@ -370,7 +370,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/diagnostics", "/diagnostics"),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/input/trajectory", "/planning/scenario_planning/trajectory"),
-            ("~/metrics", "/diagnostic/control_evaluator/metrics"),
+            ("~/output/metrics", "~/metrics"),
         ],
     )