Skip to content

Commit

Permalink
Merge pull request #442 from tier4/hotfix/pr3664
Browse files Browse the repository at this point in the history 
feat(intersection): switch default/occlusion_first_stop when occlusion appeared/disappeared or approved/disapproved (autowarefoundation#3664)
  • Loading branch information
@shmpwk
shmpwk authored May 15, 2023
2 parents 4394f30 + 32529e7 commit d20f670
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 66 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 @@ -150,15 +150,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 @@ -186,9 +180,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 @@ -210,7 +210,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker
{debug_data_.collision_stop_wall_pose}, "intersection", now),
&wall_marker, now);
}
if (!occlusion_first_stop_activated_) {
if (!activated_ && occlusion_first_stop_required_) {
appendMarkerArray(
virtual_wall_marker_creator_->createStopVirtualWallMarker(
{debug_data_.occlusion_first_stop_wall_pose}, "intersection", now),
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 @@ -183,32 +183,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 @@ -219,20 +204,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 @@ -253,9 +227,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
31 changes: 13 additions & 18 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;

/* get current pose */
const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose;
Expand Down Expand Up @@ -266,7 +264,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 @@ -299,19 +299,18 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
RCLCPP_DEBUG(logger_, "===== plan end =====");
return true;
}
if (
before_creep_state_machine_.getState() == StateMachine::State::GO &&
!ext_occlusion_requested) {
if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
occlusion_stop_required = true;
stop_line_idx = occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;
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 =
(dist_1st_stopline < planner_param_.common.stop_overshoot_margin);
(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
Expand Down Expand Up @@ -387,13 +386,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;
} else {
/* collision */
Expand All @@ -414,7 +410,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
}
}

if (!occlusion_first_stop_activated_ && occlusion_first_stop_line_idx) {
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 @@ -427,10 +423,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 @@ -455,6 +448,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
}
RCLCPP_DEBUG(logger_, "===== plan end =====");
return true;
}

is_go_out_ = true;
Expand Down

0 comments on commit d20f670

Please sign in to comment.