From 3f8923134c15b159abcbce83f0412637db4c32a1 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 26 Sep 2023 11:15:13 +0900 Subject: [PATCH] fix(crosswalk): fix module launch condition Signed-off-by: Takayuki Murooka --- .../src/manager.cpp | 51 +++++++------------ .../src/manager.hpp | 2 - .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 - 4 files changed, 20 insertions(+), 37 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 5a9de20d79477..304b4bdf6f5e7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -125,15 +125,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - std::ostringstream string_stream; - string_stream << "use crosswalk regulatory element: "; - string_stream << std::boolalpha << *opt_use_regulatory_element_; - RCLCPP_INFO_STREAM(logger_, string_stream.str()); - } - const auto launch = [this, &path](const auto id) { + const auto launch = [this, &path](const auto id, const auto & use_regulatory_element) { if (isModuleRegistered(id)) { return; } @@ -143,26 +136,23 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - node_, id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); + node_, id, lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); generateUUID(id); updateRTCStatus(getUUID(id), true, std::numeric_limits::lowest(), path.header.stamp); }; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->id()); - } - } 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->id(), true); + } - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk.id()); - } + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk.id(), false); } } @@ -173,17 +163,14 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set crosswalk_id_set; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + crosswalk_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - crosswalk_id_set.insert(crosswalk.first->id()); - } - } else { - crosswalk_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + crosswalk_id_set.insert(crosswalk.first->id()); } return [crosswalk_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 481a246af80dd..38bb63c121699 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -51,8 +51,6 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - - std::optional opt_use_regulatory_element_{std::nullopt}; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index f9c222f662e82..8995c2362a8cd 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -45,7 +45,7 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) { + 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) { diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index 555e3a73db103..453fcdb40f0db 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -50,8 +50,6 @@ class WalkwayModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - - std::optional opt_use_regulatory_element_{std::nullopt}; }; class WalkwayModulePlugin : public PluginWrapper