Skip to content

Commit c8f1cdf

Browse files
authored
Merge branch 'main' into prefix-behavior_velocity_planner
2 parents 12fc1d9 + fe30833 commit c8f1cdf

File tree

57 files changed

+272
-330
lines changed

Some content is hidden

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

57 files changed

+272
-330
lines changed

.github/CODEOWNERS

+1-1
Original file line numberDiff line numberDiff line change
@@ -156,14 +156,14 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong
156156
perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
157157
perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
158158
perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp
159+
planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
159160
planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
160161
planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp
161162
planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai
162163
planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
163164
planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
164165
planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
165166
planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
166-
planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
167167
planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
168168
planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
169169
planning/behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp

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
});

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -82,12 +82,12 @@
8282
/>
8383
<let
8484
name="behavior_path_planner_launch_modules"
85-
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, '&quot;)"
85+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, '&quot;)"
8686
if="$(var launch_external_request_lane_change_right_module)"
8787
/>
8888
<let
8989
name="behavior_path_planner_launch_modules"
90-
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, '&quot;)"
90+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, '&quot;)"
9191
if="$(var launch_external_request_lane_change_left_module)"
9292
/>
9393
<let

localization/ekf_localizer/CMakeLists.txt

+8-9
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ include_directories(
1313

1414
ament_auto_find_build_dependencies()
1515

16-
ament_auto_add_library(ekf_localizer_lib SHARED
16+
ament_auto_add_library(${PROJECT_NAME} SHARED
1717
src/ekf_localizer.cpp
1818
src/covariance.cpp
1919
src/diagnostics.cpp
@@ -24,21 +24,20 @@ ament_auto_add_library(ekf_localizer_lib SHARED
2424
src/ekf_module.cpp
2525
)
2626

27-
target_link_libraries(ekf_localizer_lib Eigen3::Eigen)
28-
29-
ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp)
30-
31-
target_compile_options(ekf_localizer PUBLIC -g -Wall -Wextra -Wpedantic -Werror)
27+
rclcpp_components_register_node(${PROJECT_NAME}
28+
PLUGIN "EKFLocalizer"
29+
EXECUTABLE ${PROJECT_NAME}_node
30+
EXECUTOR SingleThreadedExecutor
31+
)
3232

33-
target_link_libraries(ekf_localizer ekf_localizer_lib)
34-
target_include_directories(ekf_localizer PUBLIC include)
33+
target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
3534

3635
function(add_testcase filepath)
3736
get_filename_component(filename ${filepath} NAME)
3837
string(REGEX REPLACE ".cpp" "" test_name ${filename})
3938

4039
ament_add_gtest(${test_name} ${filepath})
41-
target_link_libraries("${test_name}" ekf_localizer_lib)
40+
target_link_libraries("${test_name}" ${PROJECT_NAME})
4241
ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
4342
endfunction()
4443

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ class Simple1DFilter
102102
class EKFLocalizer : public rclcpp::Node
103103
{
104104
public:
105-
EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options);
105+
explicit EKFLocalizer(const rclcpp::NodeOptions & options);
106106

107107
private:
108108
const std::shared_ptr<Warning> warning_;

localization/ekf_localizer/launch/ekf_localizer.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
<arg name="output_twist_name" default="ekf_twist"/>
1818
<arg name="output_twist_with_covariance_name" default="ekf_twist_with_covariance"/>
1919

20-
<node pkg="ekf_localizer" exec="ekf_localizer" name="ekf_localizer" output="screen">
20+
<node pkg="ekf_localizer" exec="ekf_localizer_node" output="both">
2121
<remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>
2222

2323
<remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>

localization/ekf_localizer/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
<depend>kalman_filter</depend>
3030
<depend>nav_msgs</depend>
3131
<depend>rclcpp</depend>
32+
<depend>rclcpp_components</depend>
3233
<depend>std_srvs</depend>
3334
<depend>tf2</depend>
3435
<depend>tf2_ros</depend>

localization/ekf_localizer/src/ekf_localizer.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -40,8 +40,8 @@
4040

4141
using std::placeholders::_1;
4242

43-
EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options)
44-
: rclcpp::Node(node_name, node_options),
43+
EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
44+
: rclcpp::Node("ekf_localizer", node_options),
4545
warning_(std::make_shared<Warning>(this)),
4646
params_(this),
4747
ekf_dt_(params_.ekf_dt),
@@ -479,3 +479,6 @@ void EKFLocalizer::serviceTriggerNode(
479479
res->success = true;
480480
return;
481481
}
482+
483+
#include <rclcpp_components/register_node_macro.hpp>
484+
RCLCPP_COMPONENTS_REGISTER_NODE(EKFLocalizer)

localization/ekf_localizer/src/ekf_localizer_node.cpp

-28
This file was deleted.

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)"/>

0 commit comments

Comments
 (0)