feat(behavior_velocity): update grid map collision judgement using ob…

…ject info (tier4#568)

* feature(behavior_velocity): update grid map utils

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): remove unused

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): early continue

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add object info to occ grid and update collision judgement

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add debug options

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): replace to planning utils

* chore(behavior_velocity): parametrize pedestrian radius and use planner data

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): update doc and minor change

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): minor change

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): update docs and update parameter from experiment

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add compare polygon iterator and line iterator

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): reduce test verbose

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): to silent g-test

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): chores

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add offset from front bumper

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): use clippled path as marker

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add extract close partition

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(behavior_velocity): fix remaining

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): update debug marker

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add ray option and tune param

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(behavior_velocity): fix predicted path

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

planning/behavior_velocity_planner/CMakeLists.txt

@@ -218,13 +218,31 @@ target_link_libraries(scene_module_virtual_traffic_light scene_module_lib)
 
 
 # SceneModule OcclusionSpot
-ament_auto_add_library(scene_module_occlusion_spot SHARED
+# Util
+ament_auto_add_library(occlusion_spot_lib SHARED
   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_include_directories(occlusion_spot_lib
+  SYSTEM PUBLIC
+    ${BOOST_INCLUDE_DIRS}
+    ${tf2_geometry_msgs_INCLUDE_DIRS}
+)
+
+target_include_directories(occlusion_spot_lib
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(occlusion_spot_lib ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES})
+
+ament_auto_add_library(scene_module_occlusion_spot SHARED
   src/scene_module/occlusion_spot/manager.cpp
   src/scene_module/occlusion_spot/debug.cpp
   src/scene_module/occlusion_spot/scene_occlusion_spot.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
@@ -235,7 +253,7 @@ target_include_directories(scene_module_occlusion_spot
 
 ament_target_dependencies(scene_module_occlusion_spot ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES})
 
-target_link_libraries(scene_module_occlusion_spot scene_module_lib)
+target_link_libraries(scene_module_occlusion_spot scene_module_lib occlusion_spot_lib)
 
 # Scene Module Manager
 ament_auto_add_library(scene_module_manager SHARED

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -1,11 +1,17 @@
 /**:
   ros__parameters:
     occlusion_spot:
-      detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object"
-      pass_judge: "current_velocity"       # [-] candidate is "current_velocity""
-      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
+      detection_method: "predicted_object"  # [-] candidate is "occupancy_grid" or "predicted_object"
+      pass_judge: "current_velocity"        # [-] candidate is "current_velocity"
+      filter_occupancy_grid: true           # [-] whether to filter occupancy grid by morphologyEx or not
+      use_object_info: true                 # [-] whether to reflect object info to occupancy grid map or not
+      use_partition_lanelet: true           # [-] whether to use partition lanelet map data
+      pedestrian_vel: 1.5                   # [m/s] assume pedestrian is dashing from occlusion at this velocity
+      pedestrian_radius: 0.3                # [m] assume pedestrian width(0.2m) + margin(0.1m)
+      debug:                                # !Note: default should be false for performance
+        is_show_occlusion: false            # [-] whether to show occlusion point markers.
+        is_show_cv_window: false            # [-] whether to show open_cv debug window
+        is_show_processing_time: false      # [-] whether to show processing time
       threshold:
         detection_area_length: 100.0        # [m] the length of path to consider perception range
         stuck_vehicle_vel: 1.0              # [m/s] velocity below this value is assumed to stop
@@ -17,12 +23,12 @@
         non_effective_jerk: -0.3            # [m/s^3] weak jerk for velocity planning.
         non_effective_acceleration: -1.0    # [m/s^2] weak deceleration for velocity planning.
         min_allowed_velocity: 1.0           # [m/s] minimum velocity allowed
-        delay_time: 0.1                     # [s] safety time buffer for delay system response
-        safe_margin: 1.0                    # [m] maximum safety distance for any error
+        safe_margin: 2.0                    # [m] margin for detection failure(0.5m) + pedestrian radius(0.5m) + safe margin(1.0m)
       detection_area:
         min_occlusion_spot_size: 1.0     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
         slice_length: 10.0               # [m] size of slices in both length and distance relative to the ego path.
-        max_lateral_distance: 4.0        # [m] buffer around the ego path used to build the detection area.
+        min_longitudinal_offset: 1.0     # [m] detection area safety buffer from front bumper.
+        max_lateral_distance: 6.0        # [m] buffer around the ego path used to build the detection 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