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

Jump to bottom

feat(intersection): send intersection/intersection_occlusion rtc status(fix #3511) #3512

Merged
merged 6 commits into from
Apr 24, 2023
Merged

feat(intersection): send intersection/intersection_occlusion rtc status(fix #3511) #3512

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f173800
Merge feat/intersection-occlusion-latest (#8)
soblin Apr 20, 2023
93baf1f
fixed virtual wall marker
soblin Apr 21, 2023
72bab34
use is_occluded flag
soblin Apr 21, 2023
2c85ec3
3 rtc intersection appears
soblin Apr 24, 2023
ab638a0
clearCooperateStatus before sendRTC not to duplicate default/occlusio…
soblin Apr 24, 2023
8312408
clearCooperateStatus at the beginning of send RTC
soblin Apr 24, 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
13 changes: 3 additions & 10 deletions 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 @@ -104,7 +104,6 @@ void IntersectionModuleManager::launchNewModules(
const auto lanelets =
planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose);
// run occlusion detection only in the first intersection
bool enable_occlusion_detection = intersection_param_.occlusion.enable;
for (size_t i = 0; i < lanelets.size(); i++) {
const auto ll = lanelets.at(i);
const auto lane_id = ll.id();
Expand All @@ -125,12 +124,10 @@ void IntersectionModuleManager::launchNewModules(
const auto assoc_ids =
planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph);
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_);
module_id, lane_id, planner_data_, intersection_param_, assoc_ids, true, node_,
logger_.get_child("intersection_module"), clock_);
registerModule(std::move(new_module));
generateUUID(module_id);

enable_occlusion_detection = false;
}
}

Expand Down Expand Up @@ -175,22 +172,20 @@ 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 auto occlusion_uuid = intersection_module->getOcclusionUUID();
const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID();
if (!is_occluded) {
rtc_interface_.clearCooperateStatus();
// default
updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
occlusion_rtc_interface_.updateCooperateStatus(
occlusion_uuid, true, std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::lowest(), stamp);
// send {default, occlusion} RTC status
} else {
rtc_interface_.clearCooperateStatus();
// occlusion
const auto occlusion_safety = intersection_module->getOcclusionSafety();
const auto occlusion_distance = intersection_module->getOcclusionDistance();
Expand All @@ -203,8 +198,6 @@ void IntersectionModuleManager::sendRTC(const Time & stamp)
rtc_interface_.updateCooperateStatus(
occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance,
occlusion_first_stop_distance, stamp);
// send {first_stop, occlusion} RTC status
// rtc_interface_.removeCooperateStatus(uuid);
}
}
rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus()
Expand Down