fix(behavior_velocity): fix conflicting case of intersection and merg…

…e from private (#447) * fix(behavior_velocity): fix conflicting case of intersection and merge from private Signed-off-by: tanaka3 <[email protected]> * refactor(behavior_velocity): refactor Param to double Signed-off-by: tanaka3 <[email protected]> * fix(behavior_velocity): revert offset for wall pose Signed-off-by: tanaka3 <[email protected]>
autowarefoundation · Mar 2, 2022 · 9b605cb · 9b605cb
1 parent 407c802
commit 9b605cb
Show file tree

Hide file tree

Showing 9 changed files with 83 additions and 25 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/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;

diff --git a/...vior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/...vior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp
@@ -99,7 +99,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
 public:
   struct PlannerParam
   {
-    IntersectionModule::PlannerParam intersection_param;
+    double decel_velocity;
+    double detection_area_length;
+    double stop_line_margin;
     double stop_duration_sec;
   };
 

diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp
@@ -52,7 +52,7 @@ bool hasDuplicatedPoint(
  */
 bool getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const IntersectionModule::PlannerParam & planner_param,
+  const int lane_id, const double detection_area_length,
   std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
   std::vector<lanelet::ConstLanelets> * objective_lanelets_result, const rclcpp::Logger logger);
 
@@ -69,8 +69,7 @@ bool getObjectiveLanelets(
  */
 bool generateStopLine(
   const int lane_id, const std::vector<lanelet::CompoundPolygon3d> detection_areas,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const IntersectionModule::PlannerParam & planner_param,
+  const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx,
   int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger);

diff --git a/planning/behavior_velocity_planner/src/node.cpp b/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));

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/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;
 
@@ -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);
   }
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
@@ -83,10 +83,18 @@ 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.decel_velocity = node.get_parameter("intersection.decel_velocity").as_double();
+  mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double();
+  mp.stop_line_margin = node.get_parameter("intersection.stop_line_margin").as_double();
 }
 
 void IntersectionModuleManager::launchNewModules(
@@ -102,6 +110,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 +155,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 +168,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
diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -93,8 +93,8 @@ bool IntersectionModule::modifyPathVelocity(
   std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
 
   util::getObjectiveLanelets(
-    lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_, &conflicting_area_lanelets,
-    &detection_area_lanelets, logger_);
+    lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
+    &conflicting_area_lanelets, &detection_area_lanelets, logger_);
   std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
     conflicting_area_lanelets, planner_param_.detection_area_length);
   std::vector<lanelet::CompoundPolygon3d> detection_areas = util::getPolygon3dFromLaneletsVec(
@@ -115,8 +115,8 @@ bool IntersectionModule::modifyPathVelocity(
   int pass_judge_line_idx = -1;
   int first_idx_inside_lane = -1;
   if (!util::generateStopLine(
-        lane_id_, conflicting_areas, planner_data_, planner_param_, path, *path, &stop_line_idx,
-        &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) {
+        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, *path,
+        &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
     RCLCPP_DEBUG(logger_, "===== plan end =====");
     return false;

diff --git a/...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp
@@ -69,10 +69,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
 
   util::getObjectiveLanelets(
-    lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.intersection_param,
+    lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
     &conflicting_area_lanelets, &detection_area_lanelets, logger_);
   std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
-    conflicting_area_lanelets, planner_param_.intersection_param.detection_area_length);
+    conflicting_area_lanelets, planner_param_.detection_area_length);
   if (conflicting_areas.empty()) {
     RCLCPP_DEBUG(logger_, "no detection area. skip computation.");
     return true;
@@ -86,7 +86,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   const auto private_path =
     extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
   if (!util::generateStopLine(
-        lane_id_, conflicting_areas, planner_data_, planner_param_.intersection_param, path,
+        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path,
         private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane,
         logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
@@ -108,7 +108,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   /* set stop speed */
   if (state_machine_.getState() == State::STOP) {
     constexpr double stop_vel = 0.0;
-    const double decel_vel = planner_param_.intersection_param.decel_velocity;
+    const double decel_vel = planner_param_.decel_velocity;
     double v = (has_traffic_light_ && turn_direction_ == "straight") ? decel_vel : stop_vel;
     util::setVelocityFrom(stop_line_idx, v, path);
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
@@ -162,8 +162,7 @@ int getFirstPointInsidePolygons(
 
 bool generateStopLine(
   const int lane_id, const std::vector<lanelet::CompoundPolygon3d> detection_areas,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const IntersectionModule::PlannerParam & planner_param,
+  const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx,
   int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger)
@@ -178,7 +177,7 @@ bool generateStopLine(
   /* set parameters */
   constexpr double interval = 0.2;
 
-  const int margin_idx_dist = std::ceil(planner_param.stop_line_margin / interval);
+  const int margin_idx_dist = std::ceil(stop_line_margin / interval);
   const int base2front_idx_dist =
     std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval);
   const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval);
@@ -322,7 +321,7 @@ bool getStopPoseIndexFromMap(
 
 bool getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const IntersectionModule::PlannerParam & planner_param,
+  const int lane_id, const double detection_area_length,
   std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
   std::vector<lanelet::ConstLanelets> * objective_lanelets_result, const rclcpp::Logger logger)
 {
@@ -379,7 +378,7 @@ bool getObjectiveLanelets(
   }
 
   // get possible lanelet path that reaches conflicting_lane longer than given length
-  const double length = planner_param.detection_area_length;
+  const double length = detection_area_length;
   std::vector<lanelet::ConstLanelets> objective_lanelets_sequences;
   for (const auto & ll : objective_lanelets) {
     // Preceding lanes does not include objective_lane so add them at the end