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 b66d6295ca54b..6357544743801 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 @@ -105,13 +105,6 @@ struct PoseWithString } }; -struct SelectedPullOverPath -{ - PullOverPath path; - rclcpp::Time selected_time; - std::optional last_path_idx_increment_time; -}; - struct PullOverContextData { PullOverContextData() = delete; @@ -136,8 +129,11 @@ struct PullOverContextData const bool is_stopped; const LaneParkingResponse lane_parking_response; const FreespaceParkingResponse freespace_parking_response; - // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it) - std::optional selected_pull_over_path_opt; + // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it), + // and bind following three member, rename as "SelectedPullOverPath" + std::optional pull_over_path_opt; + std::optional last_path_update_time; + std::optional last_path_idx_increment_time; }; bool isOnModifiedGoal( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index ed2766bcc7cac..244c0be624f23 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -46,8 +46,7 @@ class LaneParkingRequest void update( const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, - const std::optional> & selected_pull_over_path, - const PathDecisionState & prev_data); + const std::optional & pull_over_path, const PathDecisionState & prev_data); const GoalPlannerParameters parameters_; const autoware::universe_utils::LinearRing2d vehicle_footprint_; @@ -63,10 +62,7 @@ class LaneParkingRequest { return last_previous_module_output_; } - const std::optional> & get_selected_pull_over_path() const - { - return selected_pull_over_path_; - } + const std::optional & get_pull_over_path() const { return pull_over_path_; } const PathDecisionState & get_prev_data() const { return prev_data_; } LaneParkingRequest clone() const; @@ -75,8 +71,7 @@ class LaneParkingRequest ModuleStatus current_status_; BehaviorModuleOutput previous_module_output_; BehaviorModuleOutput last_previous_module_output_; - std::optional> - selected_pull_over_path_; // pull_over_path_; //> & selected_pull_over_path, - const bool is_stopped); + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped); const GoalPlannerParameters parameters_; const autoware::universe_utils::LinearRing2d vehicle_footprint_; @@ -116,9 +111,10 @@ class FreespaceParkingRequest { return occupancy_grid_map_; } - const std::optional> & get_selected_pull_over_path() const + const std::optional & get_pull_over_path() const { return pull_over_path_; } + const std::optional & get_last_path_update_time() const { - return selected_pull_over_path_; + return last_path_update_time_; } bool is_stopped() const { return is_stopped_; } @@ -128,7 +124,8 @@ class FreespaceParkingRequest std::shared_ptr planner_data_; ModuleStatus current_status_; std::shared_ptr occupancy_grid_map_; - std::optional> selected_pull_over_path_; + std::optional pull_over_path_; + std::optional last_path_update_time_; bool is_stopped_; void initializeOccupancyGridMap( 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 8589bf3cfdb69..169c42136a8ef 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 @@ -339,7 +339,7 @@ void LaneParkingPlanner::onTimer() const auto & current_status = local_request.get_current_status(); const auto & previous_module_output = local_request.get_previous_module_output(); const auto & last_previous_module_output = local_request.get_last_previous_module_output(); - const auto & selected_pull_over_path = local_request.get_selected_pull_over_path(); + const auto & pull_over_path_opt = local_request.get_pull_over_path(); const auto & prev_data = local_request.get_prev_data(); if (current_status == ModuleStatus::IDLE) { @@ -365,8 +365,8 @@ void LaneParkingPlanner::onTimer() } } const std::optional modified_goal_opt = - selected_pull_over_path - ? std::make_optional(selected_pull_over_path.value().first.modified_goal()) + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) : std::nullopt; if (isOnModifiedGoal( local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { @@ -491,7 +491,8 @@ void FreespaceParkingPlanner::onTimer() const auto & parameters = local_request.parameters_; const auto & local_planner_data = local_request.get_planner_data(); const auto & current_status = local_request.get_current_status(); - const auto & selected_pull_over_path = local_request.get_selected_pull_over_path(); + const auto & pull_over_path_opt = local_request.get_pull_over_path(); + const auto & last_path_update_time = local_request.get_last_path_update_time(); const auto & occupancy_grid_map = local_request.get_occupancy_grid_map(); if (current_status == ModuleStatus::IDLE) { @@ -507,8 +508,8 @@ void FreespaceParkingPlanner::onTimer() } const std::optional modified_goal_opt = - selected_pull_over_path - ? std::make_optional(selected_pull_over_path.value().first.modified_goal()) + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) : std::nullopt; if (isOnModifiedGoal( local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { @@ -539,16 +540,11 @@ void FreespaceParkingPlanner::onTimer() const bool is_new_costmap = (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; - const auto selected_time_opt = - selected_pull_over_path - ? std::make_optional(selected_pull_over_path.value().second) - : std::nullopt; - if ( isStuck(static_target_objects, dynamic_target_objects, local_request) && is_new_costmap && needPathUpdate( local_planner_data->self_odometry->pose.pose, path_update_duration, clock_->now(), - modified_goal_opt, selected_time_opt, parameters)) { + modified_goal_opt, last_path_update_time, parameters)) { const auto freespace_path_opt = planFreespacePath(local_request, static_target_objects, freespace_planner_); if (freespace_path_opt) { @@ -566,15 +562,10 @@ std::pair GoalPlannerModule::sync // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to // gp_planner_data_ here - const bool found_pull_over_path = - context_data_ ? context_data_.value().selected_pull_over_path_opt.has_value() : false; - std::optional> selected_pull_over_path; - if (found_pull_over_path) { - const auto & ctx_data_pull_over_path = - context_data_.value().selected_pull_over_path_opt.value(); - selected_pull_over_path.emplace( - ctx_data_pull_over_path.path, ctx_data_pull_over_path.selected_time); - } + std::optional pull_over_path = + context_data_ ? context_data_.value().pull_over_path_opt : std::nullopt; + std::optional last_path_update_time = + context_data_ ? context_data_.value().last_path_update_time : std::nullopt; // NOTE: onTimer/onFreespaceParkingTimer copies gp_planner_data_ to their local clone, so we need // to lock gp_planner_data_ here to avoid data race. But the following clone process is @@ -596,7 +587,7 @@ std::pair GoalPlannerModule::sync // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) // and if these two coincided, only the reference count is affected lane_parking_request.update( - *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), selected_pull_over_path, + *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path, path_decision_controller_.get_current_state()); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since @@ -622,7 +613,7 @@ std::pair GoalPlannerModule::sync auto & freespace_parking_request = freespace_parking_request_.value(); constexpr double stuck_time = 5.0; freespace_parking_request.update( - *planner_data_, getCurrentStatus(), selected_pull_over_path, + *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time, isStopped(odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time)); // GoalPlannerModule::occupancy_grid_map_ and gp_planner_data.occupancy_grid_map share the // ownership, and gp_planner_data.occupancy_grid_map maybe also shared by the local @@ -681,11 +672,11 @@ void GoalPlannerModule::updateData() path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); const bool found_pull_over_path = - context_data_ ? context_data_.value().selected_pull_over_path_opt.has_value() : false; + context_data_ ? context_data_.value().pull_over_path_opt.has_value() : false; std::optional pull_over_path = - found_pull_over_path ? std::make_optional( - context_data_.value().selected_pull_over_path_opt.value().path) - : std::nullopt; + found_pull_over_path + ? std::make_optional(context_data_.value().pull_over_path_opt.value()) + : std::nullopt; const auto modified_goal_pose = [&]() -> std::optional { if (!pull_over_path) { @@ -718,10 +709,10 @@ void GoalPlannerModule::updateData() } if (hasFinishedCurrentPath(ctx_data)) { - if (ctx_data.selected_pull_over_path_opt) { - auto & pull_over_path = ctx_data.selected_pull_over_path_opt.value().path; + if (ctx_data.pull_over_path_opt) { + auto & pull_over_path = ctx_data.pull_over_path_opt.value(); if (pull_over_path.incrementPathIndex()) { - ctx_data.selected_pull_over_path_opt.value().last_path_idx_increment_time = clock_->now(); + ctx_data.last_path_idx_increment_time = clock_->now(); } } } @@ -730,8 +721,7 @@ void GoalPlannerModule::updateData() last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); // TODO(soblin): do not "plan" in updateData - if (ctx_data.selected_pull_over_path_opt) - decideVelocity(ctx_data.selected_pull_over_path_opt.value().path); + if (ctx_data.pull_over_path_opt) decideVelocity(ctx_data.pull_over_path_opt.value()); } } @@ -891,12 +881,10 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return false; } - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return false; } - if ( - context_data.selected_pull_over_path_opt.value().path.type() == - PullOverPlannerType::FREESPACE) { + if (context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { return false; } // TODO(soblin): return from freespace to lane is disabled temporarily, because if @@ -904,7 +892,7 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte // deleted, freespace path is set again // So context_data need to have old_selected_lane_pull_over_path also, which is only updated // against lane_pull_over_path in selectPullOverPath() - const auto & lane_parking_path = context_data.selected_pull_over_path_opt.value().path; + const auto & lane_parking_path = context_data.pull_over_path_opt.value(); const auto & path = lane_parking_path.full_path(); const auto & curvatures = lane_parking_path.full_path_curvatures(); @@ -1257,7 +1245,7 @@ void GoalPlannerModule::setOutput( return; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); if ( parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { @@ -1296,9 +1284,8 @@ void GoalPlannerModule::setDrivableAreaInfo( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if ( - context_data.selected_pull_over_path_opt && - context_data.selected_pull_over_path_opt.value().path.type() == - PullOverPlannerType::FREESPACE) { + context_data.pull_over_path_opt && + context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1317,10 +1304,10 @@ void GoalPlannerModule::setModifiedGoal( const PullOverContextData & context_data, BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (context_data.selected_pull_over_path_opt) { + if (context_data.pull_over_path_opt) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = context_data.selected_pull_over_path_opt.value().path.modified_goal_pose(); + modified_goal.pose = context_data.pull_over_path_opt.value().modified_goal_pose(); modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -1408,7 +1395,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate(PullOverContextD output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return output; } @@ -1434,35 +1421,32 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData which was originally generated by either road_parking or freespace thread */ auto pull_over_path_with_velocity_opt = - context_data.selected_pull_over_path_opt - ? std::make_optional(context_data.selected_pull_over_path_opt.value().path) + context_data.pull_over_path_opt + ? std::make_optional(context_data.pull_over_path_opt.value()) : std::nullopt; const bool is_freespace = pull_over_path_with_velocity_opt && pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE; const auto selected_modified_goal_opt = - context_data.selected_pull_over_path_opt - ? std::make_optional( - context_data.selected_pull_over_path_opt.value().path.modified_goal()) - : std::nullopt; - const auto selected_time_opt = - context_data.selected_pull_over_path_opt - ? std::make_optional( - context_data.selected_pull_over_path_opt.value().selected_time) + context_data.pull_over_path_opt + ? std::make_optional(context_data.pull_over_path_opt.value().modified_goal()) : std::nullopt; + const auto & last_path_update_time = context_data.last_path_update_time; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), - selected_modified_goal_opt, selected_time_opt, *parameters_)) { + selected_modified_goal_opt, last_path_update_time, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); - context_data.selected_pull_over_path_opt = std::nullopt; + context_data.pull_over_path_opt = std::nullopt; + context_data.last_path_update_time = std::nullopt; + context_data.last_path_idx_increment_time = std::nullopt; // update goal candidates auto goal_candidates = goal_candidates_; @@ -1492,8 +1476,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData * As the next action item, only set this selected pull_over_path to only * FreespaceThreadSafeData. */ - context_data.selected_pull_over_path_opt = - SelectedPullOverPath{pull_over_path, clock_->now(), std::nullopt}; + context_data.pull_over_path_opt = pull_over_path; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; 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() @@ -1514,8 +1499,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData // return to lane parking if it is possible if (is_freespace && canReturnToLaneParking(context_data)) { if (pull_over_path_with_velocity_opt) { - context_data.selected_pull_over_path_opt = - SelectedPullOverPath{pull_over_path_with_velocity_opt.value(), clock_->now(), std::nullopt}; + context_data.pull_over_path_opt = pull_over_path_with_velocity_opt; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; } } @@ -1548,11 +1534,11 @@ void GoalPlannerModule::postProcess() const bool has_decided_path = path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { context_data_ = std::nullopt; return; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); const auto distance_to_path_change = calcDistanceToPathChange(context_data); @@ -1593,10 +1579,10 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); const auto & full_path = pull_over_path.full_path(); @@ -1678,10 +1664,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath(const PullOverContextData & c // 4. feasible stop const auto stop_pose_with_info = std::invoke([&]() -> std::optional> { - if (context_data.selected_pull_over_path_opt) { + if (context_data.pull_over_path_opt) { return std::make_pair( - context_data.selected_pull_over_path_opt.value().path.start_pose(), - "stop at selected start pose"); + context_data.pull_over_path_opt.value().start_pose(), "stop at selected start pose"); } if (context_data.lane_parking_response.closest_start_pose) { return std::make_pair( @@ -1768,15 +1753,12 @@ bool FreespaceParkingPlanner::isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const FreespaceParkingRequest & req) const { - if (!req.get_selected_pull_over_path()) { - return false; - } - const auto & parameters = req.parameters_; const auto & planner_data = req.get_planner_data(); - const auto & pull_over_path = req.get_selected_pull_over_path().value().first; const std::optional modified_goal = - std::make_optional(pull_over_path.modified_goal()); + req.get_pull_over_path() + ? std::make_optional(req.get_pull_over_path().value().modified_goal()) + : std::nullopt; if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal, parameters)) { return false; } @@ -1785,8 +1767,12 @@ bool FreespaceParkingPlanner::isStuck( return false; } + if (!req.get_pull_over_path()) { + return true; + } + if (parameters.use_object_recognition) { - const auto & path = pull_over_path.getCurrentPath(); + const auto & path = req.get_pull_over_path().value().getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); std::vector ego_polygons_expanded; if (goal_planner_utils::checkObjectsCollision( @@ -1801,7 +1787,8 @@ bool FreespaceParkingPlanner::isStuck( if ( parameters.use_occupancy_grid_for_path_collision_check && - checkOccupancyGridCollision(pull_over_path.getCurrentPath(), req.get_occupancy_grid_map())) { + checkOccupancyGridCollision( + req.get_pull_over_path().value().getCurrentPath(), req.get_occupancy_grid_map())) { return true; } @@ -1829,26 +1816,28 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d return false; } - // check if self pose is near the end of current path - if (!ctx_data.selected_pull_over_path_opt) { - return false; - } - - const auto & [pull_over_path, selected_time, last_path_index_increment_time] = - ctx_data.selected_pull_over_path_opt.value(); - // require increment only when the time passed is enough // to prevent increment before driving // when the end of the current path is close to the current pose // this value should be `keep_stop_time` in keepStoppedWithCurrentPath + if (!ctx_data.last_path_idx_increment_time) { + return false; + } constexpr double keep_current_idx_time = 4.0; const bool has_passed_enough_time_from_increment = - (clock_->now() - selected_time).seconds() > keep_current_idx_time; + (clock_->now() - ctx_data.last_path_idx_increment_time.value()).seconds() > + keep_current_idx_time; if (!has_passed_enough_time_from_increment) { return false; } - const auto & current_path_end = pull_over_path.getCurrentPath().points.back(); + // check if self pose is near the end of current path + if (!ctx_data.pull_over_path_opt) { + return false; + } + + const auto & current_path_end = + ctx_data.pull_over_path_opt.value().getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1858,10 +1847,10 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return {}; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); const auto & path = pull_over_path.full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; @@ -1972,11 +1961,10 @@ bool GoalPlannerModule::hasEnoughDistance( void GoalPlannerModule::keepStoppedWithCurrentPath( const PullOverContextData & ctx_data, PathWithLaneId & path) const { - if (!ctx_data.selected_pull_over_path_opt) { + if (!ctx_data.last_path_idx_increment_time) { return; } - const auto last_path_idx_increment_time = - ctx_data.selected_pull_over_path_opt.value().last_path_idx_increment_time; + const auto last_path_idx_increment_time = ctx_data.last_path_idx_increment_time; constexpr double keep_stop_time = 2.0; if (!last_path_idx_increment_time) { return; @@ -2438,8 +2426,8 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); // Visualize path and related pose - if (context_data.selected_pull_over_path_opt) { - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); add( createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -2544,17 +2532,16 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = context_data.selected_pull_over_path_opt - ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = context_data.pull_over_path_opt ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = context_data.selected_pull_over_path_opt - ? context_data.selected_pull_over_path_opt.value().path.modified_goal_pose() + marker.pose = context_data.pull_over_path_opt + ? context_data.pull_over_path_opt.value().modified_goal_pose() : planner_data_->self_odometry->pose.pose; - if (context_data.selected_pull_over_path_opt) { - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); marker.text = magic_enum::enum_name(pull_over_path.type()); marker.text += " " + std::to_string(pull_over_path.path_idx()) + "/" + std::to_string(pull_over_path.partial_paths().size() - 1); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp index a6deacb8b43e0..6abfd714f4f89 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -20,15 +20,14 @@ namespace autoware::behavior_path_planner void LaneParkingRequest::update( const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, - const std::optional> & selected_pull_over_path, - const PathDecisionState & prev_data) + const std::optional & pull_over_path, const PathDecisionState & prev_data) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); current_status_ = current_status; last_previous_module_output_ = previous_module_output_; previous_module_output_ = previous_module_output; - selected_pull_over_path_ = selected_pull_over_path; + pull_over_path_ = pull_over_path; prev_data_ = prev_data; } @@ -37,7 +36,7 @@ LaneParkingRequest LaneParkingRequest::clone() const LaneParkingRequest request( parameters_, vehicle_footprint_, goal_candidates_, previous_module_output_); request.update( - *planner_data_, current_status_, previous_module_output_, selected_pull_over_path_, prev_data_); + *planner_data_, current_status_, previous_module_output_, pull_over_path_, prev_data_); return request; } @@ -59,14 +58,15 @@ void FreespaceParkingRequest::initializeOccupancyGridMap( void FreespaceParkingRequest::update( const PlannerData & planner_data, const ModuleStatus & current_status, - const std::optional> & selected_pull_over_path, - const bool is_stopped) + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); current_status_ = current_status; occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); - selected_pull_over_path_ = selected_pull_over_path; + pull_over_path_ = pull_over_path; + last_path_update_time_ = last_path_update_time; is_stopped_ = is_stopped; } @@ -74,7 +74,8 @@ FreespaceParkingRequest FreespaceParkingRequest::clone() const { FreespaceParkingRequest request( parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); - request.update(*planner_data_, current_status_, selected_pull_over_path_, is_stopped_); + request.update( + *planner_data_, current_status_, pull_over_path_, last_path_update_time_, is_stopped_); return request; }