Skip to content

Commit

Permalink
feat(intersection): ensure temporal stop before peeking into oncoming…
Browse files Browse the repository at this point in the history
… lane (#5047)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
soblin authored Sep 25, 2023
1 parent 770cf17 commit 9e4b4e9
Show file tree
Hide file tree
Showing 6 changed files with 78 additions and 20 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
possible_object_bbox: [1.5, 2.5] # [m x m]
ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h
stop_release_margin_time: 1.5 # [s]
temporal_stop_before_attention_area: false

enable_rtc:
intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
getOrDeclareParameter<double>(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold");
ip.occlusion.stop_release_margin_time =
getOrDeclareParameter<double>(node, ns + ".occlusion.stop_release_margin_time");
ip.occlusion.temporal_stop_before_attention_area =
getOrDeclareParameter<bool>(node, ns + ".occlusion.temporal_stop_before_attention_area");
}

void IntersectionModuleManager::launchNewModules(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,11 @@ IntersectionModule::IntersectionModule(
stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area);
stuck_private_area_timeout_.setState(StateMachine::State::STOP);
}
{
temporal_stop_before_attention_state_machine_.setMarginTime(
planner_param_.occlusion.before_creep_stop_time);
temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP);
}

decision_state_pub_ =
node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
Expand Down Expand Up @@ -480,7 +485,10 @@ void reactRTCApprovalByDecisionResult(
// NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved

if (!rtc_occlusion_approved && planner_param.occlusion.enable) {
const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx;
const size_t occlusion_peeking_stop_line =
decision_result.temporal_stop_before_attention_required
? decision_result.first_attention_stop_line_idx
: decision_result.occlusion_stop_line_idx;
if (planner_param.occlusion.enable_creeping) {
const size_t closest_idx = decision_result.closest_idx;
for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) {
Expand Down Expand Up @@ -544,7 +552,9 @@ void reactRTCApprovalByDecisionResult(
}
}
if (!rtc_occlusion_approved && planner_param.occlusion.enable) {
const auto stop_line_idx = decision_result.occlusion_stop_line_idx;
const auto stop_line_idx = decision_result.temporal_stop_before_attention_required
? decision_result.first_attention_stop_line_idx
: decision_result.occlusion_stop_line_idx;
planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path);
debug_data->occlusion_stop_wall_pose =
planning_utils::getAheadPose(stop_line_idx, baselink2front, *path);
Expand Down Expand Up @@ -784,7 +794,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
const auto & intersection_stop_lines = intersection_stop_lines_opt.value();
const auto
[closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt,
occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines;
first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] =
intersection_stop_lines;

const auto & conflicting_area = intersection_lanelets_.value().conflicting_area();
const auto path_lanelets_opt = util::generatePathLanelets(
Expand Down Expand Up @@ -866,12 +877,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
return IntersectionModule::Indecisive{};
}

if (!occlusion_peeking_stop_line_idx_opt) {
if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) {
RCLCPP_DEBUG(logger_, "occlusion stop line is null");
return IntersectionModule::Indecisive{};
}
const auto collision_stop_line_idx =
is_over_default_stop_line ? closest_idx : default_stop_line_idx;
const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value();
const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value();

const auto & attention_lanelets = intersection_lanelets_.value().attention();
Expand Down Expand Up @@ -949,35 +961,62 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
if (
occlusion_stop_state_machine_.getState() == StateMachine::State::STOP ||
ext_occlusion_requested) {
const double dist_stopline = motion_utils::calcSignedArcLength(
const double dist_default_stopline = motion_utils::calcSignedArcLength(
path->points, path->points.at(closest_idx).point.pose.position,
path->points.at(default_stop_line_idx).point.pose.position);
const bool approached_stop_line =
(std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin);
const bool over_stop_line = (dist_stopline < 0.0);
const bool is_stopped =
const bool approached_default_stop_line =
(std::fabs(dist_default_stopline) < planner_param_.common.stop_overshoot_margin);
const bool over_default_stop_line = (dist_default_stopline < 0.0);
const bool is_stopped_at_default =
planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time);
if (over_stop_line) {
if (over_default_stop_line) {
before_creep_state_machine_.setState(StateMachine::State::GO);
}
if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
const double dist_first_attention_stopline = motion_utils::calcSignedArcLength(
path->points, path->points.at(closest_idx).point.pose.position,
path->points.at(first_attention_stop_line_idx).point.pose.position);
const bool approached_first_attention_stop_line =
(std::fabs(dist_first_attention_stopline) < planner_param_.common.stop_overshoot_margin);
const bool over_first_attention_stop_line = (dist_first_attention_stopline < 0.0);
const bool is_stopped_at_first_attention =
planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time);
if (planner_param_.occlusion.temporal_stop_before_attention_area) {
if (over_first_attention_stop_line) {
temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO);
}
if (is_stopped_at_first_attention && approached_first_attention_stop_line) {
temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO);
}
}
const bool temporal_stop_before_attention_required =
planner_param_.occlusion.temporal_stop_before_attention_area &&
temporal_stop_before_attention_state_machine_.getState() == StateMachine::State::STOP;
if (has_collision_with_margin) {
return IntersectionModule::OccludedCollisionStop{
is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx,
occlusion_stop_line_idx};
return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin,
temporal_stop_before_attention_required,
closest_idx,
collision_stop_line_idx,
first_attention_stop_line_idx,
occlusion_stop_line_idx};
} else {
return IntersectionModule::PeekingTowardOcclusion{
is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx,
occlusion_stop_line_idx};
return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin,
temporal_stop_before_attention_required,
closest_idx,
collision_stop_line_idx,
first_attention_stop_line_idx,
occlusion_stop_line_idx};
}
} else {
if (is_stopped && approached_stop_line) {
if (is_stopped_at_default && approached_default_stop_line) {
// start waiting at the first stop line
before_creep_state_machine_.setState(StateMachine::State::GO);
}
const auto occlusion_stop_line = planner_param_.occlusion.temporal_stop_before_attention_area
? first_attention_stop_line_idx
: occlusion_stop_line_idx;
return IntersectionModule::FirstWaitBeforeOcclusion{
is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx,
occlusion_stop_line_idx};
is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line};
}
} else if (has_collision_with_margin) {
return IntersectionModule::NonOccludedCollisionStop{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,7 @@ class IntersectionModule : public SceneModuleInterface
std::vector<double> possible_object_bbox;
double ignore_parked_vehicle_speed_threshold;
double stop_release_margin_time;
bool temporal_stop_before_attention_area;
} occlusion;
};

Expand Down Expand Up @@ -141,15 +142,19 @@ class IntersectionModule : public SceneModuleInterface
// NOTE: if intersection_occlusion is disapproved externally through RTC,
// it indicates "is_forcefully_occluded"
bool is_actually_occlusion_cleared{false};
bool temporal_stop_before_attention_required{false};
size_t closest_idx{0};
size_t collision_stop_line_idx{0};
size_t first_attention_stop_line_idx{0};
size_t occlusion_stop_line_idx{0};
};
struct OccludedCollisionStop
{
bool is_actually_occlusion_cleared{false};
bool temporal_stop_before_attention_required{false};
size_t closest_idx{0};
size_t collision_stop_line_idx{0};
size_t first_attention_stop_line_idx{0};
size_t occlusion_stop_line_idx{0};
};
struct Safe
Expand Down Expand Up @@ -222,6 +227,7 @@ class IntersectionModule : public SceneModuleInterface
StateMachine collision_state_machine_; //! for stable collision checking
StateMachine before_creep_state_machine_; //! for two phase stop
StateMachine occlusion_stop_state_machine_;
StateMachine temporal_stop_before_attention_state_machine_;
// NOTE: uuid_ is base member

// for stuck vehicle detection
Expand Down
8 changes: 8 additions & 0 deletions planning/behavior_velocity_intersection_module/src/util.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -279,6 +279,8 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
}
}
}
const auto first_attention_stop_line_ip = static_cast<size_t>(occlusion_peeking_line_ip_int);
const bool first_attention_stop_line_valid = occlusion_peeking_line_valid;
occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds);
const auto occlusion_peeking_line_ip = static_cast<size_t>(
std::clamp<int>(occlusion_peeking_line_ip_int, 0, static_cast<int>(path_ip.points.size()) - 1));
Expand Down Expand Up @@ -324,6 +326,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
size_t closest_idx{0};
size_t stuck_stop_line{0};
size_t default_stop_line{0};
size_t first_attention_stop_line{0};
size_t occlusion_peeking_stop_line{0};
size_t pass_judge_line{0};
};
Expand All @@ -333,6 +336,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
{&closest_idx_ip, &intersection_stop_lines_temp.closest_idx},
{&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line},
{&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line},
{&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line},
{&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line},
{&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line},
};
Expand Down Expand Up @@ -367,6 +371,10 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
if (default_stop_line_valid) {
intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line;
}
if (first_attention_stop_line_valid) {
intersection_stop_lines.first_attention_stop_line =
intersection_stop_lines_temp.first_attention_stop_line;
}
if (occlusion_peeking_line_valid) {
intersection_stop_lines.occlusion_peeking_stop_line =
intersection_stop_lines_temp.occlusion_peeking_stop_line;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -128,8 +128,10 @@ struct IntersectionStopLines
size_t closest_idx{0};
// NOTE: null if path does not conflict with first_conflicting_area
std::optional<size_t> stuck_stop_line{std::nullopt};
// NOTE: null if path is over map stop_line OR its value is calculated negative area
// NOTE: null if path is over map stop_line OR its value is calculated negative
std::optional<size_t> default_stop_line{std::nullopt};
// NOTE: null if the index is calculated negative
std::optional<size_t> first_attention_stop_line{std::nullopt};
// NOTE: null if footprints do not change from outside to inside of detection area
std::optional<size_t> occlusion_peeking_stop_line{std::nullopt};
// if the value is calculated negative, its value is 0
Expand Down

0 comments on commit 9e4b4e9

Please sign in to comment.