Merge branch 'main' into remove-maintainer-centerpoint

common/autoware_auto_perception_rviz_plugin/package.xml

@@ -8,7 +8,8 @@
   <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
   <maintainer email="[email protected]">Taiki Tanaka</maintainer>
   <maintainer email="[email protected]">Takeshi Miura</maintainer>
-
+  <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

common/object_recognition_utils/package.xml

@@ -8,6 +8,7 @@
   <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
   <maintainer email="[email protected]">Yusuke Muramatsu</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

common/perception_utils/package.xml

@@ -7,6 +7,7 @@
   <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
   <maintainer email="[email protected]">Yusuke Muramatsu</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

control/control_validator/package.xml

@@ -7,6 +7,9 @@
   <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
   <maintainer email="[email protected]">Takamasa Horibe</maintainer>
   <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <maintainer email="[email protected]">Mamoru Sobue</maintainer>
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
+
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Kyoichi Sugahara</author>

map/map_loader/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The map_loader package</description>
   <maintainer email="[email protected]">Ryohsuke Mitsudome</maintainer>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
   <maintainer email="[email protected]">Ryu Yamamoto</maintainer>
   <maintainer email="[email protected]">Koji Minoda</maintainer>
   <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -90,8 +90,9 @@ std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::getPCDMetadata(
 {
   std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
   if (pcd_paths.size() != 1) {
-    if (!fs::exists(pcd_metadata_path)) {
-      throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path);
+    while (!fs::exists(pcd_metadata_path)) {
+      RCLCPP_ERROR_STREAM(get_logger(), "PCD metadata file not found: " << pcd_metadata_path);
+      std::this_thread::sleep_for(std::chrono::seconds(1));
     }
 
     pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path);

mkdocs_macros.py

@@ -42,6 +42,8 @@ def format_param_range(param):
 def extract_parameter_info(parameters, namespace=""):
     params = []
     for k, v in parameters.items():
+        if "$ref" in v.keys():
+            continue
         if v["type"] != "object":
             param = {}
             param["Name"] = namespace + k

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -493,6 +493,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<BehaviorModuleOutput> last_previous_module_output_{};
   bool hasPreviousModulePathShapeChanged() const;
   bool hasDeviatedFromLastPreviousModulePath() const;
+  bool hasDeviatedFromCurrentPreviousModulePath() const;
 
   // timer for generating pull over path candidates in a separate thread
   void onTimer();

planning/behavior_path_goal_planner_module/package.xml

@@ -11,6 +11,8 @@
   <maintainer email="[email protected]">Tomoya Kimura</maintainer>
   <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
   <maintainer email="[email protected]">Tomohito Ando</maintainer>
+  <maintainer email="[email protected]">Mamoru Sobue</maintainer>
+  <maintainer email="[email protected]">Daniel Sanchez</maintainer>
 
   <license>Apache License 2.0</license>
 

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -161,6 +161,13 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const
            planner_data_->self_odometry->pose.pose.position)) > 0.3;
 }
 
+bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const
+{
+  return std::abs(motion_utils::calcLateralOffset(
+           getPreviousModuleOutput().path.points,
+           planner_data_->self_odometry->pose.pose.position)) > 0.3;
+}
+
 // generate pull over candidate paths
 void GoalPlannerModule::onTimer()
 {
@@ -179,6 +186,10 @@ void GoalPlannerModule::onTimer()
 
   // check if new pull over path candidates are needed to be generated
   const bool need_update = std::invoke([&]() {
+    if (hasDeviatedFromCurrentPreviousModulePath()) {
+      RCLCPP_ERROR(getLogger(), "has deviated from current previous module path");
+      return false;
+    }
     if (thread_safe_data_.get_pull_over_path_candidates().empty()) {
       return true;
     }
@@ -1554,27 +1565,9 @@ bool GoalPlannerModule::checkObjectsCollision(
   }
 
   /* Expand ego collision check polygon
-   *   - `collision_check_margin` in all directions
-   *   - `extra_stopping_margin` in the moving direction
-   *   - `extra_lateral_margin` in external direction of path curve
-   *
-   *
-   *                                        ^ moving direction
-   *                                        x
-   *                                        x
-   *                                        x
-   *          +----------------------+------x--+
-   *          |                      |      x  |
-   *          |   +---------------+  |    xx   |
-   *          |   |               |  |  xx     |
-   *          |   | ego footprint |xxxxx       |
-   *          |   |               |  |         |
-   *          |   +---------------+  | extra_stopping_margin
-   *          |       margin         |         |
-   *          +----------------------+         |
-   *          |       extra_lateral_margin     |
-   *          +--------------------------------+
-   *
+   *   - `collision_check_margin` is added in all directions.
+   *   - `extra_stopping_margin` adds stopping margin under deceleration constraints forward.
+   *   - `extra_lateral_margin` adds the lateral margin on curves.
    */
   std::vector<Polygon2d> ego_polygons_expanded{};
   const auto curvatures = motion_utils::calcCurvature(path.points);
@@ -1585,19 +1578,19 @@ bool GoalPlannerModule::checkObjectsCollision(
       std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration,
       parameters_->object_recognition_collision_check_max_extra_stopping_margin);
 
-    double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps *
-                                  std::abs(p.point.longitudinal_velocity_mps);
-    extra_lateral_margin =
-      std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin);
+    // The square is meant to imply centrifugal force, but it is not a very well-founded formula.
+    // TODO(kosuke55): It is needed to consider better way because there is an inherently different
+    // conception of the inside and outside margins.
+    const double extra_lateral_margin = std::min(
+      extra_stopping_margin,
+      std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2)));
 
-    const auto lateral_offset_pose =
-      tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0);
     const auto ego_polygon = tier4_autoware_utils::toFootprint(
-      lateral_offset_pose,
+      p.point.pose,
       planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin,
       planner_data_->parameters.base_link2rear + collision_check_margin,
       planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 +
-        std::abs(extra_lateral_margin));
+        extra_lateral_margin * 2.0);
     ego_polygons_expanded.push_back(ego_polygon);
   }
 

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -5,15 +5,14 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
-      collision_check_margins: [2.0, 1.5, 1.0]
-      collision_check_distance_from_end: 1.0
+      collision_check_margins: [2.0, 1.0, 0.5, 0.1]
       collision_check_margin_from_front_object: 5.0
       th_moving_object_velocity: 1.0
       th_distance_to_middle_of_the_road: 0.5
       center_line_path_interval: 1.0
       # shift pull out
       enable_shift_pull_out: true
-      check_shift_path_lane_departure: false
+      shift_collision_check_distance_from_end: -10.0
       minimum_shift_pull_out_distance: 0.0
       deceleration_interval: 15.0
       lateral_jerk: 0.5
@@ -23,6 +22,7 @@
       maximum_curvature: 0.07
       # geometric pull out
       enable_geometric_pull_out: true
+      geometric_collision_check_distance_from_end: 0.0
       divide_pull_out_path: true
       geometric_pull_out_velocity: 1.0
       arc_path_interval: 1.0
@@ -32,7 +32,7 @@
       # search start pose backward
       enable_back: true
       search_priority: "efficient_path"  # "efficient_path" or "short_back_distance"
-      max_back_distance: 30.0
+      max_back_distance: 20.0
       backward_search_resolution: 2.0
       backward_path_update_duration: 3.0
       ignore_distance_from_lane_end: 15.0

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

@@ -63,14 +63,14 @@ struct StartPlannerParameters
   double intersection_search_length{0.0};
   double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
   std::vector<double> collision_check_margins{};
-  double collision_check_distance_from_end{0.0};
   double collision_check_margin_from_front_object{0.0};
   double th_moving_object_velocity{0.0};
   double center_line_path_interval{0.0};
 
   // shift pull out
   bool enable_shift_pull_out{false};
   bool check_shift_path_lane_departure{false};
+  double shift_collision_check_distance_from_end{0.0};
   double minimum_shift_pull_out_distance{0.0};
   int lateral_acceleration_sampling_num{0};
   double lateral_jerk{0.0};
@@ -80,6 +80,7 @@ struct StartPlannerParameters
   double deceleration_interval{0.0};
   // geometric pull out
   bool enable_geometric_pull_out{false};
+  double geometric_collision_check_distance_from_end;
   bool divide_pull_out_path{false};
   ParallelParkingParameters parallel_parking_parameters{};
   // search start pose backward

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -174,7 +174,8 @@ class StartPlannerModule : public SceneModuleInterface
     const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
     const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin);
 
-  PathWithLaneId extractCollisionCheckSection(const PullOutPath & path);
+  PathWithLaneId extractCollisionCheckSection(
+    const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type);
   void updateStatusWithCurrentPath(
     const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
     const behavior_path_planner::PlannerType & planner_type);

planning/behavior_path_start_planner_module/package.xml

@@ -11,6 +11,8 @@
   <maintainer email="[email protected]">Tomoya Kimura</maintainer>
   <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
   <maintainer email="[email protected]">Tomohito Ando</maintainer>
+  <maintainer email="[email protected]">Mamoru Sobue</maintainer>
+  <maintainer email="[email protected]">Daniel Sanchez</maintainer>
 
   <license>Apache License 2.0</license>
 

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
     ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
   p.collision_check_margins =
     node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
-  p.collision_check_distance_from_end =
-    node->declare_parameter<double>(ns + "collision_check_distance_from_end");
   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");
@@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   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.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 =
@@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   p.deceleration_interval = node->declare_parameter<double>(ns + "deceleration_interval");
   // 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");

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -657,8 +657,8 @@ bool StartPlannerModule::findPullOutPath(
 
   // check collision
   if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-        vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects,
-        collision_check_margin)) {
+        vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()),
+        pull_out_lane_stop_objects, collision_check_margin)) {
     return false;
   }
 
@@ -672,8 +672,17 @@ bool StartPlannerModule::findPullOutPath(
   return true;
 }
 
-PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path)
+PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
+  const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type)
 {
+  const std::map<PlannerType, double> collision_check_distances = {
+    {behavior_path_planner::PlannerType::SHIFT,
+     parameters_->shift_collision_check_distance_from_end},
+    {behavior_path_planner::PlannerType::GEOMETRIC,
+     parameters_->geometric_collision_check_distance_from_end}};
+
+  const double collision_check_distance_from_end = collision_check_distances.at(planner_type);
+
   PathWithLaneId combined_path;
   for (const auto & partial_path : path.partial_paths) {
     combined_path.points.insert(
@@ -682,7 +691,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat
   // calculate collision check end idx
   const size_t collision_check_end_idx = std::invoke([&]() {
     const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
-      combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end);
+      combined_path.points, path.end_pose.position, collision_check_distance_from_end);
 
     if (collision_check_end_pose) {
       return motion_utils::findNearestIndex(
@@ -1403,9 +1412,14 @@ void StartPlannerModule::setDebugData()
   // visualize collision_check_end_pose and footprint
   {
     const auto local_footprint = vehicle_info_.createFootprint();
+    std::map<PlannerType, double> collision_check_distances = {
+      {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
+      {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
+
+    double collision_check_distance_from_end = collision_check_distances[status_.planner_type];
     const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
       getFullPath().points, status_.pull_out_path.end_pose.position,
-      parameters_->collision_check_distance_from_end);
+      collision_check_distance_from_end);
     if (collision_check_end_pose) {
       add(createPoseMarkerArray(
         *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));

planning/rtc_interface/package.xml

@@ -7,6 +7,7 @@
   <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
   <maintainer email="[email protected]">Taiki Tanaka</maintainer>
   <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
+  <maintainer email="[email protected]">Satoshi Ota</maintainer>
 
   <license>Apache License 2.0</license>
 

sensing/radar_scan_to_pointcloud2/package.xml

@@ -5,6 +5,9 @@
   <description>radar_scan_to_pointcloud2</description>
   <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
+  <maintainer email="[email protected]">Taekjin Lee</maintainer>
+
   <author email="[email protected]">Satoshi Tanaka</author>
   <license>Apache License 2.0</license>
 

sensing/radar_static_pointcloud_filter/package.xml

@@ -5,6 +5,9 @@
   <description>radar_static_pointcloud_filter</description>
   <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
+  <maintainer email="[email protected]">Taekjin Lee</maintainer>
+
   <author email="[email protected]">Satoshi Tanaka</author>
   <license>Apache License 2.0</license>
 

sensing/radar_threshold_filter/package.xml

@@ -5,6 +5,9 @@
   <description>radar_threshold_filter</description>
   <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
+  <maintainer email="[email protected]">Taekjin Lee</maintainer>
+
   <author email="[email protected]">Satoshi Tanaka</author>
   <license>Apache License 2.0</license>
 

sensing/radar_tracks_noise_filter/package.xml

@@ -6,6 +6,9 @@
   <description>radar_tracks_noise_filter</description>
   <maintainer email="[email protected]">Sathshi Tanaka</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
+  <maintainer email="[email protected]">Taekjin Lee</maintainer>
+
   <license>Apache License 2.0</license>
   <author email="[email protected]">Sathshi Tanaka</author>