velocity handling

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Oct 2, 2024 · fcc0eef · fcc0eef
1 parent 188f0c9
commit fcc0eef
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 9 deletions.
diff --git a/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -122,7 +122,8 @@ struct PullOverContextData
   const bool is_stable_safe_path;
   const PredictedObjects static_target_objects;
   const PredictedObjects dynamic_target_objects;
-  const std::optional<PullOverPath> pull_over_path_opt;
+  // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it)
+  std::optional<PullOverPath> pull_over_path_opt;
   const std::vector<PullOverPath> pull_over_path_candidates;
   // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose
 };

diff --git a/...e/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/...e/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp
@@ -47,6 +47,7 @@ struct PullOverPath
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPath(const PullOverPath & other);
+  PullOverPath & operator=(const PullOverPath & other) = default;
 
   PullOverPlannerType type() const { return type_; }
   size_t goal_id() const { return goal_id_; }

diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1342,9 +1342,15 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(
     return getPreviousModuleOutput();
   }
 
-  const auto & pull_over_path_opt = context_data.pull_over_path_opt;
+  auto context_data_with_velocity = context_data;
+  /**
+     NOTE(soblin): this path originates from the previously selected(by main thread) pull_over_path
+     which was originally generated by either road_parking or freespace thread
+   */
+  auto & pull_over_path_with_velocity_opt = context_data_with_velocity.pull_over_path_opt;
   const bool is_freespace =
-    pull_over_path_opt && pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE;
+    pull_over_path_with_velocity_opt &&
+    pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE;
   if (
     path_decision_controller_.get_current_state().state ==
       PathDecisionState::DecisionKind::NOT_DECIDED &&
@@ -1371,8 +1377,24 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(
     // update thread_safe_data_
     if (path_and_goal_opt) {
       auto [pull_over_path, modified_goal] = *path_and_goal_opt;
-      deceleratePath(pull_over_path);
+      /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old
+       * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed
+       * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of
+       * member variable
+       *
+       * set this selected pull_over_path to ThreadSafeData, but actually RoadParking thread does
+       * not use pull_over_path, but only FreespaceParking thread use this selected pull_over_path.
+       * As the next action item, only set this selected pull_over_path to only
+       * FreespaceThreadSafeData.
+       */
       thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal);
+      if (pull_over_path_with_velocity_opt) {
+        auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value();
+        // copy the path for later setOutput()
+        pull_over_path_with_velocity = pull_over_path;
+        // modify the velocity for latest setOutput()
+        deceleratePath(pull_over_path_with_velocity);
+      }
       RCLCPP_DEBUG(
         getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(),
         modified_goal.id);
@@ -1383,26 +1405,27 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(
 
   // set output and status
   BehaviorModuleOutput output{};
-  setOutput(context_data, output);
+  setOutput(context_data_with_velocity, output);
 
   // return to lane parking if it is possible
-  if (is_freespace && canReturnToLaneParking(context_data)) {
+  if (is_freespace && canReturnToLaneParking(context_data_with_velocity)) {
     thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
   }
 
   // For debug
-  setDebugData(context_data);
+  setDebugData(context_data_with_velocity);
 
   if (parameters_->print_debug_info) {
     // For evaluations
     printParkingPositionError();
   }
 
-  if (!context_data.pull_over_path_opt) {
+  if (!pull_over_path_with_velocity_opt) {
     return output;
   }
 
-  path_candidate_ = std::make_shared<PathWithLaneId>(pull_over_path_opt.value().full_path());
+  path_candidate_ =
+    std::make_shared<PathWithLaneId>(pull_over_path_with_velocity_opt.value().full_path());
 
   return output;
 }