fix(behavior_velocity): fix conflicting case of intersection and merge from private

planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp

@@ -37,6 +37,21 @@ class IntersectionModuleManager : public SceneModuleManagerInterface
 
 private:
   IntersectionModule::PlannerParam intersection_param_;
+
+  void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+
+  std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+};
+
+class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
+{
+public:
+  explicit MergeFromPrivateModuleManager(rclcpp::Node & node);
+
+  const char * getModuleName() override { return "merge_from_private"; }
+
+private:
   MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_;
 
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;

planning/behavior_velocity_planner/src/node.cpp

@@ -135,7 +135,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
     planner_manager_.launchSceneModule(std::make_shared<TrafficLightModuleManager>(*this));
   }
   if (this->declare_parameter("launch_intersection", true)) {
+    // intersection module should be before merge from private to declare intersection parameters
     planner_manager_.launchSceneModule(std::make_shared<IntersectionModuleManager>(*this));
+    planner_manager_.launchSceneModule(std::make_shared<MergeFromPrivateModuleManager>(*this));
   }
   if (this->declare_parameter("launch_blind_spot", true)) {
     planner_manager_.launchSceneModule(std::make_shared<BlindSpotModuleManager>(*this));

planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp

@@ -150,7 +150,8 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray(
 }
 
 visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray(
-  const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor)
+  const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor,
+  const double offset_z = 0.0)
 {
   visualization_msgs::msg::MarkerArray msg;
 
@@ -162,7 +163,7 @@ visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray(
   marker_virtual_wall.type = visualization_msgs::msg::Marker::CUBE;
   marker_virtual_wall.action = visualization_msgs::msg::Marker::ADD;
   marker_virtual_wall.pose = pose;
-  marker_virtual_wall.pose.position.z += 1.0;
+  marker_virtual_wall.pose.position.z += 1.0 + offset_z;
   marker_virtual_wall.scale = createMarkerScale(0.1, 5.0, 2.0);
   marker_virtual_wall.color = createMarkerColor(1.0, 0.0, 0.0, 0.5);
   msg.markers.push_back(marker_virtual_wall);
@@ -175,7 +176,7 @@ visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray(
   marker_factor_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
   marker_factor_text.action = visualization_msgs::msg::Marker::ADD;
   marker_factor_text.pose = pose;
-  marker_factor_text.pose.position.z += 2.0;
+  marker_factor_text.pose.position.z += 2.0 + offset_z;
   marker_factor_text.scale = createMarkerScale(0.0, 0.0, 1.0);
   marker_factor_text.color = createMarkerColor(1.0, 1.0, 1.0, 0.999);
   marker_factor_text.text = stop_factor;
@@ -345,7 +346,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark
 
     appendMarkerArray(
       createVirtualStopWallMarkerArray(
-        debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road"),
+        debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road", -1.0),
       current_time, &debug_marker_array);
   }
 

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -83,10 +83,21 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0);
   ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0);
   ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0);
+}
+
+MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node)
+: SceneModuleManagerInterface(node, getModuleName())
+{
+  const std::string ns(getModuleName());
   auto & mp = merge_from_private_area_param_;
   mp.stop_duration_sec =
     node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0);
-  mp.intersection_param = intersection_param_;
+  mp.intersection_param.decel_velocity =
+    node.get_parameter("intersection.decel_velocity").as_double();
+  mp.intersection_param.detection_area_length =
+    node.get_parameter("intersection.detection_area_length").as_double();
+  mp.intersection_param.stop_line_margin =
+    node.get_parameter("intersection.stop_line_margin").as_double();
 }
 
 void IntersectionModuleManager::launchNewModules(
@@ -102,6 +113,32 @@ void IntersectionModuleManager::launchNewModules(
       continue;
     }
 
+    // Is intersection?
+    const std::string turn_direction = ll.attributeOr("turn_direction", "else");
+    const auto is_intersection =
+      turn_direction == "right" || turn_direction == "left" || turn_direction == "straight";
+    if (!is_intersection) {
+      continue;
+    }
+    registerModule(std::make_shared<IntersectionModule>(
+      module_id, lane_id, planner_data_, intersection_param_,
+      logger_.get_child("intersection_module"), clock_));
+  }
+}
+
+void MergeFromPrivateModuleManager::launchNewModules(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map);
+  for (size_t i = 0; i < lanelets.size(); i++) {
+    const auto ll = lanelets.at(i);
+    const auto lane_id = ll.id();
+    const auto module_id = lane_id;
+
+    if (isModuleRegistered(module_id)) {
+      continue;
+    }
+
     // Is intersection?
     const std::string turn_direction = ll.attributeOr("turn_direction", "else");
     const auto is_intersection =
@@ -121,10 +158,6 @@ void IntersectionModuleManager::launchNewModules(
           logger_.get_child("merge_from_private_road_module"), clock_));
       }
     }
-
-    registerModule(std::make_shared<IntersectionModule>(
-      module_id, lane_id, planner_data_, intersection_param_,
-      logger_.get_child("intersection_module"), clock_));
   }
 }
 
@@ -138,4 +171,14 @@ IntersectionModuleManager::getModuleExpiredFunction(
     return lane_id_set.count(scene_module->getModuleId()) == 0;
   };
 }
+std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
+MergeFromPrivateModuleManager::getModuleExpiredFunction(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  const auto lane_id_set = getLaneIdSetOnPath(path);
+
+  return [lane_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
+    return lane_id_set.count(scene_module->getModuleId()) == 0;
+  };
+}
 }  // namespace behavior_velocity_planner