diff --git a/.github/actions/docker-build-and-push/action.yaml b/.github/actions/docker-build-and-push/action.yaml index 5cc0b734e4..65dd991c72 100644 --- a/.github/actions/docker-build-and-push/action.yaml +++ b/.github/actions/docker-build-and-push/action.yaml @@ -77,6 +77,30 @@ runs: flavor: | latest=false + - name: Docker meta for planning-control-fail + id: meta-planning-control-fail + uses: docker/metadata-action@v5 + with: + images: ghcr.io/${{ github.repository_owner }}/${{ inputs.target-image }} + tags: | + type=raw,value=planning-control-fail-${{ inputs.platform }} + type=raw,value=planning-control-fail-${{ steps.date.outputs.date }}-${{ inputs.platform }} + bake-target: docker-metadata-action-planning-control-fail + flavor: | + latest=false + + - name: Docker meta for planning-control-pass + id: meta-planning-control-pass + uses: docker/metadata-action@v5 + with: + images: ghcr.io/${{ github.repository_owner }}/${{ inputs.target-image }} + tags: | + type=raw,value=planning-control-pass-${{ inputs.platform }} + type=raw,value=planning-control-pass-${{ steps.date.outputs.date }}-${{ inputs.platform }} + bake-target: docker-metadata-action-planning-control-pass + flavor: | + latest=false + - name: Login to GitHub Container Registry uses: docker/login-action@v3 with: @@ -91,5 +115,7 @@ runs: files: | docker/docker-bake.hcl ${{ steps.meta-simulator-visualizer.outputs.bake-file }} + ${{ steps.meta-planning-control-fail.outputs.bake-file }} + ${{ steps.meta-planning-control-pass.outputs.bake-file }} set: | ${{ inputs.build-args }} diff --git a/.github/workflows/docker-build-and-push-arm64.yaml b/.github/workflows/docker-build-and-push-arm64.yaml index 00ac8a9e85..bf2945516d 100644 --- a/.github/workflows/docker-build-and-push-arm64.yaml +++ b/.github/workflows/docker-build-and-push-arm64.yaml @@ -51,7 +51,7 @@ jobs: uses: ./.github/actions/docker-build-and-push with: platform: arm64 - target-image: autoware_snapshots + target-image: demo_packages build-args: | *.platform=linux/arm64 *.args.ROS_DISTRO=${{ needs.load-env.outputs.rosdistro }} diff --git a/.github/workflows/docker-build-and-push.yaml b/.github/workflows/docker-build-and-push.yaml index 57c688f282..5d90c8b78b 100644 --- a/.github/workflows/docker-build-and-push.yaml +++ b/.github/workflows/docker-build-and-push.yaml @@ -46,7 +46,7 @@ jobs: uses: ./.github/actions/docker-build-and-push with: platform: amd64 - target-image: autoware_snapshots + target-image: demo_packages build-args: | *.platform=linux/amd64 *.args.ROS_DISTRO=${{ needs.load-env.outputs.rosdistro }} diff --git a/docker/build.sh b/docker/build.sh index b962137b79..fe571da04d 100755 --- a/docker/build.sh +++ b/docker/build.sh @@ -2,110 +2,18 @@ set -e -# Function to print help message -print_help() { - echo "Usage: build.sh [OPTIONS]" - echo "Options:" - echo " --help Display this help message" - echo " -h Display this help message" - echo " --no-cuda Disable CUDA support" - echo " --platform Specify the platform (default: current platform)" - echo " --devel-only Build devel image only" - echo "" - echo "Note: The --platform option should be one of 'linux/amd64' or 'linux/arm64'." -} - SCRIPT_DIR=$(readlink -f "$(dirname "$0")") WORKSPACE_ROOT="$SCRIPT_DIR/.." -# Parse arguments -parse_arguments() { - while [ "$1" != "" ]; do - case "$1" in - --help | -h) - print_help - exit 1 - ;; - --no-cuda) - option_no_cuda=true - ;; - --platform) - option_platform="$2" - shift - ;; - --devel-only) - option_devel_only=true - ;; - *) - echo "Unknown option: $1" - print_help - exit 1 - ;; - esac - shift - done -} - -# Set CUDA options -set_cuda_options() { - if [ "$option_no_cuda" = "true" ]; then - setup_args="--no-nvidia" - image_name_suffix="" - else - image_name_suffix="-cuda" - fi -} - -# Set build options -set_build_options() { - if [ "$option_devel_only" = "true" ]; then - targets=("universe-devel") - else - targets=() - fi -} - -# Set platform -set_platform() { - if [ -n "$option_platform" ]; then - platform="$option_platform" - else - platform="linux/amd64" - if [ "$(uname -m)" = "aarch64" ]; then - platform="linux/arm64" - fi - fi -} - -# Set arch lib dir -set_arch_lib_dir() { - if [ "$platform" = "linux/arm64" ]; then - lib_dir="aarch64" - elif [ "$platform" = "linux/amd64" ]; then - lib_dir="x86_64" - else - echo "Unsupported platform: $platform" - exit 1 - fi -} - -# Load env -load_env() { - source "$WORKSPACE_ROOT/amd64.env" - if [ "$platform" = "linux/arm64" ]; then - source "$WORKSPACE_ROOT/arm64.env" - fi -} - # Clone repositories clone_repositories() { cd "$WORKSPACE_ROOT" if [ ! -d "src" ]; then mkdir -p src - vcs import src - + ros2 launch + scenario_test_runner + scenario_test_runner.launch.py + architecture_type:=awf/universe/20240605 + record:=false + sensor_model:=sample_sensor_kit + vehicle_model:=sample_vehicle + scenario:=/autoware/simulation/yield_maneuver_demo.yaml + map_path:=/autoware/map + global_timeout:=240 + initialize_duration:=90 + launch_autoware:=false + launch_rviz:=true + + planning-control: + image: ghcr.io/autowarefoundation/autoware:universe + network_mode: "host" + volumes: + - /dev/shm:/dev/shm + - /etc/localtime:/etc/localtime:ro + - ./etc/map:/autoware/map + environment: + - ROS_DOMAIN_ID=88 + command: >- + ros2 launch + autoware_launch + planning_simulator.launch.xml + scenario_simulation:=true + vehicle_model:=sample_vehicle + sensor_model:=sample_sensor_kit + map_path:=/autoware/map + use_foa:=false + perception/enable_traffic_light:=false + rviz:=false diff --git a/docker/planning-control/entrypoint.sh b/docker/planning-control/entrypoint.sh new file mode 100644 index 0000000000..286db2474f --- /dev/null +++ b/docker/planning-control/entrypoint.sh @@ -0,0 +1,6 @@ +#!/usr/bin/env bash +# shellcheck disable=SC1090,SC1091 + +source "/opt/ros/$ROS_DISTRO/setup.bash" +source /opt/autoware/setup.bash +exec "$@" diff --git a/docker/planning-control/fail/Dockerfile b/docker/planning-control/fail/Dockerfile new file mode 100644 index 0000000000..dcc708f8af --- /dev/null +++ b/docker/planning-control/fail/Dockerfile @@ -0,0 +1,9 @@ +FROM ghcr.io/autowarefoundation/autoware:universe AS planning-control-fail + +# Copy launch file +COPY docker/planning-control/fail/fail_scenario.launch.xml /opt/autoware/share/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml + +# Copy entrypoint script +COPY docker/planning-control/entrypoint.sh /entrypoint.sh +RUN chmod +x /entrypoint.sh +ENTRYPOINT ["/entrypoint.sh"] diff --git a/docker/planning-control/fail/fail_scenario.launch.xml b/docker/planning-control/fail/fail_scenario.launch.xml new file mode 100644 index 0000000000..2233379ae7 --- /dev/null +++ b/docker/planning-control/fail/fail_scenario.launch.xml @@ -0,0 +1,337 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + ros__parameters: + avoidance: + resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER + resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "current_lane" + # drivable area setting + use_intersection_areas: true + use_hatched_road_markings: true + use_freespace_areas: true + + # avoidance is performed for the object type with true + target_object: + car: + th_moving_speed: 1.0 # [m/s] + th_moving_time: 2.0 # [s] + longitudinal_margin: 0.0 # [m] + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] + max_expand_ratio: 0.0 # [-] FOR DEVELOPER + envelope_buffer_margin: 0.1 # [m] FOR DEVELOPER + th_error_eclipse_long_radius : 0.6 # [m] + truck: + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + bus: + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + trailer: + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + unknown: + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: -0.2 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + bicycle: + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 + motorcycle: + th_moving_speed: 1.0 + th_moving_time: 1.0 + longitudinal_margin: 1.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.3 + hard_margin_for_parked_vehicle: 0.3 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 + pedestrian: + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 + lower_distance_for_polygon_expansion: 30.0 # [m] FOR DEVELOPER + upper_distance_for_polygon_expansion: 100.0 # [m] FOR DEVELOPER + + # For target object filtering + target_filtering: + # avoidance target type + target_type: + car: false # [-] + truck: false # [-] + bus: false # [-] + trailer: false # [-] + unknown: false # [-] + bicycle: false # [-] + motorcycle: false # [-] + pedestrian: false # [-] + # detection range + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] + # lost object compensation + max_compensation_time: 2.0 + + # detection area generation parameters + detection_area: + static: false # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + + # filtering parking objects + parked_vehicle: + th_offset_from_centerline: 1.0 # [m] + th_shiftable_ratio: 0.8 # [-] + min_road_shoulder_width: 0.5 # [m] FOR DEVELOPER + + # for merging/deviating vehicle + merging_vehicle: + th_overhang_distance: 0.0 # [m] + + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. + avoidance_for_ambiguous_vehicle: + # policy for ego behavior for ambiguous vehicle. + # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. + # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. + # "ignore" : never avoid it. + policy: "manual" # [-] + condition: + th_stopped_time: 3.0 # [s] + th_moving_distance: 1.0 # [m] + ignore_area: + traffic_light: + front_distance: 20.0 # [m] + crosswalk: + front_distance: 20.0 # [m] + behind_distance: 0.0 # [m] + wait_and_see: + target_behaviors: ["MERGING", "DEVIATING"] # [-] + th_closest_distance: 10.0 # [m] + + # params for filtering objects that are in intersection + intersection: + yaw_deviation: 0.349 # [rad] (default 20.0deg) + + freespace: + condition: + th_stopped_time: 5.0 # [-] + + # For safety check + safety_check: + # safety check target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: false # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] + # safety check configuration + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] + # collision check parameters + check_all_predicted_path: false # [-] + safety_check_backward_distance: 100.0 # [m] + hysteresis_factor_expand_rate: 1.5 # [-] + hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] + # predicted path parameters + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 3.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] + # rss parameters + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.0 # [s] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + th_avoid_execution: 0.09 # [m] FOR DEVELOPER + th_small_shift_length: 0.101 # [m] FOR DEVELOPER + soft_drivable_bound_margin: 0.3 # [m] + hard_drivable_bound_margin: 0.3 # [m] + max_right_shift_length: 5.0 # [m] FOR DEVELOPER + max_left_shift_length: 5.0 # [m] FOR DEVELOPER + max_deviation_from_lane: 0.2 # [m] + # approve the next shift line after reaching this percentage of the current shift line length. + # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. + # this feature can be disabled by setting this parameter to 0.0. + ratio_for_return_shift_approval: 0.5 # [-] + # avoidance distance parameters + longitudinal: + min_prepare_time: 1.0 # [s] + max_prepare_time: 3.0 # [s] + min_prepare_distance: 1.0 # [m] + min_slow_down_speed: 1.38 # [m/s] + buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER + nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER + consider_front_overhang: true # [-] + consider_rear_overhang: true # [-] + # return dead line + return_dead_line: + goal: + enable: true # [-] + buffer: 3.0 # [m] + traffic_light: + enable: true # [-] + buffer: 3.0 # [m] + + # For cancel maneuver + cancel: + enable: true # [-] + force: + duration_time: 2.0 # [s] + + # For yield maneuver + yield: + enable: true # [-] + enable_during_shifting: false # [-] + + # For stop maneuver + stop: + max_distance: 20.0 # [m] + stop_buffer: 1.0 # [m] FOR DEVELOPER + + policy: + # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver". + # "per_shift_line": request approval for each shift line. + # "per_avoidance_maneuver": request approval for avoidance maneuver (avoid + return). + make_approval_request: "per_shift_line" + # policy for vehicle slow down behavior. select "best_effort" or "reliable". + # "best_effort": slow down deceleration & jerk are limited by constraints. + # but there is a possibility that the vehicle can't stop in front of the vehicle. + # "reliable": insert stop or slow down point with ignoring decel/jerk constraints. + # make it possible to increase chance to avoid but uncomfortable deceleration maybe happen. + deceleration: "best_effort" # [-] + # policy for avoidance lateral margin. select "best_effort" or "reliable". + # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal + # margin to avoid. + # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid + # with expected lateral margin. + lateral_margin: "best_effort" # [-] + # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. + use_shorten_margin_immediately: true # [-] + + # -------------------------------------- + # FOLLOWING PARAMETERS ARE FOR DEVELOPER + # -------------------------------------- + + constraints: + # lateral constraints + lateral: + velocity: [1.39, 4.17, 11.1] # [m/s] + max_accel_values: [0.5, 0.5, 0.5] # [m/ss] + min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -1.5 # [m/ss] + max_jerk: 1.0 # [m/sss] + max_acceleration: 0.5 # [m/ss] + min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] + + # path generation method. select "shift_line_base" or "optimization_base" or "both". + # "shift_line_base" : Create avoidance path based on shift line. + # User can control avoidance maneuver execution via RTC. + # However, this method doesn't support complex avoidance scenario (e.g. S-shape maneuver). + # "optimization_base": This module selects avoidance target object + # and bpp module clips drivable area based on avoidance target object polygon shape. + # But this module doesn't modify the path shape. + # On the other hand, autoware_path_optimizer module optimizes path shape instead of this module + # so that the path can be within drivable area. This method is able to deal with complex avoidance scenario. + # However, user can't control avoidance manuever execution. + # "both" : Use both method. + path_generation_method: "shift_line_base" + + shift_line_pipeline: + trim: + quantize_size: 0.1 + th_similar_grad_1: 0.1 + th_similar_grad_2: 0.2 + th_similar_grad_3: 0.5 + + # for debug + debug: + enable_other_objects_marker: true + enable_other_objects_info: true + enable_detection_area_marker: false + enable_drivable_bound_marker: false + enable_safety_check_marker: false + enable_shift_line_marker: false + enable_lane_marker: false + enable_misc_marker: false \ No newline at end of file diff --git a/docker/planning-control/pass/Dockerfile b/docker/planning-control/pass/Dockerfile new file mode 100644 index 0000000000..8ff64a3893 --- /dev/null +++ b/docker/planning-control/pass/Dockerfile @@ -0,0 +1,9 @@ +FROM ghcr.io/autowarefoundation/autoware:universe AS planning-control-pass + +# Copy launch file +COPY docker/planning-control/pass/pass_scenario.launch.xml /opt/autoware/share/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml + +# Copy entrypoint script +COPY docker/planning-control/entrypoint.sh /entrypoint.sh +RUN chmod +x /entrypoint.sh +ENTRYPOINT ["/entrypoint.sh"] diff --git a/docker/planning-control/pass/pass_scenario.launch.xml b/docker/planning-control/pass/pass_scenario.launch.xml new file mode 100644 index 0000000000..87596dad36 --- /dev/null +++ b/docker/planning-control/pass/pass_scenario.launch.xml @@ -0,0 +1,337 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + ros__parameters: + avoidance: + resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER + resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" + # drivable area setting + use_intersection_areas: true + use_hatched_road_markings: true + use_freespace_areas: true + + # avoidance is performed for the object type with true + target_object: + car: + th_moving_speed: 1.0 # [m/s] + th_moving_time: 2.0 # [s] + longitudinal_margin: 0.0 # [m] + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] + max_expand_ratio: 0.0 # [-] FOR DEVELOPER + envelope_buffer_margin: 0.1 # [m] FOR DEVELOPER + th_error_eclipse_long_radius : 0.6 # [m] + truck: + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + bus: + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + trailer: + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + unknown: + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 0.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: -0.2 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 + bicycle: + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 + motorcycle: + th_moving_speed: 1.0 + th_moving_time: 1.0 + longitudinal_margin: 1.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.3 + hard_margin_for_parked_vehicle: 0.3 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 + pedestrian: + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 + lateral_margin: + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 + lower_distance_for_polygon_expansion: 30.0 # [m] FOR DEVELOPER + upper_distance_for_polygon_expansion: 100.0 # [m] FOR DEVELOPER + + # For target object filtering + target_filtering: + # avoidance target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] + # detection range + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] + # lost object compensation + max_compensation_time: 2.0 + + # detection area generation parameters + detection_area: + static: false # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + + # filtering parking objects + parked_vehicle: + th_offset_from_centerline: 1.0 # [m] + th_shiftable_ratio: 0.8 # [-] + min_road_shoulder_width: 0.5 # [m] FOR DEVELOPER + + # for merging/deviating vehicle + merging_vehicle: + th_overhang_distance: 0.0 # [m] + + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. + avoidance_for_ambiguous_vehicle: + # policy for ego behavior for ambiguous vehicle. + # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. + # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. + # "ignore" : never avoid it. + policy: "manual" # [-] + condition: + th_stopped_time: 3.0 # [s] + th_moving_distance: 1.0 # [m] + ignore_area: + traffic_light: + front_distance: 20.0 # [m] + crosswalk: + front_distance: 20.0 # [m] + behind_distance: 0.0 # [m] + wait_and_see: + target_behaviors: ["MERGING", "DEVIATING"] # [-] + th_closest_distance: 10.0 # [m] + + # params for filtering objects that are in intersection + intersection: + yaw_deviation: 0.349 # [rad] (default 20.0deg) + + freespace: + condition: + th_stopped_time: 5.0 # [-] + + # For safety check + safety_check: + # safety check target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: false # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] + # safety check configuration + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] + # collision check parameters + check_all_predicted_path: false # [-] + safety_check_backward_distance: 100.0 # [m] + hysteresis_factor_expand_rate: 1.5 # [-] + hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] + # predicted path parameters + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 3.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] + # rss parameters + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.0 # [s] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + th_avoid_execution: 0.09 # [m] FOR DEVELOPER + th_small_shift_length: 0.101 # [m] FOR DEVELOPER + soft_drivable_bound_margin: 0.3 # [m] + hard_drivable_bound_margin: 0.3 # [m] + max_right_shift_length: 5.0 # [m] FOR DEVELOPER + max_left_shift_length: 5.0 # [m] FOR DEVELOPER + max_deviation_from_lane: 0.2 # [m] + # approve the next shift line after reaching this percentage of the current shift line length. + # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. + # this feature can be disabled by setting this parameter to 0.0. + ratio_for_return_shift_approval: 0.5 # [-] + # avoidance distance parameters + longitudinal: + min_prepare_time: 1.0 # [s] + max_prepare_time: 3.0 # [s] + min_prepare_distance: 1.0 # [m] + min_slow_down_speed: 1.38 # [m/s] + buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER + nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER + consider_front_overhang: true # [-] + consider_rear_overhang: true # [-] + # return dead line + return_dead_line: + goal: + enable: true # [-] + buffer: 3.0 # [m] + traffic_light: + enable: true # [-] + buffer: 3.0 # [m] + + # For cancel maneuver + cancel: + enable: true # [-] + force: + duration_time: 2.0 # [s] + + # For yield maneuver + yield: + enable: true # [-] + enable_during_shifting: false # [-] + + # For stop maneuver + stop: + max_distance: 20.0 # [m] + stop_buffer: 1.0 # [m] FOR DEVELOPER + + policy: + # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver". + # "per_shift_line": request approval for each shift line. + # "per_avoidance_maneuver": request approval for avoidance maneuver (avoid + return). + make_approval_request: "per_shift_line" + # policy for vehicle slow down behavior. select "best_effort" or "reliable". + # "best_effort": slow down deceleration & jerk are limited by constraints. + # but there is a possibility that the vehicle can't stop in front of the vehicle. + # "reliable": insert stop or slow down point with ignoring decel/jerk constraints. + # make it possible to increase chance to avoid but uncomfortable deceleration maybe happen. + deceleration: "best_effort" # [-] + # policy for avoidance lateral margin. select "best_effort" or "reliable". + # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal + # margin to avoid. + # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid + # with expected lateral margin. + lateral_margin: "best_effort" # [-] + # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. + use_shorten_margin_immediately: true # [-] + + # -------------------------------------- + # FOLLOWING PARAMETERS ARE FOR DEVELOPER + # -------------------------------------- + + constraints: + # lateral constraints + lateral: + velocity: [1.39, 4.17, 11.1] # [m/s] + max_accel_values: [0.5, 0.5, 0.5] # [m/ss] + min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -1.5 # [m/ss] + max_jerk: 1.0 # [m/sss] + max_acceleration: 0.5 # [m/ss] + min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] + + # path generation method. select "shift_line_base" or "optimization_base" or "both". + # "shift_line_base" : Create avoidance path based on shift line. + # User can control avoidance maneuver execution via RTC. + # However, this method doesn't support complex avoidance scenario (e.g. S-shape maneuver). + # "optimization_base": This module selects avoidance target object + # and bpp module clips drivable area based on avoidance target object polygon shape. + # But this module doesn't modify the path shape. + # On the other hand, autoware_path_optimizer module optimizes path shape instead of this module + # so that the path can be within drivable area. This method is able to deal with complex avoidance scenario. + # However, user can't control avoidance manuever execution. + # "both" : Use both method. + path_generation_method: "shift_line_base" + + shift_line_pipeline: + trim: + quantize_size: 0.1 + th_similar_grad_1: 0.1 + th_similar_grad_2: 0.2 + th_similar_grad_3: 0.5 + + # for debug + debug: + enable_other_objects_marker: true + enable_other_objects_info: true + enable_detection_area_marker: false + enable_drivable_bound_marker: false + enable_safety_check_marker: false + enable_shift_line_marker: false + enable_lane_marker: false + enable_misc_marker: false \ No newline at end of file diff --git a/docker/run.sh b/docker/run.sh index a1c920906b..73695c80b1 100755 --- a/docker/run.sh +++ b/docker/run.sh @@ -11,35 +11,14 @@ NC='\033[0m' # No Color SCRIPT_DIR=$(readlink -f "$(dirname "$0")") WORKSPACE_ROOT="$SCRIPT_DIR/.." -source "$WORKSPACE_ROOT/amd64.env" -if [ "$(uname -m)" = "aarch64" ]; then - source "$WORKSPACE_ROOT/arm64.env" -fi - -# Default values -option_no_nvidia=false -option_devel=false -option_headless=false -MAP_PATH="" -WORKSPACE_PATH="" -USER_ID="" -WORKSPACE="" -DEFAULT_LAUNCH_CMD="ros2 launch autoware_launch autoware.launch.xml map_path:=/autoware_map vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit" # Function to print help message print_help() { - echo -e "\n------------------------------------------------------------" - echo -e "${RED}Note:${NC} The --map-path option is mandatory for the runtime. For development environment with shell access, use --devel option." - echo -e " Default launch command: ${GREEN}${DEFAULT_LAUNCH_CMD}${NC}" - echo -e "------------------------------------------------------------" - echo -e "${RED}Usage:${NC} run.sh [OPTIONS] [LAUNCH_CMD](optional)" - echo -e "Options:" - echo -e " ${GREEN}--help/-h${NC} Display this help message" - echo -e " ${GREEN}--map-path${NC} Specify to mount map files into /autoware_map (mandatory for runtime)" - echo -e " ${GREEN}--devel${NC} Launch the latest Autoware development environment with shell access" - echo -e " ${GREEN}--workspace${NC} (--devel only)Specify the directory to mount into /workspace, by default it uses current directory (pwd)" - echo -e " ${GREEN}--no-nvidia${NC} Disable NVIDIA GPU support" - echo -e " ${GREEN}--headless${NC} Run Autoware in headless mode (default: false)" + echo "${GREEN}Usage: run.sh [OPTIONS]${NC}" + echo "Options:" + echo " --help Display this help message" + echo " -h Display this help message" + echo " --web Run web simulation" echo "" } @@ -51,138 +30,27 @@ parse_arguments() { print_help exit 1 ;; - --no-nvidia) - option_no_nvidia=true - ;; - --devel) - option_devel=true - ;; - --headless) - option_headless=true - ;; - --workspace) - WORKSPACE_PATH="$2" - shift - ;; - --map-path) - MAP_PATH="$2" - shift - ;; - --*) - echo "Unknown option: $1" - print_help - exit 1 + --web) + option_web=true ;; - -*) + *) echo "Unknown option: $1" print_help exit 1 ;; - *) - LAUNCH_CMD="$@" - break - ;; esac shift done } -# Set the docker image and workspace variables -set_variables() { - if [ "$option_devel" = "true" ]; then - # Set image based on option - IMAGE="ghcr.io/autowarefoundation/autoware:universe-devel" - - # Set workspace path, if not provided use the current directory - if [ "$WORKSPACE_PATH" = "" ]; then - WORKSPACE_PATH=$(pwd) - fi - WORKSPACE="-v ${WORKSPACE_PATH}:/workspace" - - # Set user ID and group ID to match the local user - USER_ID="-e LOCAL_UID=$(id -u) -e LOCAL_GID=$(id -g) -e LOCAL_USER=$(id -un) -e LOCAL_GROUP=$(id -gn)" - - # Set map path - if [ "$MAP_PATH" != "" ]; then - MAP="-v ${MAP_PATH}:/autoware_map:ro" - fi - - # Set launch command - if [ "$LAUNCH_CMD" = "" ]; then - LAUNCH_CMD="/bin/bash" - fi - else - # Set image based on option - IMAGE="ghcr.io/autowarefoundation/autoware:universe" - - # Set map path - if [ "$MAP_PATH" = "" ]; then - echo -e "\n------------------------------------------------------------" - echo -e "${RED}Note:${NC} The --map-path option is mandatory for the universe(runtime image). For development environment with shell access, use --devel option." - echo -e "------------------------------------------------------------" - exit 1 - else - MAP="-v ${MAP_PATH}:/autoware_map:ro" - fi - - # Set default launch command if not provided - if [ "$LAUNCH_CMD" = "" ]; then - LAUNCH_CMD=${DEFAULT_LAUNCH_CMD} - fi - fi -} - -# Set GPU flag based on option -set_gpu_flag() { - if [ "$option_no_nvidia" = "true" ]; then - GPU_FLAG="" +# Run simulation +run_simulation() { + if [ "$option_web" = "true" ]; then + docker compose -f "${SCRIPT_DIR}/web-simulation.docker-compose.yaml" up else - GPU_FLAG="--gpus all" - IMAGE=${IMAGE}-cuda - fi -} - -# Set X display variables -set_x_display() { - MOUNT_X="" - if [ "$option_headless" = "false" ]; then - MOUNT_X="-e DISPLAY=$DISPLAY -v /tmp/.X11-unix/:/tmp/.X11-unix" - xhost + >/dev/null + docker compose -f "${SCRIPT_DIR}/local-simulation.docker-compose.yaml" up fi } -# Main script execution -main() { - # Parse arguments - parse_arguments "$@" - set_variables - set_gpu_flag - set_x_display - - if [ "$option_devel" = "true" ]; then - echo -e "${GREEN}-----------------------------------------------------------------${NC}" - echo -e "${BLUE}Launching Autoware development environment${NC}" - else - echo -e "${GREEN}-----------------------------------------------------------------${NC}" - echo -e "${GREEN}Launching Autoware${NC}" - fi - echo -e "${GREEN}IMAGE:${NC} ${IMAGE}" - if [ "$option_devel" = "true" ]; then - echo -e "${GREEN}WORKSPACE PATH(mounted):${NC} ${WORKSPACE_PATH}:/workspace" - fi - if [ "$MAP_PATH" != "" ]; then - echo -e "${GREEN}MAP PATH(mounted):${NC} ${MAP_PATH}:/autoware_map" - fi - echo -e "${GREEN}LAUNCH CMD:${NC} ${LAUNCH_CMD}" - echo -e "${GREEN}-----------------------------------------------------------------${NC}" - - # Launch the container - set -x - docker run -it --rm --net=host ${GPU_FLAG} ${USER_ID} ${MOUNT_X} \ - -e XAUTHORITY=${XAUTHORITY} -e XDG_RUNTIME_DIR=$XDG_RUNTIME_DIR -e NVIDIA_DRIVER_CAPABILITIES=all -v /etc/localtime:/etc/localtime:ro \ - ${WORKSPACE} ${MAP} ${IMAGE} \ - ${LAUNCH_CMD} -} - -# Execute the main script -main "$@" +parse_arguments "$@" +run_simulation diff --git a/docker/simulator-visualizer/entrypoint.sh b/docker/simulator-visualizer/entrypoint.sh index 72aa019c83..b42395425b 100644 --- a/docker/simulator-visualizer/entrypoint.sh +++ b/docker/simulator-visualizer/entrypoint.sh @@ -6,7 +6,7 @@ source /opt/autoware/setup.bash # Start VNC server echo "Starting VNC server..." -vncserver :14 -auth $HOME/.Xauthority -geometry 1024x768 -depth 16 -pixelformat rgb565 >/dev/null 2>&1 +vncserver :1 -auth $HOME/.Xauthority -geometry 1024x768 -depth 16 -pixelformat rgb565 >/dev/null 2>&1 VNC_RESULT=$? if [ $VNC_RESULT -ne 0 ]; then @@ -35,6 +35,9 @@ if [ -n "$NGROK_AUTHTOKEN" ]; then NOVNC_URL=https://$NGROK_URL fi +# Start Rviz +rviz2 -d /autoware/simulation/scenario_simulator.rviz + # Print message echo -e "\033[32m-------------------------------------------------------------------------\033[0m" echo -e "Note: In order to access VNC and NoVNC on localhost, you need to expose ports 5901 and 6080 to the outside world respectively." diff --git a/docker/docker-compose.yaml b/docker/web-simulation.docker-compose.yaml similarity index 85% rename from docker/docker-compose.yaml rename to docker/web-simulation.docker-compose.yaml index 09eeb24afd..5de8a81f41 100644 --- a/docker/docker-compose.yaml +++ b/docker/web-simulation.docker-compose.yaml @@ -1,19 +1,17 @@ services: simulator: - image: ghcr.io/autowarefoundation/openadkit_demo.autoware:simulator-visualizer + image: ghcr.io/autowarefoundation/autoware_snapshots:simulator-visualizer-amd64 network_mode: "host" volumes: - /dev/shm:/dev/shm - /etc/localtime:/etc/localtime:ro - ./etc/map:/autoware/map - ./etc/simulation:/autoware/simulation - - /tmp/.X11-unix:/tmp/.X11-unix:rw environment: - ROS_DOMAIN_ID=88 - NGROK_AUTHTOKEN=2guulYxBHQKJpe37Qp4PexfzXQm_3HSWKfiUgtKyxKmz2PJBo - NGROK_URL=simviz.openadkit.ngrok.app - - DISPLAY=${DISPLAY} - - XAUTHORITY=/home/user/.Xauthority + - VNC_ENABLED=true command: >- ros2 launch scenario_test_runner