Feature/porting occlusion spot (autowarefoundation#309)

* add occulusion_spot marker (autowarefoundation#266)

* add blindspot marker

* to occlusion spot slow down

* remove debug info from marker (autowarefoundation#287)

* remove debug info from marker

* remove debug arrow

* fix format

* update behavior launch

* apply pep8

* fix format

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

autoware_launch/rviz/autoware.rviz

@@ -1005,6 +1005,24 @@ Visualization Manager:
                         Reliability Policy: Reliable
                         Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area
                       Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: OcclusionSpot
+                      Namespaces:
+                        arrow: false
+                        occlusion spot slow down: true
+                        collision: false
+                        info_obstacle: false
+                        obstacle: false
+                        sidewalk: false
+                        slow factor_text: true
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot
+                      Value: true
                     - Class: rviz_plugins/Path
                       Color Border Vel Max: 3
                       Enabled: true

planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -174,6 +174,14 @@ def generate_launch_description():
     with open(traffic_light_param_path, 'r') as f:
         traffic_light_param = yaml.safe_load(f)['/**']['ros__parameters']
 
+    occlusion_spot_param_path = os.path.join(
+        get_package_share_directory('behavior_velocity_planner'),
+        'config',
+        'occlusion_spot.param.yaml',
+    )
+    with open(occlusion_spot_param_path, 'r') as f:
+        occlusion_spot_param = yaml.safe_load(f)['/**']['ros__parameters']
+
     behavior_velocity_planner_component = ComposableNode(
         package='behavior_velocity_planner',
         plugin='behavior_velocity_planner::BehaviorVelocityPlannerNode',
@@ -189,6 +197,7 @@ def generate_launch_description():
              '/perception/traffic_light_recognition/traffic_light_states'),
             ('~/input/external_traffic_light_states',
              '/awapi/traffic_light/put/traffic_light_status'),
+            ('~/input/occupancy_grid', '/sensing/lidar/occupancy_grid'),
             ('~/output/path', 'path'),
             ('~/output/stop_reasons',
              '/planning/scenario_planning/status/stop_reasons'),
@@ -202,6 +211,7 @@ def generate_launch_description():
                 'launch_intersection': True,
                 'launch_blind_spot': True,
                 'launch_detection_area': True,
+                'launch_occlusion_spot': True,
                 'forward_path_length': 1000.0,
                 'backward_path_length': 5.0,
                 'max_accel': -2.8,
@@ -212,7 +222,8 @@ def generate_launch_description():
             detection_area_param,
             intersection_param,
             stop_line_param,
-            traffic_light_param
+            traffic_light_param,
+            occlusion_spot_param
         ],
         extra_arguments=[{
             'use_intra_process_comms': LaunchConfiguration('use_intra_process')