feat: merge beta/v0.29.0 1 to beta/x2_gen2_v0.29.0

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -488,6 +488,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
   // calculate the target motion for delay compensation
   constexpr double min_running_dist = 0.01;
   if (control_data.state_after_delay.running_distance > min_running_dist) {
+    control_data.interpolated_traj.points =
+      autoware::motion_utils::removeOverlapPoints(control_data.interpolated_traj.points);
     const auto target_pose = longitudinal_utils::findTrajectoryPoseAfterDistance(
       control_data.nearest_idx, control_data.state_after_delay.running_distance,
       control_data.interpolated_traj);

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -14,6 +14,7 @@
   <!-- Parameter files -->
   <arg name="localization_error_monitor_param_path"/>
   <arg name="ekf_localizer_param_path"/>
+  <arg name="stop_filter_param_path"/>
   <arg name="pose_initializer_param_path"/>
   <arg name="eagleye_param_path"/>
   <arg name="ar_tag_based_localizer_param_path"/>

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -22,6 +22,7 @@
       <arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
       <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
       <arg name="output_odom_name" value="/localization/kinematic_state"/>
+      <arg name="param_path" value="$(var stop_filter_param_path)"/>
     </include>
   </group>
 

planning/autoware_path_optimizer/src/mpt_optimizer.cpp

@@ -477,8 +477,13 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData
 
   const auto get_prev_optimized_traj_points = [&]() {
     if (prev_optimized_traj_points_ptr_) {
+      RCLCPP_WARN(logger_, "return the previous optimized_trajectory as exceptional behavior.");
       return *prev_optimized_traj_points_ptr_;
     }
+    RCLCPP_WARN(
+      logger_,
+      "Try to return the previous optimized_trajectory as exceptional behavior, "
+      "but this failure also. Then return path_smoother output.");
     return traj_points;
   };
 
@@ -505,8 +510,7 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData
   // 6. optimize steer angles
   const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat);
   if (!optimized_variables) {
-    RCLCPP_INFO_EXPRESSION(
-      logger_, enable_debug_info_, "return std::nullopt since could not solve qp");
+    RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp");
     return get_prev_optimized_traj_points();
   }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1259,8 +1259,6 @@ bool GoalPlannerModule::hasNotDecidedPath(
   const std::shared_ptr<SafetyCheckParams> & safety_check_params,
   const std::shared_ptr<GoalSearcherBase> goal_searcher) const
 {
-  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
-
   return checkDecidingPathStatus(
            planner_data, occupancy_grid_map, parameters, ego_predicted_path_params,
            objects_filtering_params, safety_check_params, goal_searcher)
@@ -2371,8 +2369,6 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath(
   const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
   const std::shared_ptr<SafetyCheckParams> & safety_check_params) const
 {
-  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
-
   if (!thread_safe_data_.get_pull_over_path()) {
     return {false, false};
   }

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

@@ -293,7 +293,7 @@ PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTi
 
   // return identity if old_stamp is newer than new_stamp
   if (old_stamp > new_stamp) {
-    RCLCPP_WARN_STREAM_THROTTLE(
+    RCLCPP_DEBUG_STREAM_THROTTLE(
       get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(),
       "old_stamp is newer than new_stamp,");
     return Eigen::Matrix4f::Identity();