feat(control_evaluator): implement a control evaluator (#6959)

* add control evaluator module

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

* make the evaluator depend on messages from AEB

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

* update output msg

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

* delete extra new line

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

* update/fix details

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

* add a package mantainer

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

* Add a timer to maintain a constant rate of msg publishing

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

---------

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

evaluator/control_evaluator/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.14)
+project(control_evaluator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(pluginlib REQUIRED)
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/${PROJECT_NAME}_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "control_diagnostics::controlEvaluatorNode"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)

evaluator/control_evaluator/README.md

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

evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp

@@ -0,0 +1,61 @@
+// 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__CONTROL_EVALUATOR_NODE_HPP_
+#define CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+
+#include <array>
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace control_diagnostics
+{
+
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+
+/**
+ * @brief Node for control evaluation
+ */
+class controlEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options);
+
+  /**
+   * @brief publish the given metric statistic
+   */
+  DiagnosticStatus generateDiagnosticStatus(const bool is_emergency_brake) const;
+  void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
+  void onTimer();
+
+private:
+  rclcpp::Subscription<DiagnosticArray>::SharedPtr control_diag_sub_;
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
+
+  // Calculator
+  // Metrics
+  std::deque<rclcpp::Time> stamps_;
+  DiagnosticArray metrics_msg_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace control_diagnostics
+
+#endif  // CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_

evaluator/control_evaluator/launch/control_evaluator.launch.xml

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="input/diagnostics" default="/diagnostics"/>
+
+  <!-- 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="~/metrics" to="/diagnostic/control_evaluator/metrics"/>
+    </node>
+  </group>
+</launch>

evaluator/control_evaluator/package.xml

@@ -0,0 +1,29 @@
+<?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>control_evaluator</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>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</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>

evaluator/control_evaluator/param/control_evaluator.defaults.yaml

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

evaluator/control_evaluator/src/control_evaluator_node.cpp

@@ -0,0 +1,86 @@
+// 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/control_evaluator_node.hpp"
+
+#include <fstream>
+#include <iostream>
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace control_diagnostics
+{
+controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options)
+: Node("control_evaluator", node_options)
+{
+  using std::placeholders::_1;
+
+  control_diag_sub_ = create_subscription<DiagnosticArray>(
+    "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1));
+
+  // Publisher
+  metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
+
+  // Timer callback to publish evaluator diagnostics
+  using namespace std::literals::chrono_literals;
+  timer_ =
+    rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this));
+}
+
+DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus(const bool is_emergency_brake) const
+{
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = "autonomous_emergency_braking";
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "decision";
+  key_value.value = (is_emergency_brake) ? "stop" : "none";
+  status.values.push_back(key_value);
+  return status;
+}
+
+void controlEvaluatorNode::onTimer()
+{
+  if (!metrics_msg_.status.empty()) {
+    metrics_pub_->publish(metrics_msg_);
+    metrics_msg_.status.clear();
+  }
+}
+
+void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg)
+{
+  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;
+    });
+
+  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));
+
+  const auto runtime = (now() - start).seconds();
+  RCLCPP_DEBUG(get_logger(), "control evaluation calculation time: %2.2f ms", runtime * 1e3);
+}
+
+}  // namespace control_diagnostics
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode)

launch/tier4_control_launch/launch/control.launch.py

@@ -343,6 +343,22 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
+    # control evaluator
+    control_evaluator_component = ComposableNode(
+        package="control_evaluator",
+        plugin="control_diagnostics::controlEvaluatorNode",
+        name="control_evaluator",
+        remappings=[
+            ("~/input/diagnostics", "/diagnostics"),
+            ("~/output/metrics", "~/metrics"),
+        ],
+    )
+
+    control_evaluator_loader = LoadComposableNodes(
+        composable_node_descriptions=[control_evaluator_component],
+        target_container="/control/control_container",
+    )
+
     # control validator checker
     control_validator_component = ComposableNode(
         package="control_validator",
@@ -369,6 +385,7 @@ def launch_setup(context, *args, **kwargs):
             obstacle_collision_checker_loader,
             autonomous_emergency_braking_loader,
             predicted_path_checker_loader,
+            control_evaluator_loader,
         ]
     )
 

launch/tier4_control_launch/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <exec_depend>control_evaluator</exec_depend>
   <exec_depend>external_cmd_converter</exec_depend>
   <exec_depend>external_cmd_selector</exec_depend>
   <exec_depend>lane_departure_checker</exec_depend>