fix(behavior_velocity_planner): use only forward path for launching m…

…odule (#1028) (#96)

* fix(behavior_velocity_planner): use only foward path for launching module

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Make functions common

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Fix precommit include_what_you_use

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Move functions to utils

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp

@@ -222,6 +222,10 @@ bool getClosestLanelet(
   const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
   ConstLanelet * closest_lanelet_ptr);
 
+bool getCurrentLanelets(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelets * current_lanelets_ptr);
+
 /**
  * [getSucceedingLaneletSequences retrieves a sequence of lanelets after the given lanelet.
  * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence

map/lanelet2_extension/lib/query.cpp

@@ -741,6 +741,30 @@ bool query::getClosestLanelet(
   return found;
 }
 
+bool query::getCurrentLanelets(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelets * current_lanelets_ptr)
+{
+  if (current_lanelets_ptr == nullptr) {
+    std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet"
+              << std::endl;
+    return false;
+  }
+
+  if (lanelets.empty()) {
+    return false;
+  }
+
+  lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y);
+  for (const auto & llt : lanelets) {
+    if (!lanelet::geometry::inside(llt, search_point)) {
+      current_lanelets_ptr->push_back(llt);
+    }
+  }
+
+  return !current_lanelets_ptr->empty();  // return found
+}
+
 std::vector<std::deque<lanelet::ConstLanelet>> getSucceedingLaneletSequencesRecursive(
   const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
   const double length)

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

@@ -25,6 +25,8 @@
 
 #include <functional>
 #include <memory>
+#include <set>
+#include <vector>
 
 namespace behavior_velocity_planner
 {
@@ -38,6 +40,17 @@ class CrosswalkModuleManager : public SceneModuleManagerInterface
 private:
   CrosswalkModule::PlannerParam crosswalk_planner_param_;
   WalkwayModule::PlannerParam walkway_planner_param_;
+
+  std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const lanelet::LaneletMapPtr lanelet_map,
+    const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
+
+  std::set<int64_t> getCrosswalkIdSetOnPath(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const lanelet::LaneletMapPtr lanelet_map,
+    const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
+
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -47,16 +47,6 @@
 #include <utility>
 #include <vector>
 
-namespace tier4_autoware_utils
-{
-template <>
-inline geometry_msgs::msg::Pose getPose(
-  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
-{
-  return p.point.pose;
-}
-}  // namespace tier4_autoware_utils
-
 namespace behavior_velocity_planner
 {
 using autoware_auto_perception_msgs::msg::ObjectClassification;

planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

@@ -17,6 +17,8 @@
 
 #include "behavior_velocity_planner/planner_data.hpp"
 
+#include <utilization/util.hpp>
+
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_planning_msgs/msg/stop_reason.hpp>

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

@@ -23,9 +23,14 @@
 
 #include <functional>
 #include <memory>
+#include <set>
+#include <utility>
+#include <vector>
 
 namespace behavior_velocity_planner
 {
+using StopLineWithLaneId = std::pair<lanelet::ConstLineString3d, int64_t>;
+
 class StopLineModuleManager : public SceneModuleManagerInterface
 {
 public:
@@ -35,6 +40,15 @@ class StopLineModuleManager : public SceneModuleManagerInterface
 
 private:
   StopLineModule::PlannerParam planner_param_;
+
+  std::vector<StopLineWithLaneId> getStopLinesWithLaneIdOnPath(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const lanelet::LaneletMapPtr lanelet_map);
+
+  std::set<int64_t> getStopLineIdSetOnPath(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const lanelet::LaneletMapPtr lanelet_map);
+
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -46,7 +46,11 @@
 
 #include <algorithm>
 #include <limits>
+#include <memory>
+#include <set>
 #include <string>
+#include <unordered_map>
+#include <utility>
 #include <vector>
 
 namespace tier4_autoware_utils
@@ -57,6 +61,12 @@ inline geometry_msgs::msg::Point getPoint(
 {
   return p.point.pose.position;
 }
+template <>
+inline geometry_msgs::msg::Pose getPose(
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+{
+  return p.point.pose;
+}
 }  // namespace tier4_autoware_utils
 
 namespace behavior_velocity_planner
@@ -303,6 +313,80 @@ std::vector<T> concatVector(const std::vector<T> & vec1, const std::vector<T> &
   return concat_vec;
 }
 
+template <class T>
+std::unordered_map<typename std::shared_ptr<const T>, lanelet::ConstLanelet> getRegElemMapOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
+{
+  std::unordered_map<typename std::shared_ptr<const T>, lanelet::ConstLanelet> reg_elem_map_on_path;
+  std::set<int64_t> unique_lane_ids;
+  auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
+    path.points, current_pose, std::numeric_limits<double>::max(), M_PI_2);
+
+  // Add current lane id
+  lanelet::ConstLanelets current_lanes;
+  if (
+    lanelet::utils::query::getCurrentLanelets(
+      lanelet::utils::query::laneletLayer(lanelet_map), current_pose, &current_lanes) &&
+    nearest_segment_idx) {
+    for (const auto & ll : current_lanes) {
+      if (
+        ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) ||
+        ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) {
+        unique_lane_ids.insert(ll.id());
+      }
+    }
+  }
+
+  // Add forward path lane_id
+  const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
+  for (size_t i = start_idx; i < path.points.size(); i++) {
+    unique_lane_ids.insert(
+      path.points.at(i).lane_ids.at(0));  // should we iterate ids? keep as it was.
+  }
+
+  for (const auto lane_id : unique_lane_ids) {
+    const auto ll = lanelet_map->laneletLayer.get(lane_id);
+
+    for (const auto & reg_elem : ll.regulatoryElementsAs<const T>()) {
+      reg_elem_map_on_path.insert(std::make_pair(reg_elem, ll));
+    }
+  }
+
+  return reg_elem_map_on_path;
+}
+
+template <class T>
+std::set<int64_t> getRegElemIdSetOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
+{
+  std::set<int64_t> reg_elem_id_set;
+  for (const auto & m : getRegElemMapOnPath<const T>(path, lanelet_map, current_pose)) {
+    reg_elem_id_set.insert(m.first->id());
+  }
+  return reg_elem_id_set;
+}
+
+template <class T>
+std::set<int64_t> getLaneletIdSetOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
+{
+  std::set<int64_t> id_set;
+  for (const auto & m : getRegElemMapOnPath<const T>(path, lanelet_map, current_pose)) {
+    id_set.insert(m.second.id());
+  }
+  return id_set;
+}
+
+std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose);
+
+std::set<int64_t> getLaneIdSetOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose);
 }  // namespace planning_utils
 }  // namespace behavior_velocity_planner
 

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

@@ -25,36 +25,6 @@
 
 namespace behavior_velocity_planner
 {
-namespace
-{
-std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const lanelet::LaneletMapPtr lanelet_map)
-{
-  std::vector<lanelet::ConstLanelet> lanelets;
-
-  for (const auto & p : path.points) {
-    const auto lane_id = p.lane_ids.at(0);
-    lanelets.push_back(lanelet_map->laneletLayer.get(lane_id));
-  }
-
-  return lanelets;
-}
-
-std::set<int64_t> getLaneIdSetOnPath(const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
-{
-  std::set<int64_t> lane_id_set;
-
-  for (const auto & p : path.points) {
-    const auto lane_id = p.lane_ids.at(0);
-    lane_id_set.insert(lane_id);
-  }
-
-  return lane_id_set;
-}
-
-}  // namespace
-
 BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
 : SceneModuleManagerInterface(node, getModuleName())
 {
@@ -71,8 +41,9 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
 void BlindSpotModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  for (const auto & ll :
-       getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
+  for (const auto & ll : planning_utils::getLaneletsOnPath(
+         path, planner_data_->route_handler_->getLaneletMapPtr(),
+         planner_data_->current_pose.pose)) {
     const auto lane_id = ll.id();
     const auto module_id = lane_id;
 
@@ -96,7 +67,8 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 BlindSpotModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto lane_id_set = getLaneIdSetOnPath(path);
+  const auto lane_id_set = planning_utils::getLaneIdSetOnPath(
+    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;