Merge branch 'awf-latest' into RT1-8920-revise-current-lane-objects-filtering

Author: mkquda

.cppcheck_suppressions

@@ -1,4 +1,5 @@
 *:*/test/*
+*:*/examples/*
 
 checkersReport
 missingInclude

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -14,7 +14,8 @@
       - lateral_deviation
       - yaw_deviation
       - velocity_deviation
-      - trajectory_lateral_displacement
+      - lateral_trajectory_displacement_local
+      - lateral_trajectory_displacement_lookahead
       - stability
       - stability_frechet
       - obstacle_distance

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp

@@ -45,7 +45,7 @@ Accumulator<double> calcLateralDeviation(const Trajectory & ref, const Trajector
  * @param [in] base_pose base pose
  * @return calculated statistics
  */
-Accumulator<double> calcLateralTrajectoryDisplacement(
+Accumulator<double> calcLocalLateralTrajectoryDisplacement(
   const Trajectory & prev, const Trajectory & traj, const Pose & base_pose);
 
 /**

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp

@@ -37,10 +37,10 @@ enum class Metric {
   lateral_deviation,
   yaw_deviation,
   velocity_deviation,
-  lateral_trajectory_displacement,
+  lateral_trajectory_displacement_local,
+  lateral_trajectory_displacement_lookahead,
   stability,
   stability_frechet,
-  trajectory_lateral_displacement,
   obstacle_distance,
   obstacle_ttc,
   modified_goal_longitudinal_deviation,
@@ -64,10 +64,10 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"lateral_deviation", Metric::lateral_deviation},
   {"yaw_deviation", Metric::yaw_deviation},
   {"velocity_deviation", Metric::velocity_deviation},
-  {"lateral_trajectory_displacement", Metric::lateral_trajectory_displacement},
+  {"lateral_trajectory_displacement_local", Metric::lateral_trajectory_displacement_local},
+  {"lateral_trajectory_displacement_lookahead", Metric::lateral_trajectory_displacement_lookahead},
   {"stability", Metric::stability},
   {"stability_frechet", Metric::stability_frechet},
-  {"trajectory_lateral_displacement", Metric::trajectory_lateral_displacement},
   {"obstacle_distance", Metric::obstacle_distance},
   {"obstacle_ttc", Metric::obstacle_ttc},
   {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation},
@@ -86,10 +86,10 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::lateral_deviation, "lateral_deviation"},
   {Metric::yaw_deviation, "yaw_deviation"},
   {Metric::velocity_deviation, "velocity_deviation"},
-  {Metric::lateral_trajectory_displacement, "lateral_trajectory_displacement"},
+  {Metric::lateral_trajectory_displacement_local, "lateral_trajectory_displacement_local"},
+  {Metric::lateral_trajectory_displacement_lookahead, "lateral_trajectory_displacement_lookahead"},
   {Metric::stability, "stability"},
   {Metric::stability_frechet, "stability_frechet"},
-  {Metric::trajectory_lateral_displacement, "trajectory_lateral_displacement"},
   {Metric::obstacle_distance, "obstacle_distance"},
   {Metric::obstacle_ttc, "obstacle_ttc"},
   {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"},
@@ -109,10 +109,10 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::lateral_deviation, "Lateral_deviation[m]"},
   {Metric::yaw_deviation, "Yaw_deviation[rad]"},
   {Metric::velocity_deviation, "Velocity_deviation[m/s]"},
-  {Metric::lateral_trajectory_displacement, "Nearest Pose Lateral Deviation[m]"},
+  {Metric::lateral_trajectory_displacement_local, "Nearest Pose Lateral Deviation[m]"},
+  {Metric::lateral_trajectory_displacement_lookahead, "Lateral_Offset_Over_Distance_Ahead[m]"},
   {Metric::stability, "Stability[m]"},
   {Metric::stability_frechet, "StabilityFrechet[m]"},
-  {Metric::trajectory_lateral_displacement, "Trajectory_lateral_displacement[m]"},
   {Metric::obstacle_distance, "Obstacle_distance[m]"},
   {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"},
   {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"},

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp

@@ -56,7 +56,7 @@ Accumulator<double> calcLateralDistance(const Trajectory & traj1, const Trajecto
  * @param [in]  trajectory_eval_time_s time duration for trajectory evaluation in seconds
  * @return statistical accumulator containing the total lateral displacement
  */
-Accumulator<double> calcTrajectoryLateralDisplacement(
+Accumulator<double> calcLookaheadLateralTrajectoryDisplacement(
   const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom,
   const double trajectory_eval_time_s);
 

evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp

@@ -45,7 +45,7 @@ Accumulator<double> calcLateralDeviation(const Trajectory & ref, const Trajector
   return stat;
 }
 
-Accumulator<double> calcLateralTrajectoryDisplacement(
+Accumulator<double> calcLocalLateralTrajectoryDisplacement(
   const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose)
 {
   Accumulator<double> stat;

evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp

@@ -98,7 +98,7 @@ Accumulator<double> calcLateralDistance(const Trajectory & traj1, const Trajecto
   return stat;
 }
 
-Accumulator<double> calcTrajectoryLateralDisplacement(
+Accumulator<double> calcLookaheadLateralTrajectoryDisplacement(
   const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom,
   const double trajectory_eval_time_s)
 {

evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp

@@ -50,8 +50,11 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
       return metrics::calcYawDeviation(reference_trajectory_, traj);
     case Metric::velocity_deviation:
       return metrics::calcVelocityDeviation(reference_trajectory_, traj);
-    case Metric::lateral_trajectory_displacement:
-      return metrics::calcLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_);
+    case Metric::lateral_trajectory_displacement_local:
+      return metrics::calcLocalLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_);
+    case Metric::lateral_trajectory_displacement_lookahead:
+      return metrics::calcLookaheadLateralTrajectoryDisplacement(
+        previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s);
     case Metric::stability_frechet:
       return metrics::calcFrechetDistance(
         metrics::utils::get_lookahead_trajectory(
@@ -68,9 +71,6 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
         metrics::utils::get_lookahead_trajectory(
           traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m,
           parameters.trajectory.lookahead.max_time_s));
-    case Metric::trajectory_lateral_displacement:
-      return metrics::calcTrajectoryLateralDisplacement(
-        previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s);
     case Metric::obstacle_distance:
       return metrics::calcDistanceToObstacle(dynamic_objects_, traj);
     case Metric::obstacle_ttc:

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml

@@ -23,6 +23,7 @@
   <depend>autoware_behavior_path_planner</depend>
   <depend>autoware_behavior_path_planner_common</depend>
   <depend>autoware_bezier_sampler</depend>
+  <depend>autoware_freespace_planning_algorithms</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_test_utils</depend>

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml

@@ -22,6 +22,7 @@
 
   <depend>autoware_behavior_path_planner</depend>
   <depend>autoware_behavior_path_planner_common</depend>
+  <depend>autoware_freespace_planning_algorithms</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_universe_utils</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp

@@ -19,7 +19,7 @@
 
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
+#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
@@ -42,8 +42,8 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 
-  std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
-    const tier4_planning_msgs::msg::PathWithLaneId & path) override;
+  std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
+  getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 };
 
 class BlindSpotModulePlugin : public PluginWrapper<BlindSpotModuleManager>

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp

@@ -16,8 +16,8 @@
 #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_
 
 #include <autoware/behavior_velocity_blind_spot_module/util.hpp>
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/utilization/state_machine.hpp>
+#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
@@ -59,7 +59,7 @@ struct Safe
 
 using BlindSpotDecision = std::variant<InternalError, OverPassJudge, Unsafe, Safe>;
 
-class BlindSpotModule : public SceneModuleInterface
+class BlindSpotModule : public SceneModuleInterfaceWithRTC
 {
 public:
   struct DebugData

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml

@@ -18,6 +18,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_behavior_velocity_rtc_interface</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp

@@ -83,7 +83,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa
   }
 }
 
-std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
+std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
 BlindSpotModuleManager::getModuleExpiredFunction(
   const tier4_planning_msgs::msg::PathWithLaneId & path)
 {

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp

@@ -41,7 +41,7 @@ BlindSpotModule::BlindSpotModule(
   const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
   const std::shared_ptr<const PlannerData> planner_data, const PlannerParam & planner_param,
   const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
-: SceneModuleInterface(module_id, logger, clock),
+: SceneModuleInterfaceWithRTC(module_id, logger, clock),
   lane_id_(lane_id),
   planner_param_{planner_param},
   turn_direction_(turn_direction),

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml

@@ -22,6 +22,7 @@
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_behavior_velocity_rtc_interface</depend>
   <depend>autoware_grid_map_utils</depend>
   <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_interpolation</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp

@@ -216,7 +216,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
   }
 }
 
-std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
+std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
 CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
 {
   const auto rh = planner_data_->route_handler_;
@@ -233,7 +233,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
     crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id());
   }
 
-  return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
+  return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
     return crosswalk_id_set.count(scene_module->getModuleId()) == 0;
   };
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp

@@ -19,7 +19,7 @@
 
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
+#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
 #include <autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -48,8 +48,8 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   void launchNewModules(const PathWithLaneId & path) override;
 
-  std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
-    const PathWithLaneId & path) override;
+  std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
+  getModuleExpiredFunction(const PathWithLaneId & path) override;
 };
 
 class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -176,7 +176,7 @@ CrosswalkModule::CrosswalkModule(
   const std::optional<int64_t> & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const PlannerParam & planner_param, const rclcpp::Logger & logger,
   const rclcpp::Clock::SharedPtr clock)
-: SceneModuleInterface(module_id, logger, clock),
+: SceneModuleInterfaceWithRTC(module_id, logger, clock),
   module_id_(module_id),
   planner_param_(planner_param),
   use_regulatory_element_(reg_elem_id)

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -17,7 +17,7 @@
 
 #include "autoware/behavior_velocity_crosswalk_module/util.hpp"
 
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
+#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp>
@@ -112,7 +112,7 @@ double InterpolateMap(
 }
 }  // namespace
 
-class CrosswalkModule : public SceneModuleInterface
+class CrosswalkModule : public SceneModuleInterfaceWithRTC
 {
 public:
   struct PlannerParam

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml

@@ -18,6 +18,7 @@
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_behavior_velocity_rtc_interface</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp

@@ -74,16 +74,17 @@ void DetectionAreaModuleManager::launchNewModules(
   }
 }
 
-std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
+std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
 DetectionAreaModuleManager::getModuleExpiredFunction(
   const tier4_planning_msgs::msg::PathWithLaneId & path)
 {
   const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath<DetectionArea>(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
-  return [detection_area_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
-    return detection_area_id_set.count(scene_module->getModuleId()) == 0;
-  };
+  return
+    [detection_area_id_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
+      return detection_area_id_set.count(scene_module->getModuleId()) == 0;
+    };
 }
 
 }  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp

@@ -19,7 +19,7 @@
 
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
+#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
@@ -41,8 +41,8 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 
-  std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
-    const tier4_planning_msgs::msg::PathWithLaneId & path) override;
+  std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
+  getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 };
 
 class DetectionAreaModulePlugin : public PluginWrapper<DetectionAreaModuleManager>

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp

@@ -37,7 +37,7 @@ DetectionAreaModule::DetectionAreaModule(
   const lanelet::autoware::DetectionArea & detection_area_reg_elem,
   const PlannerParam & planner_param, const rclcpp::Logger & logger,
   const rclcpp::Clock::SharedPtr clock)
-: SceneModuleInterface(module_id, logger, clock),
+: SceneModuleInterfaceWithRTC(module_id, logger, clock),
   lane_id_(lane_id),
   detection_area_reg_elem_(detection_area_reg_elem),
   state_(State::GO),

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp

@@ -23,8 +23,8 @@
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
 #include <autoware_lanelet2_extension/regulatory_elements/detection_area.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -38,7 +38,7 @@ using PathIndexWithPoint2d = std::pair<size_t, Point2d>;                // front
 using PathIndexWithOffset = std::pair<size_t, double>;                  // front index, offset
 using tier4_planning_msgs::msg::PathWithLaneId;
 
-class DetectionAreaModule : public SceneModuleInterface
+class DetectionAreaModule : public SceneModuleInterfaceWithRTC
 {
 public:
   enum class State { GO, STOP };

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml

@@ -20,6 +20,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_behavior_velocity_rtc_interface</depend>
   <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp

@@ -353,14 +353,14 @@ void IntersectionModuleManager::launchNewModules(
   }
 }
 
-std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
+std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
 IntersectionModuleManager::getModuleExpiredFunction(
   const tier4_planning_msgs::msg::PathWithLaneId & path)
 {
   const auto lane_set = planning_utils::getLaneletsOnPath(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
-  return [lane_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
+  return [lane_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
     const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
     const auto & associative_ids = intersection_module->getAssociativeIds();
     for (const auto & lane : lane_set) {