feat(behavior_velocity): occlusion spot add partition lanelet (autowa…

…refoundation#482)

* feat(behavior_velocity): add partition lanelet

Signed-off-by: taikitanaka <[email protected]>

* feat(behavior_velocity): add get partition lanelet to utils

Signed-off-by: taikitanaka <[email protected]>

* chore(behavior_velocity): remove unused function

Signed-off-by: taikitanaka <[email protected]>

* chore(behavior_velocity): unite variables

Signed-off-by: taikitanaka <[email protected]>

* feat(behavior_velocity): apply concept of partition

Signed-off-by: taikitanaka <[email protected]>

* chore(behavior_velocity): change partition marker color

Signed-off-by: tanaka3 <[email protected]>

* doc(behavior_velocity): update doc

Signed-off-by: tanaka3 <[email protected]>

* chore(behavior_velocity): to default setting

Signed-off-by: tanaka3 <[email protected]>

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -3,6 +3,7 @@
     occlusion_spot:
       method: "predicted_object"           # [-] candidate is "occupancy_grid" or "predicted_object"
       debug: false                         # [-] whether to publish debug markers. Note Default should be false for performance
+      use_partition_lanelet: true          # [-] whether to use partition lanelet map data
       pedestrian_vel: 1.0                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
       threshold:
         detection_area_length: 100.0        # [m] the length of path to consider perception range

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

@@ -38,6 +38,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -61,6 +62,7 @@ using lanelet::LaneletMapPtr;
 using lanelet::geometry::fromArcCoordinates;
 using lanelet::geometry::toArcCoordinates;
 using DetectionAreaIdx = boost::optional<std::pair<double, double>>;
+using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
 
 namespace occlusion_spot_utils
 {
@@ -98,7 +100,8 @@ struct LatLon
 struct PlannerParam
 {
   METHOD method;
-  bool debug;  // [-]
+  bool debug;                  // [-]
+  bool use_partition_lanelet;  // [-]
   // parameters in yaml
   double detection_area_length;      // [m]
   double detection_area_max_length;  // [m]
@@ -177,6 +180,27 @@ struct PossibleCollisionInfo
   }
 };
 
+struct DebugData
+{
+  double z;
+  std::string road_type = "";
+  std::string detection_type = "";
+  std::vector<Slice> detection_area_polygons;
+  std::vector<lanelet::BasicPolygon2d> partition_lanelets;
+  std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
+  std::vector<PossibleCollisionInfo> possible_collisions;
+  std::vector<geometry_msgs::msg::Point> occlusion_points;
+  PathWithLaneId path_raw;
+  PathWithLaneId interp_path;
+  void resetData()
+  {
+    detection_area_polygons.clear();
+    parked_vehicle_point.clear();
+    possible_collisions.clear();
+    occlusion_points.clear();
+  }
+};
+
 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);
@@ -186,7 +210,7 @@ bool isStuckVehicle(PredictedObject obj, const double min_vel);
 double offsetFromStartToEgo(
   const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx);
 std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
-  std::vector<PredictedObject> & objs, const std::vector<Slice> polys);
+  std::vector<PredictedObject> & objs, const std::vector<Slice> & polys);
 std::vector<PredictedObject> getParkedVehicles(
   const PredictedObjects & dyn_objects, const PlannerParam & param,
   std::vector<Point> & debug_point);
@@ -209,21 +233,16 @@ void createPossibleCollisionBehindParkedVehicle(
 void calcSlowDownPointsForPossibleCollision(
   const int closest_idx, const PathWithLaneId & path, const double offset,
   std::vector<PossibleCollisionInfo> & possible_collisions);
-//!< @brief extract lanelet that includes target_road_type only
-DetectionAreaIdx extractTargetRoadArcLength(
-  const LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path,
-  const ROAD_TYPE & target_road_type);
 //!< @brief convert a set of occlusion spots found on detection_area slice
 boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
   const grid_map::GridMap & grid, const std::vector<grid_map::Position> & occlusion_spot_positions,
-  const double offset_from_start_to_ego, const BasicPoint2d basic_point,
-  const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param);
+  const double offset_from_start_to_ego, const BasicPoint2d base_point,
+  const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data);
 //!< @brief generate possible collisions coming from occlusion spots on the side of the path
 void createPossibleCollisionsInDetectionArea(
-  const std::vector<Slice> & detection_area_polygons,
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
   const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param,
-  std::vector<Point> & debug_points);
+  DebugData & debug_data);
 
 }  // namespace occlusion_spot_utils
 }  // namespace behavior_velocity_planner

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

@@ -40,19 +40,9 @@ namespace behavior_velocity_planner
 class OcclusionSpotModule : public SceneModuleInterface
 {
   using PlannerParam = occlusion_spot_utils::PlannerParam;
+  using DebugData = occlusion_spot_utils::DebugData;
 
 public:
-  struct DebugData
-  {
-    double z;
-    std::string road_type = "occupancy";
-    std::vector<lanelet::BasicPolygon2d> detection_areas;
-    std::vector<occlusion_spot_utils::PossibleCollisionInfo> possible_collisions;
-    std::vector<geometry_msgs::msg::Point> occlusion_points;
-    PathWithLaneId path_raw;
-    PathWithLaneId interp_path;
-  };
-
   OcclusionSpotModule(
     const int64_t module_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const rclcpp::Logger logger,

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

@@ -39,20 +39,9 @@ namespace behavior_velocity_planner
 class OcclusionSpotInPublicModule : public SceneModuleInterface
 {
   using PlannerParam = occlusion_spot_utils::PlannerParam;
+  using DebugData = occlusion_spot_utils::DebugData;
 
 public:
-  struct DebugData
-  {
-    std::string road_type = "object";
-    std::vector<lanelet::BasicPolygon2d> detection_areas;
-    std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
-    std::vector<occlusion_spot_utils::PossibleCollisionInfo> possible_collisions;
-    std::vector<geometry_msgs::msg::Point> occlusion_points;
-    PathWithLaneId path_raw;
-    PathWithLaneId interp_path;
-    double z;
-  };
-
   OcclusionSpotInPublicModule(
     const int64_t module_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const rclcpp::Logger logger,

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -69,6 +69,7 @@ struct PointWithSearchRangeIndex
 };
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using Point2d = boost::geometry::model::d2::point_xy<double>;
+using Polygons2d = std::vector<lanelet::BasicPolygon2d>;
 namespace planning_utils
 {
 inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; }
@@ -106,7 +107,7 @@ inline geometry_msgs::msg::Pose getPose(
 {
   return traj.points.at(idx).pose;
 }
-
+void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys);
 inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); }
 
 inline double square(const double & a) { return a * a; }

planning/behavior_velocity_planner/occlusion-spot-design.md

@@ -85,6 +85,11 @@ The maximum length of detection area depends on ego current vehicle velocity and
 
 ![brief](./docs/occlusion_spot/detection_area_poly.svg)
 
+##### Partition Lanelet
+
+By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible to remove unwanted occlusion spot.
+![brief](./docs/occlusion_spot/occlusion_spot_partition.svg)
+
 #### Module Parameters
 
 | Parameter        | Type   | Description                                                               |
@@ -125,7 +130,7 @@ The maximum length of detection area depends on ego current vehicle velocity and
 
 ```plantuml
 @startuml
-title modifyPathVelocity (Private/Public) Road
+title modifyPathVelocity (Occupancy/PredictedObject)
 start
 
 partition process_path {
@@ -135,16 +140,12 @@ note right
   using spline interpolation and interpolate (x,y,z,v)
 end note
 :calc closest path point from ego;
-:extract target road pair;
-note right
-extract target road type start and end pair and early return if none
-end note
 }
 
 partition process_sensor_data {
-if (road type is PUBLIC) then (yes)
+if (road type is PredictedObject) then (yes)
   :preprocess dynamic object;
-else if (road type is PRIVATE) then (yes)
+else if (road type is Occupancy) then (yes)
   :preprocess occupancy grid map info;
 else (no)
   stop
@@ -154,14 +155,14 @@ endif
 partition generate_detection_area_polygon {
 :convert path to path lanelet;
 :generate left/right slice of polygon that starts from path start;
-:generate interpolated polygon which is created from ego TTC and lateral distance that pedestrian can reach within ego TTC.;
+:generate interpolated polygon created from ego TTC and lateral distance that pedestrian can reach within ego TTC.;
 }
 partition find_possible_collision {
 :generate possible collision;
 :calculate collision path point and intersection point;
 note right
-  - occlusion spot is calculated by longitudinally closest point of unknown cells.
-  - intersection point is where ego front bumper and darting object will crash.
+  - occlusion spot is calculated by the longitudinally closest point of unknown cells.
+  - intersection point is where ego front bumper and the darting object will crash.
   - collision path point is calculated by arc coordinate consider ego vehicle's geometry.
   - safe velocity and safe margin is calculated from performance of ego emergency braking system.
 end note
@@ -197,11 +198,11 @@ stop
 @enduml
 ```
 
-##### Detail process for public road
+##### Detail process for predicted object
 
 ```plantuml
 @startuml
-title modifyPathVelocity For Public Road
+title modifyPathVelocity
 start
 
 partition process_path {
@@ -253,7 +254,7 @@ stop
 
 ```plantuml
 @startuml
-title modifyPathVelocity For Private Road
+title modifyPathVelocity For Occupancy
 start
 
 partition process_path {
@@ -285,7 +286,11 @@ partition generate_possible_collision {
 :generate possible collision from occlusion spot;
 note right
   - occlusion spot candidate is N by N size unknown cells.
-  - consider occlusion which is nearer than `lateral_distance_threshold`.
+  - consider occlusion spot in detection area polygon.
+end note
+:filter occlusion spot by partition lanelets;
+note right
+  - filter occlusion spot by partition lanelets which prevent pedestrians come out.
 end note
 :calculate collision path point and intersection point;
 :calculate safe velocity and safe margin for possible collision;

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

@@ -28,6 +28,8 @@ namespace behavior_velocity_planner
 namespace
 {
 using builtin_interfaces::msg::Time;
+using BasicPolygons = std::vector<lanelet::BasicPolygon2d>;
+using Slices = std::vector<occlusion_spot_utils::Slice>;
 
 visualization_msgs::msg::Marker makeArrowMarker(
   const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id)
@@ -51,7 +53,7 @@ visualization_msgs::msg::Marker makeArrowMarker(
 
 std::vector<visualization_msgs::msg::Marker> makeSlowDownMarkers(
   const occlusion_spot_utils::PossibleCollisionInfo & possible_collision,
-  const std::string road_type, const int id)
+  const std::string detection_type, const int id)
 {
   // virtual wall
   std::vector<visualization_msgs::msg::Marker> debug_markers;
@@ -86,7 +88,7 @@ std::vector<visualization_msgs::msg::Marker> makeSlowDownMarkers(
   debug_markers.emplace_back(slowdown_reason_marker);
   slowdown_reason_marker.scale = tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.5);
   slowdown_reason_marker.id = id + 100;
-  slowdown_reason_marker.text = "\n \n" + road_type;
+  slowdown_reason_marker.text = "\n \n" + detection_type;
   debug_markers.emplace_back(slowdown_reason_marker);
   debug_markers.push_back(makeArrowMarker(possible_collision, id));
   return debug_markers;
@@ -142,7 +144,7 @@ std::vector<visualization_msgs::msg::Marker> makeCollisionMarkers(
 }
 
 visualization_msgs::msg::MarkerArray makePolygonMarker(
-  const std::vector<lanelet::BasicPolygon2d> & polygons, const int id, const double z)
+  const BasicPolygons & polygons, const std::string ns, const int id, const double z)
 {
   visualization_msgs::msg::MarkerArray debug_markers;
   visualization_msgs::msg::Marker debug_marker;
@@ -154,9 +156,9 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z);
   debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
   debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
-  debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3);
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.5);
   debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
-  debug_marker.ns = "detection_area";
+  debug_marker.ns = ns;
   for (const auto & poly : polygons) {
     for (const auto & p : poly) {
       geometry_msgs::msg::Point point =
@@ -170,6 +172,35 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   return debug_markers;
 }
 
+visualization_msgs::msg::MarkerArray makeSlicePolygonMarker(
+  const Slices & slices, const std::string ns, const int id, const double z)
+{
+  visualization_msgs::msg::MarkerArray debug_markers;
+  visualization_msgs::msg::Marker debug_marker;
+  debug_marker.header.frame_id = "map";
+  debug_marker.header.stamp = rclcpp::Time(0);
+  debug_marker.id = planning_utils::bitShift(id);
+  debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+  debug_marker.action = visualization_msgs::msg::Marker::ADD;
+  debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z);
+  debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
+  debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3);
+  debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
+  debug_marker.ns = ns;
+  for (const auto & slice : slices) {
+    for (const auto & p : slice.polygon) {
+      geometry_msgs::msg::Point point =
+        tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0);
+      debug_marker.points.push_back(point);
+    }
+    debug_markers.markers.push_back(debug_marker);
+    debug_marker.id++;
+    debug_marker.points.clear();
+  }
+  return debug_markers;
+}
+
 visualization_msgs::msg::MarkerArray createPathMarkerArray(
   const PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const double r,
   const double g, const double b)
@@ -211,7 +242,7 @@ visualization_msgs::msg::MarkerArray createPossibleCollisionMarkers(
   int id = planning_utils::bitShift(module_id_);
   for (const auto & possible_collision : possible_collisions) {
     std::vector<visualization_msgs::msg::Marker> collision_markers =
-      makeSlowDownMarkers(possible_collision, debug_data.road_type, id++);
+      makeSlowDownMarkers(possible_collision, debug_data.detection_type, id++);
     occlusion_spot_slowdown_markers.markers.insert(
       occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(),
       collision_markers.end());
@@ -261,10 +292,11 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar
     appendMarkerArray(
       createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array);
   }
-  if (!debug_data_.detection_areas.empty()) {
+  if (!debug_data_.detection_area_polygons.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time,
-      &debug_marker_array);
+      makeSlicePolygonMarker(
+        debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z),
+      current_time, &debug_marker_array);
   }
 
   return debug_marker_array;
@@ -278,10 +310,16 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray
     appendMarkerArray(
       createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array);
   }
-  if (!debug_data_.detection_areas.empty()) {
+  if (!debug_data_.detection_area_polygons.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time,
-      &debug_marker_array);
+      makeSlicePolygonMarker(
+        debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z),
+      current_time, &debug_marker_array);
+  }
+  if (!debug_data_.partition_lanelets.empty()) {
+    appendMarkerArray(
+      makePolygonMarker(debug_data_.partition_lanelets, "partition", module_id_, debug_data_.z),
+      current_time, &debug_marker_array);
   }
   if (!debug_data_.interp_path.points.empty()) {
     appendMarkerArray(

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

@@ -46,6 +46,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
     throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"};
   }
   pp.debug = node.declare_parameter(ns + ".debug", false);
+  pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", false);
   pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0);
   pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0);
   pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel", 1.0);