fix(crosswalk): fix duplicated crosswalk launch

Signed-off-by: Takayuki Murooka <[email protected]>

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -128,33 +128,38 @@ 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<int64_t> & reg_elem_id) {
+    if (isModuleRegistered(lane_id)) {
       return;
     }
 
     const auto & p = crosswalk_planner_param_;
     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<CrosswalkModule>(
-      node_, id, lanelet_map_ptr, p, use_regulatory_element, logger, clock_));
-    generateUUID(id);
-    updateRTCStatus(getUUID(id), true, std::numeric_limits<double>::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<double>::lowest(), path.header.stamp);
   };
 
   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->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<SceneModuleInterface> & scene_module) {

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<int64_t> & 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<const lanelet::autoware::Crosswalk>(
-      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 {

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<int64_t> & 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;