Merge pull request #51 from booars/hotfix/sync_booars_launch

hotfix: fix booars_launch for update

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml

@@ -5,16 +5,16 @@
   <arg name="sensor_model"/>
   <arg name="launch_vehicle_interface"/>
 
-  <log message="The arguments for aichallenge_submit_launch."/>
+  <log message="The arguments for booars_launch."/>
   <log message=" - simulation: $(var simulation)"/>
   <log message=" - use_sim_time: $(var use_sim_time)"/>
   <log message=" - sensor_model: $(var sensor_model)"/>
   <log message=" - launch_vehicle_interface: $(var launch_vehicle_interface)"/>
 
-  <include file="$(find-pkg-share aichallenge_submit_launch)/launch/reference.launch.xml" >
+  <include file="$(find-pkg-share booars_launch)/launch/booars.launch.xml" >
     <arg name="vehicle_model" value="racing_kart"/>
     <arg name="sensor_model" value="$(var sensor_model)"/>
-    <arg name="map_path" value="$(find-pkg-share aichallenge_submit_launch)/map"/>
+    <arg name="map_path" value="$(find-pkg-share booars_launch)/map"/>
     <arg name="use_sim_time" value="$(var use_sim_time)"/>
     <arg name="simulation" value="$(var simulation)"/>
     <arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>

aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt

@@ -3,4 +3,4 @@ project(booars_launch)
 
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
-ament_auto_package(INSTALL_TO_SHARE launch map config)
+ament_auto_package(INSTALL_TO_SHARE launch map config data)

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner.param.yaml

@@ -0,0 +1,69 @@
+/**:
+  ros__parameters:
+    verbose: false
+
+    planning_hz: 10.0
+    backward_path_length: 5.0
+    forward_path_length: 300.0
+    backward_length_buffer_for_end_of_pull_over: 5.0
+    backward_length_buffer_for_end_of_pull_out: 5.0
+    minimum_pull_over_length: 16.0
+
+    refine_goal_search_radius_range: 7.5
+
+    # parameters for turn signal decider
+    turn_signal_intersection_search_distance: 30.0
+    turn_signal_intersection_angle_threshold_deg: 15.0
+    turn_signal_minimum_search_distance: 10.0
+    turn_signal_search_time: 3.0
+    turn_signal_shift_length_threshold: 0.3
+    turn_signal_on_swerving: true
+
+    path_interval: 1.0
+
+    visualize_maximum_drivable_area: true
+
+    lateral_distance_max_threshold: 2.0
+    longitudinal_distance_min_threshold: 3.0
+
+    expected_front_deceleration: -1.0
+    expected_rear_deceleration: -1.0
+
+    expected_front_deceleration_for_abort: -1.0
+    expected_rear_deceleration_for_abort: -2.0
+
+    rear_vehicle_reaction_time: 2.0
+    rear_vehicle_safety_time_margin: 1.0
+
+    # USE ONLY WHEN THE OPTION COMPILE_WITH_OLD_ARCHITECTURE IS SET TO FALSE.
+    # https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/CMakeLists.txt
+    # NOTE: The smaller the priority number is, the higher the module priority is.
+    lane_change:
+      enable_module: true
+      enable_simultaneous_execution: false
+      priority: 4
+      max_module_size: 1
+
+    pull_out:
+      enable_module: true
+      enable_simultaneous_execution: false
+      priority: 0
+      max_module_size: 1
+
+    side_shift:
+      enable_module: true
+      enable_simultaneous_execution: false
+      priority: 2
+      max_module_size: 1
+
+    pull_over:
+      enable_module: true
+      enable_simultaneous_execution: false
+      priority: 1
+      max_module_size: 1
+
+    avoidance:
+      enable_module: true
+      enable_simultaneous_execution: false
+      priority: 3
+      max_module_size: 1

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner_tree.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Fallback>
+            <Action ID="LaneFollowing_Plan" output="{output}"/>
+        </Fallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="a">
+        <ReactiveFallback>
+            <Condition ID="LaneChange_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="LaneChange_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="b">
+        <ReactiveFallback>
+            <Condition ID="Avoidance_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="Avoidance_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Condition ID="Avoidance_CheckApproval"/>
+        <Action ID="Avoidance_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="Avoidance_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="Avoidance_Request"/>
+        <Condition ID="ExternalApproval"/>
+        <Condition ID="LaneChange_CheckApproval"/>
+        <Action ID="LaneChange_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="LaneChange_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="LaneChange_Request"/>
+        <Action ID="LaneFollowing_Plan">
+            <output_port name="output" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
+        </Action>
+        <Action ID="LaneFollowing_PlanCandidate">
+            <output_port name="output_candidate" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
+        </Action>
+        <Condition ID="LaneFollowing_Request"/>
+        <Action ID="PullOut_Plan">
+            <output_port name="output"/>
+        </Action>
+        <Condition ID="PullOut_Request"/>
+        <Action ID="PullOver_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="PullOver_Request"/>
+        <Condition ID="SideShift_CheckApproval"/>
+        <Action ID="SideShift_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="SideShift_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="SideShift_Request"/>
+        <SubTree ID="SubTree"/>
+        <SubTree ID="a"/>
+        <SubTree ID="b"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/map/lanelet2_map_loader.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    lanelet2_map_projector_type: MGRS  # Options: MGRS, UTM
+    center_line_resolution: 5.0        # [m]

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml

@@ -8,7 +8,7 @@
     multi_layered_costmap:
       layers:
         - "cached_lanelet_layer"
-        - "predicted_object_layer"
+        # - "predicted_object_layer"
       cached_lanelet_layer:
         type: "cached_lanelet"
         map_topic: "/map/vector_map"

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    map_frame: map
+    arrival_check_angle_deg: 45.0
+    arrival_check_distance: 1.0
+    arrival_check_duration: 1.0
+    goal_angle_threshold_deg: 45.0
+    enable_correct_goal_pose: false

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/scenario_planning/common/nearest_search.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    # ego
+    ego_nearest_dist_threshold: 3.0 # [m]
+    ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -0,0 +1,116 @@
+# see AvoidanceParameters description in avoidance_module_data.hpp for description.
+/**:
+  ros__parameters:
+    avoidance:
+      resample_interval_for_planning: 0.3               # [m]
+      resample_interval_for_output: 4.0                 # [m]
+      detection_area_right_expand_dist: 0.0             # [m]
+      detection_area_left_expand_dist: 1.0              # [m]
+      drivable_area_right_bound_offset: 0.0             # [m]
+      drivable_area_left_bound_offset: 0.0              # [m]
+      object_envelope_buffer: 0.3                       # [m]
+
+      # avoidance module common setting
+      enable_bound_clipping: false
+      enable_avoidance_over_same_direction: true
+      enable_avoidance_over_opposite_direction: true
+      enable_update_path_when_object_is_gone: false
+      enable_force_avoidance_for_stopped_vehicle: false
+      enable_safety_check: false
+      enable_yield_maneuver: false
+      disable_path_update: false
+
+      # for debug
+      publish_debug_marker: false
+      print_debug_info: false
+
+      # avoidance is performed for the object type with true
+      target_object:
+        car: true
+        truck: true
+        bus: true
+        trailer: true
+        unknown: false
+        bicycle: false
+        motorcycle: false
+        pedestrian: false
+
+      # For target object filtering
+      target_filtering:
+        # filtering moving objects
+        threshold_speed_object_is_stopped: 1.0          # [m/s]
+        threshold_time_object_is_moving: 1.0            # [s]
+        threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s]
+        # detection range
+        object_check_force_avoidance_clearance: 30.0    # [m]
+        object_check_forward_distance: 150.0            # [m]
+        object_check_backward_distance: 2.0             # [m]
+        object_check_goal_distance: 20.0                # [m]
+        # filtering parking objects
+        threshold_distance_object_is_on_center: 1.0     # [m]
+        object_check_shiftable_ratio: 0.6               # [-]
+        object_check_min_road_shoulder_width: 0.5       # [m]
+        # lost object compensation
+        object_last_seen_threshold: 2.0
+
+      # For safety check
+      safety_check:
+        safety_check_backward_distance: 100.0            # [m]
+        safety_check_time_horizon: 10.0                  # [s]
+        safety_check_idling_time: 1.5                    # [s]
+        safety_check_accel_for_rss: 2.5                  # [m/ss]
+        safety_check_hysteresis_factor: 2.0              # [-]
+
+      # For avoidance maneuver
+      avoidance:
+        # avoidance lateral parameters
+        lateral:
+          lateral_collision_margin: 1.0                 # [m]
+          lateral_collision_safety_buffer: 0.7          # [m]
+          lateral_passable_safety_buffer: 0.0           # [m]
+          road_shoulder_safety_margin: 0.3              # [m]
+          avoidance_execution_lateral_threshold: 0.499
+          max_right_shift_length: 5.0
+          max_left_shift_length: 5.0
+        # avoidance distance parameters
+        longitudinal:
+          prepare_time: 2.0                             # [s]
+          longitudinal_collision_safety_buffer: 0.0     # [m]
+          min_prepare_distance: 1.0                     # [m]
+          min_avoidance_distance: 10.0                  # [m]
+          min_nominal_avoidance_speed: 7.0              # [m/s]
+          min_sharp_avoidance_speed: 1.0                # [m/s]
+
+      # For yield maneuver
+      yield:
+        yield_velocity: 2.78                            # [m/s]
+
+      # For stop maneuver
+      stop:
+        min_distance: 10.0                              # [m]
+        max_distance: 20.0                              # [m]
+
+      constraints:
+        # vehicle slows down under longitudinal constraints
+        use_constraints_for_decel: false                # [-]
+
+        # lateral constraints
+        lateral:
+          nominal_lateral_jerk: 0.2                     # [m/s3]
+          max_lateral_jerk: 1.0                         # [m/s3]
+
+        # longitudinal constraints
+        longitudinal:
+          nominal_deceleration: -1.0                    # [m/ss]
+          nominal_jerk: 0.5                             # [m/sss]
+          max_deceleration: -2.0                        # [m/ss]
+          max_jerk: 1.0
+          # For prevention of large acceleration while avoidance
+          min_avoidance_speed_for_acc_prevention: 3.0   # [m/s]
+          max_avoidance_acceleration: 0.5               # [m/ss]
+
+      target_velocity_matrix:
+        col_size: 2
+        matrix:
+          [2.78, 13.9,                                  # velocity [m/s]
+           0.50, 1.00]                                  # margin [m]

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml

@@ -0,0 +1,58 @@
+/**:
+  ros__parameters:
+    # Static expansion
+    avoidance:
+      drivable_area_right_bound_offset: 0.0
+      drivable_area_left_bound_offset: 0.0
+      drivable_area_types_to_skip: [road_border]
+    lane_change:
+      drivable_area_right_bound_offset: 0.0
+      drivable_area_left_bound_offset: 0.0
+      drivable_area_types_to_skip: [road_border]
+    lane_following:
+      drivable_area_right_bound_offset: 0.0
+      drivable_area_left_bound_offset: 0.0
+      drivable_area_types_to_skip: [road_border]
+    pull_out:
+      drivable_area_right_bound_offset: 0.0
+      drivable_area_left_bound_offset: 0.0
+      drivable_area_types_to_skip: [road_border]
+    pull_over:
+      drivable_area_right_bound_offset: 0.0
+      drivable_area_left_bound_offset: 0.0
+      drivable_area_types_to_skip: [road_border]
+    side_shift:
+      drivable_area_right_bound_offset: 0.0
+      drivable_area_left_bound_offset: 0.0
+      drivable_area_types_to_skip: [road_border]
+
+    # Dynamic expansion by projecting the ego footprint along the path
+    dynamic_expansion:
+      enabled: false
+      ego:
+        extra_footprint_offset:
+          front: 0.5 # [m] extra length to add to the front of the ego footprint
+          rear: 0.5 # [m] extra length to add to the rear of the ego footprint
+          left: 0.5 # [m] extra length to add to the left of the ego footprint
+          right: 0.5 # [m] extra length to add to the rear of the ego footprint
+      dynamic_objects:
+        avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects
+        extra_footprint_offset:
+          front: 0.5 # [m] extra length to add to the front of the dynamic object footprint
+          rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
+          left: 0.5 # [m] extra length to add to the left of the dynamic object footprint
+          right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
+      expansion:
+        method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'.
+                         # 'lanelet': add lanelets overlapped by the ego footprints
+                         # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area
+        max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit)
+        max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit)
+        extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint
+      avoid_linestring:
+        types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area
+          - road_border
+        distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid
+        compensate:
+          enable: true  # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction
+          extra_distance: 3.0 # [m] extra distance to add to the compensation