Skip to content

Commit dd917f3

Browse files
Merge branch 'main' into fix/diag_level_of_ndt_scan_matcher
2 parents 8161670 + 98cc9f2 commit dd917f3

25 files changed

+160
-168
lines changed

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp

+5
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <iostream>
2121
#include <string>
2222
#include <unordered_map>
23+
#include <variant>
2324
#include <vector>
2425

2526
namespace perception_diagnostics
@@ -36,7 +37,11 @@ enum class Metric {
3637
SIZE,
3738
};
3839

40+
// Each metric has a different return type. (statistic or just a one value etc).
41+
// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant
3942
using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
43+
using MetricValueMap = std::unordered_map<std::string, double>;
44+
using MetricsMap = std::variant<MetricStatMap, MetricValueMap>;
4045

4146
struct PredictedPathDeviationMetrics
4247
{

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,7 @@ class MetricsCalculator
9696
* @param [in] metric Metric enum value
9797
* @return map of string describing the requested metric and the calculated value
9898
*/
99-
std::optional<MetricStatMap> calculate(const Metric & metric) const;
99+
std::optional<MetricsMap> calculate(const Metric & metric) const;
100100

101101
/**
102102
* @brief set the dynamic objects used to calculate obstacle metrics
@@ -143,7 +143,7 @@ class MetricsCalculator
143143
PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics(
144144
const PredictedObjects & objects, const double time_horizon) const;
145145
MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const;
146-
MetricStatMap calcObjectsCountMetrics() const;
146+
MetricValueMap calcObjectsCountMetrics() const;
147147

148148
bool hasPassedTime(const rclcpp::Time stamp) const;
149149
bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;

evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
6161

6262
DiagnosticStatus generateDiagnosticStatus(
6363
const std::string metric, const Stat<double> & metric_stat) const;
64+
DiagnosticStatus generateDiagnosticStatus(
65+
const std::string metric, const double metric_value) const;
6466

6567
private:
6668
// Subscribers and publishers

evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
prediction_time_horizons: [1.0, 2.0, 3.0, 5.0]
1414

1515
stopped_velocity_threshold: 1.0
16-
detection_radius_list: [50.0, 100.0, 150.0, 200.0]
16+
detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0]
1717
detection_height_list: [10.0]
1818
detection_count_purge_seconds: 36000.0
1919
objects_count_window_seconds: 1.0

evaluator/perception_online_evaluator/src/metrics_calculator.cpp

+7-9
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ namespace perception_diagnostics
2626
using object_recognition_utils::convertLabelToString;
2727
using tier4_autoware_utils::inverseTransformPoint;
2828

29-
std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric) const
29+
std::optional<MetricsMap> MetricsCalculator::calculate(const Metric & metric) const
3030
{
3131
// clang-format off
3232
const bool use_past_objects = metric == Metric::lateral_deviation ||
@@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas
455455
return metric_stat_map;
456456
}
457457

458-
MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const
458+
MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const
459459
{
460-
MetricStatMap metric_stat_map;
460+
MetricValueMap metric_stat_map;
461461
// calculate the average number of objects in the detection area in all past frames
462462
const auto overall_average_count = detection_counter_.getOverallAverageCount();
463463
for (const auto & [label, range_and_count] : overall_average_count) {
464464
for (const auto & [range, count] : range_and_count) {
465-
metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add(
466-
count);
465+
metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
467466
}
468467
}
469468
// calculate the average number of objects in the detection area in the past
@@ -472,17 +471,16 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const
472471
detection_counter_.getAverageCount(parameters_->objects_count_window_seconds);
473472
for (const auto & [label, range_and_count] : average_count) {
474473
for (const auto & [range, count] : range_and_count) {
475-
metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range]
476-
.add(count);
474+
metric_stat_map
475+
["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
477476
}
478477
}
479478

480479
// calculate the total number of objects in the detection area
481480
const auto total_count = detection_counter_.getTotalCount();
482481
for (const auto & [label, range_and_count] : total_count) {
483482
for (const auto & [range, count] : range_and_count) {
484-
metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add(
485-
count);
483+
metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count;
486484
}
487485
}
488486

evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp

+32-7
Original file line numberDiff line numberDiff line change
@@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics()
6969

7070
// calculate metrics
7171
for (const Metric & metric : parameters_->metrics) {
72-
const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric));
73-
if (!metric_stat_map.has_value()) {
72+
const auto metric_result = metrics_calculator_.calculate(Metric(metric));
73+
if (!metric_result.has_value()) {
7474
continue;
7575
}
7676

77-
for (const auto & [metric, stat] : metric_stat_map.value()) {
78-
if (stat.count() > 0) {
79-
metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat));
80-
}
81-
}
77+
std::visit(
78+
[&metrics_msg, this](auto && arg) {
79+
using T = std::decay_t<decltype(arg)>;
80+
for (const auto & [metric, value] : arg) {
81+
if constexpr (std::is_same_v<T, MetricStatMap>) {
82+
if (value.count() > 0) {
83+
metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
84+
}
85+
} else if constexpr (std::is_same_v<T, MetricValueMap>) {
86+
metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
87+
}
88+
}
89+
},
90+
metric_result.value());
8291
}
8392

8493
// publish metrics
@@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
111120
return status;
112121
}
113122

123+
DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
124+
const std::string metric, const double value) const
125+
{
126+
DiagnosticStatus status;
127+
128+
status.level = status.OK;
129+
status.name = metric;
130+
131+
diagnostic_msgs::msg::KeyValue key_value;
132+
key_value.key = "metric_value";
133+
key_value.value = std::to_string(value);
134+
status.values.push_back(key_value);
135+
136+
return status;
137+
}
138+
114139
void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
115140
{
116141
metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_);

evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

+13-1
Original file line numberDiff line numberDiff line change
@@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test
141141
[=](const DiagnosticArray::ConstSharedPtr msg) {
142142
const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
143143
if (it != msg->status.end()) {
144-
metric_value_ = boost::lexical_cast<double>(it->values[2].value);
144+
const auto mean_it = std::find_if(
145+
it->values.begin(), it->values.end(),
146+
[](const auto & key_value) { return key_value.key == "mean"; });
147+
if (mean_it != it->values.end()) {
148+
metric_value_ = boost::lexical_cast<double>(mean_it->value);
149+
} else {
150+
const auto metric_value_it = std::find_if(
151+
it->values.begin(), it->values.end(),
152+
[](const auto & key_value) { return key_value.key == "metric_value"; });
153+
if (metric_value_it != it->values.end()) {
154+
metric_value_ = boost::lexical_cast<double>(metric_value_it->value);
155+
}
156+
}
145157
metric_updated_ = true;
146158
}
147159
});

localization/gyro_odometer/CMakeLists.txt

+7-7
Original file line numberDiff line numberDiff line change
@@ -4,17 +4,12 @@ project(gyro_odometer)
44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

7-
ament_auto_add_executable(${PROJECT_NAME}
8-
src/gyro_odometer_node.cpp
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
98
src/gyro_odometer_core.cpp
109
)
1110

1211
target_link_libraries(${PROJECT_NAME} fmt)
1312

14-
ament_auto_add_library(gyro_odometer_node SHARED
15-
src/gyro_odometer_core.cpp
16-
)
17-
1813
if(BUILD_TESTING)
1914
ament_add_ros_isolated_gtest(test_gyro_odometer
2015
test/test_main.cpp
@@ -25,10 +20,15 @@ if(BUILD_TESTING)
2520
rclcpp
2621
)
2722
target_link_libraries(test_gyro_odometer
28-
gyro_odometer_node
23+
${PROJECT_NAME}
2924
)
3025
endif()
3126

27+
rclcpp_components_register_node(${PROJECT_NAME}
28+
PLUGIN "autoware::gyro_odometer::GyroOdometerNode"
29+
EXECUTABLE ${PROJECT_NAME}_node
30+
EXECUTOR SingleThreadedExecutor
31+
)
3232

3333
ament_auto_package(INSTALL_TO_SHARE
3434
launch

localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

+8-3
Original file line numberDiff line numberDiff line change
@@ -36,14 +36,17 @@
3636
#include <memory>
3737
#include <string>
3838

39-
class GyroOdometer : public rclcpp::Node
39+
namespace autoware::gyro_odometer
40+
{
41+
42+
class GyroOdometerNode : public rclcpp::Node
4043
{
4144
private:
4245
using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
4346

4447
public:
45-
explicit GyroOdometer(const rclcpp::NodeOptions & options);
46-
~GyroOdometer();
48+
explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options);
49+
~GyroOdometerNode();
4750

4851
private:
4952
void callbackVehicleTwist(
@@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node
7578
std::deque<sensor_msgs::msg::Imu> gyro_queue_;
7679
};
7780

81+
} // namespace autoware::gyro_odometer
82+
7883
#endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_

localization/gyro_odometer/launch/gyro_odometer.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111

1212
<arg name="config_file" default="$(find-pkg-share gyro_odometer)/config/gyro_odometer.param.yaml"/>
1313

14-
<node pkg="gyro_odometer" exec="gyro_odometer" name="gyro_odometer" output="screen">
14+
<node pkg="gyro_odometer" exec="gyro_odometer_node" output="both">
1515
<remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
1616

1717
<remap from="imu" to="$(var input_imu_topic)"/>

localization/gyro_odometer/package.xml

+2-3
Original file line numberDiff line numberDiff line change
@@ -19,14 +19,13 @@
1919

2020
<depend>fmt</depend>
2121
<depend>geometry_msgs</depend>
22+
<depend>rclcpp</depend>
23+
<depend>rclcpp_components</depend>
2224
<depend>sensor_msgs</depend>
2325
<depend>tf2</depend>
2426
<depend>tf2_geometry_msgs</depend>
2527
<depend>tier4_autoware_utils</depend>
2628

27-
<exec_depend>rclcpp</exec_depend>
28-
<exec_depend>rclcpp_components</exec_depend>
29-
3029
<test_depend>ament_cmake_ros</test_depend>
3130
<test_depend>ament_lint_auto</test_depend>
3231
<test_depend>autoware_lint_common</test_depend>

localization/gyro_odometer/src/gyro_odometer_core.cpp

+46-35
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,8 @@
1414

1515
#include "gyro_odometer/gyro_odometer_core.hpp"
1616

17+
#include <rclcpp/rclcpp.hpp>
18+
1719
#ifdef ROS_DISTRO_GALACTIC
1820
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
1921
#else
@@ -25,6 +27,42 @@
2527
#include <memory>
2628
#include <string>
2729

30+
namespace autoware::gyro_odometer
31+
{
32+
33+
GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options)
34+
: Node("gyro_odometer", node_options),
35+
output_frame_(declare_parameter<std::string>("output_frame")),
36+
message_timeout_sec_(declare_parameter<double>("message_timeout_sec")),
37+
vehicle_twist_arrived_(false),
38+
imu_arrived_(false)
39+
{
40+
transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
41+
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
42+
43+
vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
44+
"vehicle/twist_with_covariance", rclcpp::QoS{100},
45+
std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1));
46+
47+
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
48+
"imu", rclcpp::QoS{100},
49+
std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1));
50+
51+
twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
52+
twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
53+
"twist_with_covariance_raw", rclcpp::QoS{10});
54+
55+
twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
56+
twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
57+
"twist_with_covariance", rclcpp::QoS{10});
58+
59+
// TODO(YamatoAndo) createTimer
60+
}
61+
62+
GyroOdometerNode::~GyroOdometerNode()
63+
{
64+
}
65+
2866
std::array<double, 9> transformCovariance(const std::array<double, 9> & cov)
2967
{
3068
using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
@@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer(
100138
return twist_with_cov;
101139
}
102140

103-
GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options)
104-
: Node("gyro_odometer", options),
105-
output_frame_(declare_parameter<std::string>("output_frame")),
106-
message_timeout_sec_(declare_parameter<double>("message_timeout_sec")),
107-
vehicle_twist_arrived_(false),
108-
imu_arrived_(false)
109-
{
110-
transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
111-
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
112-
113-
vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
114-
"vehicle/twist_with_covariance", rclcpp::QoS{100},
115-
std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1));
116-
117-
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
118-
"imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1));
119-
120-
twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
121-
twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
122-
"twist_with_covariance_raw", rclcpp::QoS{10});
123-
124-
twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
125-
twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
126-
"twist_with_covariance", rclcpp::QoS{10});
127-
128-
// TODO(YamatoAndo) createTimer
129-
}
130-
131-
GyroOdometer::~GyroOdometer()
132-
{
133-
}
134-
135-
void GyroOdometer::callbackVehicleTwist(
141+
void GyroOdometerNode::callbackVehicleTwist(
136142
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr)
137143
{
138144
vehicle_twist_arrived_ = true;
@@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist(
173179
gyro_queue_.clear();
174180
}
175181

176-
void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
182+
void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
177183
{
178184
imu_arrived_ = true;
179185
if (!vehicle_twist_arrived_) {
@@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
241247
gyro_queue_.clear();
242248
}
243249

244-
void GyroOdometer::publishData(
250+
void GyroOdometerNode::publishData(
245251
const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw)
246252
{
247253
geometry_msgs::msg::TwistStamped twist_raw;
@@ -269,3 +275,8 @@ void GyroOdometer::publishData(
269275
twist_pub_->publish(twist);
270276
twist_with_covariance_pub_->publish(twist_with_covariance);
271277
}
278+
279+
} // namespace autoware::gyro_odometer
280+
281+
#include <rclcpp_components/register_node_macro.hpp>
282+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode)

0 commit comments

Comments
 (0)