feat(planning_evaluator,control_evaluator, evaluator utils): add diag…

…nostics subscriber to planning eval (#7849)

* add utils and diagnostics subscription to planning_evaluator

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add diagnostics eval

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* fix input diag in launch

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: kosuke55 <kosuke.tnp@gmail.com>

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

@@ -52,13 +52,6 @@ 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(

evaluator/autoware_control_evaluator/package.xml

@@ -14,6 +14,7 @@
   <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>

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -14,6 +14,8 @@
 
 #include "autoware/control_evaluator/control_evaluator_node.hpp"
 
+#include "autoware/evaluator_utils/evaluator_utils.hpp"
+
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 
@@ -64,57 +66,11 @@ void ControlEvaluatorNode::getRouteData()
   }
 }
 
-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());
+    autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_);
   }
 }
 

evaluator/autoware_evaluator_utils/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_evaluator_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(evaluator_utils SHARED
+  src/evaluator_utils.cpp
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+)

evaluator/autoware_evaluator_utils/README.md

@@ -0,0 +1,5 @@
+# Evaluator Utils
+
+## Purpose
+
+This package provides utils functions for other evaluator packages

evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp

@@ -0,0 +1,45 @@
+// 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__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_
+#define AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <deque>
+#include <optional>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace autoware::evaluator_utils
+{
+
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using DiagnosticQueue = std::deque<std::pair<DiagnosticStatus, rclcpp::Time>>;
+
+void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue);
+void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue);
+void addDiagnostic(
+  const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue);
+void updateDiagnosticQueue(
+  const DiagnosticArray & input_diagnostics, const std::string & function,
+  const rclcpp::Time & stamp, DiagnosticQueue & diag_queue);
+
+}  // namespace autoware::evaluator_utils
+
+#endif  // AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_

evaluator/autoware_evaluator_utils/package.xml

@@ -0,0 +1,24 @@
+<?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_evaluator_utils</name>
+  <version>0.1.0</version>
+  <description>ROS 2 node for evaluating control</description>
+  <maintainer email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki MUROOKA</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</author>
+  <author email="takayuki.murooka@tier4.jp">Takayuki MUROOKA</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp

@@ -0,0 +1,66 @@
+// 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/evaluator_utils/evaluator_utils.hpp"
+
+#include <algorithm>
+
+namespace autoware::evaluator_utils
+{
+void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue)
+{
+  constexpr double KEEP_TIME = 1.0;
+  diag_queue.erase(
+    std::remove_if(
+      diag_queue.begin(), diag_queue.end(),
+      [stamp](const std::pair<DiagnosticStatus, rclcpp::Time> & p) {
+        return (stamp - p.second).seconds() > KEEP_TIME;
+      }),
+    diag_queue.end());
+}
+
+void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue)
+{
+  diag_queue.erase(
+    std::remove_if(
+      diag_queue.begin(), diag_queue.end(),
+      [&name](const std::pair<DiagnosticStatus, rclcpp::Time> & p) {
+        return p.first.name.find(name) != std::string::npos;
+      }),
+    diag_queue.end());
+}
+
+void addDiagnostic(
+  const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue)
+{
+  diag_queue.push_back(std::make_pair(diag, stamp));
+}
+
+void updateDiagnosticQueue(
+  const DiagnosticArray & input_diagnostics, const std::string & function,
+  const rclcpp::Time & stamp, DiagnosticQueue & diag_queue)
+{
+  const auto it = std::find_if(
+    input_diagnostics.status.begin(), input_diagnostics.status.end(),
+    [&function](const DiagnosticStatus & diag) {
+      return diag.name.find(function) != std::string::npos;
+    });
+  if (it != input_diagnostics.status.end()) {
+    removeDiagnosticsByName(it->name, diag_queue);
+    addDiagnostic(*it, input_diagnostics.header.stamp, diag_queue);
+  }
+
+  removeOldDiagnostics(stamp, diag_queue);
+}
+}  // namespace autoware::evaluator_utils

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp

@@ -32,13 +32,14 @@
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <diagnostic_msgs/msg/detail/diagnostic_status__struct.hpp>
 
 #include <array>
 #include <deque>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
-
 namespace planning_diagnostics
 {
 using autoware_perception_msgs::msg::PredictedObjects;
@@ -93,12 +94,22 @@ class PlanningEvaluatorNode : public rclcpp::Node
     const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg,
     const Odometry::ConstSharedPtr ego_state_ptr);
 
+  /**
+   * @brief obtain diagnostics information
+   */
+  void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
+
   /**
    * @brief publish the given metric statistic
    */
   DiagnosticStatus generateDiagnosticStatus(
     const Metric & metric, const Stat<double> & metric_stat) const;
 
+  /**
+   * @brief publish current ego lane info
+   */
+  DiagnosticStatus generateDiagnosticEvaluationStatus(const DiagnosticStatus & diag);
+
   /**
    * @brief publish current ego lane info
    */
@@ -127,6 +138,10 @@ class PlanningEvaluatorNode : public rclcpp::Node
    * @brief fetch topic data
    */
   void fetchData();
+  // 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 planning_diag_sub_;
 
   // ROS
   autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
@@ -164,6 +179,9 @@ class PlanningEvaluatorNode : public rclcpp::Node
   std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
 
   rclcpp::TimerBase::SharedPtr timer_;
+  // queue for diagnostics and time stamp
+  std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
+  const std::vector<std::string> target_functions_ = {"obstacle_cruise_planner"};
   std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
 };
 }  // namespace planning_diagnostics

evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml

@@ -7,6 +7,7 @@
   <arg name="input/modified_goal" default="/planning/scenario_planning/modified_goal"/>
   <arg name="input/map_topic" default="/map/vector_map"/>
   <arg name="input/route_topic" default="/planning/mission_planning/route"/>
+  <arg name="input/diagnostics" default="/diagnostics"/>
 
   <!-- planning evaluator -->
   <group>
@@ -15,6 +16,7 @@
       <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/diagnostics" to="$(var input/diagnostics)"/>
       <remap from="~/input/reference_trajectory" to="$(var input/reference_trajectory)"/>
       <remap from="~/input/objects" to="$(var input/objects)"/>
       <remap from="~/input/modified_goal" to="$(var input/modified_goal)"/>

evaluator/autoware_planning_evaluator/package.xml

@@ -13,6 +13,7 @@
   <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_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>