diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 687d37ab63b1..12d564237db3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_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 pull_over_path_opt; + // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it) + std::optional pull_over_path_opt; const std::vector pull_over_path_candidates; // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 1c868e8c5d22..47367164b251 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -47,6 +47,7 @@ struct PullOverPath const std::vector> & 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/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index c196c6d953ca..55cbd4c0e29d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_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(pull_over_path_opt.value().full_path()); + path_candidate_ = + std::make_shared(pull_over_path_with_velocity_opt.value().full_path()); return output; }