fix(goal_planner): fix multithread and magin

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -37,6 +37,7 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 
+#include <atomic>
 #include <deque>
 #include <limits>
 #include <memory>
@@ -290,6 +291,35 @@ class GoalPlannerModule : public SceneModuleInterface
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map);
 
+  ~GoalPlannerModule()
+  {
+    if (lane_parking_timer_) {
+      lane_parking_timer_->cancel();
+    }
+    if (freespace_parking_timer_) {
+      freespace_parking_timer_->cancel();
+    }
+
+    while (is_lane_parking_cb_running_.load() || is_freespace_parking_cb_running_.load()) {
+      const std::string running_callbacks = std::invoke([&]() {
+        if (is_lane_parking_cb_running_ && is_freespace_parking_cb_running_) {
+          return "lane parking and freespace parking";
+        }
+        if (is_lane_parking_cb_running_) {
+          return "lane parking";
+        }
+        return "freespace parking";
+      });
+      RCLCPP_INFO_THROTTLE(
+        getLogger(), *clock_, 1000, "Waiting for %s callback to finish...",
+        running_callbacks.c_str());
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    RCLCPP_INFO_THROTTLE(
+      getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished");
+  }
+
   void updateModuleParams(const std::any & parameters) override
   {
     parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
@@ -330,6 +360,18 @@ class GoalPlannerModule : public SceneModuleInterface
   CandidateOutput planCandidate() const override { return CandidateOutput{}; }
 
 private:
+  // Flag class for managing whether a certain callback is running in multi-threading
+  class ScopedFlag
+  {
+  public:
+    explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }
+
+    ~ScopedFlag() { flag_.store(false); }
+
+  private:
+    std::atomic<bool> & flag_;
+  };
+
   /*
    * state transitions and plan function used in each state
    *
@@ -404,10 +446,12 @@ class GoalPlannerModule : public SceneModuleInterface
   // pre-generate lane parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr lane_parking_timer_;
   rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
+  std::atomic<bool> is_lane_parking_cb_running_;
 
   // generate freespace parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
   rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
+  std::atomic<bool> is_freespace_parking_cb_running_;
 
   // debug
   mutable GoalPlannerDebugData debug_data_;
@@ -493,6 +537,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

@@ -62,6 +62,8 @@ GoalPlannerModule::GoalPlannerModule(
   parameters_{parameters},
   vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
   thread_safe_data_{mutex_, clock_},
+  is_lane_parking_cb_running_{false},
+  is_freespace_parking_cb_running_{false},
   debug_stop_pose_with_info_{&stop_pose_}
 {
   LaneDepartureChecker lane_departure_checker{};
@@ -161,9 +163,18 @@ 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()
 {
+  const ScopedFlag flag(is_lane_parking_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
@@ -179,15 +190,19 @@ void GoalPlannerModule::onTimer()
 
   // check if new pull over path candidates are needed to be generated
   const bool need_update = std::invoke([&]() {
+    if (hasDeviatedFromCurrentPreviousModulePath()) {
+      RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path");
+      return false;
+    }
     if (thread_safe_data_.get_pull_over_path_candidates().empty()) {
       return true;
     }
     if (hasPreviousModulePathShapeChanged()) {
-      RCLCPP_ERROR(getLogger(), "has previous module path shape changed");
+      RCLCPP_DEBUG(getLogger(), "has previous module path shape changed");
       return true;
     }
     if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) {
-      RCLCPP_ERROR(getLogger(), "has deviated from last previous module path");
+      RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path");
       return true;
     }
     return false;
@@ -276,6 +291,8 @@ void GoalPlannerModule::onTimer()
 
 void GoalPlannerModule::onFreespaceParkingTimer()
 {
+  const ScopedFlag flag(is_freespace_parking_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
@@ -1554,27 +1571,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 +1584,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/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -35,6 +35,7 @@
 
 #include <tf2/utils.h>
 
+#include <atomic>
 #include <deque>
 #include <memory>
 #include <string>
@@ -83,6 +84,21 @@ class StartPlannerModule : public SceneModuleInterface
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map);
 
+  ~StartPlannerModule()
+  {
+    if (freespace_planner_timer_) {
+      freespace_planner_timer_->cancel();
+    }
+
+    while (is_freespace_planner_cb_running_.load()) {
+      RCLCPP_INFO_THROTTLE(
+        getLogger(), *clock_, 1000, "Waiting for freespace planner callback to finish...");
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    RCLCPP_INFO_THROTTLE(getLogger(), *clock_, 1000, "freespace planner callback finished");
+  }
+
   void updateModuleParams(const std::any & parameters) override
   {
     parameters_ = std::any_cast<std::shared_ptr<StartPlannerParameters>>(parameters);
@@ -135,6 +151,18 @@ class StartPlannerModule : public SceneModuleInterface
   bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; }
 
 private:
+  // Flag class for managing whether a certain callback is running in multi-threading
+  class ScopedFlag
+  {
+  public:
+    explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }
+
+    ~ScopedFlag() { flag_.store(false); }
+
+  private:
+    std::atomic<bool> & flag_;
+  };
+
   bool canTransitSuccessState() override;
 
   bool canTransitFailureState() override { return false; }
@@ -202,6 +230,8 @@ class StartPlannerModule : public SceneModuleInterface
   std::unique_ptr<PullOutPlannerBase> freespace_planner_;
   rclcpp::TimerBase::SharedPtr freespace_planner_timer_;
   rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_;
+  std::atomic<bool> is_freespace_planner_cb_running_;
+
   // TODO(kosuke55)
   // Currently, we only do lock when updating a member of status_.
   // However, we need to ensure that the value does not change when referring to it.

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -54,7 +54,8 @@ StartPlannerModule::StartPlannerModule(
     objects_of_interest_marker_interface_ptr_map)
 : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{parameters},
-  vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}
+  vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
+  is_freespace_planner_cb_running_{false}
 {
   lane_departure_checker_ = std::make_shared<LaneDepartureChecker>();
   lane_departure_checker_->setVehicleInfo(vehicle_info_);
@@ -85,6 +86,8 @@ StartPlannerModule::StartPlannerModule(
 
 void StartPlannerModule::onFreespacePlannerTimer()
 {
+  const ScopedFlag flag(is_freespace_planner_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }