diff --git a/.cspell.json b/.cspell.json
index 2c51a351..f5bfb9fa 100644
--- a/.cspell.json
+++ b/.cspell.json
@@ -66,6 +66,7 @@
"cyclonedds",
"dallara",
"dcmake",
+ "decel",
"distro",
"downsample",
"freespace",
diff --git a/aichallenge/workspace/aichallenge_launch.sh b/aichallenge/workspace/aichallenge_launch.sh
new file mode 100644
index 00000000..9c8fa071
--- /dev/null
+++ b/aichallenge/workspace/aichallenge_launch.sh
@@ -0,0 +1,5 @@
+#!/usr/bin/bash
+
+source install/setup.bash
+
+ros2 launch aichallenge_launch aichallenge_system.launch.xml
diff --git a/aichallenge/workspace/colcon_build.bash b/aichallenge/workspace/colcon_build.bash
new file mode 100755
index 00000000..c9991efd
--- /dev/null
+++ b/aichallenge/workspace/colcon_build.bash
@@ -0,0 +1,2 @@
+#!/usr/bin/bash
+MAKEFLAGS=-j4 colcon build --symlink-install --packages-up-to aichallenge_launch --executor sequential
diff --git a/aichallenge/workspace/depends.repos b/aichallenge/workspace/depends.repos
new file mode 100644
index 00000000..75ab280d
--- /dev/null
+++ b/aichallenge/workspace/depends.repos
@@ -0,0 +1,27 @@
+repositories:
+ # used for autoware cmake and lanelet2 related packages
+ autoware_common:
+ type: git
+ url: https://github.com/autowarefoundation/autoware_common.git
+ version: awsim-stable
+ # used for goal pose setter
+ autoware_adapi_msgs:
+ type: git
+ url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
+ version: 9679b5a7a1f4cfff2fa50b80d2759d3937f2f953
+ autoware_auto_msgs:
+ type: git
+ url: https://github.com/tier4/autoware_auto_msgs.git
+ version: f6642370c6f4f42a5dc0b6c1fc4a21396d4dc34c
+ autoware_msgs:
+ type: git
+ url: https://github.com/autowarefoundation/autoware_msgs.git
+ version: 90ed3f1f3371d75eb30b6e7d2acb88ccec4e8030
+ tier4_autoware_msgs:
+ type: git
+ url: https://github.com/tier4/tier4_autoware_msgs.git
+ version: 87febc35abc9310eaec0b6dd3ce2a4845ce515aa
+ autoware.universe:
+ type: git
+ url: https://github.com/autowarefoundation/autoware.universe.git
+ version: awsim-stable
diff --git a/aichallenge/workspace/depends/.gitignore b/aichallenge/workspace/depends/.gitignore
new file mode 100644
index 00000000..d6b7ef32
--- /dev/null
+++ b/aichallenge/workspace/depends/.gitignore
@@ -0,0 +1,2 @@
+*
+!.gitignore
diff --git a/aichallenge/workspace/src/aichallenge_launch/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_launch/CMakeLists.txt
new file mode 100644
index 00000000..231f1738
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/CMakeLists.txt
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.14)
+project(aichallenge_launch)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_package(INSTALL_TO_SHARE launch config data)
diff --git a/aichallenge/workspace/src/aichallenge_launch/config/converter.param.yaml b/aichallenge/workspace/src/aichallenge_launch/config/converter.param.yaml
new file mode 100644
index 00000000..3e98b794
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/config/converter.param.yaml
@@ -0,0 +1,18 @@
+/**:
+ ros__parameters:
+ use_steer_ff: true
+ use_steer_fb: true
+ is_debugging: false
+ steer_pid:
+ kp: 150.0
+ ki: 15.0
+ kd: 0.0
+ max: 8.0
+ min: -8.0
+ max_p: 8.0
+ min_p: -8.0
+ max_i: 8.0
+ min_i: -8.0
+ max_d: 0.0
+ min_d: 0.0
+ invalid_integration_decay: 0.97
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_launch/config/lanelet2_map_loader.param.yaml
similarity index 100%
rename from aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml
rename to aichallenge/workspace/src/aichallenge_launch/config/lanelet2_map_loader.param.yaml
diff --git a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv
new file mode 100644
index 00000000..979af2be
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv
@@ -0,0 +1,8 @@
+default,0.0,1.0,2.0,3.0,4.0,5.0
+0.0,-0.3,-0.3,-0.3,-0.3,-0.3,-0.3
+0.1,0.0,0.0,0.0,0.0,0.0,0.0
+0.2,0.0,0.0,0.0,0.0,0.0,0.0
+0.3,0.05,0.05,0.05,0.05,0.05,0.05
+0.4,0.1,0.1,0.1,0.1,0.1,0.1
+0.5,0.175,0.175,0.175,0.175,0.175,0.175
+0.6,0.25,0.25,0.25,0.25,0.25,0.25
diff --git a/aichallenge/workspace/src/aichallenge_launch/data/brake_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/brake_map.csv
new file mode 100644
index 00000000..adf2c809
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/data/brake_map.csv
@@ -0,0 +1,10 @@
+default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
+0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
+0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51
+0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96
+0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633
+0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106
+0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116
+0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126
+0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136
+0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955
diff --git a/aichallenge/workspace/src/aichallenge_launch/data/steer_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/steer_map.csv
new file mode 100644
index 00000000..3da0e0cd
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/data/steer_map.csv
@@ -0,0 +1,14 @@
+default,-0.6,-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4,0.5,0.6
+-12,-0.29,-0.3234236132,-0.3468991362,-0.37274847,-0.4288688461,-0.4696573,-0.4781014157,-0.49,-0.51,-0.52,-0.53,-0.54,-0.55
+-10,-0.2,-0.2451308686,-0.2667782734,-0.3090752469,-0.3575200568,-0.3874868999,-0.4041493831,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47
+-8,-0.1,-0.1586739777,-0.2020698818,-0.2317654357,-0.2778998646,-0.3029839842,-0.3188172953,-0.32,-0.33,-0.34,-0.35,-0.36,-0.37
+-6,-0.0404761584,-0.0806574159,-0.1208386734,-0.1559621241,-0.1888907059,-0.210790018,-0.2322336736,-0.24,-0.25,-0.26,-0.27,-0.28,-0.29
+-4,0.01,-0.01428569928,-0.04,-0.06805018099,-0.09718925222,-0.1189509092,-0.1361293637,-0.14,-0.153044463,-0.1688321844,-0.18,-0.2,-0.22
+-2,0.06,0.05,0.03,0.01,-0.006091655083,-0.03546033903,-0.05319488695,-0.06097627388,-0.07576251508,-0.09165178816,-0.1,-0.12,-0.14
+0,0.11,0.09,0.07,0.05,0.03,0.01,-0.0004106386541,-0.01,-0.03,-0.05,-0.07,-0.09,-0.11
+2,0.15,0.13,0.11,0.09,0.07812978496,0.06023545297,0.04386326928,0.02314813566,0.003779338416,-0.01390526686,-0.0324840879,-0.05106290894,-0.06964172998
+4,0.24,0.22,0.2,0.1839795042,0.1649754952,0.1455879759,0.1274837062,0.1020632549,0.0861964856,0.06971503353,0.04541294018,0.01308869356,-0.01923555307
+6,0.33,0.31,0.29,0.274963497,0.2447295892,0.2337353319,0.2126138313,0.1767700907,0.1578757866,0.1440584148,0.1136331403,0.0827443595,0.05185557872
+8,0.43,0.41,0.39,0.3714915121,0.3299578073,0.3156403746,0.2997845963,0.2500495071,0.2339668568,0.2153133621,0.1815127859,0.156071251,0.1306297162
+10,0.5,0.49,0.48,0.4629455165,0.4210353203,0.3977027236,0.3865034288,0.3250957262,0.312237913,0.2885123051,0.2474220915,0.2123900615,0.1773580315
+12,0.56,0.54,0.53,0.5151036525,0.5046070554,0.4897214196,0.4687756354,0.4036994672,0.3812674227,0.3496883008,0.32482655,0.2783538423,0.2318811345
diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml
new file mode 100644
index 00000000..4b46cba5
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml
@@ -0,0 +1,279 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_submit.launch.xml
new file mode 100644
index 00000000..7c029bc4
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_submit.launch.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml
new file mode 100644
index 00000000..d8978772
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml
new file mode 100644
index 00000000..83238406
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_launch/package.xml
@@ -0,0 +1,50 @@
+
+
+
+ aichallenge_launch
+ 0.0.0
+ TODO: Package description
+ Takagi, Isamu
+ TODO: License declaration
+
+ ament_cmake_auto
+ autoware_cmake
+ aichallenge_submit_launch
+ aichallenge_system_launch
+ tier4_vehicle_launch
+ global_parameter_loader
+ goal_pose_setter
+ map_loader
+ map_tf_generator
+ imu_corrector
+ imu_gnss_poser
+ gyro_odometer
+ ekf_localizer
+ twist2accel
+ raw_vehicle_cmd_converter
+ robot_state_publisher
+ vehicle_velocity_converter
+ racing_kart_description
+ racing_kart_sensor_kit_description
+
+ dummy_perception_publisher
+ mission_planner
+ behavior_path_planner
+ path_to_trajectory
+ default_ad_api
+ ad_api_adaptors
+ simple_pure_pursuit
+
+ tier4_planning_rviz_plugin
+ tier4_localization_rviz_plugin
+ tier4_control_rviz_plugin
+ tier4_vehicle_rviz_plugin
+
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml
new file mode 100755
index 00000000..61c41ee8
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_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]
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml
new file mode 100644
index 00000000..a801eca3
--- /dev/null
+++ b/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
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml
new file mode 100644
index 00000000..eb670976
--- /dev/null
+++ b/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]
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
new file mode 100644
index 00000000..c14f59f6
--- /dev/null
+++ b/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]
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml
new file mode 100644
index 00000000..902a93ce
--- /dev/null
+++ b/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
diff --git a/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 b/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
new file mode 100644
index 00000000..295769ac
--- /dev/null
+++ b/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
diff --git a/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 b/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
new file mode 100644
index 00000000..b6a9574b
--- /dev/null
+++ b/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
diff --git a/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 b/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
new file mode 100644
index 00000000..68000747
--- /dev/null
+++ b/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
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml
new file mode 100644
index 00000000..ca1eb740
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml
@@ -0,0 +1,95 @@
+/**:
+ ros__parameters:
+ pull_over:
+ request_length: 100.0
+ th_arrived_distance: 1.0
+ th_stopped_velocity: 0.01
+ th_stopped_time: 2.0 # It must be greater than the state_machine's.
+ pull_over_velocity: 3.0
+ pull_over_minimum_velocity: 1.38
+ margin_from_boundary: 0.5
+ decide_path_distance: 10.0
+ maximum_deceleration: 0.5
+ # goal research
+ enable_goal_research: true
+ search_priority: "efficient_path" # "efficient_path" or "close_goal"
+ forward_goal_search_length: 20.0
+ backward_goal_search_length: 20.0
+ goal_search_interval: 2.0
+ longitudinal_margin: 3.0
+ max_lateral_offset: 0.5
+ lateral_offset_interval: 0.25
+ ignore_distance_from_lane_start: 15.0
+ # occupancy grid map
+ use_occupancy_grid: true
+ use_occupancy_grid_for_longitudinal_margin: false
+ occupancy_grid_collision_check_margin: 0.0
+ theta_size: 360
+ obstacle_threshold: 60
+ # object recognition
+ use_object_recognition: true
+ object_recognition_collision_check_margin: 1.0
+ # shift path
+ enable_shift_parking: true
+ pull_over_sampling_num: 4
+ maximum_lateral_jerk: 2.0
+ minimum_lateral_jerk: 0.5
+ deceleration_interval: 15.0
+ after_pull_over_straight_distance: 1.0
+ # freespace parking
+ enable_freespace_parking: true
+ freespace_parking:
+ planning_algorithm: "astar" # options: astar, rrtstar
+ velocity: 1.0
+ vehicle_shape_margin: 1.0
+ time_limit: 3000.0
+ minimum_turning_radius: 5.0
+ maximum_turning_radius: 5.0
+ turning_radius_size: 1
+ # search configs
+ theta_size: 144
+ angle_goal_range: 6.0
+ curve_weight: 1.2
+ reverse_weight: 1.0
+ lateral_goal_range: 0.5
+ longitudinal_goal_range: 2.0
+ # costmap configs
+ obstacle_threshold: 30
+ # -- A* search Configurations --
+ astar:
+ only_behind_solutions: false
+ use_back: false
+ distance_heuristic_weight: 1.0
+ # -- RRT* search Configurations --
+ rrtstar:
+ enable_update: true
+ use_informed_sampling: true
+ max_planning_time: 150.0
+ neighbor_radius: 8.0
+ margin: 1.0
+ # parallel parking path
+ enable_arc_forward_parking: true
+ enable_arc_backward_parking: true
+ after_forward_parking_straight_distance: 2.0
+ after_backward_parking_straight_distance: 2.0
+ forward_parking_velocity: 1.38
+ backward_parking_velocity: -1.38
+ forward_parking_lane_departure_margin: 0.0
+ backward_parking_lane_departure_margin: 0.0
+ arc_path_interval: 1.0
+ pull_over_max_steer_angle: 0.35 # 20deg
+ # hazard on when parked
+ hazard_on_threshold_distance: 1.0
+ hazard_on_threshold_velocity: 0.5
+ # check safety with dynamic objects. Not used now.
+ pull_over_duration: 2.0
+ pull_over_prepare_duration: 4.0
+ min_stop_distance: 5.0
+ stop_time: 2.0
+ hysteresis_buffer_distance: 2.0
+ prediction_time_resolution: 0.5
+ enable_collision_check_at_prepare_phase: false
+ use_predicted_path_outside_lanelet: false
+ use_all_predicted_path: false
+ # debug
+ print_debug_info: false
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml
new file mode 100644
index 00000000..79044041
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml
@@ -0,0 +1,8 @@
+/**:
+ ros__parameters:
+ side_shift:
+ min_distance_to_start_shifting: 5.0
+ time_to_start_shifting: 1.0
+ shifting_lateral_jerk: 0.2
+ min_shifting_distance: 5.0
+ min_shifting_speed: 5.56
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv
index 32e639ca..979af2be 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv
@@ -1,7 +1,8 @@
-default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
-0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
-0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28
-0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03
-0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58
-0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1
-0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61
+default,0.0,1.0,2.0,3.0,4.0,5.0
+0.0,-0.3,-0.3,-0.3,-0.3,-0.3,-0.3
+0.1,0.0,0.0,0.0,0.0,0.0,0.0
+0.2,0.0,0.0,0.0,0.0,0.0,0.0
+0.3,0.05,0.05,0.05,0.05,0.05,0.05
+0.4,0.1,0.1,0.1,0.1,0.1,0.1
+0.5,0.175,0.175,0.175,0.175,0.175,0.175
+0.6,0.25,0.25,0.25,0.25,0.25,0.25
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
index ac984457..e2c21a40 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -115,7 +115,7 @@
-
+
@@ -180,14 +180,14 @@
-
-
-
-
-
-
+
+
+
+
+
+
-
+
@@ -227,7 +227,7 @@
-
+
@@ -236,7 +236,7 @@
-
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
index 1778c130..29ba8499 100644
--- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
+++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
@@ -1,15 +1,15 @@
sensor_kit_base_link:
gnss_link:
- x: 0.0
+ x: -0.26
y: 0.0
z: 0.0
roll: 0.0
pitch: 0.0
yaw: 0.0
imu_link:
- x: 0.0
+ x: 0.85
y: 0.0
z: 0.0
roll: 0.0
pitch: 0.0
- yaw: 0.0
+ yaw: -1.57
diff --git a/aichallenge/workspace/utils/initialize_workspace.bash b/aichallenge/workspace/utils/initialize_workspace.bash
new file mode 100755
index 00000000..9be731f7
--- /dev/null
+++ b/aichallenge/workspace/utils/initialize_workspace.bash
@@ -0,0 +1,10 @@
+#!/usr/bin/bash
+WORKSPACE=$(readlink -f "$(dirname "$0")/..")
+cd "${WORKSPACE}" || exit
+vcs import --shallow --input depends.repos depends
+
+touch depends/autoware.universe/localization/gyro_odometer/COLCON_IGNORE
+touch depends/autoware.universe/sensing/imu_corrector/COLCON_IGNORE
+
+# shellcheck disable=SC2046
+rosdep install --ignore-src --from-paths $(colcon list --paths-only --packages-up-to aichallenge_launch)