From 816fc7507f7f4d00d69481154f2e2205953f2e46 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Fri, 25 Oct 2024 17:26:26 +0900 Subject: [PATCH] rewrite diagnostic_convertor to scenario_simulator_v2_adapter, supporting metric_msgs. --- .github/CODEOWNERS | 2 +- evaluator/diagnostic_converter/README.md | 53 -------- .../src/converter_node.cpp | 101 -------------- .../test/test_converter_node.cpp | 128 ------------------ .../CMakeLists.txt | 4 +- .../scenario_simulator_v2_adapter/README.md | 42 ++++++ .../scenario_simulator_v2_adapter.param.yaml | 7 + .../converter_node.hpp | 38 +++--- .../package.xml | 8 +- .../scenario_simulator_v2_adapter.schema.json | 45 ++++++ .../src/converter_node.cpp | 85 ++++++++++++ .../test/test_converter_node.cpp | 112 +++++++++++++++ .../launch/simulator.launch.xml | 10 +- 13 files changed, 322 insertions(+), 313 deletions(-) delete mode 100644 evaluator/diagnostic_converter/README.md delete mode 100644 evaluator/diagnostic_converter/src/converter_node.cpp delete mode 100644 evaluator/diagnostic_converter/test/test_converter_node.cpp rename evaluator/{diagnostic_converter => scenario_simulator_v2_adapter}/CMakeLists.txt (89%) create mode 100644 evaluator/scenario_simulator_v2_adapter/README.md create mode 100644 evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml rename evaluator/{diagnostic_converter/include/diagnostic_converter => scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter}/converter_node.hpp (53%) rename evaluator/{diagnostic_converter => scenario_simulator_v2_adapter}/package.xml (78%) create mode 100644 evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json create mode 100644 evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp create mode 100644 evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 768f59e832c05..6df21aaa9ebfa 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -62,7 +62,7 @@ control/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp temkei.kem@tier4.jp evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp +evaluator/scenario_simulator_v2_adapter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp diff --git a/evaluator/diagnostic_converter/README.md b/evaluator/diagnostic_converter/README.md deleted file mode 100644 index adc84c3967d25..0000000000000 --- a/evaluator/diagnostic_converter/README.md +++ /dev/null @@ -1,53 +0,0 @@ -# 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 diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp deleted file mode 100644 index 2a2574732694c..0000000000000 --- a/evaluator/diagnostic_converter/src/converter_node.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// 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 "diagnostic_converter/converter_node.hpp" - -#include - -namespace -{ -std::string removeInvalidTopicString(const std::string & input_string) -{ - std::regex pattern{R"([a-zA-Z0-9/_]+)"}; - - std::string result; - for (std::sregex_iterator itr(std::begin(input_string), std::end(input_string), pattern), end; - itr != end; ++itr) { - result += itr->str(); - } - return result; -} - -std::string removeUnitString(const std::string & input_string) -{ - for (size_t i = 0; i < input_string.size(); ++i) { - if (input_string.at(i) == '[') { - if (i != 0 && input_string.at(i - 1) == ' ') { - // Blank is also removed - return std::string{input_string.begin(), input_string.begin() + i - 1}; - } - return std::string{input_string.begin(), input_string.begin() + i}; - } - } - return input_string; -} -} // namespace - -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 diagnostic_topics; - declare_parameter>("diagnostic_topics", std::vector()); - get_parameter>("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 fn = - std::bind(&DiagnosticConverter::onDiagnostic, this, _1, sub_counter++, diagnostic_topic); - diagnostics_sub_.push_back(create_subscription(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) { - const auto valid_topic_name = removeInvalidTopicString(status_topic + "_" + key_value.key); - getPublisher(valid_topic_name, 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 = removeUnitString(key_value.value); - return param_msg; -} - -rclcpp::Publisher::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(topic, 1); - } - return pubs.at(topic); -} -} // namespace diagnostic_converter - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_converter::DiagnosticConverter) diff --git a/evaluator/diagnostic_converter/test/test_converter_node.cpp b/evaluator/diagnostic_converter/test/test_converter_node.cpp deleted file mode 100644 index 167421f0777df..0000000000000 --- a/evaluator/diagnostic_converter/test/test_converter_node.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// 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 "diagnostic_converter/converter_node.hpp" - -#include - -#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 - -#include -#include -#include -#include - -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 generateCallback( - bool & flag, UserDefinedValue & msg) -{ - return [&](UserDefinedValue::ConstSharedPtr received_msg) { - flag = true; - msg = *received_msg; - }; -} - -TEST(ConverterNode, ConvertDiagnostics) -{ - const std::vector input_topics = {"/test1/diag", "/test2/diag"}; - - rclcpp::init(0, nullptr); - rclcpp::Node::SharedPtr dummy_node = std::make_shared("converter_test_node"); - - rclcpp::NodeOptions options; - options.append_parameter_override("diagnostic_topics", rclcpp::ParameterValue(input_topics)); - auto node = std::make_shared(options); - - { // Simple case with 1 resulting UserDefinedValue - bool msg_received = false; - UserDefinedValue param; - // DiagnosticArray publishers - const auto diag_pub = dummy_node->create_publisher(input_topics[0], 1); - // UserDefinedValue subscribers - const auto param_sub_a = dummy_node->create_subscription( - 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(input_topics[1], 1); - // UserDefinedValue subscribers - const auto param_sub_xa = dummy_node->create_subscription( - input_topics[1] + "_x_a", 1, generateCallback(msg_received_xa, param_xa)); - const auto param_sub_xb = dummy_node->create_subscription( - input_topics[1] + "_x_b", 1, generateCallback(msg_received_xb, param_xb)); - const auto param_sub_ya = dummy_node->create_subscription( - input_topics[1] + "_y_a", 1, generateCallback(msg_received_ya, param_ya)); - const auto param_sub_yb = dummy_node->create_subscription( - 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(); -} diff --git a/evaluator/diagnostic_converter/CMakeLists.txt b/evaluator/scenario_simulator_v2_adapter/CMakeLists.txt similarity index 89% rename from evaluator/diagnostic_converter/CMakeLists.txt rename to evaluator/scenario_simulator_v2_adapter/CMakeLists.txt index 3f47eb0386a1c..576bd9682725a 100644 --- a/evaluator/diagnostic_converter/CMakeLists.txt +++ b/evaluator/scenario_simulator_v2_adapter/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version -project(diagnostic_converter) +project(scenario_simulator_v2_adapter) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -21,7 +21,7 @@ ament_auto_add_library(${PROJECT_NAME}_node SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "diagnostic_converter::DiagnosticConverter" + PLUGIN "scenario_simulator_v2_adapter::MetricConverter" EXECUTABLE ${PROJECT_NAME} ) diff --git a/evaluator/scenario_simulator_v2_adapter/README.md b/evaluator/scenario_simulator_v2_adapter/README.md new file mode 100644 index 0000000000000..640680d97cf44 --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/README.md @@ -0,0 +1,42 @@ +# scenario_simulator_v2 Adapter + +## Purpose + +This package provides a node to convert various messages from the Autoware into `tier4_simulation_msgs::msg::UserDefinedValue` messages for the scenario_simulator_v2. +Currently, this node supports conversion of: +- `tier4_metric_msgs::msg::MetricArray` for metric topics + +## Inner-workings / Algorithms + +- For `tier4_metric_msgs::msg::MetricArray`, +The node subscribes to all topics listed in the parameter `metric_topic_list`. +Each time such message is received, it is converted into as many `UserDefinedValue` messages as the number of `Metric` objects. +The format of the output topic is detailed in the _output_ section. + +## Inputs / Outputs + +### Inputs + +The node listens to `MetricArray` messages on the topics specified in `metric_topic_list`. + +### Outputs + +The node outputs `UserDefinedValue` messages that are converted from the received messages. + +- For `MetricArray` messages, +The name of the output topics are generated from the corresponding input topic, the name of the metric. + - For example, we might listen to topic `/planning/planning_evaluator/metrics` and receive a `MetricArray` with 2 metrics: + - metric with `name: "metricA/x"` + - metric with `name: "metricA/y"` + - The resulting topics to publish the `UserDefinedValue` are as follows: + - `/planning/planning_evaluator/metrics/metricA/x` + - `/planning/planning_evaluator/metrics/metricA/y` + +## Parameters +{{ json_to_markdown("src/autoware/universe/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json") }} + +## Assumptions / Known limitsmetrics_x + +Values in the `Metric` objects of a `MetricArray` are assumed to be of type `double`. + +## Future extensions / Unimplemented parts diff --git a/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml b/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml new file mode 100644 index 0000000000000..20a745e353de2 --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + update_rate: 10.0 + metric_topic_list: + - /planning/planning_evaluator/metrics + - /control/control_evaluator/metrics + - /system/processing_time_checker/metrics \ No newline at end of file diff --git a/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp b/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp similarity index 53% rename from evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp rename to evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp index 59bb02ebf301f..040b75850b798 100644 --- a/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp +++ b/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ -#define DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ +#ifndef SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ +#define SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ #include -#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include +#include #include "tier4_simulation_msgs/msg/user_defined_value.hpp" #include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" @@ -26,41 +27,40 @@ #include #include -namespace diagnostic_converter +namespace scenario_simulator_v2_adapter { -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; -using diagnostic_msgs::msg::KeyValue; +using tier4_metric_msgs::msg::Metric; +using tier4_metric_msgs::msg::MetricArray; using tier4_simulation_msgs::msg::UserDefinedValue; using tier4_simulation_msgs::msg::UserDefinedValueType; /** - * @brief Node for converting from DiagnosticArray to UserDefinedValue + * @brief Node for converting Autoware's messages to UserDefinedValue */ -class DiagnosticConverter : public rclcpp::Node +class MetricConverter : public rclcpp::Node { public: - explicit DiagnosticConverter(const rclcpp::NodeOptions & node_options); + explicit MetricConverter(const rclcpp::NodeOptions & node_options); /** - * @brief callback for DiagnosticArray msgs that publishes equivalent UserDefinedValue msgs - * @param [in] diag_msg received diagnostic message + * @brief callback for MetricArray msgs that publishes equivalent UserDefinedValue msgs + * @param [in] metrics_msg received metrics message */ - void onDiagnostic( - const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx, + void onMetrics( + const MetricArray::ConstSharedPtr metrics_msg, const size_t topic_idx, const std::string & topic); - UserDefinedValue createUserDefinedValue(const KeyValue & key_value) const; + UserDefinedValue createUserDefinedValue(const Metric & metric) const; rclcpp::Publisher::SharedPtr getPublisher( - const std::string & topic, const size_t pub_idx); + const std::string & topic, const size_t topic_idx); private: // ROS - std::vector::SharedPtr> diagnostics_sub_; + std::vector::SharedPtr> metrics_sub_; std::vector::SharedPtr>> params_pub_; }; -} // namespace diagnostic_converter +} // namespace scenario_simulator_v2_adapter -#endif // DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ +#endif // SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ diff --git a/evaluator/diagnostic_converter/package.xml b/evaluator/scenario_simulator_v2_adapter/package.xml similarity index 78% rename from evaluator/diagnostic_converter/package.xml rename to evaluator/scenario_simulator_v2_adapter/package.xml index be9266f39b6e0..4557e1e144bef 100644 --- a/evaluator/diagnostic_converter/package.xml +++ b/evaluator/scenario_simulator_v2_adapter/package.xml @@ -1,18 +1,18 @@ - diagnostic_converter + scenario_simulator_v2_adapter 0.5.6 - Node for converting diagnostic messages into ParameterDeclaration messages + Node for converting autoware's messages into UserDefinedValue messages Kyoichi Sugahara Maxime CLEMENT Takamasa Horibe + Takamasa Horibe Apache License 2.0 ament_cmake_auto autoware_cmake - - diagnostic_msgs + tier4_metric_msgs pluginlib rclcpp rclcpp_components diff --git a/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json b/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json new file mode 100644 index 0000000000000..274faa280937b --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json @@ -0,0 +1,45 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for scenario_simulator_v2 Adapter", + "type": "object", + "definitions": { + "scenario_simulator_v2_adapter": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + }, + "processing_time_topic_list": { + "type": "array", + "items": { + "type": "string" + }, + "description": "The topic name list of the processing time." + }, + "metric_topic_list": { + "type": "array", + "items": { + "type": "string" + }, + "description": "The topic name list of the metrics." + } + }, + "required": ["update_rate", "processing_time_topic_list", "metric_topic_list"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/scenario_simulator_v2_adapter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} \ No newline at end of file diff --git a/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp b/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp new file mode 100644 index 0000000000000..98d36327793fe --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp @@ -0,0 +1,85 @@ +// 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 "scenario_simulator_v2_adapter/converter_node.hpp" + +#include + +namespace +{ +std::string removeInvalidTopicString(const std::string & input_string) +{ + std::regex pattern{R"([a-zA-Z0-9/_]+)"}; + + std::string result; + for (std::sregex_iterator itr(std::begin(input_string), std::end(input_string), pattern), end; + itr != end; ++itr) { + result += itr->str(); + } + return result; +} +} // namespace + +namespace scenario_simulator_v2_adapter +{ +MetricConverter::MetricConverter(const rclcpp::NodeOptions & node_options) +: Node("scenario_simulator_v2_adapter", node_options) +{ + using std::placeholders::_1; + + size_t sub_counter = 0; + std::vector metric_topic_list; + declare_parameter>("metric_topic_list", std::vector()); + get_parameter>("metric_topic_list", metric_topic_list); + for (const std::string & metric_topic : metric_topic_list) { + // std::function required with multiple arguments https://answers.ros.org/question/289207 + const std::function fn = + std::bind(&MetricConverter::onMetrics, this, _1, sub_counter++, metric_topic); + metrics_sub_.push_back(create_subscription(metric_topic, 1, fn)); + } + params_pub_.resize(metrics_sub_.size()); +} + +void MetricConverter::onMetrics( + const MetricArray::ConstSharedPtr metrics_msg, const size_t topic_idx, + const std::string & base_topic_name) +{ + for (const auto & metric : metrics_msg->metric_array) { + std::string metric_name = base_topic_name + (metric.name.empty() ? "" : "/" + metric.name); + const auto valid_topic_name = removeInvalidTopicString(metric_name); + getPublisher(valid_topic_name, topic_idx)->publish(createUserDefinedValue(metric)); + } +} + +UserDefinedValue MetricConverter::createUserDefinedValue(const Metric & metric) const +{ + UserDefinedValue param_msg; + param_msg.type.data = UserDefinedValueType::DOUBLE; + param_msg.value = metric.value; + return param_msg; +} + +rclcpp::Publisher::SharedPtr MetricConverter::getPublisher( + const std::string & topic_name, const size_t topic_idx) +{ + auto & pubs = params_pub_[topic_idx]; + if (pubs.count(topic_name) == 0) { + pubs[topic_name] = create_publisher(topic_name, 1); + } + return pubs.at(topic_name); +} +} // namespace scenario_simulator_v2_adapter + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(scenario_simulator_v2_adapter::MetricConverter) diff --git a/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp b/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp new file mode 100644 index 0000000000000..3acdee3bd4e5e --- /dev/null +++ b/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp @@ -0,0 +1,112 @@ +// 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 "scenario_simulator_v2_adapter/converter_node.hpp" + +#include + +#include +#include +#include "tier4_simulation_msgs/msg/user_defined_value.hpp" +#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" + +#include + +#include +#include +#include +#include + +using ConverterNode = scenario_simulator_v2_adapter::MetricConverter; +using tier4_metric_msgs::msg::Metric; +using tier4_metric_msgs::msg::MetricArray; +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 generateCallback( + bool & flag, UserDefinedValue & msg) +{ + return [&](UserDefinedValue::ConstSharedPtr received_msg) { + flag = true; + msg = *received_msg; + }; +} + +TEST(ConverterNode, ConvertMetrics) +{ + const std::vector input_topics = {"/test1/metrics", "/test2/metrics"}; + + rclcpp::init(0, nullptr); + rclcpp::Node::SharedPtr dummy_node = std::make_shared("converter_test_node"); + + rclcpp::NodeOptions options; + options.append_parameter_override("metric_topic_list", rclcpp::ParameterValue(input_topics)); + auto node = std::make_shared(options); + + { // Simple case with 1 resulting UserDefinedValue + bool msg_received = false; + UserDefinedValue param; + // MetricArray publishers + const auto metric_pub = dummy_node->create_publisher(input_topics[0], 1); + // UserDefinedValue subscribers + const auto param_sub_a = dummy_node->create_subscription( + input_topics[0] + "/a", 1, generateCallback(msg_received, param)); + MetricArray metrics; + Metric metric; + metric.name = "a"; + metric.value = "1"; + metrics.metric_array.push_back(metric); + metric_pub->publish(metrics); + waitForMsg(msg_received, node, dummy_node); + EXPECT_EQ(param.value, "1"); + } + { // Case with multiple UserDefinedValue converted from one MetricArray + bool msg_received_x = false; + bool msg_received_y = false; + UserDefinedValue param_x; + UserDefinedValue param_y; + // MetricArray publishers + const auto metric_pub = dummy_node->create_publisher(input_topics[1], 1); + // UserDefinedValue subscribers + const auto param_sub_x = dummy_node->create_subscription( + input_topics[1] + "/x", 1, generateCallback(msg_received_x, param_x)); + const auto param_sub_y = dummy_node->create_subscription( + input_topics[1] + "/y", 1, generateCallback(msg_received_y, param_y)); + MetricArray metrics; + Metric metric; + metric.name = "x"; + metric.value = "2"; + metrics.metric_array.push_back(metric); + metric.name = "y"; + metric.value = "3.33333"; + metrics.metric_array.push_back(metric); + metric_pub->publish(metrics); + waitForMsg(msg_received_x, node, dummy_node); + EXPECT_EQ(param_x.value, "2"); + waitForMsg(msg_received_y, node, dummy_node); + EXPECT_EQ(param_y.value, "3.33333"); + } + + rclcpp::shutdown(); +} diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 7be992e2e4764..e1226ee63f5e2 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -20,7 +20,7 @@ description="Select localization mode. Options are 'none', 'api' or 'pose_twist_estimator'. 'pose_twist_estimator' starts most of the localization modules except for the ndt_scan_matcher. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process." /> - + @@ -203,10 +203,10 @@ - - - - + + + +