Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(intersection): fix occlusion rtc distance #3539

Merged
merged 4 commits into from
Apr 26, 2023
Merged

fix(intersection): fix occlusion rtc distance #3539

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
999968d
always set RTC distance to default stop line/occlusion stop line
soblin Apr 26, 2023
ab8d519
initialize RTC status as non occluded at intersection module construc…
soblin Apr 26, 2023
b70057e
removed debug print
soblin Apr 26, 2023
0d69ae7
keep waiting at 1st stop lineif externally occlusion is requested
soblin Apr 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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