Merge pull request #40 from booars/sync-upstream

chore: sync upstream

.cspell.json

@@ -50,22 +50,26 @@
     "gird"
   ],
   "words": [
+    "accelmap",
     "ackermann",
     "adapi",
     "aichallenge",
     "astar",
     "autocompute",
+    "autodetermine",
     "automotiveaichallenge",
     "autoware",
     "awsim",
     "booars",
+    "brakemap",
     "buildtool",
     "colcon",
     "costmap",
     "cuda",
     "cyclonedds",
     "dallara",
     "dcmake",
+    "decel",
     "distro",
     "downsample",
     "freespace",

Dockerfile

@@ -11,8 +11,12 @@ RUN apt-get -y install ros-humble-rqt-graph
 # PATH="$PATH:/root/.local/bin"
 # PATH="/usr/local/cuda/bin:$PATH"
 ENV XDG_RUNTIME_DIR=/tmp/xdg
-ENV ROS_LOCALHOST_ONLY=1
+ENV ROS_LOCALHOST_ONLY=0
 ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+ENV ROS_DOMAIN_ID=71
+ENV CYCLONEDDS_URI=file:///opt/autoware/cyclonedds.xml
+
+COPY cyclonedds.xml /opt/autoware/cyclonedds.xml
 
 FROM common AS dev
 

aichallenge/pkill_all.sh

@@ -1,3 +1,5 @@
+#!/bin/bash
+
 pkill -9 AWSIM.x86_64
 pkill -9 component_conta
 pkill -9 dummy_objects_p
@@ -18,4 +20,4 @@ pkill -9 routing_adaptor
 pkill -9 rviz2
 pkill -9 simple_pure_pur
 pkill -9 twist2accel
-pkill -9 vehicle_velocit
+pkill -9 vehicle_velocit

aichallenge/run_evaluation.bash

@@ -47,9 +47,33 @@ wmctrl -a "AWSIM" && wmctrl -r "AWSIM" -e 0,0,0,900,1043
 ros2 service call /debug/service/capture_screen std_srvs/srv/Trigger >/dev/null
 sleep 1
 
+# Set initial pose
+echo "Set initial pose"
+ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{ 
+  header: {
+    frame_id: 'map'
+  },
+  pose: {
+    pose: {
+      position: {
+        x: 89633.29,
+        y: 43127.57,
+        z: 0.0
+      },
+      orientation: {
+        x: 0.0,
+        y: 0.0,
+        z: 0.8778,
+        w: 0.4788
+      }
+    }
+  }
+}" >/dev/null
+sleep 1
+
 # Start driving and wait for the simulation to finish
 echo "Waiting for the simulation"
-ros2 topic pub --once /control/control_mode_request_topic std_msgs/msg/Bool '{data: true}' >/dev/null
+ros2 service call /control/control_mode_request autoware_auto_vehicle_msgs/srv/ControlModeCommand '{mode: 1}' >/dev/null
 wait $PID_AWSIM
 
 # Stop recording rviz2

aichallenge/workspace/run_aichallenge_launch_replay_rosbag.sh

@@ -0,0 +1,5 @@
+#!/usr/bin/bash
+
+source install/setup.bash
+
+ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=false use_sim_time:=true run_rviz:=true

aichallenge/workspace/run_aichallenge_launch_run_vehicle.sh

@@ -0,0 +1,5 @@
+#!/usr/bin/bash
+
+source install/setup.bash
+
+ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=false use_sim_time:=false run_rviz:=false

aichallenge/workspace/run_aichallenge_launch_simulation.sh

@@ -0,0 +1,5 @@
+#!/usr/bin/bash
+
+source install/setup.bash
+
+ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=true use_sim_time:=true run_rviz:=true

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_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/aichallenge_submit_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/aichallenge_submit_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/aichallenge_submit_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

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml

@@ -0,0 +1,34 @@
+/**:
+  ros__parameters:
+    lane_change:
+      lane_change_prepare_duration: 4.0         # [s]
+      lane_changing_safety_check_duration: 8.0  # [s]
+
+      minimum_lane_change_prepare_distance: 2.0 # [m]
+      minimum_lane_change_length: 16.5          # [m]
+      backward_length_buffer_for_end_of_lane: 3.0 # [m]
+      lane_change_finish_judge_buffer: 2.0      # [m]
+
+      lane_changing_lateral_jerk: 0.5           # [m/s3]
+      lane_changing_lateral_acc: 0.5            # [m/s2]
+
+      minimum_lane_change_velocity: 2.78        # [m/s]
+      prediction_time_resolution: 0.5           # [s]
+      maximum_deceleration: 1.0                 # [m/s2]
+      lane_change_sampling_num: 3
+
+      # collision check
+      enable_collision_check_at_prepare_phase: false
+      prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s]
+      use_predicted_path_outside_lanelet: false
+      use_all_predicted_path: false
+
+      # abort
+      enable_cancel_lane_change: true
+      enable_abort_lane_change: false
+
+      abort_delta_time: 3.0                      # [s]
+      abort_max_lateral_jerk: 1000.0             # [m/s3]
+
+      # debug
+      publish_debug_marker: false

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    lane_following:
+      lane_change_prepare_duration: 2.0

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml

@@ -0,0 +1,31 @@
+/**:
+  ros__parameters:
+    pull_out:
+      th_arrived_distance: 1.0
+      th_stopped_velocity: 0.01
+      th_stopped_time: 1.0
+      collision_check_margin: 1.0
+      collision_check_distance_from_end: 1.0
+      # shift pull out
+      enable_shift_pull_out: true
+      shift_pull_out_velocity: 2.0
+      pull_out_sampling_num: 4
+      minimum_shift_pull_out_distance: 20.0
+      maximum_lateral_jerk: 2.0
+      minimum_lateral_jerk: 0.5
+      deceleration_interval: 15.0
+      # geometric pull out
+      enable_geometric_pull_out: true
+      divide_pull_out_path: false
+      geometric_pull_out_velocity: 1.0
+      arc_path_interval: 1.0
+      lane_departure_margin: 0.2
+      backward_velocity: -1.0
+      pull_out_max_steer_angle: 0.26  # 15deg
+      # search start pose backward
+      enable_back: true
+      search_priority: "efficient_path"  # "efficient_path" or "short_back_distance"
+      max_back_distance: 30.0
+      backward_search_resolution: 2.0
+      backward_path_update_duration: 3.0
+      ignore_distance_from_lane_end: 15.0