feat(behavior_path_planner): enable lane change in intersection (auto…

…warefoundation#2720) fix(behavior_path_planner): enable lane change in intersection Signed-off-by: Fumiya Watanabe <[email protected]> Signed-off-by: Fumiya Watanabe <[email protected]>
tier4 · Jan 24, 2023 · 2729925 · 2729925
1 parent a7de504
commit 2729925
Show file tree

Hide file tree

Showing 9 changed files with 155 additions and 57 deletions.
diff --git a/...ehavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/...ehavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp
@@ -297,18 +297,12 @@ PathWithLaneId ExternalRequestLaneChangeModule::getReferencePath() const
 
   const int num_lane_change =
     std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
-  double optional_lengths{0.0};
-  const auto isInIntersection = util::checkLaneIsInIntersection(
-    *route_handler, reference_path, current_lanes, common_parameters, num_lane_change,
-    optional_lengths);
-  if (isInIntersection) {
-    reference_path = util::getCenterLinePath(
-      *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
-      common_parameters.forward_path_length, common_parameters, optional_lengths);
-  }
+  reference_path = util::getCenterLinePath(
+    *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
+    common_parameters.forward_path_length, common_parameters, 0.0);
 
   const double lane_change_buffer =
-    util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths);
+    util::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0);
 
   reference_path = util::setDecelerationVelocity(
     *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration,

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
@@ -305,18 +305,13 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
 
   const int num_lane_change =
     std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
-  double optional_lengths{0.0};
-  const auto isInIntersection = util::checkLaneIsInIntersection(
-    *route_handler, reference_path, current_lanes, common_parameters, num_lane_change,
-    optional_lengths);
-  if (isInIntersection) {
-    reference_path = util::getCenterLinePath(
-      *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
-      common_parameters.forward_path_length, common_parameters, optional_lengths);
-  }
+
+  reference_path = util::getCenterLinePath(
+    *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
+    common_parameters.forward_path_length, common_parameters, 0.0);
 
   const double lane_change_buffer =
-    util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths);
+    util::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0);
 
   reference_path = util::setDecelerationVelocity(
     *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration,

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
@@ -455,30 +455,22 @@ bool hasEnoughDistance(
     return false;
   }
 
-  if (
-    lane_change_total_distance + lane_change_required_distance >
-    util::getDistanceToNextIntersection(current_pose, current_lanes)) {
-    return false;
-  }
-
   if (
     route_handler.isInGoalRouteSection(current_lanes.back()) &&
     lane_change_total_distance + lane_change_required_distance >
       util::getSignedDistance(current_pose, goal_pose, current_lanes)) {
     return false;
   }
 
-  if (
-    lane_change_total_distance + lane_change_required_distance >
-    util::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs)) {
-    return false;
-  }
-
   // return is there is no target lanes. Else continue checking
   if (target_lanes.empty()) {
     return true;
   }
 
+  if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, target_lanes)) {
+    return false;
+  }
+
   return true;
 }
 
@@ -1001,26 +993,13 @@ bool hasEnoughDistanceToLaneChangeAfterAbort(
     return false;
   }
 
-  if (
-    abort_plus_lane_change_distance >
-    util::getDistanceToNextIntersection(current_pose, current_lanes)) {
-    return false;
-  }
-
   if (
     route_handler.isInGoalRouteSection(current_lanes.back()) &&
     abort_plus_lane_change_distance >
       util::getSignedDistance(current_pose, route_handler.getGoalPose(), current_lanes)) {
     return false;
   }
 
-  if (
-    abort_plus_lane_change_distance >
-    util::getDistanceToCrosswalk(
-      current_pose, current_lanes, *route_handler.getOverallGraphPtr())) {
-    return false;
-  }
-
   return true;
 }
 }  // namespace behavior_path_planner::lane_change_utils
diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp
@@ -42,6 +42,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+
+  bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const;
 };
 
 class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
@@ -58,6 +60,8 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+
+  bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const;
 };
 }  // namespace behavior_velocity_planner
 

diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp
@@ -43,6 +43,12 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override;
 
+  bool isModuleRegisteredFromRegElement(const lanelet::Id & id) const;
+
+  bool hasSameTrafficLight(
+    const lanelet::TrafficLightConstPtr element,
+    const lanelet::TrafficLightConstPtr registered_element) const;
+
   // Debug
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::LookingTrafficSignal>::SharedPtr
     pub_tl_state_;

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
@@ -26,6 +26,18 @@
 
 namespace behavior_velocity_planner
 {
+bool hasSameLanelet(const lanelet::ConstLanelets & lanes1, const lanelet::ConstLanelets & lanes2)
+{
+  for (const auto & lane1 : lanes1) {
+    for (const auto & lane2 : lanes2) {
+      if (lane1.id() == lane2.id()) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
 IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
 : SceneModuleManagerInterfaceWithRTC(node, getModuleName())
 {
@@ -84,7 +96,7 @@ void IntersectionModuleManager::launchNewModules(
     const auto lane_id = ll.id();
     const auto module_id = lane_id;
 
-    if (isModuleRegistered(module_id)) {
+    if (hasSameParentLanelet(ll)) {
       continue;
     }
 
@@ -161,23 +173,86 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 IntersectionModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto lane_id_set = planning_utils::getLaneIdSetOnPath(
+  const auto lane_set = planning_utils::getLaneletsOnPath(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose);
 
-  return [lane_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
-    return lane_id_set.count(scene_module->getModuleId()) == 0;
-  };
+  return
+    [this, lane_set]([[maybe_unused]] const std::shared_ptr<SceneModuleInterface> & scene_module) {
+      for (const auto & lane : lane_set) {
+        const std::string turn_direction = lane.attributeOr("turn_direction", "else");
+        const auto is_intersection =
+          turn_direction == "right" || turn_direction == "left" || turn_direction == "straight";
+        if (!is_intersection) {
+          continue;
+        }
+        if (hasSameParentLanelet(lane)) {
+          return false;
+        }
+      }
+      return true;
+    };
 }
+
 std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 MergeFromPrivateModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto lane_id_set = planning_utils::getLaneIdSetOnPath(
+  const auto lane_set = planning_utils::getLaneletsOnPath(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose);
 
-  return [lane_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
-    return lane_id_set.count(scene_module->getModuleId()) == 0;
-  };
+  return
+    [this, lane_set]([[maybe_unused]] const std::shared_ptr<SceneModuleInterface> & scene_module) {
+      for (const auto & lane : lane_set) {
+        const std::string turn_direction = lane.attributeOr("turn_direction", "else");
+        const auto is_intersection =
+          turn_direction == "right" || turn_direction == "left" || turn_direction == "straight";
+        if (!is_intersection) {
+          continue;
+        }
+        if (hasSameParentLanelet(lane)) {
+          return false;
+        }
+      }
+      return true;
+    };
+}
+
+bool IntersectionModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const
+{
+  lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane);
+
+  for (const auto & id : registered_module_id_set_) {
+    const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id);
+    lanelet::ConstLanelets registered_parents =
+      planner_data_->route_handler_->getPreviousLanelets(registered_lane);
+    for (const auto & ll : registered_parents) {
+      auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll);
+      neighbor_lanes.push_back(ll);
+      if (hasSameLanelet(parents, neighbor_lanes)) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+bool MergeFromPrivateModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const
+{
+  lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane);
+
+  for (const auto & id : registered_module_id_set_) {
+    const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id);
+    lanelet::ConstLanelets registered_parents =
+      planner_data_->route_handler_->getPreviousLanelets(registered_lane);
+    for (const auto & ll : registered_parents) {
+      auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll);
+      neighbor_lanes.push_back(ll);
+      if (hasSameLanelet(parents, neighbor_lanes)) {
+        return true;
+      }
+    }
+  }
+  return false;
 }
 
 }  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp
@@ -125,7 +125,7 @@ void TrafficLightModuleManager::launchNewModules(
     // Use lanelet_id to unregister module when the route is changed
     const auto lane_id = traffic_light_reg_elem.second.id();
     const auto module_id = lane_id;
-    if (!isModuleRegistered(module_id)) {
+    if (!isModuleRegisteredFromRegElement(module_id)) {
       registerModule(std::make_shared<TrafficLightModule>(
         module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second,
         planner_param_, logger_.get_child("traffic_light_module"), clock_));
@@ -143,9 +143,47 @@ TrafficLightModuleManager::getModuleExpiredFunction(
   const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath<TrafficLight>(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose);
 
-  return [lanelet_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
-    return lanelet_id_set.count(scene_module->getModuleId()) == 0;
+  return [this, lanelet_id_set](
+           [[maybe_unused]] const std::shared_ptr<SceneModuleInterface> & scene_module) {
+    for (const auto & id : lanelet_id_set) {
+      if (isModuleRegisteredFromRegElement(id)) {
+        return false;
+      }
+    }
+    return true;
   };
 }
 
+bool TrafficLightModuleManager::isModuleRegisteredFromRegElement(const lanelet::Id & id) const
+{
+  const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id);
+
+  for (const auto & registered_id : registered_module_id_set_) {
+    const auto registered_lane =
+      planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(registered_id);
+    for (const auto & registered_element : registered_lane.regulatoryElementsAs<TrafficLight>()) {
+      for (const auto & element : lane.regulatoryElementsAs<TrafficLight>()) {
+        if (hasSameTrafficLight(element, registered_element)) {
+          return true;
+        }
+      }
+    }
+  }
+  return false;
+}
+
+bool TrafficLightModuleManager::hasSameTrafficLight(
+  const lanelet::TrafficLightConstPtr element,
+  const lanelet::TrafficLightConstPtr registered_element) const
+{
+  for (const auto & traffic_light : element->trafficLights()) {
+    for (const auto & registered_traffic_light : registered_element->trafficLights()) {
+      if (traffic_light.id() == registered_traffic_light.id()) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
 }  // namespace behavior_velocity_planner
diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -102,6 +102,7 @@ class RouteHandler
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const;
   bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const;
+  lanelet::ConstLanelets getLaneChangeableNeighbors(const lanelet::ConstLanelet & lanelet) const;
 
   /**
    * @brief Check if same-direction lane is available at the right side of the lanelet

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
@@ -426,6 +426,12 @@ bool RouteHandler::isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const
   return !getNextLaneletWithinRoute(lanelet, &next_lanelet);
 }
 
+lanelet::ConstLanelets RouteHandler::getLaneChangeableNeighbors(
+  const lanelet::ConstLanelet & lanelet) const
+{
+  return lanelet::utils::query::getLaneChangeableNeighbors(routing_graph_ptr_, lanelet);
+}
+
 lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter(
   const lanelet::ConstLanelet & lanelet, const double min_length) const
 {