Skip to content

Commit

Permalink
fix(walkway): fix module launch condition (autowarefoundation#4823)
Browse files Browse the repository at this point in the history
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota authored and 0x126 committed Sep 7, 2023
1 parent 34224ed commit f537e9c
Showing 1 changed file with 19 additions and 28 deletions.
47 changes: 19 additions & 28 deletions planning/behavior_velocity_walkway_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,8 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node)
void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
{
const auto rh = planner_data_->route_handler_;
if (!opt_use_regulatory_element_) {
opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr());
}

const auto launch = [this, &path](const auto & lanelet) {
const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) {
const auto attribute =
lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string(""));
if (attribute != lanelet::AttributeValueString::Walkway) {
Expand All @@ -64,24 +61,21 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();

registerModule(std::make_shared<WalkwayModule>(
lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_));
};

if (*opt_use_regulatory_element_) {
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);

for (const auto & crosswalk : crosswalk_leg_elem_map) {
launch(crosswalk.first->crosswalkLanelet());
}
} else {
const auto crosswalk_lanelets = getCrosswalksOnPath(
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
rh->getOverallGraphPtr());
for (const auto & crosswalk : crosswalk_leg_elem_map) {
launch(crosswalk.first->crosswalkLanelet(), true);
}

for (const auto & crosswalk : crosswalk_lanelets) {
launch(crosswalk);
}
const auto crosswalk_lanelets = getCrosswalksOnPath(
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());

for (const auto & crosswalk : crosswalk_lanelets) {
launch(crosswalk, false);
}
}

Expand All @@ -92,17 +86,14 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)

std::set<int64_t> walkway_id_set;

if (*opt_use_regulatory_element_) {
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
walkway_id_set = getCrosswalkIdSetOnPath(
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());

for (const auto & crosswalk : crosswalk_leg_elem_map) {
walkway_id_set.insert(crosswalk.first->id());
}
} else {
walkway_id_set = getCrosswalkIdSetOnPath(
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
rh->getOverallGraphPtr());
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);

for (const auto & crosswalk : crosswalk_leg_elem_map) {
walkway_id_set.insert(crosswalk.first->id());
}

return [walkway_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
Expand Down

0 comments on commit f537e9c

Please sign in to comment.