Skip to content

Commit

Permalink
feat(intersection): switch default/occlusion_first_stop when occlusio…
Browse files Browse the repository at this point in the history 
…n appeared/disappeared or approved/disapproved (autowarefoundation#3664)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Jun 13, 2023
1 parent 224a22c commit d29cf55
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 84 deletions.
12 changes: 2 additions & 10 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 @@ -155,15 +155,9 @@ class IntersectionModule : public SceneModuleInterface
UUID getOcclusionUUID() const { return occlusion_uuid_; }
bool getOcclusionSafety() const { return occlusion_safety_; }
double getOcclusionDistance() const { return occlusion_stop_distance_; }
UUID getOcclusionFirstStopUUID() const { return occlusion_first_stop_uuid_; }
bool getOcclusionFirstStopSafety() const { return occlusion_first_stop_safety_; }
double getOcclusionFirstStopDistance() const { return occlusion_first_stop_distance_; }
void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
void setOcclusionFirstStopActivation(const bool activation)
{
occlusion_first_stop_activated_ = activation;
}
bool isOccluded() const { return is_actually_occluded_ || is_forcefully_occluded_; }
bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; }

private:
rclcpp::Node & node_;
Expand Down Expand Up @@ -191,9 +185,7 @@ class IntersectionModule : public SceneModuleInterface
bool occlusion_activated_ = true;
// for first stop in two-phase stop
const UUID occlusion_first_stop_uuid_;
bool occlusion_first_stop_safety_ = true;
double occlusion_first_stop_distance_;
bool occlusion_first_stop_activated_ = true;
bool occlusion_first_stop_required_ = false;

StateMachine collision_state_machine_; //! for stable collision checking
StateMachine before_creep_state_machine_; //! for two phase stop
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,7 +217,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker
&wall_marker, now);
uid++;
}
if (!occlusion_first_stop_activated_) {
if (!activated_ && occlusion_first_stop_required_) {
appendMarkerArray(
motion_utils::createStopVirtualWallMarker(
debug_data_.occlusion_first_stop_wall_pose, "intersection", now, uid),
Expand Down
46 changes: 9 additions & 37 deletions planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,32 +186,17 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister

void IntersectionModuleManager::sendRTC(const Time & stamp)
{
rtc_interface_.clearCooperateStatus();
for (const auto & scene_module : scene_modules_) {
const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
const bool is_occluded = intersection_module->isOccluded();
const UUID uuid = getUUID(scene_module->getModuleId());
const bool safety =
scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired());
updateRTCStatus(uuid, safety, scene_module->getDistance(), stamp);
const auto occlusion_uuid = intersection_module->getOcclusionUUID();
const auto occlusion_distance = intersection_module->getOcclusionDistance();
const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID();
if (!is_occluded) {
// default
updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
occlusion_rtc_interface_.updateCooperateStatus(
occlusion_uuid, true, occlusion_distance, occlusion_distance, stamp);
} else {
// occlusion
const auto occlusion_safety = intersection_module->getOcclusionSafety();
occlusion_rtc_interface_.updateCooperateStatus(
occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);

const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety();
const auto occlusion_first_stop_distance =
intersection_module->getOcclusionFirstStopDistance();
rtc_interface_.updateCooperateStatus(
occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance,
occlusion_first_stop_distance, stamp);
}
const auto occlusion_safety = intersection_module->getOcclusionSafety();
occlusion_rtc_interface_.updateCooperateStatus(
occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
}
rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus()
occlusion_rtc_interface_.publishCooperateStatus(stamp);
Expand All @@ -222,20 +207,9 @@ void IntersectionModuleManager::setActivation()
for (const auto & scene_module : scene_modules_) {
const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
const auto occlusion_uuid = intersection_module->getOcclusionUUID();
const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID();
const bool is_occluded = intersection_module->isOccluded();
if (!is_occluded) {
// default
scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId())));
intersection_module->setOcclusionActivation(
occlusion_rtc_interface_.isActivated(occlusion_uuid));
} else {
// occlusion
intersection_module->setOcclusionActivation(
occlusion_rtc_interface_.isActivated(occlusion_uuid));
intersection_module->setOcclusionFirstStopActivation(
rtc_interface_.isActivated(occlusion_first_stop_uuid));
}
scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId())));
intersection_module->setOcclusionActivation(
occlusion_rtc_interface_.isActivated(occlusion_uuid));
}
}

Expand All @@ -256,9 +230,7 @@ void IntersectionModuleManager::deleteExpiredModules(
// occlusion
const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
const auto occlusion_uuid = intersection_module->getOcclusionUUID();
const auto occlusion_first_uuid = intersection_module->getOcclusionFirstStopUUID();
occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid);
rtc_interface_.removeCooperateStatus(occlusion_first_uuid);
unregisterModule(scene_module);
}
}
Expand Down
58 changes: 22 additions & 36 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 @@ -70,8 +70,7 @@ IntersectionModule::IntersectionModule(
assoc_ids_(assoc_ids),
enable_occlusion_detection_(enable_occlusion_detection),
detection_divisions_(std::nullopt),
occlusion_uuid_(tier4_autoware_utils::generateUUID()),
occlusion_first_stop_uuid_(tier4_autoware_utils::generateUUID())
occlusion_uuid_(tier4_autoware_utils::generateUUID())
{
velocity_factor_.init(VelocityFactor::INTERSECTION);
planner_param_ = planner_param;
Expand Down Expand Up @@ -104,8 +103,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
// occlusion
occlusion_safety_ = true;
occlusion_stop_distance_ = std::numeric_limits<double>::lowest();
occlusion_first_stop_safety_ = true;
occlusion_first_stop_distance_ = std::numeric_limits<double>::lowest();
occlusion_first_stop_required_ = false;

const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
/* get current pose */
Expand Down Expand Up @@ -296,7 +294,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
/* calculate final stop lines */
std::optional<size_t> stop_line_idx = default_stop_line_idx_opt;
std::optional<size_t> occlusion_peeking_line_idx =
occlusion_peeking_line_idx_opt; // TODO(Mamoru Sobue): different position depending on the flag
occlusion_peeking_line_idx_opt
? std::make_optional<size_t>(occlusion_peeking_line_idx_opt.value())
: std::nullopt;
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;

Expand Down Expand Up @@ -324,34 +324,24 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
is_actually_occluded_ = !is_occlusion_cleared;
is_forcefully_occluded_ = ext_occlusion_requested;
if (!is_occlusion_cleared || ext_occlusion_requested) {
const bool approached_stop_line =
(std::fabs(dist_1st_stopline) < planner_param_.common.stop_overshoot_margin);
const bool is_stopped = planner_data_->isVehicleStopped();
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;
} else if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
if (!has_collision) {
occlusion_stop_required = true;
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());
occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
} else {
collision_stop_required = true;
stop_line_idx = default_stop_line_idx_opt;
occlusion_stop_required = true;
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());
occlusion_state_ = OcclusionState::COLLISION_DETECTED;
}
}
if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
occlusion_stop_required = true;
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());
occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
} else {
const bool approached_stop_line =
(std::fabs(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
before_creep_state_machine_.setStateWithMarginTime(
Expand Down Expand Up @@ -429,13 +419,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
logger_.get_child("collision state_machine"), *clock_);

/* set RTC safety respectively */
occlusion_first_stop_distance_ = dist_1st_stopline;
occlusion_stop_distance_ = dist_2nd_stopline;
setDistance(dist_1st_stopline);
if (occlusion_stop_required) {
if (first_phase_stop_required) {
occlusion_first_stop_safety_ = false;
}
occlusion_first_stop_required_ = first_phase_stop_required;
occlusion_safety_ = is_occlusion_cleared;
}
/* collision */
Expand All @@ -461,7 +448,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
}
}

if (!occlusion_first_stop_activated_) {
if (!isActivated() && occlusion_first_stop_required_ && occlusion_first_stop_line_idx) {
planning_utils::setVelocityFromIndex(
occlusion_first_stop_line_idx.value(), 0.0 /* [m/s] */, path);
debug_data_.occlusion_first_stop_wall_pose =
Expand All @@ -474,10 +461,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
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 =====");
return true;
}

if (!isActivated() /* collision*/) {
Expand All @@ -502,6 +486,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
velocity_factor_.set(
path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN);
}
RCLCPP_DEBUG(logger_, "===== plan end =====");
return true;
}

is_go_out_ = true;
Expand Down

0 comments on commit d29cf55

Please sign in to comment.