Skip to content

Commit f2694f6

Browse files
satoshi-otasatoshi-ota
committed
chore(evaluator): move evaluators
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent ad35c13 commit f2694f6

File tree

78 files changed

+10337
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

78 files changed

+10337
-0
lines changed

‎evaluator/control_evaluator/CMakeLists.txt

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(control_evaluator)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(pluginlib REQUIRED)
8+
9+
ament_auto_add_library(${PROJECT_NAME}_node SHARED
10+
src/${PROJECT_NAME}_node.cpp
11+
)
12+
13+
rclcpp_components_register_node(${PROJECT_NAME}_node
14+
PLUGIN "control_diagnostics::controlEvaluatorNode"
15+
EXECUTABLE ${PROJECT_NAME}
16+
)
17+
18+
19+
ament_auto_package(
20+
INSTALL_TO_SHARE
21+
param
22+
launch
23+
)

‎evaluator/control_evaluator/README.md

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# Planning Evaluator
2+
3+
## Purpose
4+
5+
This package provides nodes that generate metrics to evaluate the quality of control.

‎evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp

+61
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
16+
#define CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
17+
18+
#include "rclcpp/rclcpp.hpp"
19+
20+
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
21+
22+
#include <array>
23+
#include <deque>
24+
#include <memory>
25+
#include <string>
26+
#include <vector>
27+
28+
namespace control_diagnostics
29+
{
30+
31+
using diagnostic_msgs::msg::DiagnosticArray;
32+
using diagnostic_msgs::msg::DiagnosticStatus;
33+
34+
/**
35+
* @brief Node for control evaluation
36+
*/
37+
class controlEvaluatorNode : public rclcpp::Node
38+
{
39+
public:
40+
explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options);
41+
42+
/**
43+
* @brief publish the given metric statistic
44+
*/
45+
DiagnosticStatus generateDiagnosticStatus(const bool is_emergency_brake) const;
46+
void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
47+
void onTimer();
48+
49+
private:
50+
rclcpp::Subscription<DiagnosticArray>::SharedPtr control_diag_sub_;
51+
rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
52+
53+
// Calculator
54+
// Metrics
55+
std::deque<rclcpp::Time> stamps_;
56+
DiagnosticArray metrics_msg_;
57+
rclcpp::TimerBase::SharedPtr timer_;
58+
};
59+
} // namespace control_diagnostics
60+
61+
#endif // CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_

‎evaluator/control_evaluator/launch/control_evaluator.launch.xml

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<launch>
2+
<arg name="input/diagnostics" default="/diagnostics"/>
3+
4+
<!-- control evaluator -->
5+
<group>
6+
<node name="control_evaluator" exec="control_evaluator" pkg="control_evaluator">
7+
<param from="$(find-pkg-share control_evaluator)/param/control_evaluator.defaults.yaml"/>
8+
<remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
9+
<remap from="~/metrics" to="/diagnostic/control_evaluator/metrics"/>
10+
</node>
11+
</group>
12+
</launch>

‎evaluator/control_evaluator/package.xml

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>control_evaluator</name>
5+
<version>0.1.0</version>
6+
<description>ROS 2 node for evaluating control</description>
7+
<maintainer email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</maintainer>
8+
<maintainer email="takayuki.murooka@tier4.jp">takayuki MUROOKA</maintainer>
9+
<license>Apache License 2.0</license>
10+
11+
<author email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</author>
12+
<author email="takayuki.murooka@tier4.jp">takayuki MUROOKA</author>
13+
14+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
15+
<buildtool_depend>autoware_cmake</buildtool_depend>
16+
17+
<depend>diagnostic_msgs</depend>
18+
<depend>pluginlib</depend>
19+
<depend>rclcpp</depend>
20+
<depend>rclcpp_components</depend>
21+
22+
<test_depend>ament_cmake_ros</test_depend>
23+
<test_depend>ament_lint_auto</test_depend>
24+
<test_depend>autoware_lint_common</test_depend>
25+
26+
<export>
27+
<build_type>ament_cmake</build_type>
28+
</export>
29+
</package>

‎evaluator/control_evaluator/param/control_evaluator.defaults.yaml

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
/**:
2+
ros__parameters:

‎evaluator/control_evaluator/src/control_evaluator_node.cpp

+86
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "control_evaluator/control_evaluator_node.hpp"
16+
17+
#include <fstream>
18+
#include <iostream>
19+
#include <map>
20+
#include <memory>
21+
#include <string>
22+
#include <utility>
23+
#include <vector>
24+
25+
namespace control_diagnostics
26+
{
27+
controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options)
28+
: Node("control_evaluator", node_options)
29+
{
30+
using std::placeholders::_1;
31+
32+
control_diag_sub_ = create_subscription<DiagnosticArray>(
33+
"~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1));
34+
35+
// Publisher
36+
metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
37+
38+
// Timer callback to publish evaluator diagnostics
39+
using namespace std::literals::chrono_literals;
40+
timer_ =
41+
rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this));
42+
}
43+
44+
DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus(const bool is_emergency_brake) const
45+
{
46+
DiagnosticStatus status;
47+
status.level = status.OK;
48+
status.name = "autonomous_emergency_braking";
49+
diagnostic_msgs::msg::KeyValue key_value;
50+
key_value.key = "decision";
51+
key_value.value = (is_emergency_brake) ? "stop" : "none";
52+
status.values.push_back(key_value);
53+
return status;
54+
}
55+
56+
void controlEvaluatorNode::onTimer()
57+
{
58+
if (!metrics_msg_.status.empty()) {
59+
metrics_pub_->publish(metrics_msg_);
60+
metrics_msg_.status.clear();
61+
}
62+
}
63+
64+
void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg)
65+
{
66+
const auto start = now();
67+
const auto aeb_status =
68+
std::find_if(diag_msg->status.begin(), diag_msg->status.end(), [](const auto & status) {
69+
const bool aeb_found = status.name.find("autonomous_emergency_braking") != std::string::npos;
70+
return aeb_found;
71+
});
72+
73+
if (aeb_status == diag_msg->status.end()) return;
74+
75+
const bool is_emergency_brake = (aeb_status->level == DiagnosticStatus::ERROR);
76+
metrics_msg_.header.stamp = now();
77+
metrics_msg_.status.emplace_back(generateDiagnosticStatus(is_emergency_brake));
78+
79+
const auto runtime = (now() - start).seconds();
80+
RCLCPP_DEBUG(get_logger(), "control evaluation calculation time: %2.2f ms", runtime * 1e3);
81+
}
82+
83+
} // namespace control_diagnostics
84+
85+
#include "rclcpp_components/register_node_macro.hpp"
86+
RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode)

‎evaluator/diagnostic_converter/CMakeLists.txt

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version
2+
3+
project(diagnostic_converter)
4+
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
find_package(ament_cmake_auto REQUIRED)
14+
find_package(pluginlib REQUIRED)
15+
16+
ament_auto_find_build_dependencies()
17+
18+
19+
ament_auto_add_library(${PROJECT_NAME}_node SHARED
20+
src/converter_node.cpp
21+
)
22+
23+
rclcpp_components_register_node(${PROJECT_NAME}_node
24+
PLUGIN "diagnostic_converter::DiagnosticConverter"
25+
EXECUTABLE ${PROJECT_NAME}
26+
)
27+
28+
if(BUILD_TESTING)
29+
find_package(ament_lint_auto REQUIRED)
30+
ament_lint_auto_find_test_dependencies()
31+
32+
ament_add_gtest(test_${PROJECT_NAME}
33+
test/test_converter_node.cpp
34+
)
35+
target_link_libraries(test_${PROJECT_NAME}
36+
${PROJECT_NAME}_node
37+
)
38+
endif()
39+
40+
ament_auto_package()

‎evaluator/diagnostic_converter/README.md

+53
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
# Planning Evaluator
2+
3+
## Purpose
4+
5+
This package provides a node to convert `diagnostic_msgs::msg::DiagnosticArray` messages
6+
into `tier4_simulation_msgs::msg::UserDefinedValue` messages.
7+
8+
## Inner-workings / Algorithms
9+
10+
The node subscribes to all topics listed in the parameters and assumes they publish
11+
`DiagnosticArray` messages.
12+
Each time such message is received,
13+
it is converted into as many `UserDefinedValue` messages as the number of `KeyValue` objects.
14+
The format of the output topic is detailed in the _output_ section.
15+
16+
## Inputs / Outputs
17+
18+
### Inputs
19+
20+
The node listens to `DiagnosticArray` messages on the topics specified in the parameters.
21+
22+
### Outputs
23+
24+
The node outputs `UserDefinedValue` messages that are converted from the received `DiagnosticArray`.
25+
26+
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.
27+
For example, we might listen to topic `/diagnostic_topic` and receive a `DiagnosticArray` with 2 status:
28+
29+
- Status with `name: "x"`.
30+
- Key: `a`.
31+
- Key: `b`.
32+
- Status with `name: "y"`.
33+
- Key: `a`.
34+
- Key: `c`.
35+
36+
The resulting topics to publish the `UserDefinedValue` are as follows:
37+
38+
- `/metrics_x_a`.
39+
- `/metrics_x_b`.
40+
- `/metrics_y_a`.
41+
- `/metrics_y_c`.
42+
43+
## Parameters
44+
45+
| Name | Type | Description |
46+
| :------------------ | :--------------- | :------------------------------------------------------------ |
47+
| `diagnostic_topics` | list of `string` | list of DiagnosticArray topics to convert to UserDefinedValue |
48+
49+
## Assumptions / Known limits
50+
51+
Values in the `KeyValue` objects of a `DiagnosticStatus` are assumed to be of type `double`.
52+
53+
## Future extensions / Unimplemented parts

‎evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
// Copyright 2023 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_
16+
#define DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
21+
#include "tier4_simulation_msgs/msg/user_defined_value.hpp"
22+
#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp"
23+
24+
#include <memory>
25+
#include <string>
26+
#include <unordered_map>
27+
#include <vector>
28+
29+
namespace diagnostic_converter
30+
{
31+
using diagnostic_msgs::msg::DiagnosticArray;
32+
using diagnostic_msgs::msg::DiagnosticStatus;
33+
using diagnostic_msgs::msg::KeyValue;
34+
using tier4_simulation_msgs::msg::UserDefinedValue;
35+
using tier4_simulation_msgs::msg::UserDefinedValueType;
36+
37+
/**
38+
* @brief Node for converting from DiagnosticArray to UserDefinedValue
39+
*/
40+
class DiagnosticConverter : public rclcpp::Node
41+
{
42+
public:
43+
explicit DiagnosticConverter(const rclcpp::NodeOptions & node_options);
44+
45+
/**
46+
* @brief callback for DiagnosticArray msgs that publishes equivalent UserDefinedValue msgs
47+
* @param [in] diag_msg received diagnostic message
48+
*/
49+
void onDiagnostic(
50+
const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx,
51+
const std::string & topic);
52+
53+
UserDefinedValue createUserDefinedValue(const KeyValue & key_value) const;
54+
55+
rclcpp::Publisher<UserDefinedValue>::SharedPtr getPublisher(
56+
const std::string & topic, const size_t pub_idx);
57+
58+
private:
59+
// ROS
60+
std::vector<rclcpp::Subscription<DiagnosticArray>::SharedPtr> diagnostics_sub_;
61+
std::vector<std::unordered_map<std::string, rclcpp::Publisher<UserDefinedValue>::SharedPtr>>
62+
params_pub_;
63+
};
64+
} // namespace diagnostic_converter
65+
66+
#endif // DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_

0 commit comments

Comments
 (0)