Skip to content

Commit

Permalink
does not do pullover
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Dec 12, 2024
1 parent a235d71 commit 64c0ae0
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 25 deletions.
36 changes: 25 additions & 11 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,8 @@ struct PullOverContextData
explicit PullOverContextData(
const bool is_stable_safe_path, const PredictedObjects & static_objects,
const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state,
const bool is_stopped, LaneParkingResponse && lane_parking_response,
FreespaceParkingResponse && freespace_parking_response)
const bool is_stopped, const LaneParkingResponse & lane_parking_response,
const FreespaceParkingResponse & freespace_parking_response)
: is_stable_safe_path(is_stable_safe_path),
static_target_objects(static_objects),
dynamic_target_objects(dynamic_objects),
Expand All @@ -99,18 +99,32 @@ struct PullOverContextData
freespace_parking_response(freespace_parking_response)
{
}
const bool is_stable_safe_path;
const PredictedObjects static_target_objects;
const PredictedObjects dynamic_target_objects;
const PathDecisionState prev_state_for_debug;
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),
// and bind following three member, rename as "SelectedPullOverPath"
// TODO(soblin): make following variables private
bool is_stable_safe_path;
PredictedObjects static_target_objects;
PredictedObjects dynamic_target_objects;
PathDecisionState prev_state_for_debug;
bool is_stopped;
LaneParkingResponse lane_parking_response;
FreespaceParkingResponse freespace_parking_response;
std::optional<PullOverPath> pull_over_path_opt;
std::optional<rclcpp::Time> last_path_update_time;
std::optional<rclcpp::Time> last_path_idx_increment_time;

void update(
const bool is_stable_safe_path_, const PredictedObjects static_target_objects_,
const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_,
const bool is_stopped_, const LaneParkingResponse & lane_parking_response_,
const FreespaceParkingResponse & freespace_parking_response_)
{
is_stable_safe_path = is_stable_safe_path_;
static_target_objects = static_target_objects_;
dynamic_target_objects = dynamic_target_objects_;
prev_state_for_debug = prev_state_for_debug_;
is_stopped = is_stopped_;
lane_parking_response = lane_parking_response_;
freespace_parking_response = freespace_parking_response_;
}
};

bool isOnModifiedGoal(
Expand Down
47 changes: 33 additions & 14 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -329,7 +329,7 @@ void LaneParkingPlanner::onTimer()
std::lock_guard<std::mutex> guard(mutex_);
if (request_) {
auto & request = request_.value();
local_request_opt.emplace(std::move(request));
local_request_opt.emplace(request);
}
}
// end of critical section
Expand Down Expand Up @@ -467,9 +467,11 @@ void LaneParkingPlanner::onTimer()
// set response
{
std::lock_guard<std::mutex> guard(mutex_);
response_.pull_over_path_candidates = std::move(path_candidates);
response_.pull_over_path_candidates = path_candidates;
response_.closest_start_pose = closest_start_pose;
RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size());
RCLCPP_INFO(
getLogger(), "generated %lu pull over path candidates",
response_.pull_over_path_candidates.size());
}
}

Expand Down Expand Up @@ -605,7 +607,7 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
// `planner_data_.is_route_handler_updated` variable is set true by behavior_path_planner
// (although this flag is not implemented yet). In that case, gp_planner_data members except
// for route_handler should be copied from planner_data_
lane_parking_response = std::move(lane_parking_response_);
lane_parking_response = lane_parking_response_;
}

FreespaceParkingResponse freespace_parking_response;
Expand All @@ -628,7 +630,7 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
// is thread-safe because gp_planner_data.occupancy_grid_map is only re-pointed here and its
// prior resource is still owned by the onFreespaceParkingTimer thread locally.
occupancy_grid_map_ = freespace_parking_request.get_occupancy_grid_map();
freespace_parking_response = std::move(freespace_parking_response_);
freespace_parking_response = freespace_parking_response_;
}
// end of critical section
return {lane_parking_response, freespace_parking_response};
Expand Down Expand Up @@ -704,13 +706,23 @@ void GoalPlannerModule::updateData()
modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_,
goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded);

context_data_.emplace(
path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
dynamic_target_objects, prev_decision_state,
isStopped(
odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time,
parameters_->th_stopped_velocity),
std::move(lane_parking_response), std::move(freespace_parking_response));
if (context_data_) {
context_data_.value().update(
path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
dynamic_target_objects, prev_decision_state,
isStopped(
odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time,
parameters_->th_stopped_velocity),
lane_parking_response, freespace_parking_response);
} else {
context_data_.emplace(
path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
dynamic_target_objects, prev_decision_state,
isStopped(
odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time,
parameters_->th_stopped_velocity),
lane_parking_response, freespace_parking_response);
}
auto & ctx_data = context_data_.value();

if (!isActivated()) {
Expand Down Expand Up @@ -1259,7 +1271,7 @@ void GoalPlannerModule::setOutput(
return;
}

const auto & pull_over_path = context_data.pull_over_path_opt.value();
const auto & pull_over_path = selected_pull_over_path_with_velocity_opt.value();
if (
parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path &&
isActivated()) {
Expand Down Expand Up @@ -1460,7 +1472,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData
// 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");
RCLCPP_INFO(
getLogger(), "Update %lu pull over path candidates",
context_data.lane_parking_response.pull_over_path_candidates.size());

context_data.pull_over_path_opt = std::nullopt;
context_data.last_path_update_time = std::nullopt;
Expand Down Expand Up @@ -1496,6 +1510,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData
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()
Expand Down Expand Up @@ -1554,6 +1569,8 @@ void GoalPlannerModule::postProcess()
path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED;

if (!context_data.pull_over_path_opt) {
// TODO(soblin): now context_data has part of thread_safe_data, so must not be nullfied because
// this function is called in every cycle
context_data_ = std::nullopt;
return;
}
Expand All @@ -1572,6 +1589,8 @@ void GoalPlannerModule::postProcess()

setVelocityFactor(pull_over_path.full_path());

// TODO(soblin): now context_data has part of thread_safe_data, so must not be nullfied because
// this function is called in every cycle
context_data_ = std::nullopt;
}

Expand Down

0 comments on commit 64c0ae0

Please sign in to comment.