Merge branch 'main' into prefix-behavior_velocity_planner

autowarefoundation · May 24, 2024 · c8f1cdf · c8f1cdf
2 parents 12fc1d9 + fe30833
commit c8f1cdf
Show file tree

Hide file tree

Showing 57 changed files with 272 additions and 330 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -156,14 +156,14 @@ perception/traffic_light_map_based_detector/** [email protected] tao.zhong
 perception/traffic_light_multi_camera_fusion/** [email protected] [email protected]
 perception/traffic_light_occlusion_predictor/** [email protected] [email protected]
 perception/traffic_light_visualization/** [email protected] [email protected]
+planning/autoware_behavior_path_external_request_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/autoware_behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/autoware_planning_test_manager/** [email protected] [email protected]
 planning/autoware_remaining_distance_time_calculator/** [email protected]
 planning/autoware_static_centerline_generator/** [email protected] [email protected]
 planning/behavior_path_avoidance_by_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_avoidance_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_dynamic_avoidance_module/** [email protected] [email protected] [email protected] [email protected]
-planning/behavior_path_external_request_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_goal_planner_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]

diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp
@@ -20,6 +20,7 @@
 #include <iostream>
 #include <string>
 #include <unordered_map>
+#include <variant>
 #include <vector>
 
 namespace perception_diagnostics
@@ -36,7 +37,11 @@ enum class Metric {
   SIZE,
 };
 
+// Each metric has a different return type. (statistic or just a one value etc).
+// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant
 using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
+using MetricValueMap = std::unordered_map<std::string, double>;
+using MetricsMap = std::variant<MetricStatMap, MetricValueMap>;
 
 struct PredictedPathDeviationMetrics
 {

diff --git a/...or/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/...or/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp
@@ -96,7 +96,7 @@ class MetricsCalculator
    * @param [in] metric Metric enum value
    * @return map of string describing the requested metric and the calculated value
    */
-  std::optional<MetricStatMap> calculate(const Metric & metric) const;
+  std::optional<MetricsMap> calculate(const Metric & metric) const;
 
   /**
    * @brief set the dynamic objects used to calculate obstacle metrics
@@ -143,7 +143,7 @@ class MetricsCalculator
   PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics(
     const PredictedObjects & objects, const double time_horizon) const;
   MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const;
-  MetricStatMap calcObjectsCountMetrics() const;
+  MetricValueMap calcObjectsCountMetrics() const;
 
   bool hasPassedTime(const rclcpp::Time stamp) const;
   bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;

diff --git a/...online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/...online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp
@@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
 
   DiagnosticStatus generateDiagnosticStatus(
     const std::string metric, const Stat<double> & metric_stat) const;
+  DiagnosticStatus generateDiagnosticStatus(
+    const std::string metric, const double metric_value) const;
 
 private:
   // Subscribers and publishers

diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml
@@ -13,7 +13,7 @@
     prediction_time_horizons: [1.0, 2.0, 3.0, 5.0]
 
     stopped_velocity_threshold: 1.0
-    detection_radius_list: [50.0, 100.0, 150.0, 200.0]
+    detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0]
     detection_height_list: [10.0]
     detection_count_purge_seconds: 36000.0
     objects_count_window_seconds: 1.0

diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp
@@ -26,7 +26,7 @@ namespace perception_diagnostics
 using object_recognition_utils::convertLabelToString;
 using tier4_autoware_utils::inverseTransformPoint;
 
-std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric) const
+std::optional<MetricsMap> MetricsCalculator::calculate(const Metric & metric) const
 {
   // clang-format off
   const bool use_past_objects = metric == Metric::lateral_deviation        ||
@@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas
   return metric_stat_map;
 }
 
-MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const
+MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const
 {
-  MetricStatMap metric_stat_map;
+  MetricValueMap metric_stat_map;
   // calculate the average number of objects in the detection area in all past frames
   const auto overall_average_count = detection_counter_.getOverallAverageCount();
   for (const auto & [label, range_and_count] : overall_average_count) {
     for (const auto & [range, count] : range_and_count) {
-      metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add(
-        count);
+      metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
     }
   }
   // calculate the average number of objects in the detection area in the past
@@ -472,17 +471,16 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const
     detection_counter_.getAverageCount(parameters_->objects_count_window_seconds);
   for (const auto & [label, range_and_count] : average_count) {
     for (const auto & [range, count] : range_and_count) {
-      metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range]
-        .add(count);
+      metric_stat_map
+        ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count;
     }
   }
 
   // calculate the total number of objects in the detection area
   const auto total_count = detection_counter_.getTotalCount();
   for (const auto & [label, range_and_count] : total_count) {
     for (const auto & [range, count] : range_and_count) {
-      metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add(
-        count);
+      metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count;
     }
   }
 

diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp
@@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics()
 
   // calculate metrics
   for (const Metric & metric : parameters_->metrics) {
-    const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric));
-    if (!metric_stat_map.has_value()) {
+    const auto metric_result = metrics_calculator_.calculate(Metric(metric));
+    if (!metric_result.has_value()) {
       continue;
     }
 
-    for (const auto & [metric, stat] : metric_stat_map.value()) {
-      if (stat.count() > 0) {
-        metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat));
-      }
-    }
+    std::visit(
+      [&metrics_msg, this](auto && arg) {
+        using T = std::decay_t<decltype(arg)>;
+        for (const auto & [metric, value] : arg) {
+          if constexpr (std::is_same_v<T, MetricStatMap>) {
+            if (value.count() > 0) {
+              metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
+            }
+          } else if constexpr (std::is_same_v<T, MetricValueMap>) {
+            metrics_msg.status.push_back(generateDiagnosticStatus(metric, value));
+          }
+        }
+      },
+      metric_result.value());
   }
 
   // publish metrics
@@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
   return status;
 }
 
+DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
+  const std::string metric, const double value) const
+{
+  DiagnosticStatus status;
+
+  status.level = status.OK;
+  status.name = metric;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "metric_value";
+  key_value.value = std::to_string(value);
+  status.values.push_back(key_value);
+
+  return status;
+}
+
 void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
 {
   metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_);

diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp
@@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test
       [=](const DiagnosticArray::ConstSharedPtr msg) {
         const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
         if (it != msg->status.end()) {
-          metric_value_ = boost::lexical_cast<double>(it->values[2].value);
+          const auto mean_it = std::find_if(
+            it->values.begin(), it->values.end(),
+            [](const auto & key_value) { return key_value.key == "mean"; });
+          if (mean_it != it->values.end()) {
+            metric_value_ = boost::lexical_cast<double>(mean_it->value);
+          } else {
+            const auto metric_value_it = std::find_if(
+              it->values.begin(), it->values.end(),
+              [](const auto & key_value) { return key_value.key == "metric_value"; });
+            if (metric_value_it != it->values.end()) {
+              metric_value_ = boost::lexical_cast<double>(metric_value_it->value);
+            }
+          }
           metric_updated_ = true;
         }
       });

diff --git a/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -82,12 +82,12 @@
   />
   <let
     name="behavior_path_planner_launch_modules"
-    value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, '&quot;)"
+    value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, '&quot;)"
     if="$(var launch_external_request_lane_change_right_module)"
   />
   <let
     name="behavior_path_planner_launch_modules"
-    value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, '&quot;)"
+    value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, '&quot;)"
     if="$(var launch_external_request_lane_change_left_module)"
   />
   <let

diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt
@@ -13,7 +13,7 @@ include_directories(
 
 ament_auto_find_build_dependencies()
 
-ament_auto_add_library(ekf_localizer_lib SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/ekf_localizer.cpp
   src/covariance.cpp
   src/diagnostics.cpp
@@ -24,21 +24,20 @@ ament_auto_add_library(ekf_localizer_lib SHARED
   src/ekf_module.cpp
 )
 
-target_link_libraries(ekf_localizer_lib Eigen3::Eigen)
-
-ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp)
-
-target_compile_options(ekf_localizer PUBLIC -g -Wall -Wextra -Wpedantic -Werror)
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "EKFLocalizer"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
 
-target_link_libraries(ekf_localizer ekf_localizer_lib)
-target_include_directories(ekf_localizer PUBLIC include)
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
 
 function(add_testcase filepath)
   get_filename_component(filename ${filepath} NAME)
   string(REGEX REPLACE ".cpp" "" test_name ${filename})
 
   ament_add_gtest(${test_name} ${filepath})
-  target_link_libraries("${test_name}" ekf_localizer_lib)
+  target_link_libraries("${test_name}" ${PROJECT_NAME})
   ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
 endfunction()
 

diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -102,7 +102,7 @@ class Simple1DFilter
 class EKFLocalizer : public rclcpp::Node
 {
 public:
-  EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options);
+  explicit EKFLocalizer(const rclcpp::NodeOptions & options);
 
 private:
   const std::shared_ptr<Warning> warning_;

diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml
@@ -17,7 +17,7 @@
   <arg name="output_twist_name" default="ekf_twist"/>
   <arg name="output_twist_with_covariance_name" default="ekf_twist_with_covariance"/>
 
-  <node pkg="ekf_localizer" exec="ekf_localizer" name="ekf_localizer" output="screen">
+  <node pkg="ekf_localizer" exec="ekf_localizer_node" output="both">
     <remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>
 
     <remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>

diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml
@@ -29,6 +29,7 @@
   <depend>kalman_filter</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -40,8 +40,8 @@
 
 using std::placeholders::_1;
 
-EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options)
-: rclcpp::Node(node_name, node_options),
+EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
+: rclcpp::Node("ekf_localizer", node_options),
   warning_(std::make_shared<Warning>(this)),
   params_(this),
   ekf_dt_(params_.ekf_dt),
@@ -479,3 +479,6 @@ void EKFLocalizer::serviceTriggerNode(
   res->success = true;
   return;
 }
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(EKFLocalizer)
diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp
diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt
@@ -4,17 +4,12 @@ project(gyro_odometer)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_executable(${PROJECT_NAME}
-  src/gyro_odometer_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/gyro_odometer_core.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} fmt)
 
-ament_auto_add_library(gyro_odometer_node SHARED
-  src/gyro_odometer_core.cpp
-)
-
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_gyro_odometer
     test/test_main.cpp
@@ -25,10 +20,15 @@ if(BUILD_TESTING)
     rclcpp
   )
   target_link_libraries(test_gyro_odometer
-    gyro_odometer_node
+    ${PROJECT_NAME}
   )
 endif()
 
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::gyro_odometer::GyroOdometerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
 
 ament_auto_package(INSTALL_TO_SHARE
   launch

diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
@@ -36,14 +36,17 @@
 #include <memory>
 #include <string>
 
-class GyroOdometer : public rclcpp::Node
+namespace autoware::gyro_odometer
+{
+
+class GyroOdometerNode : public rclcpp::Node
 {
 private:
   using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
 
 public:
-  explicit GyroOdometer(const rclcpp::NodeOptions & options);
-  ~GyroOdometer();
+  explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options);
+  ~GyroOdometerNode();
 
 private:
   void callbackVehicleTwist(
@@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node
   std::deque<sensor_msgs::msg::Imu> gyro_queue_;
 };
 
+}  // namespace autoware::gyro_odometer
+
 #endif  // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml
@@ -11,7 +11,7 @@
 
   <arg name="config_file" default="$(find-pkg-share gyro_odometer)/config/gyro_odometer.param.yaml"/>
 
-  <node pkg="gyro_odometer" exec="gyro_odometer" name="gyro_odometer" output="screen">
+  <node pkg="gyro_odometer" exec="gyro_odometer_node" output="both">
     <remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
 
     <remap from="imu" to="$(var input_imu_topic)"/>