fix(behavior_velocity_planner): support u-turn for detection_area, vt…

…l and stop line (autowarefoundation#1700)

* fix(behavior_velocity_planner): deal with u-turn for detection_area, vtl, stop_line

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

* enable behavior velocity test and fix bug

Signed-off-by: Takayuki Murooka <[email protected]>

* fix bug

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

* fix collision check

Signed-off-by: Takayuki Murooka <[email protected]>

* add test

Signed-off-by: Takayuki Murooka <[email protected]>

* remove unnecessary debug output

Signed-off-by: Takayuki Murooka <[email protected]>

Signed-off-by: Takayuki Murooka <[email protected]>

common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp

@@ -20,13 +20,14 @@
 
 #include <boost/optional.hpp>
 
+#include <algorithm>
 #include <utility>
 #include <vector>
 
 namespace motion_utils
 {
 inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id)
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id)
 {
   size_t start_idx = 0;  // NOTE: to prevent from maybe-uninitialized error
   size_t end_idx = 0;    // NOTE: to prevent from maybe-uninitialized error
@@ -35,7 +36,7 @@ inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
   for (size_t i = 0; i < path.points.size(); ++i) {
     const auto & p = path.points.at(i);
     for (const auto & id : p.lane_ids) {
-      if (id == lane_id) {
+      if (id == target_lane_id) {
         if (!found_first_idx) {
           start_idx = i;
           found_first_idx = true;
@@ -46,6 +47,10 @@ inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
   }
 
   if (found_first_idx) {
+    // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded.
+    start_idx = static_cast<size_t>(std::max(0, static_cast<int>(start_idx) - 1));
+    end_idx = std::min(path.points.size() - 1, end_idx + 1);
+
     return std::make_pair(start_idx, end_idx);
   }
 

common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp

@@ -72,16 +72,16 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId)
     {
       const auto res = getPathIndexRangeWithLaneId(points, 3);
       EXPECT_EQ(res->first, 0U);
-      EXPECT_EQ(res->second, 1U);
+      EXPECT_EQ(res->second, 2U);
     }
     {
       const auto res = getPathIndexRangeWithLaneId(points, 1);
-      EXPECT_EQ(res->first, 2U);
-      EXPECT_EQ(res->second, 2U);
+      EXPECT_EQ(res->first, 1U);
+      EXPECT_EQ(res->second, 3U);
     }
     {
       const auto res = getPathIndexRangeWithLaneId(points, 2);
-      EXPECT_EQ(res->first, 3U);
+      EXPECT_EQ(res->first, 2U);
       EXPECT_EQ(res->second, 5U);
     }
     {
@@ -142,7 +142,7 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId)
     for (size_t i = 3; i < 9; ++i) {
       modified_path.points.at(i).lane_ids = {100};
     }
-    EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 3U);
+    EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U);
     EXPECT_EQ(
       findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U);
   }

planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp

@@ -78,6 +78,10 @@ struct PlannerData
   // occupancy grid
   nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid;
 
+  // nearest search
+  double ego_nearest_dist_threshold;
+  double ego_nearest_yaw_threshold;
+
   // other internal data
   std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> traffic_light_id_map;
   // external data

planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp

@@ -123,7 +123,7 @@ class BlindSpotModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 
 private:
-  int64_t lane_id_;
+  const int64_t lane_id_;
   TurnDirection turn_direction_;
   bool has_traffic_light_;
   bool is_over_pass_judge_line_;

planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp

@@ -66,7 +66,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
 public:
   DetectionAreaModule(
-    const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem,
+    const int64_t module_id, const int64_t lane_id,
+    const lanelet::autoware::DetectionArea & detection_area_reg_elem,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
@@ -85,6 +86,9 @@ class DetectionAreaModule : public SceneModuleInterface
   bool hasEnoughBrakingDistance(
     const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const;
 
+  // Lane id
+  int64_t lane_id_;
+
   // Key Feature
   const lanelet::autoware::DetectionArea & detection_area_reg_elem_;
 

planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp

@@ -79,7 +79,8 @@ class NoStoppingAreaModule : public SceneModuleInterface
 
 public:
   NoStoppingAreaModule(
-    const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem,
+    const int64_t module_id, const int64_t lane_id,
+    const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
@@ -91,6 +92,8 @@ class NoStoppingAreaModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 
 private:
+  const int64_t lane_id_;
+
   mutable bool pass_judged_ = false;
   mutable bool is_stoppable_ = true;
   StateMachine state_machine_;  //! for state

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

@@ -203,8 +203,8 @@ struct DebugData
 PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0);
 //!< @brief wrapper for detection area polygon generation
 bool buildDetectionAreaPolygon(
-  Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose,
-  const PlannerParam & param);
+  Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose,
+  const size_t target_seg_idx, const PlannerParam & param);
 lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);
 // Note : consider offset_from_start_to_ego and safety margin for collision here
 void handleCollisionOffset(std::vector<PossibleCollisionInfo> & possible_collisions, double offset);

planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

@@ -37,6 +37,7 @@
 #include <set>
 #include <string>
 #include <unordered_map>
+#include <vector>
 
 // Debug
 #include <rclcpp/rclcpp.hpp>
@@ -110,6 +111,22 @@ class SceneModuleInterface
 
   void setSafe(const bool safe) { safe_ = safe; }
   void setDistance(const double distance) { distance_ = distance; }
+
+  template <class T>
+  size_t findEgoSegmentIndex(const std::vector<T> & points) const
+  {
+    const auto & p = planner_data_;
+    return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
+  }
+
+  template <class T>
+  size_t findEgoIndex(const std::vector<T> & points) const
+  {
+    const auto & p = planner_data_;
+    return motion_utils::findFirstNearestIndexWithSoftConstraints(
+      points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
+  }
 };
 
 class SceneModuleManagerInterface
@@ -261,6 +278,23 @@ class SceneModuleManagerInterface
     scene_modules_.erase(scene_module);
   }
 
+  template <class T>
+  size_t findEgoSegmentIndex(const std::vector<T> & points) const
+  {
+    const auto & p = planner_data_;
+    return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
+  }
+
+  template <class T>
+  size_t findEgoIndex(
+    const std::vector<T> & points, const geometry_msgs::msg::Pose & ego_pose) const
+  {
+    const auto & p = planner_data_;
+    return motion_utils::findFirstNearestIndexWithSoftConstraints(
+      points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
+  }
+
   std::set<std::shared_ptr<SceneModuleInterface>> scene_modules_;
   std::set<int64_t> registered_module_id_set_;
 

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

@@ -90,14 +90,8 @@ class StopLineModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 
 private:
-  int64_t module_id_;
-
   geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line);
 
-  boost::optional<StopLineModule::SegmentIndexWithPoint2d> findCollision(
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
-    const SearchRangeIndex & search_index);
-
   boost::optional<StopLineModule::SegmentIndexWithOffset> findOffsetSegment(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const StopLineModule::SegmentIndexWithPoint2d & collision);
@@ -111,8 +105,11 @@ class StopLineModule : public SceneModuleInterface
     const StopLineModule::SegmentIndexWithPose & insert_index_with_pose,
     tier4_planning_msgs::msg::StopReason * stop_reason);
 
-  lanelet::ConstLineString3d stop_line_;
   int64_t lane_id_;
+
+  lanelet::ConstLineString3d stop_line_;
+
+  // State machine
   State state_;
 
   // Parameter

planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp

@@ -66,8 +66,9 @@ class TrafficLightModule : public SceneModuleInterface
 
 public:
   TrafficLightModule(
-    const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem,
-    lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
+    const int64_t module_id, const int64_t lane_id,
+    const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
+    const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
   bool modifyPathVelocity(
@@ -123,6 +124,9 @@ class TrafficLightModule : public SceneModuleInterface
   autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState(
     const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const;
 
+  // Lane id
+  const int64_t lane_id_;
+
   // Key Feature
   const lanelet::TrafficLight & traffic_light_reg_elem_;
   lanelet::ConstLanelet lane_;

planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp

@@ -75,8 +75,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface
 
 public:
   VirtualTrafficLightModule(
-    const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem,
-    lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
+    const int64_t module_id, const int64_t lane_id,
+    const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane,
+    const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
   bool modifyPathVelocity(
@@ -87,7 +88,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 
 private:
-  const int64_t module_id_;
+  const int64_t lane_id_;
   const lanelet::autoware::VirtualTrafficLight & reg_elem_;
   const lanelet::ConstLanelet lane_;
   const PlannerParam planner_param_;
@@ -101,13 +102,15 @@ class VirtualTrafficLightModule : public SceneModuleInterface
   void setStopReason(
     const geometry_msgs::msg::Pose & stop_pose, tier4_planning_msgs::msg::StopReason * stop_reason);
 
-  bool isBeforeStartLine();
+  boost::optional<size_t> getPathIndexOfFirstEndLine();
 
-  bool isBeforeStopLine();
+  bool isBeforeStartLine(const size_t end_line_idx);
 
-  bool isAfterAnyEndLine();
+  bool isBeforeStopLine(const size_t end_line_idx);
 
-  bool isNearAnyEndLine();
+  bool isAfterAnyEndLine(const size_t end_line_idx);
+
+  bool isNearAnyEndLine(const size_t end_line_idx);
 
   boost::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState> findCorrespondingState();
 
@@ -117,11 +120,11 @@ class VirtualTrafficLightModule : public SceneModuleInterface
 
   void insertStopVelocityAtStopLine(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason);
+    tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx);
 
   void insertStopVelocityAtEndLine(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason);
+    tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx);
 };
 }  // namespace behavior_velocity_planner
 #endif  // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_