Merge remote-tracking branch 'origin' into feat/ml_detection/use_cont…

…ainer

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);

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/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/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>