Feature/porting occlusion spot (#1740)

* Feature/occlusion_spot safety planner public road (#1594) * add blind spot safety planner public road * remove duplicated procesing * remove unused private param * renaming fix typo add comments * fix spell check * velocity -> relative velocity * calc2d, To param, simplify search, To original * add the num possible collision gurd * computational cost reduction * Cosmetic change for PossibleCollisionInfo * add interpolation to possible collision value * refactor codes * fix details * invalid point gurd * To Param * refacotor to occlusion spot util * cosmetic change * clean up blindspot * To Occlusion Spot * revise readme * refactor drawing * for better explanation * fix velocity profile * clean up details * cosmetic change for debug marker * use max velocity in obstacle info instead * add gtest for Too Many Possible Collision case * change case * refactor readme * minor fix * add more occlusion spot explanation * replace svg * add gtest build path lanelet * hotfix lateral distance and searching method * update g test for lateral distance * use faster search * set more realistic param * add lanelet subtype highway * cosmetic change of reviews * add occlusion spot module in private area (#1640) * add occlusion spot in private * For debugging change * add spline interpolation to path * add review changes * adding minor change * cosmetic change * Vector to retval * Blindspot To OcclusionSpot1 * To Occlusion Spot 2 * To Occlusions spot3 * update gtest with unified anchor * remove anchor * add test slice * simplify interpolation * Too Occlusion spot4 * add test buffer * To Occlusion spot * namespace gurd * correct slice and add interpolation first * handle self crossing with check for invation * to ros debug stream * removed unused interpolation * add readme to plant uml * cosmetic change * minor change * update readme * review change * change occlusion spot text color * To Offset no Throw * better debug marker * catch only inversion error * skip return in case of inversion * for better grid * simplify path lanelet at first * remove std::cout * for better path * limit ego path inside target lanelet location * remove last points * cosmetic change for markers * apply module for limited scene * fix interpolation gurd * for better params * do not includes path behind * remove dummy perception publisher * Revert "remove dummy perception publisher" This reverts commit 4acad985fe31dd9befaa21a16631495de6c3a117. * replace hard coded occupancy grid option in psim * remove respawn * add arg to params and remove redundunt launch * fix spell check * fix default value Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * Feature/occlusion spot private slice size param (#1703) * add min slice size * for a bit narrow lateral distance * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/config/occlusion_spot_param.yaml Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * Rename files Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Porting to ros2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * pre-commit fixes Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix typo Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix launch namespace Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * Fix parameter type Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
autowarefoundation · tkimura4 · Dec 7, 2021 · Sep 18, 2020 · Jul 16, 2020 · Jul 17, 2020
commit fb4331efdc91cbae489449ec111be5e285bb3147
diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(Boost REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common)
+find_package(OpenCV REQUIRED)
 
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
@@ -42,6 +43,8 @@ set(BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES
   tf2_ros
   vehicle_info_util
   visualization_msgs
+  nav_msgs
+  grid_map_ros
 )
 
 
@@ -191,6 +194,28 @@ ament_target_dependencies(scene_module_virtual_traffic_light ${BEHAVIOR_VELOCITY
 target_link_libraries(scene_module_virtual_traffic_light scene_module_lib)
 
 
+# SceneModule OcclusionSpot
+ament_auto_add_library(scene_module_occlusion_spot SHARED
+  src/scene_module/occlusion_spot/grid_utils.cpp
+  src/scene_module/occlusion_spot/geometry.cpp
+  src/scene_module/occlusion_spot/manager.cpp
+  src/scene_module/occlusion_spot/debug.cpp
+  src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp
+  src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp
+  src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
+  src/scene_module/occlusion_spot/risk_predictive_braking.cpp
+)
+
+target_include_directories(scene_module_occlusion_spot
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(scene_module_occlusion_spot ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES})
+
+target_link_libraries(scene_module_occlusion_spot scene_module_lib)
+
 # Scene Module Manager
 ament_auto_add_library(scene_module_manager SHARED
   src/planner_manager.cpp
@@ -227,6 +252,7 @@ target_link_libraries(behavior_velocity_planner
   scene_module_intersection
   scene_module_traffic_light
   scene_module_blind_spot
+  scene_module_occlusion_spot
   scene_module_detection_area
   scene_module_virtual_traffic_light
   scene_module_manager
@@ -242,6 +268,23 @@ rclcpp_components_register_node(behavior_velocity_planner
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(occlusion_spot-test
+    test/src/utils.hpp
+    test/src/test_occlusion_spot_utils.cpp
+    test/src/test_risk_predictive_braking.cpp
+    test/src/test_geometry.cpp
+    test/src/test_grid_utils.cpp
+    test/src/test_performances.cpp
+    src/scene_module/occlusion_spot/geometry.cpp
+    src/scene_module/occlusion_spot/grid_utils.cpp
+    src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
+    src/scene_module/occlusion_spot/risk_predictive_braking.cpp
+  )
+  target_link_libraries(occlusion_spot-test
+    gtest_main
+    scene_module_occlusion_spot
+  )
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE

diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md
@@ -11,6 +11,7 @@ It consists of several modules.
 - Intersection
 - Stop Line
 - Traffic Light
+- Occlusion Spot
 
 When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose.
 So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position.
@@ -45,6 +46,7 @@ So for example, in order to stop at a stop line with the vehicles' front on the
 | `launch_intersection`   | bool   | whether to launch intersection module                                               |
 | `launch_traffic_light`  | bool   | whether to launch traffic light module                                              |
 | `launch_stop_line`      | bool   | whether to launch stop_line module                                                  |
+| `launch_occlusion_spot` | bool   | whether to launch occlusion_spot module                                             |
 | `forward_path_length`   | double | forward path length                                                                 |
 | `backward_path_length`  | double | backward path length                                                                |
 | `max_accel`             | double | (to be a global parameter) max acceleration of the vehicle                          |
@@ -126,3 +128,84 @@ Then, we can get `offset segment` and `offset from segment start`.
 After that, we can calculate a offset point from `offset segment` and `offset`. This will be `stop_pose`.
 
 ![calculate_stop_pose](./docs/stop_line/calculate_stop_pose.drawio.svg)
+
+### Occlusion Spot
+
+#### Role
+
+This module plans safe velocity to slow down before reaching collision point that hidden object is darting out from `occlusion spot` where driver can't see clearly because of obstacles.
+
+![brief](./docs/occlusion_spot/occlusion_spot.svg)
+
+### Occlusion Spot Private
+
+This module only works in private road and use occupancy grid map to detect occlusion spots.
+
+### Occlusion Spot Public
+
+This module only works in public road and use dynamic objects to detect occlusion spots.
+
+#### Module Parameters
+
+| Parameter            | Type   | Description                                                               |
+| -------------------- | ------ | ------------------------------------------------------------------------- |
+| `pedestrian_vel`     | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point |
+| `safety_time_buffer` | double | [m/s] time buffer for the system delay                                    |
+
+| Parameter /threshold      | Type   | Description                                               |
+| ------------------------- | ------ | --------------------------------------------------------- |
+| `detection_area_length`　 | double | [m] the length of path to consider occlusion spot         |
+| `stuck_vehicle_vel`       | double | [m/s] velocity below this value is assumed to stop        |
+| `lateral_distance`        | double | [m] maximum lateral distance to consider hidden collision |
+
+| Parameter /(public or private)\_road | Type   | Description                                                          |
+| ------------------------------------ | ------ | -------------------------------------------------------------------- |
+| `min_velocity`                       | double | [m/s] minimum velocity to ignore occlusion spot                      |
+| `ebs_decel`                          | double | [m/s^2] maximum deceleration to assume for emergency braking system. |
+| `pbs_decel`                          | double | [m/s^2] deceleration to assume for predictive braking system         |
+
+| Parameter /sidewalk       | Type   | Description                                                     |
+| ------------------------- | ------ | --------------------------------------------------------------- |
+| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot               |
+| `focus_range`             | double | [m] buffer around the ego path used to build the sidewalk area. |
+
+| Parameter /grid  | Type   | Description                                                     |
+| ---------------- | ------ | --------------------------------------------------------------- |
+| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid    |
+| `occupied_min`   | double | [-] buffer around the ego path used to build the sidewalk area. |
+
+#### Flowchart
+
+```plantuml
+@startuml
+title modifyPathVelocity (Private/Public) Road
+start
+
+:get current road type;
+
+if (road type is PUBLIC) then (yes)
+  :use dynamic object array info;
+else if (road type is PRIVATE) then (yes)
+  :use occupancy grid map info;
+else (no)
+  stop
+endif
+
+:generate possible collision;
+
+:find possible collision between path and occlusion spot;
+
+if (possible collision is found?) then (yes)
+else (no)
+  stop
+endif
+
+:calculate collision path point;
+
+:calculate safe velocity consider lateral distance and safe velocity;
+
+:insertSafeVelocityToPath;
+
+stop
+@enduml
+```
diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    occlusion_spot:
+      pedestrian_vel: 1.0                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
+      safety_time_buffer: 0.1                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
+      threshold:
+        detection_area_length: 200.0        # [m] the length of path to consider occlusion spot
+        stuck_vehicle_vel: 1.0              # [m/s] velocity below this value is assumed to stop
+        lateral_distance: 1.5               # [m] maximum lateral distance to consider hidden collision
+      # road type parameters
+      public_road:
+        min_velocity: 5.0                   # [m/s] minimum velocity to ignore occlusion spot
+        ebs_decel: -6.0                     # [m/s^2] maximum deceleration to assume for emergency stops.
+        pbs_decel: -0.5                     # [m/s^2] deceleration to assume for predictive braking stops
+      private_road:
+        min_velocity: 1.5                   # [m/s] minimum velocity to ignore occlusion spot
+        ebs_decel: -4.0                     # [m/s^2] maximum deceleration to assume for emergency stops.
+        pbs_decel: -0.5                     # [m/s^2] deceleration to assume for predictive braking stops.
+      sidewalk:
+        min_occlusion_spot_size: 1.5     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
+        slice_size: 0.5     # [m] size of sidewalk slices in both length and distance relative to the ego trajectory.
+        focus_range: 1.5            # [m] buffer around the ego path used to build the sidewalk area.
+      grid:
+        free_space_max: 40  # maximum value of a free space cell in the occupancy grid
+        occupied_min: 60   # minimum value of an occupied cell in the occupancy grid
diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg
diff --git a/planning/behavior_velocity_planner/docs/stop_line/occlusion_spot.svg b/planning/behavior_velocity_planner/docs/stop_line/occlusion_spot.svg
diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
@@ -27,6 +27,7 @@
 #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
 #include "autoware_planning_msgs/msg/path.hpp"
 #include "autoware_planning_msgs/msg/path_with_lane_id.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 
@@ -63,6 +64,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
     sub_external_traffic_light_states_;
   rclcpp::Subscription<autoware_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
     sub_virtual_traffic_light_states_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr
+    sub_occupancy_grid_;
 
   void onTrigger(const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg);
   void onDynamicObjects(
@@ -79,6 +82,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
     const autoware_api_msgs::msg::IntersectionStatus::ConstSharedPtr msg);
   void onVirtualTrafficLightStates(
     const autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg);
+  void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg);
 
   // publisher
   rclcpp::Publisher<autoware_planning_msgs::msg::Path>::SharedPtr path_pub_;

diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp
@@ -37,6 +37,7 @@
 #include "autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "std_msgs/msg/header.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
@@ -70,6 +71,8 @@ struct PlannerData
   autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr dynamic_objects;
   pcl::PointCloud<pcl::PointXYZ>::ConstPtr no_ground_pointcloud;
   lanelet::LaneletMapPtr lanelet_map;
+  // occupancy grid
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid;
 
   // other internal data
   std::map<int, autoware_perception_msgs::msg::TrafficLightStateStamped> traffic_light_id_map;

diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp
@@ -0,0 +1,71 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_
+#define SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_
+
+#include <vector>
+
+#include "boost/geometry.hpp"
+#include "lanelet2_core/geometry/Lanelet.h"
+#include "lanelet2_core/geometry/Point.h"
+
+namespace behavior_velocity_planner
+{
+namespace geometry
+{
+namespace bg = boost::geometry;
+
+// @brief represent the range of a slice
+// (either by index on the lanelet centerline or by arc coordinates)
+struct SliceRange
+{
+  double min_length{};
+  double max_length{};
+  double min_distance{};
+  double max_distance{};
+};
+
+// @brief representation of a slice along a path
+struct Slice
+{
+  SliceRange range{};
+  lanelet::BasicPolygon2d polygon{};
+};
+
+//!< @brief build slices all along the trajectory
+// using the given range and desired slice length and width
+void buildSlices(
+  std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet,
+  const SliceRange & range, const double slice_length, const double slice_width);
+//!< @brief build an interpolated polygon between the given bounds
+void buildInterpolatedPolygon(
+  lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & from_bound,
+  const lanelet::BasicLineString2d & to_bound, const double from_length, const double to_length,
+  const double from_ratio_dist, const double to_ratio_dist);
+//!< @brief build sidewalk slice from path
+std::vector<geometry::Slice> buildSidewalkSlices(
+  const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset,
+  const double lateral_offset, const double min_size, const double lateral_max_dist);
+//!< @brief calculate interpolation between a and b at distance ratio t
+template<typename T>
+T lerp(T a, T b, double t)
+{
+  return a + t * (b - a);
+}
+
+}  // namespace geometry
+}  // namespace behavior_velocity_planner
+
+#endif  // SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_