From f2a41290ee07bdcb8b970fc7eaa53e52c77c7edc Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com>
Date: Tue, 24 Dec 2024 15:48:48 +0900
Subject: [PATCH 01/13] refactor(autoware_behavior_path_start_planner_module):
 add data_structs.cpp and init method for StartPlannerParameters (#9736)

feat(autoware_behavior_path_start_planner_module): add data_structs.cpp and init method for StartPlannerParameters

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
---
 .../CMakeLists.txt                            |   1 +
 .../data_structs.hpp                          |   1 +
 .../src/data_structs.cpp                      | 340 ++++++++++++++++++
 .../src/manager.cpp                           | 308 +---------------
 4 files changed, 346 insertions(+), 304 deletions(-)
 create mode 100644 planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp

diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt
index 3fa8ad7218fa2..e28339fb51a6c 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt
@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/freespace_pull_out.cpp
   src/geometric_pull_out.cpp
   src/shift_pull_out.cpp
+  src/data_structs.cpp
   src/util.cpp
 )
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp
index 9c439e9b3b921..5d8331318484d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp
@@ -97,6 +97,7 @@ struct StartPlannerDebugData
 
 struct StartPlannerParameters
 {
+  static StartPlannerParameters init(rclcpp::Node & node);
   double th_arrived_distance{0.0};
   double th_stopped_velocity{0.0};
   double th_stopped_time{0.0};
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp
new file mode 100644
index 0000000000000..f613b9345ce42
--- /dev/null
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp
@@ -0,0 +1,340 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/behavior_path_start_planner_module/data_structs.hpp"
+
+#include "autoware/behavior_path_start_planner_module/manager.hpp"
+
+#include <autoware/universe_utils/ros/parameter.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+namespace autoware::behavior_path_planner
+{
+
+StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node)
+{
+  using autoware::universe_utils::getOrDeclareParameter;
+  StartPlannerParameters p;
+  {
+    const std::string ns = "start_planner.";
+
+    p.th_arrived_distance = getOrDeclareParameter<double>(node, ns + "th_arrived_distance");
+    p.th_stopped_velocity = getOrDeclareParameter<double>(node, ns + "th_stopped_velocity");
+    p.th_stopped_time = getOrDeclareParameter<double>(node, ns + "th_stopped_time");
+    p.prepare_time_before_start =
+      getOrDeclareParameter<double>(node, ns + "prepare_time_before_start");
+    p.th_distance_to_middle_of_the_road =
+      getOrDeclareParameter<double>(node, ns + "th_distance_to_middle_of_the_road");
+    p.skip_rear_vehicle_check = getOrDeclareParameter<bool>(node, ns + "skip_rear_vehicle_check");
+    p.extra_width_margin_for_rear_obstacle =
+      getOrDeclareParameter<double>(node, ns + "extra_width_margin_for_rear_obstacle");
+    p.collision_check_margins =
+      getOrDeclareParameter<std::vector<double>>(node, ns + "collision_check_margins");
+    p.collision_check_margin_from_front_object =
+      getOrDeclareParameter<double>(node, ns + "collision_check_margin_from_front_object");
+    p.th_moving_object_velocity =
+      getOrDeclareParameter<double>(node, ns + "th_moving_object_velocity");
+    p.center_line_path_interval =
+      getOrDeclareParameter<double>(node, ns + "center_line_path_interval");
+    // shift pull out
+    p.enable_shift_pull_out = getOrDeclareParameter<bool>(node, ns + "enable_shift_pull_out");
+    p.check_shift_path_lane_departure =
+      getOrDeclareParameter<bool>(node, ns + "check_shift_path_lane_departure");
+    p.allow_check_shift_path_lane_departure_override =
+      getOrDeclareParameter<bool>(node, ns + "allow_check_shift_path_lane_departure_override");
+    p.shift_collision_check_distance_from_end =
+      getOrDeclareParameter<double>(node, ns + "shift_collision_check_distance_from_end");
+    p.minimum_shift_pull_out_distance =
+      getOrDeclareParameter<double>(node, ns + "minimum_shift_pull_out_distance");
+    p.lateral_acceleration_sampling_num =
+      getOrDeclareParameter<int>(node, ns + "lateral_acceleration_sampling_num");
+    p.lateral_jerk = getOrDeclareParameter<double>(node, ns + "lateral_jerk");
+    p.maximum_lateral_acc = getOrDeclareParameter<double>(node, ns + "maximum_lateral_acc");
+    p.minimum_lateral_acc = getOrDeclareParameter<double>(node, ns + "minimum_lateral_acc");
+    p.maximum_curvature = getOrDeclareParameter<double>(node, ns + "maximum_curvature");
+    p.end_pose_curvature_threshold =
+      getOrDeclareParameter<double>(node, ns + "end_pose_curvature_threshold");
+    p.maximum_longitudinal_deviation =
+      getOrDeclareParameter<double>(node, ns + "maximum_longitudinal_deviation");
+    // geometric pull out
+    p.enable_geometric_pull_out =
+      getOrDeclareParameter<bool>(node, ns + "enable_geometric_pull_out");
+    p.geometric_collision_check_distance_from_end =
+      getOrDeclareParameter<double>(node, ns + "geometric_collision_check_distance_from_end");
+    p.divide_pull_out_path = getOrDeclareParameter<bool>(node, ns + "divide_pull_out_path");
+    p.parallel_parking_parameters.pull_out_velocity =
+      getOrDeclareParameter<double>(node, ns + "geometric_pull_out_velocity");
+    p.parallel_parking_parameters.pull_out_arc_path_interval =
+      getOrDeclareParameter<double>(node, ns + "arc_path_interval");
+    p.parallel_parking_parameters.pull_out_lane_departure_margin =
+      getOrDeclareParameter<double>(node, ns + "lane_departure_margin");
+    p.lane_departure_check_expansion_margin =
+      getOrDeclareParameter<double>(node, ns + "lane_departure_check_expansion_margin");
+    p.parallel_parking_parameters.pull_out_max_steer_angle =
+      getOrDeclareParameter<double>(node, ns + "pull_out_max_steer_angle");  // 15deg
+    p.parallel_parking_parameters.center_line_path_interval =
+      p.center_line_path_interval;  // for geometric parallel parking
+    // search start pose backward
+    p.search_priority = getOrDeclareParameter<std::string>(
+      node,
+      ns + "search_priority");  // "efficient_path" or "short_back_distance"
+    p.enable_back = getOrDeclareParameter<bool>(node, ns + "enable_back");
+    p.backward_velocity = getOrDeclareParameter<double>(node, ns + "backward_velocity");
+    p.max_back_distance = getOrDeclareParameter<double>(node, ns + "max_back_distance");
+    p.backward_search_resolution =
+      getOrDeclareParameter<double>(node, ns + "backward_search_resolution");
+    p.backward_path_update_duration =
+      getOrDeclareParameter<double>(node, ns + "backward_path_update_duration");
+    p.ignore_distance_from_lane_end =
+      getOrDeclareParameter<double>(node, ns + "ignore_distance_from_lane_end");
+    // stop condition
+    p.maximum_deceleration_for_stop =
+      getOrDeclareParameter<double>(node, ns + "stop_condition.maximum_deceleration_for_stop");
+    p.maximum_jerk_for_stop =
+      getOrDeclareParameter<double>(node, ns + "stop_condition.maximum_jerk_for_stop");
+  }
+  {
+    const std::string ns = "start_planner.object_types_to_check_for_path_generation.";
+    p.object_types_to_check_for_path_generation.check_car =
+      getOrDeclareParameter<bool>(node, ns + "check_car");
+    p.object_types_to_check_for_path_generation.check_truck =
+      getOrDeclareParameter<bool>(node, ns + "check_truck");
+    p.object_types_to_check_for_path_generation.check_bus =
+      getOrDeclareParameter<bool>(node, ns + "check_bus");
+    p.object_types_to_check_for_path_generation.check_trailer =
+      getOrDeclareParameter<bool>(node, ns + "check_trailer");
+    p.object_types_to_check_for_path_generation.check_unknown =
+      getOrDeclareParameter<bool>(node, ns + "check_unknown");
+    p.object_types_to_check_for_path_generation.check_bicycle =
+      getOrDeclareParameter<bool>(node, ns + "check_bicycle");
+    p.object_types_to_check_for_path_generation.check_motorcycle =
+      getOrDeclareParameter<bool>(node, ns + "check_motorcycle");
+    p.object_types_to_check_for_path_generation.check_pedestrian =
+      getOrDeclareParameter<bool>(node, ns + "check_pedestrian");
+  }
+  // freespace planner general params
+  {
+    const std::string ns = "start_planner.freespace_planner.";
+    p.enable_freespace_planner = getOrDeclareParameter<bool>(node, ns + "enable_freespace_planner");
+    p.freespace_planner_algorithm =
+      getOrDeclareParameter<std::string>(node, ns + "freespace_planner_algorithm");
+    p.end_pose_search_start_distance =
+      getOrDeclareParameter<double>(node, ns + "end_pose_search_start_distance");
+    p.end_pose_search_end_distance =
+      getOrDeclareParameter<double>(node, ns + "end_pose_search_end_distance");
+    p.end_pose_search_interval =
+      getOrDeclareParameter<double>(node, ns + "end_pose_search_interval");
+    p.freespace_planner_velocity = getOrDeclareParameter<double>(node, ns + "velocity");
+    p.vehicle_shape_margin = getOrDeclareParameter<double>(node, ns + "vehicle_shape_margin");
+    p.freespace_planner_common_parameters.time_limit =
+      getOrDeclareParameter<double>(node, ns + "time_limit");
+    p.freespace_planner_common_parameters.max_turning_ratio =
+      getOrDeclareParameter<double>(node, ns + "max_turning_ratio");
+    p.freespace_planner_common_parameters.turning_steps =
+      getOrDeclareParameter<int>(node, ns + "turning_steps");
+  }
+  //  freespace planner search config
+  {
+    const std::string ns = "start_planner.freespace_planner.search_configs.";
+    p.freespace_planner_common_parameters.theta_size =
+      getOrDeclareParameter<int>(node, ns + "theta_size");
+    p.freespace_planner_common_parameters.angle_goal_range =
+      getOrDeclareParameter<double>(node, ns + "angle_goal_range");
+    p.freespace_planner_common_parameters.curve_weight =
+      getOrDeclareParameter<double>(node, ns + "curve_weight");
+    p.freespace_planner_common_parameters.reverse_weight =
+      getOrDeclareParameter<double>(node, ns + "reverse_weight");
+    p.freespace_planner_common_parameters.lateral_goal_range =
+      getOrDeclareParameter<double>(node, ns + "lateral_goal_range");
+    p.freespace_planner_common_parameters.longitudinal_goal_range =
+      getOrDeclareParameter<double>(node, ns + "longitudinal_goal_range");
+  }
+  //  freespace planner costmap configs
+  {
+    const std::string ns = "start_planner.freespace_planner.costmap_configs.";
+    p.freespace_planner_common_parameters.obstacle_threshold =
+      getOrDeclareParameter<int>(node, ns + "obstacle_threshold");
+  }
+  //  freespace planner astar
+  {
+    const std::string ns = "start_planner.freespace_planner.astar.";
+    p.astar_parameters.search_method =
+      getOrDeclareParameter<std::string>(node, ns + "search_method");
+    p.astar_parameters.only_behind_solutions =
+      getOrDeclareParameter<bool>(node, ns + "only_behind_solutions");
+    p.astar_parameters.use_back = getOrDeclareParameter<bool>(node, ns + "use_back");
+    p.astar_parameters.distance_heuristic_weight =
+      getOrDeclareParameter<double>(node, ns + "distance_heuristic_weight");
+  }
+  //   freespace planner rrtstar
+  {
+    const std::string ns = "start_planner.freespace_planner.rrtstar.";
+    p.rrt_star_parameters.enable_update = getOrDeclareParameter<bool>(node, ns + "enable_update");
+    p.rrt_star_parameters.use_informed_sampling =
+      getOrDeclareParameter<bool>(node, ns + "use_informed_sampling");
+    p.rrt_star_parameters.max_planning_time =
+      getOrDeclareParameter<double>(node, ns + "max_planning_time");
+    p.rrt_star_parameters.neighbor_radius =
+      getOrDeclareParameter<double>(node, ns + "neighbor_radius");
+    p.rrt_star_parameters.margin = getOrDeclareParameter<double>(node, ns + "margin");
+  }
+
+  const std::string base_ns = "start_planner.path_safety_check.";
+  // EgoPredictedPath
+  {
+    const std::string ego_path_ns = base_ns + "ego_predicted_path.";
+    p.ego_predicted_path_params.min_velocity =
+      getOrDeclareParameter<double>(node, ego_path_ns + "min_velocity");
+    p.ego_predicted_path_params.acceleration =
+      getOrDeclareParameter<double>(node, ego_path_ns + "min_acceleration");
+    p.ego_predicted_path_params.time_horizon_for_front_object =
+      getOrDeclareParameter<double>(node, ego_path_ns + "time_horizon_for_front_object");
+    p.ego_predicted_path_params.time_horizon_for_rear_object =
+      getOrDeclareParameter<double>(node, ego_path_ns + "time_horizon_for_rear_object");
+    p.ego_predicted_path_params.time_resolution =
+      getOrDeclareParameter<double>(node, ego_path_ns + "time_resolution");
+    p.ego_predicted_path_params.delay_until_departure =
+      getOrDeclareParameter<double>(node, ego_path_ns + "delay_until_departure");
+  }
+  // ObjectFilteringParams
+  const std::string obj_filter_ns = base_ns + "target_filtering.";
+  {
+    p.objects_filtering_params.safety_check_time_horizon =
+      getOrDeclareParameter<double>(node, obj_filter_ns + "safety_check_time_horizon");
+    p.objects_filtering_params.safety_check_time_resolution =
+      getOrDeclareParameter<double>(node, obj_filter_ns + "safety_check_time_resolution");
+    p.objects_filtering_params.object_check_forward_distance =
+      getOrDeclareParameter<double>(node, obj_filter_ns + "object_check_forward_distance");
+    p.objects_filtering_params.object_check_backward_distance =
+      getOrDeclareParameter<double>(node, obj_filter_ns + "object_check_backward_distance");
+    p.objects_filtering_params.ignore_object_velocity_threshold =
+      getOrDeclareParameter<double>(node, obj_filter_ns + "ignore_object_velocity_threshold");
+    p.objects_filtering_params.include_opposite_lane =
+      getOrDeclareParameter<bool>(node, obj_filter_ns + "include_opposite_lane");
+    p.objects_filtering_params.invert_opposite_lane =
+      getOrDeclareParameter<bool>(node, obj_filter_ns + "invert_opposite_lane");
+    p.objects_filtering_params.check_all_predicted_path =
+      getOrDeclareParameter<bool>(node, obj_filter_ns + "check_all_predicted_path");
+    p.objects_filtering_params.use_all_predicted_path =
+      getOrDeclareParameter<bool>(node, obj_filter_ns + "use_all_predicted_path");
+    p.objects_filtering_params.use_predicted_path_outside_lanelet =
+      getOrDeclareParameter<bool>(node, obj_filter_ns + "use_predicted_path_outside_lanelet");
+  }
+  // ObjectTypesToCheck
+  {
+    const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
+    p.objects_filtering_params.object_types_to_check.check_car =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_car");
+    p.objects_filtering_params.object_types_to_check.check_truck =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_truck");
+    p.objects_filtering_params.object_types_to_check.check_bus =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_bus");
+    p.objects_filtering_params.object_types_to_check.check_trailer =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_trailer");
+    p.objects_filtering_params.object_types_to_check.check_unknown =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_unknown");
+    p.objects_filtering_params.object_types_to_check.check_bicycle =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_bicycle");
+    p.objects_filtering_params.object_types_to_check.check_motorcycle =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_motorcycle");
+    p.objects_filtering_params.object_types_to_check.check_pedestrian =
+      getOrDeclareParameter<bool>(node, obj_types_ns + "check_pedestrian");
+  }
+  // ObjectLaneConfiguration
+  {
+    const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
+    p.objects_filtering_params.object_lane_configuration.check_current_lane =
+      getOrDeclareParameter<bool>(node, obj_lane_ns + "check_current_lane");
+    p.objects_filtering_params.object_lane_configuration.check_right_lane =
+      getOrDeclareParameter<bool>(node, obj_lane_ns + "check_right_side_lane");
+    p.objects_filtering_params.object_lane_configuration.check_left_lane =
+      getOrDeclareParameter<bool>(node, obj_lane_ns + "check_left_side_lane");
+    p.objects_filtering_params.object_lane_configuration.check_shoulder_lane =
+      getOrDeclareParameter<bool>(node, obj_lane_ns + "check_shoulder_lane");
+    p.objects_filtering_params.object_lane_configuration.check_other_lane =
+      getOrDeclareParameter<bool>(node, obj_lane_ns + "check_other_lane");
+  }
+  // SafetyCheckParams
+  const std::string safety_check_ns = base_ns + "safety_check_params.";
+  {
+    p.safety_check_params.enable_safety_check =
+      getOrDeclareParameter<bool>(node, safety_check_ns + "enable_safety_check");
+    p.safety_check_params.hysteresis_factor_expand_rate =
+      getOrDeclareParameter<double>(node, safety_check_ns + "hysteresis_factor_expand_rate");
+    p.safety_check_params.backward_path_length =
+      getOrDeclareParameter<double>(node, safety_check_ns + "backward_path_length");
+    p.safety_check_params.forward_path_length =
+      getOrDeclareParameter<double>(node, safety_check_ns + "forward_path_length");
+    p.safety_check_params.publish_debug_marker =
+      getOrDeclareParameter<bool>(node, safety_check_ns + "publish_debug_marker");
+    p.safety_check_params.collision_check_yaw_diff_threshold =
+      getOrDeclareParameter<double>(node, safety_check_ns + "collision_check_yaw_diff_threshold");
+  }
+  // RSSparams
+  {
+    const std::string rss_ns = safety_check_ns + "rss_params.";
+    p.safety_check_params.rss_params.rear_vehicle_reaction_time =
+      getOrDeclareParameter<double>(node, rss_ns + "rear_vehicle_reaction_time");
+    p.safety_check_params.rss_params.rear_vehicle_safety_time_margin =
+      getOrDeclareParameter<double>(node, rss_ns + "rear_vehicle_safety_time_margin");
+    p.safety_check_params.rss_params.lateral_distance_max_threshold =
+      getOrDeclareParameter<double>(node, rss_ns + "lateral_distance_max_threshold");
+    p.safety_check_params.rss_params.longitudinal_distance_min_threshold =
+      getOrDeclareParameter<double>(node, rss_ns + "longitudinal_distance_min_threshold");
+    p.safety_check_params.rss_params.longitudinal_velocity_delta_time =
+      getOrDeclareParameter<double>(node, rss_ns + "longitudinal_velocity_delta_time");
+    p.safety_check_params.rss_params.extended_polygon_policy =
+      getOrDeclareParameter<std::string>(node, rss_ns + "extended_polygon_policy");
+  }
+  // surround moving obstacle check
+  {
+    const std::string surround_moving_obstacle_check_ns =
+      "start_planner.surround_moving_obstacle_check.";
+    p.search_radius =
+      getOrDeclareParameter<double>(node, surround_moving_obstacle_check_ns + "search_radius");
+    p.th_moving_obstacle_velocity = getOrDeclareParameter<double>(
+      node, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity");
+    // ObjectTypesToCheck
+    {
+      const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
+      p.surround_moving_obstacles_type_to_check.check_car =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_car");
+      p.surround_moving_obstacles_type_to_check.check_truck =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_truck");
+      p.surround_moving_obstacles_type_to_check.check_bus =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_bus");
+      p.surround_moving_obstacles_type_to_check.check_trailer =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_trailer");
+      p.surround_moving_obstacles_type_to_check.check_unknown =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_unknown");
+      p.surround_moving_obstacles_type_to_check.check_bicycle =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_bicycle");
+      p.surround_moving_obstacles_type_to_check.check_motorcycle =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_motorcycle");
+      p.surround_moving_obstacles_type_to_check.check_pedestrian =
+        getOrDeclareParameter<bool>(node, obj_types_ns + "check_pedestrian");
+    }
+  }
+
+  // debug
+  {
+    const std::string debug_ns = "start_planner.debug.";
+    p.print_debug_info = getOrDeclareParameter<bool>(node, debug_ns + "print_debug_info");
+  }
+
+  return p;
+}
+}  // namespace autoware::behavior_path_planner
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
index f5739ce322248..9ea51d56ee1cc 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
@@ -29,318 +29,18 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   // init manager interface
   initInterface(node, {""});
 
-  StartPlannerParameters p;
-
-  {
-    const std::string ns = "start_planner.";
-
-    p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
-    p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
-    p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
-    p.prepare_time_before_start = node->declare_parameter<double>(ns + "prepare_time_before_start");
-    p.th_distance_to_middle_of_the_road =
-      node->declare_parameter<double>(ns + "th_distance_to_middle_of_the_road");
-    p.skip_rear_vehicle_check = node->declare_parameter<bool>(ns + "skip_rear_vehicle_check");
-    p.extra_width_margin_for_rear_obstacle =
-      node->declare_parameter<double>(ns + "extra_width_margin_for_rear_obstacle");
-    p.collision_check_margins =
-      node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
-    p.collision_check_margin_from_front_object =
-      node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
-    p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
-    p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
-    // shift pull out
-    p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
-    p.check_shift_path_lane_departure =
-      node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
-    p.allow_check_shift_path_lane_departure_override =
-      node->declare_parameter<bool>(ns + "allow_check_shift_path_lane_departure_override");
-    p.shift_collision_check_distance_from_end =
-      node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
-    p.minimum_shift_pull_out_distance =
-      node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
-    p.lateral_acceleration_sampling_num =
-      node->declare_parameter<int>(ns + "lateral_acceleration_sampling_num");
-    p.lateral_jerk = node->declare_parameter<double>(ns + "lateral_jerk");
-    p.maximum_lateral_acc = node->declare_parameter<double>(ns + "maximum_lateral_acc");
-    p.minimum_lateral_acc = node->declare_parameter<double>(ns + "minimum_lateral_acc");
-    p.maximum_curvature = node->declare_parameter<double>(ns + "maximum_curvature");
-    p.end_pose_curvature_threshold =
-      node->declare_parameter<double>(ns + "end_pose_curvature_threshold");
-    p.maximum_longitudinal_deviation =
-      node->declare_parameter<double>(ns + "maximum_longitudinal_deviation");
-    // geometric pull out
-    p.enable_geometric_pull_out = node->declare_parameter<bool>(ns + "enable_geometric_pull_out");
-    p.geometric_collision_check_distance_from_end =
-      node->declare_parameter<double>(ns + "geometric_collision_check_distance_from_end");
-    p.divide_pull_out_path = node->declare_parameter<bool>(ns + "divide_pull_out_path");
-    p.parallel_parking_parameters.pull_out_velocity =
-      node->declare_parameter<double>(ns + "geometric_pull_out_velocity");
-    p.parallel_parking_parameters.pull_out_arc_path_interval =
-      node->declare_parameter<double>(ns + "arc_path_interval");
-    p.parallel_parking_parameters.pull_out_lane_departure_margin =
-      node->declare_parameter<double>(ns + "lane_departure_margin");
-    p.lane_departure_check_expansion_margin =
-      node->declare_parameter<double>(ns + "lane_departure_check_expansion_margin");
-    p.parallel_parking_parameters.pull_out_max_steer_angle =
-      node->declare_parameter<double>(ns + "pull_out_max_steer_angle");  // 15deg
-    p.parallel_parking_parameters.center_line_path_interval =
-      p.center_line_path_interval;  // for geometric parallel parking
-    // search start pose backward
-    p.search_priority = node->declare_parameter<std::string>(
-      ns + "search_priority");  // "efficient_path" or "short_back_distance"
-    p.enable_back = node->declare_parameter<bool>(ns + "enable_back");
-    p.backward_velocity = node->declare_parameter<double>(ns + "backward_velocity");
-    p.max_back_distance = node->declare_parameter<double>(ns + "max_back_distance");
-    p.backward_search_resolution =
-      node->declare_parameter<double>(ns + "backward_search_resolution");
-    p.backward_path_update_duration =
-      node->declare_parameter<double>(ns + "backward_path_update_duration");
-    p.ignore_distance_from_lane_end =
-      node->declare_parameter<double>(ns + "ignore_distance_from_lane_end");
-    // stop condition
-    p.maximum_deceleration_for_stop =
-      node->declare_parameter<double>(ns + "stop_condition.maximum_deceleration_for_stop");
-    p.maximum_jerk_for_stop =
-      node->declare_parameter<double>(ns + "stop_condition.maximum_jerk_for_stop");
-  }
-  {
-    const std::string ns = "start_planner.object_types_to_check_for_path_generation.";
-    p.object_types_to_check_for_path_generation.check_car =
-      node->declare_parameter<bool>(ns + "check_car");
-    p.object_types_to_check_for_path_generation.check_truck =
-      node->declare_parameter<bool>(ns + "check_truck");
-    p.object_types_to_check_for_path_generation.check_bus =
-      node->declare_parameter<bool>(ns + "check_bus");
-    p.object_types_to_check_for_path_generation.check_trailer =
-      node->declare_parameter<bool>(ns + "check_trailer");
-    p.object_types_to_check_for_path_generation.check_unknown =
-      node->declare_parameter<bool>(ns + "check_unknown");
-    p.object_types_to_check_for_path_generation.check_bicycle =
-      node->declare_parameter<bool>(ns + "check_bicycle");
-    p.object_types_to_check_for_path_generation.check_motorcycle =
-      node->declare_parameter<bool>(ns + "check_motorcycle");
-    p.object_types_to_check_for_path_generation.check_pedestrian =
-      node->declare_parameter<bool>(ns + "check_pedestrian");
-  }
-  // freespace planner general params
-  {
-    const std::string ns = "start_planner.freespace_planner.";
-    p.enable_freespace_planner = node->declare_parameter<bool>(ns + "enable_freespace_planner");
-    p.freespace_planner_algorithm =
-      node->declare_parameter<std::string>(ns + "freespace_planner_algorithm");
-    p.end_pose_search_start_distance =
-      node->declare_parameter<double>(ns + "end_pose_search_start_distance");
-    p.end_pose_search_end_distance =
-      node->declare_parameter<double>(ns + "end_pose_search_end_distance");
-    p.end_pose_search_interval = node->declare_parameter<double>(ns + "end_pose_search_interval");
-    p.freespace_planner_velocity = node->declare_parameter<double>(ns + "velocity");
-    p.vehicle_shape_margin = node->declare_parameter<double>(ns + "vehicle_shape_margin");
-    p.freespace_planner_common_parameters.time_limit =
-      node->declare_parameter<double>(ns + "time_limit");
-    p.freespace_planner_common_parameters.max_turning_ratio =
-      node->declare_parameter<double>(ns + "max_turning_ratio");
-    p.freespace_planner_common_parameters.turning_steps =
-      node->declare_parameter<int>(ns + "turning_steps");
-  }
-  //  freespace planner search config
-  {
-    const std::string ns = "start_planner.freespace_planner.search_configs.";
-    p.freespace_planner_common_parameters.theta_size =
-      node->declare_parameter<int>(ns + "theta_size");
-    p.freespace_planner_common_parameters.angle_goal_range =
-      node->declare_parameter<double>(ns + "angle_goal_range");
-    p.freespace_planner_common_parameters.curve_weight =
-      node->declare_parameter<double>(ns + "curve_weight");
-    p.freespace_planner_common_parameters.reverse_weight =
-      node->declare_parameter<double>(ns + "reverse_weight");
-    p.freespace_planner_common_parameters.lateral_goal_range =
-      node->declare_parameter<double>(ns + "lateral_goal_range");
-    p.freespace_planner_common_parameters.longitudinal_goal_range =
-      node->declare_parameter<double>(ns + "longitudinal_goal_range");
-  }
-  //  freespace planner costmap configs
-  {
-    const std::string ns = "start_planner.freespace_planner.costmap_configs.";
-    p.freespace_planner_common_parameters.obstacle_threshold =
-      node->declare_parameter<int>(ns + "obstacle_threshold");
-  }
-  //  freespace planner astar
-  {
-    const std::string ns = "start_planner.freespace_planner.astar.";
-    p.astar_parameters.search_method = node->declare_parameter<std::string>(ns + "search_method");
-    p.astar_parameters.only_behind_solutions =
-      node->declare_parameter<bool>(ns + "only_behind_solutions");
-    p.astar_parameters.use_back = node->declare_parameter<bool>(ns + "use_back");
-    p.astar_parameters.distance_heuristic_weight =
-      node->declare_parameter<double>(ns + "distance_heuristic_weight");
-  }
-  //   freespace planner rrtstar
-  {
-    const std::string ns = "start_planner.freespace_planner.rrtstar.";
-    p.rrt_star_parameters.enable_update = node->declare_parameter<bool>(ns + "enable_update");
-    p.rrt_star_parameters.use_informed_sampling =
-      node->declare_parameter<bool>(ns + "use_informed_sampling");
-    p.rrt_star_parameters.max_planning_time =
-      node->declare_parameter<double>(ns + "max_planning_time");
-    p.rrt_star_parameters.neighbor_radius = node->declare_parameter<double>(ns + "neighbor_radius");
-    p.rrt_star_parameters.margin = node->declare_parameter<double>(ns + "margin");
-  }
-
-  const std::string base_ns = "start_planner.path_safety_check.";
-  // EgoPredictedPath
-  {
-    const std::string ego_path_ns = base_ns + "ego_predicted_path.";
-    p.ego_predicted_path_params.min_velocity =
-      node->declare_parameter<double>(ego_path_ns + "min_velocity");
-    p.ego_predicted_path_params.acceleration =
-      node->declare_parameter<double>(ego_path_ns + "min_acceleration");
-    p.ego_predicted_path_params.time_horizon_for_front_object =
-      node->declare_parameter<double>(ego_path_ns + "time_horizon_for_front_object");
-    p.ego_predicted_path_params.time_horizon_for_rear_object =
-      node->declare_parameter<double>(ego_path_ns + "time_horizon_for_rear_object");
-    p.ego_predicted_path_params.time_resolution =
-      node->declare_parameter<double>(ego_path_ns + "time_resolution");
-    p.ego_predicted_path_params.delay_until_departure =
-      node->declare_parameter<double>(ego_path_ns + "delay_until_departure");
-  }
-  // ObjectFilteringParams
-  const std::string obj_filter_ns = base_ns + "target_filtering.";
-  {
-    p.objects_filtering_params.safety_check_time_horizon =
-      node->declare_parameter<double>(obj_filter_ns + "safety_check_time_horizon");
-    p.objects_filtering_params.safety_check_time_resolution =
-      node->declare_parameter<double>(obj_filter_ns + "safety_check_time_resolution");
-    p.objects_filtering_params.object_check_forward_distance =
-      node->declare_parameter<double>(obj_filter_ns + "object_check_forward_distance");
-    p.objects_filtering_params.object_check_backward_distance =
-      node->declare_parameter<double>(obj_filter_ns + "object_check_backward_distance");
-    p.objects_filtering_params.ignore_object_velocity_threshold =
-      node->declare_parameter<double>(obj_filter_ns + "ignore_object_velocity_threshold");
-    p.objects_filtering_params.include_opposite_lane =
-      node->declare_parameter<bool>(obj_filter_ns + "include_opposite_lane");
-    p.objects_filtering_params.invert_opposite_lane =
-      node->declare_parameter<bool>(obj_filter_ns + "invert_opposite_lane");
-    p.objects_filtering_params.check_all_predicted_path =
-      node->declare_parameter<bool>(obj_filter_ns + "check_all_predicted_path");
-    p.objects_filtering_params.use_all_predicted_path =
-      node->declare_parameter<bool>(obj_filter_ns + "use_all_predicted_path");
-    p.objects_filtering_params.use_predicted_path_outside_lanelet =
-      node->declare_parameter<bool>(obj_filter_ns + "use_predicted_path_outside_lanelet");
-  }
-  // ObjectTypesToCheck
-  {
-    const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
-    p.objects_filtering_params.object_types_to_check.check_car =
-      node->declare_parameter<bool>(obj_types_ns + "check_car");
-    p.objects_filtering_params.object_types_to_check.check_truck =
-      node->declare_parameter<bool>(obj_types_ns + "check_truck");
-    p.objects_filtering_params.object_types_to_check.check_bus =
-      node->declare_parameter<bool>(obj_types_ns + "check_bus");
-    p.objects_filtering_params.object_types_to_check.check_trailer =
-      node->declare_parameter<bool>(obj_types_ns + "check_trailer");
-    p.objects_filtering_params.object_types_to_check.check_unknown =
-      node->declare_parameter<bool>(obj_types_ns + "check_unknown");
-    p.objects_filtering_params.object_types_to_check.check_bicycle =
-      node->declare_parameter<bool>(obj_types_ns + "check_bicycle");
-    p.objects_filtering_params.object_types_to_check.check_motorcycle =
-      node->declare_parameter<bool>(obj_types_ns + "check_motorcycle");
-    p.objects_filtering_params.object_types_to_check.check_pedestrian =
-      node->declare_parameter<bool>(obj_types_ns + "check_pedestrian");
-  }
-  // ObjectLaneConfiguration
-  {
-    const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
-    p.objects_filtering_params.object_lane_configuration.check_current_lane =
-      node->declare_parameter<bool>(obj_lane_ns + "check_current_lane");
-    p.objects_filtering_params.object_lane_configuration.check_right_lane =
-      node->declare_parameter<bool>(obj_lane_ns + "check_right_side_lane");
-    p.objects_filtering_params.object_lane_configuration.check_left_lane =
-      node->declare_parameter<bool>(obj_lane_ns + "check_left_side_lane");
-    p.objects_filtering_params.object_lane_configuration.check_shoulder_lane =
-      node->declare_parameter<bool>(obj_lane_ns + "check_shoulder_lane");
-    p.objects_filtering_params.object_lane_configuration.check_other_lane =
-      node->declare_parameter<bool>(obj_lane_ns + "check_other_lane");
-  }
-  // SafetyCheckParams
-  const std::string safety_check_ns = base_ns + "safety_check_params.";
-  {
-    p.safety_check_params.enable_safety_check =
-      node->declare_parameter<bool>(safety_check_ns + "enable_safety_check");
-    p.safety_check_params.hysteresis_factor_expand_rate =
-      node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
-    p.safety_check_params.backward_path_length =
-      node->declare_parameter<double>(safety_check_ns + "backward_path_length");
-    p.safety_check_params.forward_path_length =
-      node->declare_parameter<double>(safety_check_ns + "forward_path_length");
-    p.safety_check_params.publish_debug_marker =
-      node->declare_parameter<bool>(safety_check_ns + "publish_debug_marker");
-    p.safety_check_params.collision_check_yaw_diff_threshold =
-      node->declare_parameter<double>(safety_check_ns + "collision_check_yaw_diff_threshold");
-  }
-  // RSSparams
-  {
-    const std::string rss_ns = safety_check_ns + "rss_params.";
-    p.safety_check_params.rss_params.rear_vehicle_reaction_time =
-      node->declare_parameter<double>(rss_ns + "rear_vehicle_reaction_time");
-    p.safety_check_params.rss_params.rear_vehicle_safety_time_margin =
-      node->declare_parameter<double>(rss_ns + "rear_vehicle_safety_time_margin");
-    p.safety_check_params.rss_params.lateral_distance_max_threshold =
-      node->declare_parameter<double>(rss_ns + "lateral_distance_max_threshold");
-    p.safety_check_params.rss_params.longitudinal_distance_min_threshold =
-      node->declare_parameter<double>(rss_ns + "longitudinal_distance_min_threshold");
-    p.safety_check_params.rss_params.longitudinal_velocity_delta_time =
-      node->declare_parameter<double>(rss_ns + "longitudinal_velocity_delta_time");
-    p.safety_check_params.rss_params.extended_polygon_policy =
-      node->declare_parameter<std::string>(rss_ns + "extended_polygon_policy");
-  }
-  // surround moving obstacle check
-  {
-    const std::string surround_moving_obstacle_check_ns =
-      "start_planner.surround_moving_obstacle_check.";
-    p.search_radius =
-      node->declare_parameter<double>(surround_moving_obstacle_check_ns + "search_radius");
-    p.th_moving_obstacle_velocity = node->declare_parameter<double>(
-      surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity");
-    // ObjectTypesToCheck
-    {
-      const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
-      p.surround_moving_obstacles_type_to_check.check_car =
-        node->declare_parameter<bool>(obj_types_ns + "check_car");
-      p.surround_moving_obstacles_type_to_check.check_truck =
-        node->declare_parameter<bool>(obj_types_ns + "check_truck");
-      p.surround_moving_obstacles_type_to_check.check_bus =
-        node->declare_parameter<bool>(obj_types_ns + "check_bus");
-      p.surround_moving_obstacles_type_to_check.check_trailer =
-        node->declare_parameter<bool>(obj_types_ns + "check_trailer");
-      p.surround_moving_obstacles_type_to_check.check_unknown =
-        node->declare_parameter<bool>(obj_types_ns + "check_unknown");
-      p.surround_moving_obstacles_type_to_check.check_bicycle =
-        node->declare_parameter<bool>(obj_types_ns + "check_bicycle");
-      p.surround_moving_obstacles_type_to_check.check_motorcycle =
-        node->declare_parameter<bool>(obj_types_ns + "check_motorcycle");
-      p.surround_moving_obstacles_type_to_check.check_pedestrian =
-        node->declare_parameter<bool>(obj_types_ns + "check_pedestrian");
-    }
-  }
-
-  // debug
-  {
-    const std::string debug_ns = "start_planner.debug.";
-    p.print_debug_info = node->declare_parameter<bool>(debug_ns + "print_debug_info");
-  }
-
+  StartPlannerParameters parameters = StartPlannerParameters::init(*node);
   // validation of parameters
-  if (p.lateral_acceleration_sampling_num < 1) {
+  if (parameters.lateral_acceleration_sampling_num < 1) {
     RCLCPP_FATAL_STREAM(
       node->get_logger().get_child(name()),
       "lateral_acceleration_sampling_num must be positive integer. Given parameter: "
-        << p.lateral_acceleration_sampling_num << std::endl
+        << parameters.lateral_acceleration_sampling_num << std::endl
         << "Terminating the program...");
     exit(EXIT_FAILURE);
   }
 
-  parameters_ = std::make_shared<StartPlannerParameters>(p);
+  parameters_ = std::make_shared<StartPlannerParameters>(parameters);
 }
 
 void StartPlannerModuleManager::updateModuleParams(

From a5aeb82f54a945ae249f8305b0d23f1b790f1a54 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Tue, 24 Dec 2024 16:12:19 +0900
Subject: [PATCH 02/13] fix(diagnostic_graph_aggregator): remove unused message
 alias (#8722)

feat(diagnostic_graph_aggregator): remove unused message alias
---
 system/diagnostic_graph_aggregator/test/src/test2.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/system/diagnostic_graph_aggregator/test/src/test2.cpp b/system/diagnostic_graph_aggregator/test/src/test2.cpp
index 4a0199a89f37e..169e3017f8cd4 100644
--- a/system/diagnostic_graph_aggregator/test/src/test2.cpp
+++ b/system/diagnostic_graph_aggregator/test/src/test2.cpp
@@ -29,7 +29,6 @@ using namespace diagnostic_graph_aggregator;  // NOLINT(build/namespaces)
 
 using diagnostic_msgs::msg::DiagnosticArray;
 using diagnostic_msgs::msg::DiagnosticStatus;
-using tier4_system_msgs::msg::DiagnosticGraph;
 
 constexpr auto OK = DiagnosticStatus::OK;
 constexpr auto WARN = DiagnosticStatus::WARN;

From 3dc9605d9354260ae540d161094044c56f6c98f0 Mon Sep 17 00:00:00 2001
From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com>
Date: Tue, 24 Dec 2024 16:45:47 +0900
Subject: [PATCH 03/13] feat(autoware_traffic_light_arbiter): add current time
 validation (#9747)

* add current time validation

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

* style(pre-commit): autofix

* change ros parameter name

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

* style(pre-commit): autofix

* add validation with absolute function

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

* add timestamp of topic in test

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

* fix ci error

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

---------

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../autoware_traffic_light_arbiter/README.md  |  5 +++--
 .../config/traffic_light_arbiter.param.yaml   |  1 +
 .../traffic_light_arbiter.hpp                 |  1 +
 .../src/traffic_light_arbiter.cpp             | 19 +++++++++++++++++--
 .../test/test_node.cpp                        | 10 ++++++++++
 5 files changed, 32 insertions(+), 4 deletions(-)

diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md
index 619154e1e183b..f27afab62818e 100644
--- a/perception/autoware_traffic_light_arbiter/README.md
+++ b/perception/autoware_traffic_light_arbiter/README.md
@@ -43,7 +43,8 @@ The table below outlines how the matching process determines the output based on
 
 | Name                        | Type   | Default Value | Description                                                                                                                                                                    |
 | --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `external_time_tolerance`   | double | 5.0           | The duration in seconds an external message is considered valid for merging                                                                                                    |
-| `perception_time_tolerance` | double | 1.0           | The duration in seconds a perception message is considered valid for merging                                                                                                   |
+| `external_delay_tolerance`  | double | 5.0           | The duration in seconds an external message is considered valid for merging in comparison with current time                                                                    |
+| `external_time_tolerance`   | double | 5.0           | The duration in seconds an external message is considered valid for merging in comparison with a timestamp of perception message                                               |
+| `perception_time_tolerance` | double | 1.0           | The duration in seconds a perception message is considered valid for merging in comparison with a timestamp of external message                                                |
 | `external_priority`         | bool   | false         | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria                                               |
 | `enable_signal_matching`    | bool   | false         | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical |
diff --git a/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml
index dfe12ff352f16..36e1b8593bebc 100644
--- a/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml
+++ b/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml
@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    external_delay_tolerance: 5.0
     external_time_tolerance: 5.0
     perception_time_tolerance: 1.0
     external_priority: false
diff --git a/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp
index ccd928d52b367..916bd04a6bdd0 100644
--- a/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp
+++ b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp
@@ -51,6 +51,7 @@ class TrafficLightArbiter : public rclcpp::Node
 
   std::unique_ptr<std::unordered_set<lanelet::Id>> map_regulatory_elements_set_;
 
+  double external_delay_tolerance_;
   double external_time_tolerance_;
   double perception_time_tolerance_;
   bool external_priority_;
diff --git a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp
index e71629fa5dd28..8ce002780813f 100644
--- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp
+++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp
@@ -20,6 +20,7 @@
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
+#include <algorithm>
 #include <map>
 #include <memory>
 #include <tuple>
@@ -70,6 +71,7 @@ namespace autoware
 TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options)
 : Node("traffic_light_arbiter", options)
 {
+  external_delay_tolerance_ = this->declare_parameter<double>("external_delay_tolerance", 5.0);
   external_time_tolerance_ = this->declare_parameter<double>("external_time_tolerance", 5.0);
   perception_time_tolerance_ = this->declare_parameter<double>("perception_time_tolerance", 1.0);
   external_priority_ = this->declare_parameter<bool>("external_priority", false);
@@ -119,7 +121,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP
   latest_perception_msg_ = *msg;
 
   if (
-    (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds() >
+    std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds()) >
     external_time_tolerance_) {
     latest_external_msg_.traffic_light_groups.clear();
   }
@@ -129,10 +131,16 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP
 
 void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg)
 {
+  if (std::abs((this->now() - rclcpp::Time(msg->stamp)).seconds()) > external_delay_tolerance_) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Received outdated V2X traffic signal messages");
+    return;
+  }
+
   latest_external_msg_ = *msg;
 
   if (
-    (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() >
+    std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds()) >
     perception_time_tolerance_) {
     latest_perception_msg_.traffic_light_groups.clear();
   }
@@ -229,6 +237,13 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim
   }
 
   pub_->publish(output_signals_msg);
+
+  const auto latest_time =
+    std::max(rclcpp::Time(latest_perception_msg_.stamp), rclcpp::Time(latest_external_msg_.stamp));
+  if (rclcpp::Time(output_signals_msg.stamp) < latest_time) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Published traffic signal messages are not latest");
+  }
 }
 }  // namespace autoware
 
diff --git a/perception/autoware_traffic_light_arbiter/test/test_node.cpp b/perception/autoware_traffic_light_arbiter/test/test_node.cpp
index f993ad7cec84d..44b3ca7925fd0 100644
--- a/perception/autoware_traffic_light_arbiter/test/test_node.cpp
+++ b/perception/autoware_traffic_light_arbiter/test/test_node.cpp
@@ -196,6 +196,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyPerceptionMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  perception_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(
@@ -229,6 +232,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyExternalMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  external_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(
@@ -268,6 +274,10 @@ TEST(TrafficLightArbiterTest, testTrafficSignalBothMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  external_msg.stamp = test_target_node->now();
+  perception_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(

From e60245b2b47d69e795e399a04e1f447a2cdc7042 Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Tue, 24 Dec 2024 17:06:03 +0900
Subject: [PATCH 04/13] fix(autoware_pointcloud_preprocessor): fix image
 display in distortion corrector (#9761)

fix: fix image display

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../docs/distortion-corrector.md                    | 13 +++----------
 1 file changed, 3 insertions(+), 10 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
index 75cdccc4453ba..032798e9db125 100644
--- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
+++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md
@@ -55,16 +55,9 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
   - `hesai`: (x: 90 degrees, y: 0 degrees)
   - `others`: (x: 0 degrees, y: 90 degrees) and (x: 270 degrees, y: 0 degrees)
 
-<table>
-  <tr>
-    <td><img src="./image/velodyne.drawio.png" alt="velodyne azimuth coordinate"></td>
-    <td><img src="./image/hesai.drawio.png" alt="hesai azimuth coordinate"></td>
-   </tr>
-   <tr>
-    <td><p style="text-align: center;">Velodyne azimuth coordinate</p></td>
-    <td><p style="text-align: center;">Hesai azimuth coordinate</p></td>
-  </tr>
-</table>
+| ![Velodyne Azimuth Coordinate](./image/velodyne.drawio.png) | ![Hesai Azimuth Coordinate](./image/hesai.drawio.png) |
+| :---------------------------------------------------------: | :---------------------------------------------------: |
+|               **Velodyne azimuth coordinate**               |             **Hesai azimuth coordinate**              |
 
 ## References/External links
 

From f3a258ca04dc0dde9fbb629d05c8c3e12c38e24b Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Date: Tue, 24 Dec 2024 17:07:43 +0900
Subject: [PATCH 05/13] fix(lane_change): add metrics to valid paths
 visualization (#9737)

* fix(lane_change): add metrics to valid paths visualization

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix cpp-check error

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../utils/markers.hpp                         |  2 +-
 .../src/utils/markers.cpp                     | 60 +++++++++++++++----
 2 files changed, 51 insertions(+), 11 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp
index 4c2712f053b13..059db8e38300f 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp
@@ -35,7 +35,7 @@ using autoware::behavior_path_planner::lane_change::Debug;
 using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
 MarkerArray showAllValidLaneChangePath(
-  const std::vector<LaneChangePath> & lanes, std::string && ns);
+  const std::vector<LaneChangePath> & lane_change_paths, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp
index c7514fea01535..3277b8f031486 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp
@@ -38,34 +38,74 @@ using autoware::universe_utils::createDefaultMarker;
 using autoware::universe_utils::createMarkerScale;
 using geometry_msgs::msg::Point;
 
-MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes, std::string && ns)
+MarkerArray showAllValidLaneChangePath(
+  const std::vector<LaneChangePath> & lane_change_paths, std::string && ns)
 {
-  if (lanes.empty()) {
+  if (lane_change_paths.empty()) {
     return MarkerArray{};
   }
 
   MarkerArray marker_array;
-  int32_t id{0};
   const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
 
   const auto colors = colors::colors_list();
-  const auto loop_size = std::min(lanes.size(), colors.size());
+  const auto loop_size = std::min(lane_change_paths.size(), colors.size());
   marker_array.markers.reserve(loop_size);
 
+  const auto info_prep_to_string =
+    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
+    std::ostringstream ss_text;
+    ss_text << std::setprecision(3) << "vel: " << info.velocity.prepare
+            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.prepare
+            << "[m/s2] | t: " << info.duration.prepare << "[s] | L: " << info.length.prepare
+            << "[m]";
+    return ss_text.str();
+  };
+
+  const auto info_lc_to_string =
+    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
+    std::ostringstream ss_text;
+    ss_text << std::setprecision(3) << "vel: " << info.velocity.lane_changing
+            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.lane_changing
+            << "[m/s2] | lat_acc: " << info.lateral_acceleration
+            << "[m/s2] | t: " << info.duration.lane_changing
+            << "[s] | L: " << info.length.lane_changing << "[m]";
+    return ss_text.str();
+  };
+
   for (std::size_t idx = 0; idx < loop_size; ++idx) {
-    if (lanes.at(idx).path.points.empty()) {
+    int32_t id{0};
+    const auto & lc_path = lane_change_paths.at(idx);
+    if (lc_path.path.points.empty()) {
       continue;
     }
-
+    std::string ns_with_idx = ns + "[" + std::to_string(idx) + "]";
     const auto & color = colors.at(idx);
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color);
-    marker.points.reserve(lanes.at(idx).path.points.size());
+    const auto & points = lc_path.path.points;
+    auto marker = createDefaultMarker(
+      "map", current_time, ns_with_idx, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0),
+      color);
+    marker.points.reserve(points.size());
 
-    for (const auto & point : lanes.at(idx).path.points) {
+    for (const auto & point : points) {
       marker.points.push_back(point.point.pose.position);
     }
 
+    auto text_marker = createDefaultMarker(
+      "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.1, 0.1, 0.8), colors::yellow());
+    const auto prep_idx = points.size() / 4;
+    text_marker.pose = points.at(prep_idx).point.pose;
+    text_marker.pose.position.z += 2.0;
+    text_marker.text = info_prep_to_string(lc_path.info);
+    marker_array.markers.push_back(text_marker);
+
+    const auto lc_idx = points.size() / 2;
+    text_marker.id = ++id;
+    text_marker.pose = points.at(lc_idx).point.pose;
+    text_marker.text = info_lc_to_string(lc_path.info);
+    marker_array.markers.push_back(text_marker);
+
     marker_array.markers.push_back(marker);
   }
   return marker_array;

From a499a0a0ed7933b6e4cca5a40a9ad9ad70a27c3c Mon Sep 17 00:00:00 2001
From: kobayu858 <129580202+kobayu858@users.noreply.github.com>
Date: Tue, 24 Dec 2024 17:23:30 +0900
Subject: [PATCH 06/13] fix(autoware_tensorrt_classifier): fix
 bugprone-exception-escape (#9732)

fix: bugprone-error

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>
---
 .../src/tensorrt_classifier.cpp                      | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp
index b24e4fe56e8b8..4aa008e42c966 100644
--- a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp
+++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp
@@ -193,9 +193,15 @@ TrtClassifier::TrtClassifier(
 
 TrtClassifier::~TrtClassifier()
 {
-  if (m_cuda) {
-    if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_));
-    if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_));
+  try {
+    if (m_cuda) {
+      if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_));
+      if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_));
+    }
+  } catch (const std::exception & e) {
+    std::cerr << "Exception in TrtClassifier destructor: " << e.what() << std::endl;
+  } catch (...) {
+    std::cerr << "Unknown exception in TrtClassifier destructor" << std::endl;
   }
 }
 

From 72bc2dcf8aaa79247b3aa6dab09dd900764c44fd Mon Sep 17 00:00:00 2001
From: kobayu858 <129580202+kobayu858@users.noreply.github.com>
Date: Tue, 24 Dec 2024 17:25:54 +0900
Subject: [PATCH 07/13] fix(autoware_tensorrt_yolox): fix
 bugprone-exception-escape (#9734)

* fix: bugprone-error

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

* fix: fmt

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

* fix: fmt

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

---------

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>
---
 .../src/tensorrt_yolox.cpp                    | 24 ++++++++++++-------
 1 file changed, 15 insertions(+), 9 deletions(-)

diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp
index 06540f2b7cd19..7e2e327bf6f5e 100644
--- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp
+++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp
@@ -341,16 +341,22 @@ TrtYoloX::TrtYoloX(
 
 TrtYoloX::~TrtYoloX()
 {
-  if (use_gpu_preprocess_) {
-    if (image_buf_h_) {
-      image_buf_h_.reset();
-    }
-    if (image_buf_d_) {
-      image_buf_d_.reset();
-    }
-    if (argmax_buf_d_) {
-      argmax_buf_d_.reset();
+  try {
+    if (use_gpu_preprocess_) {
+      if (image_buf_h_) {
+        image_buf_h_.reset();
+      }
+      if (image_buf_d_) {
+        image_buf_d_.reset();
+      }
+      if (argmax_buf_d_) {
+        argmax_buf_d_.reset();
+      }
     }
+  } catch (const std::exception & e) {
+    std::cerr << "Exception in TrtYoloX destructor: " << e.what() << std::endl;
+  } catch (...) {
+    std::cerr << "Unknown exception in TrtYoloX destructor" << std::endl;
   }
 }
 

From 3402b6a7c80326967b9b5a61f079d28cf7d26232 Mon Sep 17 00:00:00 2001
From: kobayu858 <129580202+kobayu858@users.noreply.github.com>
Date: Tue, 24 Dec 2024 17:29:16 +0900
Subject: [PATCH 08/13] 
 fix(autoware_behavior_path_static_obstacle_avoidance_module): fix
 bugprone-branch-clone (#9701)

fix: bugprone-error

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>
---
 .../src/scene.cpp                                           | 6 +-----
 .../src/shift_line_generator.cpp                            | 4 +---
 2 files changed, 2 insertions(+), 8 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
index be4ce4a125c68..48729c9c4fa0c 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
@@ -233,11 +233,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
 
   std::for_each(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
-      if (!not_use_adjacent_lane) {
-        data.drivable_lanes.push_back(
-          utils::static_obstacle_avoidance::generateExpandedDrivableLanes(
-            lanelet, planner_data_, parameters_));
-      } else if (red_signal_lane_itr->id() != lanelet.id()) {
+      if (!not_use_adjacent_lane || red_signal_lane_itr->id() != lanelet.id()) {
         data.drivable_lanes.push_back(
           utils::static_obstacle_avoidance::generateExpandedDrivableLanes(
             lanelet, planner_data_, parameters_));
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
index 744af35641a59..2aba986ab680d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
@@ -264,9 +264,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
     if (!o.avoid_margin.has_value()) {
-      if (!data.red_signal_lane.has_value()) {
-        o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
-      } else if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) {
+      if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) {
         o.info = ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY;
       } else {
         o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;

From c476676c39612ebc36a20072415125571598abb2 Mon Sep 17 00:00:00 2001
From: kobayu858 <129580202+kobayu858@users.noreply.github.com>
Date: Tue, 24 Dec 2024 17:30:25 +0900
Subject: [PATCH 09/13] fix(autoware_default_adapi): fix bugprone-branch-clone
 (#9726)

fix: bugprone-error

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>
---
 system/autoware_default_adapi/src/utils/route_conversion.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/system/autoware_default_adapi/src/utils/route_conversion.cpp b/system/autoware_default_adapi/src/utils/route_conversion.cpp
index e8a748d52edee..25e8204770f8a 100644
--- a/system/autoware_default_adapi/src/utils/route_conversion.cpp
+++ b/system/autoware_default_adapi/src/utils/route_conversion.cpp
@@ -115,13 +115,13 @@ ExternalState convert_state(const InternalState & internal)
   const auto convert = [](InternalState::_state_type state) {
     switch(state) {
       // TODO(Takagi, Isamu): Add adapi state.
-      case InternalState::INITIALIZING: return ExternalState::UNSET;
+      case InternalState::INITIALIZING: return ExternalState::UNSET; // NOLINT
       case InternalState::UNSET:        return ExternalState::UNSET;
       case InternalState::ROUTING:      return ExternalState::UNSET;
       case InternalState::SET:          return ExternalState::SET;
       case InternalState::REROUTING:    return ExternalState::CHANGING;
       case InternalState::ARRIVED:      return ExternalState::ARRIVED;
-      case InternalState::ABORTED:      return ExternalState::SET;
+      case InternalState::ABORTED:      return ExternalState::SET;  // NOLINT
       case InternalState::INTERRUPTED:  return ExternalState::SET;
       default:                          return ExternalState::UNKNOWN;
     }

From 46586531056e780748a545c362403b36c9307b3e Mon Sep 17 00:00:00 2001
From: kobayu858 <129580202+kobayu858@users.noreply.github.com>
Date: Tue, 24 Dec 2024 19:21:25 +0900
Subject: [PATCH 10/13] fix(autoware_route_handler): fix
 bugprone-exception-escape (#9738)

* fix: bugprone-error

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

* fix: add include

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

---------

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>
---
 .../src/route_handler.cpp                     | 97 +++++++++++--------
 1 file changed, 57 insertions(+), 40 deletions(-)

diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp
index 01e52d85f01c3..89930fd4a23a9 100644
--- a/planning/autoware_route_handler/src/route_handler.cpp
+++ b/planning/autoware_route_handler/src/route_handler.cpp
@@ -36,6 +36,7 @@
 
 #include <algorithm>
 #include <functional>
+#include <iostream>
 #include <limits>
 #include <memory>
 #include <optional>
@@ -1163,32 +1164,40 @@ lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets(
   const bool & invert_opposite) const noexcept
 {
   lanelet::ConstLanelets linestring_shared;
-  auto lanelet_at_left = getLeftLanelet(lane);
-  auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane);
-  while (lanelet_at_left) {
-    linestring_shared.push_back(lanelet_at_left.value());
-    lanelet_at_left = getLeftLanelet(lanelet_at_left.value());
-    if (!lanelet_at_left) {
-      break;
+  try {
+    auto lanelet_at_left = getLeftLanelet(lane);
+    auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane);
+    while (lanelet_at_left) {
+      linestring_shared.push_back(lanelet_at_left.value());
+      lanelet_at_left = getLeftLanelet(lanelet_at_left.value());
+      if (!lanelet_at_left) {
+        break;
+      }
+      lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value());
     }
-    lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value());
-  }
 
-  if (!lanelet_at_left_opposite.empty() && include_opposite) {
-    if (invert_opposite) {
-      linestring_shared.push_back(lanelet_at_left_opposite.front().invert());
-    } else {
-      linestring_shared.push_back(lanelet_at_left_opposite.front());
-    }
-    auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front());
-    while (lanelet_at_right) {
+    if (!lanelet_at_left_opposite.empty() && include_opposite) {
       if (invert_opposite) {
-        linestring_shared.push_back(lanelet_at_right.value().invert());
+        linestring_shared.push_back(lanelet_at_left_opposite.front().invert());
       } else {
-        linestring_shared.push_back(lanelet_at_right.value());
+        linestring_shared.push_back(lanelet_at_left_opposite.front());
+      }
+      auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front());
+      while (lanelet_at_right) {
+        if (invert_opposite) {
+          linestring_shared.push_back(lanelet_at_right.value().invert());
+        } else {
+          linestring_shared.push_back(lanelet_at_right.value());
+        }
+        lanelet_at_right = getRightLanelet(lanelet_at_right.value());
       }
-      lanelet_at_right = getRightLanelet(lanelet_at_right.value());
     }
+  } catch (const std::exception & e) {
+    std::cerr << "Exception in getAllLeftSharedLinestringLanelets: " << e.what() << std::endl;
+    return {};
+  } catch (...) {
+    std::cerr << "Unknown exception in getAllLeftSharedLinestringLanelets" << std::endl;
+    return {};
   }
   return linestring_shared;
 }
@@ -1198,32 +1207,40 @@ lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets(
   const bool & invert_opposite) const noexcept
 {
   lanelet::ConstLanelets linestring_shared;
-  auto lanelet_at_right = getRightLanelet(lane);
-  auto lanelet_at_right_opposite = getRightOppositeLanelets(lane);
-  while (lanelet_at_right) {
-    linestring_shared.push_back(lanelet_at_right.value());
-    lanelet_at_right = getRightLanelet(lanelet_at_right.value());
-    if (!lanelet_at_right) {
-      break;
+  try {
+    auto lanelet_at_right = getRightLanelet(lane);
+    auto lanelet_at_right_opposite = getRightOppositeLanelets(lane);
+    while (lanelet_at_right) {
+      linestring_shared.push_back(lanelet_at_right.value());
+      lanelet_at_right = getRightLanelet(lanelet_at_right.value());
+      if (!lanelet_at_right) {
+        break;
+      }
+      lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value());
     }
-    lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value());
-  }
 
-  if (!lanelet_at_right_opposite.empty() && include_opposite) {
-    if (invert_opposite) {
-      linestring_shared.push_back(lanelet_at_right_opposite.front().invert());
-    } else {
-      linestring_shared.push_back(lanelet_at_right_opposite.front());
-    }
-    auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front());
-    while (lanelet_at_left) {
+    if (!lanelet_at_right_opposite.empty() && include_opposite) {
       if (invert_opposite) {
-        linestring_shared.push_back(lanelet_at_left.value().invert());
+        linestring_shared.push_back(lanelet_at_right_opposite.front().invert());
       } else {
-        linestring_shared.push_back(lanelet_at_left.value());
+        linestring_shared.push_back(lanelet_at_right_opposite.front());
+      }
+      auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front());
+      while (lanelet_at_left) {
+        if (invert_opposite) {
+          linestring_shared.push_back(lanelet_at_left.value().invert());
+        } else {
+          linestring_shared.push_back(lanelet_at_left.value());
+        }
+        lanelet_at_left = getLeftLanelet(lanelet_at_left.value());
       }
-      lanelet_at_left = getLeftLanelet(lanelet_at_left.value());
     }
+  } catch (const std::exception & e) {
+    std::cerr << "Exception in getAllRightSharedLinestringLanelets: " << e.what() << std::endl;
+    return {};
+  } catch (...) {
+    std::cerr << "Unknown exception in getAllRightSharedLinestringLanelets" << std::endl;
+    return {};
   }
   return linestring_shared;
 }

From 7102a48c93ff9616e62722cb46761266900d220d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?=
 <xmfcx@users.noreply.github.com>
Date: Tue, 24 Dec 2024 13:31:06 +0300
Subject: [PATCH 11/13] ci: introduce build-test-tidy-pr (#9709)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: M. Fatih Cırıt <mfc@autoware.org>
---
 .../build-and-test-differential/action.yaml   | 111 ---------
 .github/labeler.yaml                          |   3 -
 .../build-and-test-differential.yaml          | 219 ++++++------------
 .github/workflows/build-test-tidy-pr.yaml     |  76 ++++++
 .../workflows/clang-tidy-differential.yaml    |  75 ++++++
 5 files changed, 227 insertions(+), 257 deletions(-)
 delete mode 100644 .github/actions/build-and-test-differential/action.yaml
 create mode 100644 .github/workflows/build-test-tidy-pr.yaml
 create mode 100644 .github/workflows/clang-tidy-differential.yaml

diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml
deleted file mode 100644
index 89893e9f55fb5..0000000000000
--- a/.github/actions/build-and-test-differential/action.yaml
+++ /dev/null
@@ -1,111 +0,0 @@
-name: build-and-test-differential
-description: ""
-
-inputs:
-  rosdistro:
-    description: ""
-    required: true
-  container:
-    description: ""
-    required: true
-  container-suffix:
-    description: ""
-    required: true
-  runner:
-    description: ""
-    required: true
-  build-depends-repos:
-    description: ""
-    required: true
-  build-pre-command:
-    description: ""
-    required: true
-  codecov-token:
-    description: ""
-    required: true
-
-runs:
-  using: composite
-  steps:
-    - name: Show disk space before the tasks
-      run: df -h
-      shell: bash
-
-    - name: Show machine specs
-      run: lscpu && free -h
-      shell: bash
-
-    - name: Remove exec_depend
-      uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
-
-    - name: Get modified packages
-      id: get-modified-packages
-      uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
-
-    - name: Create ccache directory
-      run: |
-        mkdir -p ${CCACHE_DIR}
-        du -sh ${CCACHE_DIR} && ccache -s
-      shell: bash
-
-    - name: Attempt to restore ccache
-      uses: actions/cache/restore@v4
-      with:
-        path: |
-          /root/.ccache
-        key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }}
-        restore-keys: |
-          ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-
-
-    - name: Show ccache stats before build and reset stats
-      run: |
-        du -sh ${CCACHE_DIR} && ccache -s
-        ccache --zero-stats
-      shell: bash
-
-    - name: Export CUDA state as a variable for adding to cache key
-      run: |
-        build_type_cuda_state=nocuda
-        if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then
-          build_type_cuda_state=cuda
-        fi
-        echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}"
-        echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state"
-      shell: bash
-
-    - name: Build
-      if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
-      uses: autowarefoundation/autoware-github-actions/colcon-build@v1
-      with:
-        rosdistro: ${{ inputs.rosdistro }}
-        target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-        build-depends-repos: ${{ inputs.build-depends-repos }}
-        cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }}
-        build-pre-command: ${{ inputs.build-pre-command }}
-
-    - name: Show ccache stats after build
-      run: du -sh ${CCACHE_DIR} && ccache -s
-      shell: bash
-
-    - name: Test
-      id: test
-      if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
-      uses: autowarefoundation/autoware-github-actions/colcon-test@v1
-      with:
-        rosdistro: ${{ inputs.rosdistro }}
-        target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-        build-depends-repos: ${{ inputs.build-depends-repos }}
-
-    - name: Upload coverage to CodeCov
-      if: ${{ steps.test.outputs.coverage-report-files != '' }}
-      uses: codecov/codecov-action@v4
-      with:
-        files: ${{ steps.test.outputs.coverage-report-files }}
-        fail_ci_if_error: false
-        verbose: true
-        flags: differential
-        token: ${{ inputs.codecov-token }}
-
-    - name: Show disk space after the tasks
-      run: df -h
-      shell: bash
diff --git a/.github/labeler.yaml b/.github/labeler.yaml
index 115f75197f41a..47f8737ebbf39 100644
--- a/.github/labeler.yaml
+++ b/.github/labeler.yaml
@@ -39,6 +39,3 @@
   - tools/**/*
 "component:vehicle":
   - vehicle/**/*
-"tag:require-cuda-build-and-test":
-  - perception/**/*
-  - sensing/**/*
diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml
index f62904b03c6e4..0925cb0f53930 100644
--- a/.github/workflows/build-and-test-differential.yaml
+++ b/.github/workflows/build-and-test-differential.yaml
@@ -1,57 +1,35 @@
 name: build-and-test-differential
 
 on:
-  pull_request:
-    types:
-      - opened
-      - synchronize
-      - reopened
-      - labeled
-
-concurrency:
-  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }}
-  cancel-in-progress: true
+  workflow_call:
+    inputs:
+      container:
+        required: true
+        type: string
+      runner:
+        default: ubuntu-24.04
+        required: false
+        type: string
+      rosdistro:
+        default: humble
+        required: false
+        type: string
+      container-suffix:
+        required: false
+        default: ""
+        type: string
+    secrets:
+      codecov-token:
+        required: true
 
 env:
   CC: /usr/lib/ccache/gcc
   CXX: /usr/lib/ccache/g++
 
 jobs:
-  make-sure-label-is-present:
-    uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1
-    with:
-      label: tag:run-build-and-test-differential
-
-  make-sure-require-cuda-label-is-present:
-    uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1
-    with:
-      label: tag:require-cuda-build-and-test
-
-  prepare-build-and-test-differential:
-    runs-on: ubuntu-latest
-    needs: [make-sure-label-is-present, make-sure-require-cuda-label-is-present]
-    outputs:
-      cuda_build: ${{ steps.check-if-cuda-build-is-required.outputs.cuda_build }}
-    steps:
-      - name: Check if cuda-build is required
-        id: check-if-cuda-build-is-required
-        run: |
-          if ${{ needs.make-sure-require-cuda-label-is-present.outputs.result == 'true' }}; then
-            echo "cuda-build is required"
-            echo "cuda_build=true" >> $GITHUB_OUTPUT
-          else
-            echo "cuda-build is not required"
-            echo "cuda_build=false" >> $GITHUB_OUTPUT
-          fi
-        shell: bash
-      - name: Fail if the tag:run-build-and-test-differential is missing
-        if: ${{ needs.make-sure-label-is-present.outputs.result != 'true' }}
-        run: exit 1
-
   build-and-test-differential:
-    runs-on: ubuntu-latest
-    container: ghcr.io/autowarefoundation/autoware:universe-devel
-    needs: prepare-build-and-test-differential
+    runs-on: ${{ inputs.runner }}
+    container: ${{ inputs.container }}${{ inputs.container-suffix }}
     steps:
       - name: Set PR fetch depth
         run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}"
@@ -63,61 +41,13 @@ jobs:
           ref: ${{ github.event.pull_request.head.sha }}
           fetch-depth: ${{ env.PR_FETCH_DEPTH }}
 
-      - name: Run build-and-test-differential action
-        uses: ./.github/actions/build-and-test-differential
-        with:
-          rosdistro: humble
-          container: ghcr.io/autowarefoundation/autoware:universe-devel
-          container-suffix: ""
-          runner: ubuntu-latest
-          build-depends-repos: build_depends.repos
-          build-pre-command: ""
-          codecov-token: ${{ secrets.CODECOV_TOKEN }}
-
-  build-and-test-differential-cuda:
-    runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large
-    container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda
-    needs: prepare-build-and-test-differential
-    if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'true' }}
-    steps:
-      - name: Set PR fetch depth
-        run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}"
-        shell: bash
-
-      - name: Checkout PR branch and all PR commits
-        uses: actions/checkout@v4
-        with:
-          ref: ${{ github.event.pull_request.head.sha }}
-          fetch-depth: ${{ env.PR_FETCH_DEPTH }}
-
-      - name: Run build-and-test-differential action
-        uses: ./.github/actions/build-and-test-differential
-        with:
-          rosdistro: humble
-          container: ghcr.io/autowarefoundation/autoware:universe-devel
-          container-suffix: -cuda
-          runner: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large
-          build-depends-repos: build_depends.repos
-          build-pre-command: taskset --cpu-list 0-5
-          codecov-token: ${{ secrets.CODECOV_TOKEN }}
-
-  clang-tidy-differential:
-    needs: [build-and-test-differential, prepare-build-and-test-differential]
-    if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'false' }}
-    runs-on: ubuntu-latest
-    container: ghcr.io/autowarefoundation/autoware:universe-devel
-    steps:
-      - name: Set PR fetch depth
-        run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}"
-
-      - name: Checkout PR branch and all PR commits
-        uses: actions/checkout@v4
-        with:
-          ref: ${{ github.event.pull_request.head.sha }}
-          fetch-depth: ${{ env.PR_FETCH_DEPTH }}
-
       - name: Show disk space before the tasks
         run: df -h
+        shell: bash
+
+      - name: Show machine specs
+        run: lscpu && free -h
+        shell: bash
 
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
@@ -126,66 +56,69 @@ jobs:
         id: get-modified-packages
         uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
 
-      - name: Get changed files (existing files only)
-        id: get-changed-files
+      - name: Create ccache directory
         run: |
-          echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT
+          mkdir -p ${CCACHE_DIR}
+          du -sh ${CCACHE_DIR} && ccache -s
         shell: bash
 
-      - name: Run clang-tidy
-        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
-        uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
+      - name: Attempt to restore ccache
+        uses: actions/cache/restore@v4
         with:
-          rosdistro: humble
-          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-          clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci
-          clang-tidy-ignore-path: .clang-tidy-ignore
-          build-depends-repos: build_depends.repos
-          cache-key-element: cuda
+          path: |
+            /root/.ccache
+          key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }}
+          restore-keys: |
+            ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-
 
-      - name: Show disk space after the tasks
-        run: df -h
+      - name: Show ccache stats before build and reset stats
+        run: |
+          du -sh ${CCACHE_DIR} && ccache -s
+          ccache --zero-stats
+        shell: bash
 
-  clang-tidy-differential-cuda:
-    needs: build-and-test-differential-cuda
-    runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large
-    container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda
-    steps:
-      - name: Set PR fetch depth
-        run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}"
+      - name: Export CUDA state as a variable for adding to cache key
+        run: |
+          build_type_cuda_state=nocuda
+          if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then
+            build_type_cuda_state=cuda
+          fi
+          echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}"
+          echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state"
+        shell: bash
 
-      - name: Checkout PR branch and all PR commits
-        uses: actions/checkout@v4
+      - name: Build
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
         with:
-          ref: ${{ github.event.pull_request.head.sha }}
-          fetch-depth: ${{ env.PR_FETCH_DEPTH }}
-
-      - name: Show disk space before the tasks
-        run: df -h
-
-      - name: Remove exec_depend
-        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
-
-      - name: Get modified packages
-        id: get-modified-packages
-        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+          rosdistro: ${{ inputs.rosdistro }}
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          build-depends-repos: build_depends.repos
+          cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }}
 
-      - name: Get changed files (existing files only)
-        id: get-changed-files
-        run: |
-          echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT
+      - name: Show ccache stats after build
+        run: du -sh ${CCACHE_DIR} && ccache -s
         shell: bash
 
-      - name: Run clang-tidy
-        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
-        uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
+      - name: Test
+        id: test
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
         with:
-          rosdistro: humble
+          rosdistro: ${{ inputs.rosdistro }}
           target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-          clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci
-          clang-tidy-ignore-path: .clang-tidy-ignore
           build-depends-repos: build_depends.repos
-          cache-key-element: cuda
+
+      - name: Upload coverage to CodeCov
+        if: ${{ steps.test.outputs.coverage-report-files != '' }}
+        uses: codecov/codecov-action@v4
+        with:
+          files: ${{ steps.test.outputs.coverage-report-files }}
+          fail_ci_if_error: false
+          verbose: true
+          flags: differential${{ inputs.container-suffix }}
+          token: ${{ secrets.codecov-token }}
 
       - name: Show disk space after the tasks
         run: df -h
+        shell: bash
diff --git a/.github/workflows/build-test-tidy-pr.yaml b/.github/workflows/build-test-tidy-pr.yaml
new file mode 100644
index 0000000000000..df2eff132eb05
--- /dev/null
+++ b/.github/workflows/build-test-tidy-pr.yaml
@@ -0,0 +1,76 @@
+name: build-test-tidy-pr
+
+on:
+  pull_request:
+    types:
+      - opened
+      - synchronize
+      - reopened
+      - labeled
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }}
+  cancel-in-progress: true
+
+jobs:
+  require-label:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/require-label.yaml@v1
+    with:
+      label: run:build-and-test-differential
+
+  check-if-cuda-job-is-needed:
+    needs: require-label
+    runs-on: ubuntu-latest
+    outputs:
+      cuda_job_is_needed: ${{ steps.check.outputs.any_changed }}
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Check if relevant files changed
+        id: check
+        uses: tj-actions/changed-files@v45
+        with:
+          files: |
+            perception/**
+            sensing/**
+
+      - name: Output result
+        run: |
+          echo "CUDA job needed: ${{ steps.check.outputs.any_changed }}"
+        shell: bash
+
+  build-and-test-differential:
+    needs:
+      - require-label
+    uses: ./.github/workflows/build-and-test-differential.yaml
+    with:
+      container: ghcr.io/autowarefoundation/autoware:universe-devel
+    secrets:
+      codecov-token: ${{ secrets.CODECOV_TOKEN }}
+
+  build-and-test-differential-cuda:
+    needs: check-if-cuda-job-is-needed
+    if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }}
+    uses: ./.github/workflows/build-and-test-differential.yaml
+    with:
+      container: ghcr.io/autowarefoundation/autoware:universe-devel
+      container-suffix: -cuda
+    secrets:
+      codecov-token: ${{ secrets.CODECOV_TOKEN }}
+
+  clang-tidy-differential:
+    needs:
+      - check-if-cuda-job-is-needed
+      - build-and-test-differential
+    if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' }}
+    uses: ./.github/workflows/clang-tidy-differential.yaml
+    with:
+      container: ghcr.io/autowarefoundation/autoware:universe-devel
+
+  clang-tidy-differential-cuda:
+    needs:
+      - build-and-test-differential-cuda
+    uses: ./.github/workflows/clang-tidy-differential.yaml
+    with:
+      container: ghcr.io/autowarefoundation/autoware:universe-devel
+      container-suffix: -cuda
diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml
new file mode 100644
index 0000000000000..51e0a8408468c
--- /dev/null
+++ b/.github/workflows/clang-tidy-differential.yaml
@@ -0,0 +1,75 @@
+name: clang-tidy-differential
+
+on:
+  workflow_call:
+    inputs:
+      container:
+        required: true
+        type: string
+      container-suffix:
+        required: false
+        default: ""
+        type: string
+      runner:
+        default: ubuntu-24.04
+        required: false
+        type: string
+
+jobs:
+  clang-tidy-differential:
+    runs-on: ${{ inputs.runner }}
+    container: ${{ inputs.container }}${{ inputs.container-suffix }}
+    steps:
+      - name: Set PR fetch depth
+        run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}"
+
+      - name: Checkout PR branch and all PR commits
+        uses: actions/checkout@v4
+        with:
+          ref: ${{ github.event.pull_request.head.sha }}
+          fetch-depth: ${{ env.PR_FETCH_DEPTH }}
+
+      - name: Show machine specs
+        run: lscpu && free -h
+        shell: bash
+
+      - name: Show disk space before the tasks
+        run: df -h
+        shell: bash
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Get changed files (existing files only)
+        id: get-changed-files
+        run: |
+          echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT
+        shell: bash
+
+      - name: Export CUDA state as a variable for adding to cache key
+        run: |
+          build_type_cuda_state=nocuda
+          if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then
+            build_type_cuda_state=cuda
+          fi
+          echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}"
+          echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state"
+        shell: bash
+
+      - name: Run clang-tidy
+        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
+        uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
+        with:
+          rosdistro: humble
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci
+          clang-tidy-ignore-path: .clang-tidy-ignore
+          build-depends-repos: build_depends.repos
+          cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }}
+
+      - name: Show disk space after the tasks
+        run: df -h

From 24bcd0e8b9a0a5541af9c95450d0ce2f7a402abc Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Tue, 24 Dec 2024 20:57:04 +0900
Subject: [PATCH 12/13] feat(motion_velocity_planner): remove unnecessary
 tier4_planning_msgs dependency (#9757)

* feat(motion_velocity_planner): remove unnecessary tier4_planning_msgs dependency

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../package.xml                                              | 1 -
 .../package.xml                                              | 1 -
 .../autoware_motion_velocity_out_of_lane_module/package.xml  | 1 -
 .../src/out_of_lane_module.hpp                               | 2 --
 .../autoware/motion_velocity_planner_common/planner_data.hpp | 2 --
 .../autoware_motion_velocity_planner_common/package.xml      | 1 -
 .../autoware_motion_velocity_planner_node/package.xml        | 1 -
 .../autoware_motion_velocity_planner_node/src/node.cpp       | 5 -----
 8 files changed, 14 deletions(-)

diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml
index eef7b6af1f9af..1063abfb61bbb 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml
@@ -24,7 +24,6 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
-  <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml
index 68e2ead5a5ec7..e608e158b3ffe 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml
@@ -33,7 +33,6 @@
   <depend>sensor_msgs</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml
index 857716819d8cc..f0f663355821c 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml
@@ -29,7 +29,6 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
-  <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp
index c39c6e17101c5..72f20f1c63d96 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp
@@ -24,8 +24,6 @@
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <geometry_msgs/msg/pose.hpp>
-#include <tier4_planning_msgs/msg/path_point_with_lane_id.hpp>
-#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp
index a5f9badd9f4f7..9ef3bf5c7f756 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp
@@ -31,7 +31,6 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/header.hpp>
-#include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
 #include <lanelet2_core/Forward.h>
 #include <pcl/point_cloud.h>
@@ -72,7 +71,6 @@ struct PlannerData
   // last observed infomation for UNKNOWN
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
-  std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
 
   // velocity smoother
   std::shared_ptr<autoware::velocity_smoother::SmootherBase> velocity_smoother_;
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml
index 51f4d9d984365..2fc682e695f6b 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml
@@ -28,7 +28,6 @@
   <depend>geometry_msgs</depend>
   <depend>libboost-dev</depend>
   <depend>rclcpp</depend>
-  <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml
index 03a1bec5c6fe5..07b719689f51f 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml
@@ -39,7 +39,6 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_metric_msgs</depend>
-  <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
index 9a607ba3b88cf..cefc84836beda 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
@@ -357,7 +357,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
   const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry.pose.pose;
   const double v0 = planner_data.current_odometry.twist.twist.linear.x;
   const double a0 = planner_data.current_acceleration.accel.accel.linear.x;
-  const auto & external_v_limit = planner_data.external_velocity_limit;
   const auto & smoother = planner_data.velocity_smoother_;
 
   const auto traj_lateral_acc_filtered =
@@ -382,10 +381,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
   if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) {
     RCLCPP_ERROR(get_logger(), "failed to smooth");
   }
-  if (external_v_limit) {
-    autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
-      0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed);
-  }
   return traj_smoothed;
 }
 

From 421ec7d67b5624720d8dc33aa1fba04c6c5a06ef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?=
 <xmfcx@users.noreply.github.com>
Date: Tue, 24 Dec 2024 20:07:25 +0300
Subject: [PATCH 13/13] ci(build-test-tidy): introduce success condition jobs
 (#9769)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: M. Fatih Cırıt <mfc@autoware.org>
---
 .../actions/evaluate-job-result/action.yaml   | 45 ++++++++++++++
 .github/workflows/build-test-tidy-pr.yaml     | 61 +++++++++++++++++++
 2 files changed, 106 insertions(+)
 create mode 100644 .github/actions/evaluate-job-result/action.yaml

diff --git a/.github/actions/evaluate-job-result/action.yaml b/.github/actions/evaluate-job-result/action.yaml
new file mode 100644
index 0000000000000..c5c013080fd27
--- /dev/null
+++ b/.github/actions/evaluate-job-result/action.yaml
@@ -0,0 +1,45 @@
+name: Evaluate Job Result
+description: Evaluates the result of a job and updates the summary.
+inputs:
+  job_result:
+    description: Result of the job to evaluate (e.g., success, failure, skipped)
+    required: true
+    type: string
+  job_name:
+    description: Name of the job to evaluate
+    required: true
+    type: string
+  expected_results:
+    description: Comma-separated list of acceptable results (e.g., success,skipped)
+    required: true
+    type: string
+outputs:
+  failed:
+    description: Indicates if the job failed
+    value: ${{ steps.evaluate.outputs.failed }}
+
+runs:
+  using: composite
+  steps:
+    - name: Evaluate Job Result
+      id: evaluate
+      run: |
+        JOB_RESULT="${{ inputs.job_result }}"
+        IFS=',' read -ra EXPECTED <<< "${{ inputs.expected_results }}"
+        FAILED=false
+
+        for RESULT in "${EXPECTED[@]}"; do
+          if [[ "$JOB_RESULT" == "$RESULT" ]]; then
+            echo "- **${{ inputs.job_name }}:** "$JOB_RESULT" ✅" >> "$GITHUB_STEP_SUMMARY"
+            echo "failed=false" >> "$GITHUB_OUTPUT"
+            exit 0
+          fi
+        done
+
+        # If no expected result matched
+        echo "::error::${{ inputs.job_name }} failed. ❌"
+        echo "- **${{ inputs.job_name }}:** failed ❌" >> "$GITHUB_STEP_SUMMARY"
+        echo "failed=true" >> "$GITHUB_OUTPUT"
+
+        exit 1
+      shell: bash
diff --git a/.github/workflows/build-test-tidy-pr.yaml b/.github/workflows/build-test-tidy-pr.yaml
index df2eff132eb05..d1ea9b2d0f79d 100644
--- a/.github/workflows/build-test-tidy-pr.yaml
+++ b/.github/workflows/build-test-tidy-pr.yaml
@@ -58,6 +58,34 @@ jobs:
     secrets:
       codecov-token: ${{ secrets.CODECOV_TOKEN }}
 
+  build-test-pr:
+    needs:
+      - build-and-test-differential
+      - build-and-test-differential-cuda
+    if: ${{ always() }}
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Initialize Summary
+        run: echo "### Build Test PR Results" > $GITHUB_STEP_SUMMARY
+        shell: bash
+
+      - name: Evaluate build-and-test-differential
+        uses: ./.github/actions/evaluate-job-result
+        with:
+          job_result: ${{ needs.build-and-test-differential.result }}
+          job_name: build-and-test-differential
+          expected_results: success
+
+      - name: Evaluate build-and-test-differential-cuda
+        if: ${{ always() }}
+        uses: ./.github/actions/evaluate-job-result
+        with:
+          job_result: ${{ needs.build-and-test-differential-cuda.result }}
+          job_name: build-and-test-differential-cuda
+          expected_results: success,skipped
+
   clang-tidy-differential:
     needs:
       - check-if-cuda-job-is-needed
@@ -74,3 +102,36 @@ jobs:
     with:
       container: ghcr.io/autowarefoundation/autoware:universe-devel
       container-suffix: -cuda
+
+  clang-tidy-pr:
+    needs:
+      - clang-tidy-differential
+      - clang-tidy-differential-cuda
+    if: ${{ always() }}
+    runs-on: ubuntu-latest
+    steps:
+      - name: Initialize Summary
+        run: echo "### Clang Tidy PR Results" > $GITHUB_STEP_SUMMARY
+        shell: bash
+
+      - name: Check clang-tidy success
+        if: ${{ needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success' }}
+        run: |
+          echo "✅ Either one of the following has succeeded:" >> $GITHUB_STEP_SUMMARY
+
+      - name: Fail if conditions not met
+        if: ${{ !(needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success') }}
+        run: |
+          echo "::error::❌ Either one of the following should have succeeded:"
+          echo "::error::clang-tidy-differential: ${{ needs.clang-tidy-differential.result }}"
+          echo "::error::clang-tidy-differential-cuda: ${{ needs.clang-tidy-differential-cuda.result }}"
+
+          echo "❌ Either one of the following should have succeeded:" >> $GITHUB_STEP_SUMMARY
+
+          exit 1
+
+      - name: Print the results
+        if: ${{ always() }}
+        run: |
+          echo "- **clang-tidy-differential:** ${{ needs.clang-tidy-differential.result }}" >> $GITHUB_STEP_SUMMARY
+          echo "- **clang-tidy-differential-cuda:** ${{ needs.clang-tidy-differential-cuda.result }}" >> $GITHUB_STEP_SUMMARY