Merge pull request #871 from tier4/fix/crosswalk-launch-condition

fix(crosswalk): fix module launch condition
tier4 · Sep 26, 2023 · f3609c0 · f3609c0
2 parents d0b5bbe + 3f89231
commit f3609c0
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 37 deletions.
diff --git 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<CrosswalkModule>(
-      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<double>::lowest(), path.header.stamp);
   };
 
-  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->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<int64_t> crosswalk_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);
+  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<Crosswalk>(
+    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<SceneModuleInterface> & scene_module) {

diff --git 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<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const PathWithLaneId & path) override;
-
-  std::optional<bool> opt_use_regulatory_element_{std::nullopt};
 };
 
 class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>

diff --git 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
@@ -50,8 +50,6 @@ class WalkwayModuleManager : public SceneModuleManagerInterface
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const PathWithLaneId & path) override;
-
-  std::optional<bool> opt_use_regulatory_element_{std::nullopt};
 };
 
 class WalkwayModulePlugin : public PluginWrapper<WalkwayModuleManager>