diff --git a/evaluator/diagnostic_converter/CMakeLists.txt b/evaluator/diagnostic_converter/CMakeLists.txt new file mode 100644 index 000000000000..3f47eb0386a1 --- /dev/null +++ b/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() diff --git a/evaluator/diagnostic_converter/README.md b/evaluator/diagnostic_converter/README.md new file mode 100644 index 000000000000..adc84c3967d2 --- /dev/null +++ b/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 diff --git a/evaluator/diagnostic_converter/include/converter_node.hpp b/evaluator/diagnostic_converter/include/converter_node.hpp new file mode 100644 index 000000000000..53c762dac0ff --- /dev/null +++ b/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 + +#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 + +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::SharedPtr getPublisher( + const std::string & topic, const size_t pub_idx); + +private: + // ROS + std::vector::SharedPtr> diagnostics_sub_; + std::vector::SharedPtr>> + params_pub_; +}; +} // namespace diagnostic_converter + +#endif // CONVERTER_NODE_HPP_ diff --git a/evaluator/diagnostic_converter/package.xml b/evaluator/diagnostic_converter/package.xml new file mode 100644 index 000000000000..c561cfcac6c6 --- /dev/null +++ b/evaluator/diagnostic_converter/package.xml @@ -0,0 +1,29 @@ + + + + diagnostic_converter + 0.5.6 + Node for converting diagnostic messages into ParameterDeclaration messages + Kyoichi Sugahara + Maxime CLEMENT + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + diagnostic_msgs + pluginlib + rclcpp + rclcpp_components + tier4_simulation_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp new file mode 100644 index 000000000000..798515b8667b --- /dev/null +++ b/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 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) { + 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::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 new file mode 100644 index 000000000000..47f42d018ea7 --- /dev/null +++ b/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 + +#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 + +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/planning/planning_evaluator/CMakeLists.txt b/evaluator/planning_evaluator/CMakeLists.txt similarity index 96% rename from planning/planning_evaluator/CMakeLists.txt rename to evaluator/planning_evaluator/CMakeLists.txt index 6865f8eef700..4b643b3c85e4 100644 --- a/planning/planning_evaluator/CMakeLists.txt +++ b/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 diff --git a/planning/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md similarity index 100% rename from planning/planning_evaluator/README.md rename to evaluator/planning_evaluator/README.md diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/parameters.hpp b/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/parameters.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/stat.hpp b/evaluator/planning_evaluator/include/planning_evaluator/stat.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/stat.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/stat.hpp diff --git a/planning/planning_evaluator/launch/motion_evaluator.launch.xml b/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml similarity index 100% rename from planning/planning_evaluator/launch/motion_evaluator.launch.xml rename to evaluator/planning_evaluator/launch/motion_evaluator.launch.xml diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml new file mode 100644 index 000000000000..af7c5369f742 --- /dev/null +++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/planning/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml similarity index 91% rename from planning/planning_evaluator/package.xml rename to evaluator/planning_evaluator/package.xml index b288d911b4c3..73186f6b20e8 100644 --- a/planning/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -5,6 +5,7 @@ 0.1.0 ROS2 node for evaluating planners Maxime CLEMENT + Kyoichi Sugahara Apache License 2.0 Maxime CLEMENT @@ -20,6 +21,7 @@ geometry_msgs motion_utils nav_msgs + pluginlib rclcpp rclcpp_components tf2 diff --git a/planning/planning_evaluator/param/planning_evaluator.defaults.yaml b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml similarity index 100% rename from planning/planning_evaluator/param/planning_evaluator.defaults.yaml rename to evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml diff --git a/planning/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/deviation_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp diff --git a/planning/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/metrics_utils.cpp rename to evaluator/planning_evaluator/src/metrics/metrics_utils.cpp diff --git a/planning/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/obstacle_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp diff --git a/planning/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/stability_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/stability_metrics.cpp diff --git a/planning/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/trajectory_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp diff --git a/planning/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics_calculator.cpp rename to evaluator/planning_evaluator/src/metrics_calculator.cpp diff --git a/planning/planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp similarity index 100% rename from planning/planning_evaluator/src/motion_evaluator_node.cpp rename to evaluator/planning_evaluator/src/motion_evaluator_node.cpp diff --git a/planning/planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp similarity index 98% rename from planning/planning_evaluator/src/planning_evaluator_node.cpp rename to evaluator/planning_evaluator/src/planning_evaluator_node.cpp index cca9bbf7c62e..663f0482cf77 100644 --- a/planning/planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp @@ -156,7 +156,7 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m } metrics_calculator_.setPreviousTrajectory(*traj_msg); auto runtime = (now() - start).seconds(); - RCLCPP_INFO(get_logger(), "Calculation time: %2.2f ms", runtime * 1e3); + RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3); } void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg) diff --git a/planning/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp similarity index 100% rename from planning/planning_evaluator/test/test_planning_evaluator_node.cpp rename to evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index e179d410e345..f2226b57673f 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -34,5 +34,10 @@ + + + + + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 8705ba30a935..7029de7c1095 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -5,6 +5,7 @@ + @@ -102,6 +103,13 @@ + + + + + + + diff --git a/planning/planning_evaluator/launch/planning_evaluator.launch.xml b/planning/planning_evaluator/launch/planning_evaluator.launch.xml deleted file mode 100644 index b3956359c1fd..000000000000 --- a/planning/planning_evaluator/launch/planning_evaluator.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - -