feat(diagnostic_converter): add converter to use planning_evaluator's…

… output for scenario's condition (autowarefoundation#2514)

* add original diagnostic_convertor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix typo

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* change include

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete comments

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* made launch for converter

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* ci(pre-commit): autofix

* ci(pre-commit): autofix

* add diagnostic convertor in launch

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* ci(pre-commit): autofix

* change debug from info

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* change arg name to launch diagnostic convertor

* add planning_evaluator launcher in simulator.launch.xml

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix arg wrong setting

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* use simulation msg in tier4_autoware_msgs

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* fix README

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* refactoring

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* remove unnecessary dependency

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* remove unnecessary dependency

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* move folder

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* reformat

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* Update evaluator/diagnostic_converter/include/converter_node.hpp

Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

* Update evaluator/diagnostic_converter/README.md

Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

* Update evaluator/diagnostic_converter/src/converter_node.cpp

Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

* Update evaluator/diagnostic_converter/test/test_converter_node.cpp

Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

* define diagnostic_topics as parameter

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix include way

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix include way

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete ament_cmake_clang_format from package.xml

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix test_depend

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Update evaluator/diagnostic_converter/test/test_converter_node.cpp

Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

* style(pre-commit): autofix

* Update launch/tier4_simulator_launch/launch/simulator.launch.xml

Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

evaluator/diagnostic_converter/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version
+
+project(diagnostic_converter)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(pluginlib REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/converter_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "diagnostic_converter::DiagnosticConverter"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_converter_node.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package()

evaluator/diagnostic_converter/README.md

@@ -0,0 +1,53 @@
+# Planning Evaluator
+
+## Purpose
+
+This package provides a node to convert `diagnostic_msgs::msg::DiagnosticArray` messages
+into `tier4_simulation_msgs::msg::UserDefinedValue` messages.
+
+## Inner-workings / Algorithms
+
+The node subscribes to all topics listed in the parameters and assumes they publish
+`DiagnosticArray` messages.
+Each time such message is received,
+it is converted into as many `UserDefinedValue` messages as the number of `KeyValue` objects.
+The format of the output topic is detailed in the _output_ section.
+
+## Inputs / Outputs
+
+### Inputs
+
+The node listens to `DiagnosticArray` messages on the topics specified in the parameters.
+
+### Outputs
+
+The node outputs `UserDefinedValue` messages that are converted from the received `DiagnosticArray`.
+
+The name of the output topics are generated from the corresponding input topic, the name of the diagnostic status, and the key of the diagnostic.
+For example, we might listen to topic `/diagnostic_topic` and receive a `DiagnosticArray` with 2 status:
+
+- Status with `name: "x"`.
+  - Key: `a`.
+  - Key: `b`.
+- Status with `name: "y"`.
+  - Key: `a`.
+  - Key: `c`.
+
+The resulting topics to publish the `UserDefinedValue` are as follows:
+
+- `/metrics_x_a`.
+- `/metrics_x_b`.
+- `/metrics_y_a`.
+- `/metrics_y_c`.
+
+## Parameters
+
+| Name                | Type             | Description                                                   |
+| :------------------ | :--------------- | :------------------------------------------------------------ |
+| `diagnostic_topics` | list of `string` | list of DiagnosticArray topics to convert to UserDefinedValue |
+
+## Assumptions / Known limits
+
+Values in the `KeyValue` objects of a `DiagnosticStatus` are assumed to be of type `double`.
+
+## Future extensions / Unimplemented parts

evaluator/diagnostic_converter/include/converter_node.hpp

@@ -0,0 +1,66 @@
+// Copyright 2023 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 CONVERTER_NODE_HPP_
+#define CONVERTER_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp"
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace diagnostic_converter
+{
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using diagnostic_msgs::msg::KeyValue;
+using tier4_simulation_msgs::msg::UserDefinedValue;
+using tier4_simulation_msgs::msg::UserDefinedValueType;
+
+/**
+ * @brief Node for converting from DiagnosticArray to UserDefinedValue
+ */
+class DiagnosticConverter : public rclcpp::Node
+{
+public:
+  explicit DiagnosticConverter(const rclcpp::NodeOptions & node_options);
+
+  /**
+   * @brief callback for DiagnosticArray msgs that publishes equivalent UserDefinedValue msgs
+   * @param [in] diag_msg received diagnostic message
+   */
+  void onDiagnostic(
+    const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx,
+    const std::string & topic);
+
+  UserDefinedValue createUserDefinedValue(const KeyValue & key_value) const;
+
+  rclcpp::Publisher<UserDefinedValue>::SharedPtr getPublisher(
+    const std::string & topic, const size_t pub_idx);
+
+private:
+  // ROS
+  std::vector<rclcpp::Subscription<DiagnosticArray>::SharedPtr> diagnostics_sub_;
+  std::vector<std::unordered_map<std::string, rclcpp::Publisher<UserDefinedValue>::SharedPtr>>
+    params_pub_;
+};
+}  // namespace diagnostic_converter
+
+#endif  // CONVERTER_NODE_HPP_

evaluator/diagnostic_converter/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>diagnostic_converter</name>
+  <version>0.5.6</version>
+  <description>Node for converting diagnostic messages into ParameterDeclaration messages</description>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_simulation_msgs</depend>
+
+  <test_depend>ament_cmake_gtest</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/diagnostic_converter/src/converter_node.cpp

@@ -0,0 +1,70 @@
+// Copyright 2023 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 "converter_node.hpp"
+
+namespace diagnostic_converter
+{
+DiagnosticConverter::DiagnosticConverter(const rclcpp::NodeOptions & node_options)
+: Node("diagnostic_converter", node_options)
+{
+  using std::placeholders::_1;
+
+  size_t sub_counter = 0;
+  std::vector<std::string> diagnostic_topics;
+  declare_parameter<std::vector<std::string>>("diagnostic_topics", std::vector<std::string>());
+  get_parameter<std::vector<std::string>>("diagnostic_topics", diagnostic_topics);
+  for (const std::string & diagnostic_topic : diagnostic_topics) {
+    // std::function required with multiple arguments https://answers.ros.org/question/289207
+    const std::function<void(const DiagnosticArray::ConstSharedPtr)> fn =
+      std::bind(&DiagnosticConverter::onDiagnostic, this, _1, sub_counter++, diagnostic_topic);
+    diagnostics_sub_.push_back(create_subscription<DiagnosticArray>(diagnostic_topic, 1, fn));
+  }
+  params_pub_.resize(diagnostics_sub_.size());
+}
+
+void DiagnosticConverter::onDiagnostic(
+  const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx,
+  const std::string & base_topic)
+{
+  for (const auto & status : diag_msg->status) {
+    std::string status_topic = base_topic + (status.name.empty() ? "" : "_" + status.name);
+    for (const auto & key_value : status.values) {
+      getPublisher(status_topic + "_" + key_value.key, diag_idx)
+        ->publish(createUserDefinedValue(key_value));
+    }
+  }
+}
+
+UserDefinedValue DiagnosticConverter::createUserDefinedValue(const KeyValue & key_value) const
+{
+  UserDefinedValue param_msg;
+  param_msg.type.data = UserDefinedValueType::DOUBLE;
+  param_msg.value = key_value.value;
+  return param_msg;
+}
+
+rclcpp::Publisher<UserDefinedValue>::SharedPtr DiagnosticConverter::getPublisher(
+  const std::string & topic, const size_t pub_idx)
+{
+  auto & pubs = params_pub_[pub_idx];
+  if (pubs.count(topic) == 0) {
+    pubs[topic] = create_publisher<UserDefinedValue>(topic, 1);
+  }
+  return pubs.at(topic);
+}
+}  // namespace diagnostic_converter
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_converter::DiagnosticConverter)

evaluator/diagnostic_converter/test/test_converter_node.cpp

@@ -0,0 +1,127 @@
+// Copyright 2023 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 "converter_node.hpp"
+#include "gtest/gtest.h"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value.hpp"
+#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp"
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+using ConverterNode = diagnostic_converter::DiagnosticConverter;
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using diagnostic_msgs::msg::KeyValue;
+using tier4_simulation_msgs::msg::UserDefinedValue;
+
+void waitForMsg(
+  bool & flag, const rclcpp::Node::SharedPtr node1, const rclcpp::Node::SharedPtr node2)
+{
+  while (!flag) {
+    rclcpp::spin_some(node1);
+    rclcpp::spin_some(node2);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+  }
+  flag = false;
+}
+
+std::function<void(UserDefinedValue::ConstSharedPtr)> generateCallback(
+  bool & flag, UserDefinedValue & msg)
+{
+  return [&](UserDefinedValue::ConstSharedPtr received_msg) {
+    flag = true;
+    msg = *received_msg;
+  };
+}
+
+TEST(ConverterNode, ConvertDiagnostics)
+{
+  const std::vector<std::string> input_topics = {"/test1/diag", "/test2/diag"};
+
+  rclcpp::init(0, nullptr);
+  rclcpp::Node::SharedPtr dummy_node = std::make_shared<rclcpp::Node>("converter_test_node");
+
+  rclcpp::NodeOptions options;
+  options.append_parameter_override("diagnostic_topics", rclcpp::ParameterValue(input_topics));
+  auto node = std::make_shared<ConverterNode>(options);
+
+  {  // Simple case with 1 resulting UserDefinedValue
+    bool msg_received = false;
+    UserDefinedValue param;
+    // DiagnosticArray publishers
+    const auto diag_pub = dummy_node->create_publisher<DiagnosticArray>(input_topics[0], 1);
+    // UserDefinedValue subscribers
+    const auto param_sub_a = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[0] + "_a", 1, generateCallback(msg_received, param));
+    DiagnosticArray diag;
+    DiagnosticStatus status;
+    status.name = "";
+    KeyValue key_value = KeyValue().set__key("a").set__value("1");
+    status.values.push_back(key_value);
+    diag.status.push_back(status);
+    diag_pub->publish(diag);
+    waitForMsg(msg_received, node, dummy_node);
+    EXPECT_EQ(param.value, "1");
+  }
+  {  // Case with multiple UserDefinedValue converted from one DiagnosticArray
+    bool msg_received_xa = false;
+    bool msg_received_xb = false;
+    bool msg_received_ya = false;
+    bool msg_received_yb = false;
+    UserDefinedValue param_xa;
+    UserDefinedValue param_xb;
+    UserDefinedValue param_ya;
+    UserDefinedValue param_yb;
+    // DiagnosticArray publishers
+    const auto diag_pub = dummy_node->create_publisher<DiagnosticArray>(input_topics[1], 1);
+    // UserDefinedValue subscribers
+    const auto param_sub_xa = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_x_a", 1, generateCallback(msg_received_xa, param_xa));
+    const auto param_sub_xb = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_x_b", 1, generateCallback(msg_received_xb, param_xb));
+    const auto param_sub_ya = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_y_a", 1, generateCallback(msg_received_ya, param_ya));
+    const auto param_sub_yb = dummy_node->create_subscription<UserDefinedValue>(
+      input_topics[1] + "_y_b", 1, generateCallback(msg_received_yb, param_yb));
+    DiagnosticArray diag;
+    DiagnosticStatus status_x;
+    status_x.name = "x";
+    status_x.values.push_back(KeyValue().set__key("a").set__value("1"));
+    status_x.values.push_back(KeyValue().set__key("b").set__value("10"));
+    diag.status.push_back(status_x);
+    DiagnosticStatus status_y;
+    status_y.name = "y";
+    status_y.values.push_back(KeyValue().set__key("a").set__value("9"));
+    status_y.values.push_back(KeyValue().set__key("b").set__value("6"));
+    diag.status.push_back(status_y);
+    diag_pub->publish(diag);
+    waitForMsg(msg_received_xa, node, dummy_node);
+    EXPECT_EQ(param_xa.value, "1");
+    waitForMsg(msg_received_xb, node, dummy_node);
+    EXPECT_EQ(param_xb.value, "10");
+    waitForMsg(msg_received_ya, node, dummy_node);
+    EXPECT_EQ(param_ya.value, "9");
+    waitForMsg(msg_received_yb, node, dummy_node);
+    EXPECT_EQ(param_yb.value, "6");
+  }
+
+  rclcpp::shutdown();
+}

planning/planning_evaluator/CMakeLists.txt → evaluator/planning_evaluator/CMakeLists.txt

@@ -4,6 +4,8 @@ project(planning_evaluator)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+find_package(pluginlib REQUIRED)
+
 ament_auto_add_library(${PROJECT_NAME}_node SHARED
   src/metrics_calculator.cpp
   src/${PROJECT_NAME}_node.cpp