Skip to content

Commit

Permalink
fix(behavior_path_planner): is_waiting_approval (tier4#1326)
Browse files Browse the repository at this point in the history 
* fix(behavior_path_planner): is_waiting_approval

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix typos

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55 @boyali
kosuke55 authored and boyali committed Oct 3, 2022
1 parent 9bbfd92 commit 39221db
Show file tree
Hide file tree
Showing 9 changed files with 32 additions and 26 deletions.
2 changes: 1 addition & 1 deletion ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,7 @@ class PullOverModule : public SceneModuleInterface
Pose modified_goal_pose_;
std::vector<GoalCandidate> goal_candidates_;
GeometricParallelParking parallel_parking_planner_;
ParallelParkingParameters parallel_parking_prameters_;
ParallelParkingParameters parallel_parking_parameters_;
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
std::unique_ptr<LaneDepartureChecker> lane_departure_checker_;
std::unique_ptr<rclcpp::Time> last_received_time_;
Expand Down
2 changes: 1 addition & 1 deletion ...ath_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ struct SceneModuleStatus
explicit SceneModuleStatus(const std::string & n) : module_name(n) {}
std::string module_name; // TODO(Horibe) should be const
bool is_requested{false};
bool is_waiting_approval{true};
bool is_waiting_approval{false};
BT::NodeStatus status{BT::NodeStatus::IDLE};
};

Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
perception_subscriber_ = create_subscription<PredictedObjects>(
"~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1),
createSubscriptionOptions(this));
// todo: chnage to ~/input
// todo: change to ~/input
occupancy_grid_subscriber_ = create_subscription<OccupancyGrid>(
"/perception/occupancy_grid_map/map", 1,
std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1),
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2183,6 +2183,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval()
const auto candidate = planCandidate();
out.path_candidate = std::make_shared<PathWithLaneId>(candidate.path_candidate);
updateRTCStatus(candidate);
waitApproval();
return out;
}

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,7 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval()
const auto candidate = planCandidate();
out.path_candidate = std::make_shared<PathWithLaneId>(candidate.path_candidate);
updateRTCStatus(candidate);
waitApproval();
return out;
}

Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,6 +243,8 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()

out.path_candidate = std::make_shared<PathWithLaneId>(candidate_path);

waitApproval();

return out;
}

Expand Down
45 changes: 22 additions & 23 deletions planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,13 +109,13 @@ void PullOverModule::onEntry()
occupancy_grid_map_param.obstacle_threshold = parameters_.obstacle_threshold;
occupancy_grid_map_.setParam(occupancy_grid_map_param);

// inititialize when receiving new route
// initialize when receiving new route
if (
last_received_time_.get() == nullptr ||
*last_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) {
// Initialize parallel parking planner sratus
// Initialize parallel parking planner status
parallel_parking_planner_.clear();
parallel_parking_prameters_ = ParallelParkingParameters{
parallel_parking_parameters_ = ParallelParkingParameters{
parameters_.th_arrived_distance_m,
parameters_.th_stopped_velocity_mps,
parameters_.after_forward_parking_straight_distance,
Expand All @@ -138,8 +138,6 @@ void PullOverModule::onEntry()
goal_candidate.distance_from_original_goal = 0.0;
goal_candidates_.push_back(goal_candidate);
}

waitApproval();
}

void PullOverModule::onExit()
Expand Down Expand Up @@ -240,13 +238,13 @@ void PullOverModule::researchGoal()
pull_over_areas_.clear();
const Pose goal_pose_map_coords = global2local(occupancy_grid_map_.getMap(), goal_pose);
Pose start_pose = calcOffsetPose(goal_pose, dx, 0, 0);
// Serch non collision areas around the goal
// Search non collision areas around the goal
while (true) {
bool is_last_search = (dx >= parameters_.forward_goal_search_length);
Pose serach_pose = calcOffsetPose(goal_pose_map_coords, dx, 0, 0);
Pose search_pose = calcOffsetPose(goal_pose_map_coords, dx, 0, 0);
bool is_collided = occupancy_grid_map_.detectCollision(
pose2index(occupancy_grid_map_.getMap(), serach_pose, common_param.theta_size), false);
// Add area when (1) chnage non-collision -> collison or (2) last serach without collision
pose2index(occupancy_grid_map_.getMap(), search_pose, common_param.theta_size), false);
// Add area when (1) change non-collision -> collision or (2) last search without collision
if ((!prev_is_collided && is_collided) || (!is_collided && is_last_search)) {
Pose end_pose = calcOffsetPose(goal_pose, dx, 0, 0);
if (!pull_over_areas_.empty()) {
Expand Down Expand Up @@ -295,7 +293,7 @@ void PullOverModule::researchGoal()

BT::NodeStatus PullOverModule::updateState()
{
// pull_out moudule will be run when setting new goal, so not need finishing pull_over module.
// pull_out module will be run when setting new goal, so not need finishing pull_over module.
// Finishing it causes wrong lane_following path generation.
// if (hasFinishedPullOver()) {
// current_state_ = BT::NodeStatus::SUCCESS;
Expand Down Expand Up @@ -355,7 +353,7 @@ bool PullOverModule::planWithEfficientPath()
if (parameters_.enable_arc_forward_parking) {
for (const auto goal_candidate : goal_candidates_) {
modified_goal_pose_ = goal_candidate.goal_pose;
parallel_parking_planner_.setParams(planner_data_, parallel_parking_prameters_);
parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_);
if (
parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, true) &&
isLongEnoughToParkingStart(
Expand All @@ -376,7 +374,7 @@ bool PullOverModule::planWithEfficientPath()
if (parameters_.enable_arc_backward_parking) {
for (const auto goal_candidate : goal_candidates_) {
modified_goal_pose_ = goal_candidate.goal_pose;
parallel_parking_planner_.setParams(planner_data_, parallel_parking_prameters_);
parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_);
if (
parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, false) &&
isLongEnoughToParkingStart(
Expand Down Expand Up @@ -415,7 +413,7 @@ bool PullOverModule::planWithCloseGoal()
return true;
}

parallel_parking_planner_.setParams(planner_data_, parallel_parking_prameters_);
parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_);
// Generate arc forward path.
if (
parameters_.enable_arc_forward_parking &&
Expand Down Expand Up @@ -479,7 +477,7 @@ BehaviorModuleOutput PullOverModule::plan()
status_.lanes.insert(
status_.lanes.end(), status_.pull_over_lanes.begin(), status_.pull_over_lanes.end());

// Check if it needs to deciede path
// Check if it needs to decide path
if (status_.is_safe) {
const Pose parking_start_pose = getParkingStartPose();
const auto dist_to_parking_start_pose = calcSignedArcLength(
Expand All @@ -494,7 +492,7 @@ BehaviorModuleOutput PullOverModule::plan()
// Use decided path
if (status_.has_decided_path) {
if (!status_.has_requested_approval_) {
// request approval again one the final parth is decided
// request approval again one the final path is decided
waitApproval();
removeRTCStatus();
uuid_ = generateUUID();
Expand All @@ -504,7 +502,7 @@ BehaviorModuleOutput PullOverModule::plan()
clearWaitingApproval();
last_approved_time_ = std::make_unique<rclcpp::Time>(clock_->now());

// decide velocity to guarantee turn singnal lighting time
// decide velocity to guarantee turn signal lighting time
if (!status_.has_decided_velocity) {
const float vel = static_cast<float>(std::max(
planner_data_->self_odometry->twist.twist.linear.x,
Expand All @@ -518,7 +516,7 @@ BehaviorModuleOutput PullOverModule::plan()

if (isActivated() && isArcPath() && last_approved_time_ != nullptr) {
// if using arc_path and finishing current_path, get next path
// enough time for turn singal
// enough time for turn signal
const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() >
planner_data_->parameters.turn_light_on_threshold_time;

Expand All @@ -529,7 +527,7 @@ BehaviorModuleOutput PullOverModule::plan()
}

} else { // Replan shift -> arc forward -> arc backward path with each goal candidate.
// Research goal when enabling research and final path has not been decieded
// Research goal when enabling research and final path has not been decided
if (parameters_.enable_goal_research) researchGoal();

status_.path_type = PathType::NONE;
Expand All @@ -542,7 +540,7 @@ BehaviorModuleOutput PullOverModule::plan()
has_found_safe_path = planWithCloseGoal();
} else {
RCLCPP_ERROR(
getLogger(), "search_priority shoudbe efficient_path or close_goal, but %s is given.",
getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.",
parameters_.search_priority.c_str());
}

Expand All @@ -563,7 +561,7 @@ BehaviorModuleOutput PullOverModule::plan()
output.path = status_.is_safe ? std::make_shared<PathWithLaneId>(status_.path)
: std::make_shared<PathWithLaneId>(getStopPath());

// set hazard and turn singnal
// set hazard and turn signal
if (status_.has_decided_path) {
const auto hazard_info = getHazardInfo();
const auto turn_info = getTurnInfo();
Expand Down Expand Up @@ -591,8 +589,8 @@ BehaviorModuleOutput PullOverModule::plan()
}

// This const function can not change the menber variables like the goal.
// so implement generating candidate path in planWaitingApprval().
// No specific path for the cadidte. It's same to the one generated by plan().
// so implement generating candidate path in planWaitingApproval().
// No specific path for the candidate. It's same to the one generated by plan().
CandidateOutput PullOverModule::planCandidate() const { return CandidateOutput{}; }

BehaviorModuleOutput PullOverModule::planWaitingApproval()
Expand All @@ -608,6 +606,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval()

const double distance_to_path_change = calcDistanceToPathChange();
updateRTCStatus(distance_to_path_change);
waitApproval();

return out;
}
Expand Down Expand Up @@ -682,7 +681,7 @@ PathWithLaneId PullOverModule::getReferencePath() const
status_.current_lanes, s_backward, arc_position_stop_pose.length, true);
reference_path.header = route_handler->getRouteHeader();

// slow donw for turn singnal, insert stop point to stop_pose
// slow down for turn signal, insert stop point to stop_pose
reference_path = util::setDecelerationVelocityForTurnSignal(
reference_path, stop_pose, planner_data_->parameters.turn_light_on_threshold_time);

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
auto current_status = BT::NodeStatus::RUNNING;

scene_module_->onEntry();
module_status_->is_waiting_approval = scene_module_->isWaitingApproval();

const bool is_lane_following = scene_module_->name() == "LaneFollowing";

Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,8 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval()

prev_output_ = shifted_path;

waitApproval();

return output;
}

Expand Down

0 comments on commit 39221db

Please sign in to comment.