Skip to content

Commit

Permalink
Merge pull request #402 from tier4/hotfix/pr3539
Browse files Browse the repository at this point in the history 
fix(behavior_velocity_planner::intersection): strictly wait at 1st stop line when occlusion is detected
  • Loading branch information
@shmpwk
shmpwk authored May 1, 2023
2 parents 9fbb8d9 + 3117b3a commit 0e5f645
Show file tree
Hide file tree
Showing 3 changed files with 56 additions and 44 deletions.
7 changes: 4 additions & 3 deletions planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,7 @@ class IntersectionModule : public SceneModuleInterface
{
occlusion_first_stop_activated_ = activation;
}
bool isOccluded() const { return is_occluded_; }
bool isOccluded() const { return is_actually_occluded_ || is_forcefully_occluded_; }

private:
rclcpp::Node & node_;
Expand All @@ -175,7 +175,8 @@ class IntersectionModule : public SceneModuleInterface
// for occlusion detection
const bool enable_occlusion_detection_;
std::optional<std::vector<util::DetectionLaneDivision>> detection_divisions_;
bool is_occluded_ = false;
bool is_actually_occluded_ = false; //! occlusion based on occupancy_grid
bool is_forcefully_occluded_ = false; //! fake occlusion forced by external operator
OcclusionState occlusion_state_ = OcclusionState::NONE;
// NOTE: uuid_ is base member
// for occlusion clearance decision
Expand All @@ -184,7 +185,7 @@ class IntersectionModule : public SceneModuleInterface
double occlusion_stop_distance_;
bool occlusion_activated_ = true;
// for first stop in two-phase stop
const UUID occlusion_first_stop_uuid_; // TODO(Mamoru Sobue): replace with uuid_
const UUID occlusion_first_stop_uuid_;
bool occlusion_first_stop_safety_ = true;
double occlusion_first_stop_distance_;
bool occlusion_first_stop_activated_ = true;
Expand Down
11 changes: 10 additions & 1 deletion planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,8 +128,17 @@ void IntersectionModuleManager::launchNewModules(
const auto new_module = std::make_shared<IntersectionModule>(
module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection,
node_, logger_.get_child("intersection_module"), clock_);
registerModule(std::move(new_module));
generateUUID(module_id);
/* set RTC status as non_occluded status initially */
const UUID uuid = getUUID(new_module->getModuleId());
const auto occlusion_uuid = new_module->getOcclusionUUID();
rtc_interface_.updateCooperateStatus(
uuid, true, std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(),
clock_->now());
occlusion_rtc_interface_.updateCooperateStatus(
occlusion_uuid, true, std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::lowest(), clock_->now());
registerModule(std::move(new_module));
}
}

Expand Down
82 changes: 42 additions & 40 deletions planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -260,40 +260,55 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
planner_param_.occlusion.peeking_offset)
: std::nullopt;

/* a flag if front stop line is not occlusion */
bool stuck_stop_required = false;
bool collision_stop_required = false;
bool first_phase_stop_required = false;
bool occlusion_stop_required = false;

/* calculate final stop lines */
std::optional<size_t> stop_line_idx = default_stop_line_idx_opt;
std::optional<size_t> occlusion_peeking_line_idx =
default_stop_line_idx_opt; // TODO(Mamoru Sobue): maybe different position depending on the
// flag
occlusion_peeking_line_idx_opt; // TODO(Mamoru Sobue): different position depending on the flag
std::optional<size_t> occlusion_first_stop_line_idx = default_stop_line_idx_opt;
std::optional<std::pair<size_t, size_t>> insert_creep_during_occlusion = std::nullopt;
if (!is_occlusion_cleared) {

/* set RTC distance */
const double dist_1st_stopline =
default_stop_line_idx_opt
? motion_utils::calcSignedArcLength(
path_ip.points, current_pose.position,
path->points.at(default_stop_line_idx_opt.value()).point.pose.position)
: std::numeric_limits<double>::lowest();
const double dist_2nd_stopline =
occlusion_peeking_line_idx
? motion_utils::calcSignedArcLength(
path_ip.points, current_pose.position,
path->points.at(occlusion_peeking_line_idx.value()).point.pose.position)
: std::numeric_limits<double>::lowest();

bool stuck_stop_required = false;
bool collision_stop_required = false;
bool first_phase_stop_required = false;
bool occlusion_stop_required = false;

/* check safety */
const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_);
is_actually_occluded_ = !is_occlusion_cleared;
is_forcefully_occluded_ = ext_occlusion_requested;
if (!is_occlusion_cleared || ext_occlusion_requested) {
if (!default_stop_line_idx_opt) {
RCLCPP_DEBUG(logger_, "occlusion is detected but default stop line is not set or generated");
RCLCPP_DEBUG(logger_, "===== plan end =====");
return true;
}
if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
if (
before_creep_state_machine_.getState() == StateMachine::State::GO &&
!ext_occlusion_requested) {
occlusion_stop_required = true;
stop_line_idx = occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;
// clear first stop line
// insert creep velocity [closest_idx, occlusion_stop_line)
insert_creep_during_occlusion =
std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value());
is_occluded_ = true;
occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
} else {
const bool approached_stop_line =
motion_utils::calcSignedArcLength(
path_ip.points, current_pose.position,
path->points.at(default_stop_line_idx_opt.value()).point.pose.position) <
planner_param_.common.stop_overshoot_margin;
(dist_1st_stopline < planner_param_.common.stop_overshoot_margin);
const bool is_stopped = planner_data_->isVehicleStopped();
if (is_stopped && approached_stop_line) {
// start waiting at the first stop line
Expand All @@ -308,10 +323,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
// insert creep velocity [default_stop_line, occlusion_stop_line)
insert_creep_during_occlusion =
std::make_pair(default_stop_line_idx_opt.value(), occlusion_peeking_line_idx_opt.value());
is_occluded_ = true;
occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE;
}
} else if (is_occluded_) {
} else if (occlusion_state_ != OcclusionState::CLEARED) {
// previously occlusion existed, but now it is clear
if (!util::isOverTargetIndex(
*path, closest_idx, current_pose, default_stop_line_idx_opt.value())) {
Expand All @@ -323,7 +337,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
stop_line_idx = static_pass_judge_line_opt;
}
occlusion_state_ = OcclusionState::CLEARED;
is_occluded_ = false;
if (stop_line_idx && has_collision) {
// do collision checking at previous occlusion stop line
collision_stop_required = true;
Expand Down Expand Up @@ -368,30 +381,25 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
: StateMachine::State::GO,
logger_.get_child("collision state_machine"), *clock_);

/* set RTC request respectively */
/* set RTC safety respectively */
if (occlusion_stop_required) {
if (first_phase_stop_required) {
occlusion_first_stop_safety_ = false;
occlusion_first_stop_distance_ = motion_utils::calcSignedArcLength(
path->points, planner_data_->current_odometry->pose.position,
path->points.at(stop_line_idx.value()).point.pose.position);
occlusion_first_stop_distance_ = dist_1st_stopline;
}
occlusion_safety_ = false;
occlusion_stop_distance_ = motion_utils::calcSignedArcLength(
path->points, planner_data_->current_odometry->pose.position,
path->points.at(occlusion_peeking_line_idx.value()).point.pose.position);
occlusion_stop_distance_ = dist_2nd_stopline;
} else {
/* collision */
setSafe(collision_state_machine_.getState() == StateMachine::State::GO);
setDistance(motion_utils::calcSignedArcLength(
path->points, planner_data_->current_odometry->pose.position,
path->points.at(stop_line_idx.value()).point.pose.position));
setDistance(dist_1st_stopline);
}

/* make decision */
const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
if (!occlusion_activated_) {
is_go_out_ = false;

/* in case of creeping */
if (insert_creep_during_occlusion && planner_param_.occlusion.enable_creeping) {
const auto [start, end] = insert_creep_during_occlusion.value();
Expand All @@ -408,14 +416,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
planning_utils::getAheadPose(occlusion_first_stop_line_idx.value(), baselink2front, *path);
}

const auto reconciled_occlusion_peeking_line_idx =
occlusion_stop_required
? occlusion_peeking_line_idx.value()
: stop_line_idx.value(); // because intersection module may miss real occlusion
planning_utils::setVelocityFromIndex(
reconciled_occlusion_peeking_line_idx, 0.0 /* [m/s] */, path);
debug_data_.occlusion_stop_wall_pose =
planning_utils::getAheadPose(reconciled_occlusion_peeking_line_idx, baselink2front, *path);
if (occlusion_peeking_line_idx) {
planning_utils::setVelocityFromIndex(
occlusion_peeking_line_idx.value(), 0.0 /* [m/s] */, path);
debug_data_.occlusion_stop_wall_pose =
planning_utils::getAheadPose(occlusion_peeking_line_idx.value(), baselink2front, *path);
}

RCLCPP_DEBUG(logger_, "not activated. stop at the line.");
RCLCPP_DEBUG(logger_, "===== plan end =====");
Expand Down Expand Up @@ -444,10 +450,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
}

RCLCPP_DEBUG(logger_, "not activated. stop at the line.");
RCLCPP_DEBUG(logger_, "===== plan end =====");
return true;
}

is_go_out_ = true;
Expand Down

0 comments on commit 0e5f645

Please sign in to comment.