From 749effcbc5d903fcca82e8055e7aa9f41a288319 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 24 Oct 2023 00:32:24 +0900 Subject: [PATCH] fix(crosswalk): fix duplicated crosswalk launch Signed-off-by: Takayuki Murooka --- .../src/manager.cpp | 21 ++++++++++++------- .../src/scene_crosswalk.cpp | 8 +++---- .../src/scene_crosswalk.hpp | 4 ++-- 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 94e87d0174193..dd8dc95a8ad3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -128,8 +128,9 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto launch = [this, &path](const auto id, const auto & use_regulatory_element) { - if (isModuleRegistered(id)) { + const auto launch = [this, &path]( + const auto lane_id, const std::optional & reg_elem_id) { + if (isModuleRegistered(lane_id)) { return; } @@ -137,24 +138,28 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto logger = logger_.get_child("crosswalk_module"); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case + // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, id, lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); - generateUUID(id); - updateRTCStatus(getUUID(id), true, std::numeric_limits::lowest(), path.header.stamp); + node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_)); + generateUUID(lane_id); + updateRTCStatus( + getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); }; 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(), true); + // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. + launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->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); + launch(crosswalk.id(), std::nullopt); } } @@ -172,7 +177,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); for (const auto & crosswalk : crosswalk_leg_elem_map) { - crosswalk_id_set.insert(crosswalk.first->id()); + crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } return [crosswalk_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 8d7a160fbc901..4a6cacbeaf615 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -169,20 +169,20 @@ tier4_debug_msgs::msg::StringStamped createStringStampedMessage( } // namespace CrosswalkModule::CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), - use_regulatory_element_(use_regulatory_element) + use_regulatory_element_(reg_elem_id) { velocity_factor_.init(VelocityFactor::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( - lanelet_map_ptr->regulatoryElementLayer.get(module_id)); + lanelet_map_ptr->regulatoryElementLayer.get(*reg_elem_id)); stop_lines_ = reg_elem_ptr->stopLines(); crosswalk_ = reg_elem_ptr->crosswalkLanelet(); } else { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 0a80b41c32b3b..043557912ca37 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -300,8 +300,8 @@ class CrosswalkModule : public SceneModuleInterface }; CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;