Skip to content
This repository has been archived by the owner on Mar 27, 2023. It is now read-only.

Commit

Permalink
Merge pull request #743 from tier4/sync-awf-latest
Browse files Browse the repository at this point in the history
chore: sync awf-latest
  • Loading branch information
tier4-autoware-public-bot[bot] committed Feb 14, 2023
2 parents a136c23 + cc9fdbb commit e1218a6
Show file tree
Hide file tree
Showing 19 changed files with 145 additions and 89 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -70,15 +70,3 @@
update_steer_threshold: 0.035
average_num: 1000
steering_offset_limit: 0.02

# vehicle parameters
vehicle:
cg_to_front_m: 1.228
cg_to_rear_m: 1.5618
mass_kg: 2400.0
mass_fl: 600.0
mass_fr: 600.0
mass_rl: 600.0
mass_rr: 600.0
cf: 155494.663
cr: 155494.663
17 changes: 17 additions & 0 deletions autoware_launch/config/localization/ndt_scan_matcher.param.yaml
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
/**:
ros__parameters:
# Use dynamic map loading
use_dynamic_map_loading: true

# Vehicle reference frame
base_frame: "base_link"
Expand Down Expand Up @@ -64,3 +66,18 @@

# Regularization scale factor
regularization_scale_factor: 0.01

# Dynamic map loading distance
dynamic_map_loading_update_distance: 20.0

# Dynamic map loading loading radius
dynamic_map_loading_map_radius: 150.0

# Radius of input LiDAR range (used for diagnostics of dynamic map loading)
lidar_radius: 100.0

# A flag for using scan matching score based on de-grounded LiDAR scan
estimate_scores_for_degrounded_scan: false

# If lidar_point.z - base_link.z <= this threshold , the point will be removed
z_margin_for_ground_removal: 0.8
4 changes: 2 additions & 2 deletions autoware_launch/config/map/pointcloud_map_loader.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@
ros__parameters:
enable_whole_load: true
enable_downsampled_whole_load: false
enable_partial_load: false
enable_differential_load: false
enable_partial_load: true
enable_differential_load: true

# only used when downsample_whole_load enabled
leaf_size: 3.0 # downsample leaf size [m]
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,10 @@
max_dist_matrix:
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
[4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN
4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR
4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK
4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS
4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER
4.0, 2.5, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR
4.0, 2.5, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK
4.0, 2.5, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS
4.0, 2.5, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER
3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE
3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE
2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN
Expand Down Expand Up @@ -55,12 +55,12 @@
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN

min_iou_matrix: # If value is negative, it will be ignored.
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
[0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN
0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
[0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN
0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER
0.1, 0.1, 0.1, 0.1, 0.1, -1.0, 0.1, 0.1, #MOTORBIKE
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@
ros__parameters:
# constraints param for normal driving
normal:
min_acc: -0.5 # min deceleration [m/ss]
min_acc: -1.0 # min deceleration [m/ss]
max_acc: 1.0 # max acceleration [m/ss]
min_jerk: -0.5 # min jerk [m/sss]
min_jerk: -1.0 # min jerk [m/sss]
max_jerk: 1.0 # max jerk [m/sss]

# constraints to be observed
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@

# resampling parameters for optimization
max_trajectory_length: 200.0 # max trajectory length for resampling [m]
min_trajectory_length: 150.0 # min trajectory length for resampling [m]
min_trajectory_length: 180.0 # min trajectory length for resampling [m]
resample_time: 2.0 # resample total time for dense sampling [s]
dense_resample_dt: 0.2 # resample time interval for dense sampling [s]
dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
enable_update_path_when_object_is_gone: false
enable_safety_check: false
enable_yield_maneuver: false
disable_path_update: false

# for debug
publish_debug_marker: false
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Fallback>
<ReactiveSequence>
<Condition ID="PullOver_Request"/>
<Action ID="PullOver_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="PullOut_Request"/>
<Action ID="PullOut_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="SideShift_Request"/>
<Action ID="SideShift_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="ExternalRequestLaneChangeRight_Request"/>
<Action ID="ExternalRequestLaneChangeRight_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="ExternalRequestLaneChangeLeft_Request"/>
<Action ID="ExternalRequestLaneChangeLeft_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="LaneChange_Request"/>
<Action ID="LaneChange_Plan" output="{output}"/>
</ReactiveSequence>
<ReactiveSequence>
<Condition ID="Avoidance_Request"/>
<Action ID="Avoidance_Plan" output="{output}"/>
</ReactiveSequence>
<Action ID="LaneFollowing_Plan" output="{output}"/>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="Avoidance_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="Avoidance_Request"/>
<Condition ID="ExternalApproval"/>
<Action ID="ExternalRequestLaneChangeRight_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="ExternalRequestLaneChangeRight_Request"/>
<Action ID="ExternalRequestLaneChangeLeft_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="ExternalRequestLaneChangeLeft_Request"/>
<Action ID="LaneChange_Plan">
<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>
<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"/>
<Action ID="SideShift_Plan">
<output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
</Action>
<Condition ID="SideShift_Request"/>
</TreeNodesModel>
<!-- ////////// -->
</root>
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
minimum_lane_change_velocity: 2.78 # [m/s]
prediction_time_resolution: 0.5 # [s]
maximum_deceleration: 1.0 # [m/s2]
lane_change_sampling_num: 10
lane_change_sampling_num: 3

# collision check
enable_collision_check_at_prepare_phase: false
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
launch_blind_spot: true
launch_detection_area: true
launch_virtual_traffic_light: true
launch_occlusion_spot: true
launch_occlusion_spot: false
launch_no_stopping_area: true
launch_run_out: false
launch_speed_bump: false
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
intersection:
state_transit_margin_time: 1.0
stop_line_margin: 3.0
keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object"
pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity"
use_object_info: true # [-] whether to reflect object info to occupancy grid map or not
use_moving_object_ray_cast: true # [-] whether to reflect moving object ray_cast to occupancy grid map or not
use_moving_object_ray_cast: true # [-] whether to reflect moving object ray shadow grid map
use_partition_lanelet: true # [-] whether to use partition lanelet map data
pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity
pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m)
Expand All @@ -18,8 +18,8 @@
lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision
motion:
safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety
max_slow_down_jerk: -0.5 # [m/s^3] minimum jerk deceleration for safe brake.
max_slow_down_accel: -1.8 # [m/s^2] minimum accel deceleration for safe brake.
max_slow_down_jerk: -0.3 # [m/s^3] minimum jerk deceleration for safe brake.
max_slow_down_accel: -1.5 # [m/s^2] minimum accel deceleration for safe brake.
non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning.
non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning.
min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed
Expand All @@ -28,7 +28,7 @@
min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path.
min_longitudinal_offset: 1.0 # [m] detection area safety buffer from front bumper.
max_lateral_distance: 6.0 # [m] buffer around the ego path used to build the detection area.
max_lateral_distance: 5.0 # [m] buffer around the ego path used to build the detection area.
grid:
free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid
occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid
occupied_min: 57 # [-] minimum value of an occupied cell in the occupancy grid
24 changes: 2 additions & 22 deletions autoware_launch/launch/autoware.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
<!-- Map -->
<arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
<arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>
<arg name="cruise_planner_type" default="obstacle_stop_planner" description="type of cruise planner"/>
<!-- System -->
<arg name="system_run_mode" default="online" description="run mode in system"/>
<!-- Tools -->
Expand Down Expand Up @@ -109,31 +108,12 @@

<!-- Planning -->
<group if="$(var launch_planning)">
<include file="$(find-pkg-share autoware_launch)/launch/components/tier4_planning_component.launch.xml">
<arg name="vehicle_param_file" value="$(find-pkg-share $(var vehicle_model)_description)/config/vehicle_info.param.yaml"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg
name="behavior_path_planner_tree_param_path"
value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml"
/>
<arg name="cruise_planner_type" value="obstacle_stop_planner"/>
<arg name="use_experimental_lane_change_function" value="false"/>
<arg name="use_surround_obstacle_check" value="true"/>
<arg name="smoother_type" value="JerkFiltered"/>
</include>
<include file="$(find-pkg-share autoware_launch)/launch/components/tier4_planning_component.launch.xml"/>
</group>

<!-- Control -->
<group if="$(var launch_control)">
<include file="$(find-pkg-share autoware_launch)/launch/components/tier4_control_component.launch.xml">
<arg name="vehicle_param_file" value="$(find-pkg-share $(var vehicle_model)_description)/config/vehicle_info.param.yaml"/>
<arg name="vehicle_id" value="$(var vehicle_id)"/>
<arg name="enable_obstacle_collision_checker" value="false"/>
<arg name="lateral_controller_mode" value="mpc"/>
<arg name="longitudinal_controller_mode" value="pid"/>
<arg name="use_individual_control_param" value="false"/>
</include>
<include file="$(find-pkg-share autoware_launch)/launch/components/tier4_control_component.launch.xml"/>
</group>

<!-- API -->
Expand Down
Original file line number Diff line number Diff line change
@@ -1,9 +1,7 @@
<?xml version="1.0"?>
<launch>
<!-- forwarding options -->
<arg name="launch_deprecated_api" default="true"/>
<arg name="rosbridge_enabled" default="true"/>
<arg name="rosbridge_max_message_size" default="1000000000"/>
<group>
<include file="$(find-pkg-share tier4_autoware_api_launch)/launch/autoware_api.launch.xml"/>
</group>
<!-- Fork the repository and add the parameters here if needed. -->
<arg name="scenario_simulation" default="false"/>
<arg name="launch_deprecated_api" default="$(var scenario_simulation)"/>
<include file="$(find-pkg-share tier4_autoware_api_launch)/launch/autoware_api.launch.xml"/>
</launch>
Original file line number Diff line number Diff line change
@@ -1,22 +1,18 @@
<?xml version="1.0"?>
<launch>
<arg name="vehicle_param_file"/>
<arg name="vehicle_id"/>
<arg name="enable_obstacle_collision_checker"/>
<arg name="lateral_controller_mode"/>
<arg name="longitudinal_controller_mode"/>
<arg name="use_individual_control_param"/>
<!-- optional parameters are written here -->
<arg name="lateral_controller_mode" default="mpc"/>
<arg name="longitudinal_controller_mode" default="pid"/>
<arg name="use_individual_control_param" default="false"/>

<let name="latlon_controller_param_path_dir" value="$(var vehicle_id)" if="$(var use_individual_control_param)"/>
<let name="latlon_controller_param_path_dir" value="" unless="$(var use_individual_control_param)"/>

<include file="$(find-pkg-share tier4_control_launch)/launch/control.launch.py">
<arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>

<!-- option -->
<arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
<arg name="vehicle_param_file" value="$(find-pkg-share $(var vehicle_model)_description)/config/vehicle_info.param.yaml"/>
<arg name="vehicle_id" value="$(var vehicle_id)"/>
<arg name="enable_obstacle_collision_checker" value="$(var enable_obstacle_collision_checker)"/>
<arg name="enable_obstacle_collision_checker" value="false"/>
<arg name="trajectory_follower_node_param_path" value="$(find-pkg-share autoware_launch)/config/control/trajectory_follower/trajectory_follower_node.param.yaml"/>
<arg
name="lat_controller_param_path"
Expand Down
Original file line number Diff line number Diff line change
@@ -1,18 +1,15 @@
<?xml version="1.0"?>
<launch>
<arg name="vehicle_param_file"/>
<arg name="use_pointcloud_container"/>
<arg name="pointcloud_container_name"/>
<!-- NOTE: optional parameters are written here -->
<!-- behavior -->
<arg name="behavior_path_planner_tree_param_path"/>
<arg name="use_experimental_lane_change_function" default="false"/>
<!-- motion -->
<arg name="cruise_planner_type" description="options: obstacle_stop_planner, obstacle_cruise_planner, none"/>
<arg name="use_surround_obstacle_check"/>
<arg name="smoother_type" description="options: JerkFiltered, L2, Analytical, Linf(Unstable)"/>
<arg name="use_experimental_lane_change_function"/>
<arg name="cruise_planner_type" default="obstacle_stop_planner" description="options: obstacle_stop_planner, obstacle_cruise_planner, none"/>
<arg name="use_surround_obstacle_check" default="true"/>
<arg name="smoother_type" default="JerkFiltered" description="options: JerkFiltered, L2, Analytical, Linf(Unstable)"/>

<include file="$(find-pkg-share tier4_planning_launch)/launch/planning.launch.xml">
<arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
<arg name="vehicle_param_file" value="$(find-pkg-share $(var vehicle_model)_description)/config/vehicle_info.param.yaml"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="smoother_type" value="$(var smoother_type)"/>
Expand Down Expand Up @@ -47,7 +44,10 @@
name="drivable_area_expansion_param_path"
value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml"
/>
<arg name="behavior_path_planner_tree_param_path" value="$(var behavior_path_planner_tree_param_path)"/>
<arg
name="behavior_path_planner_tree_param_path"
value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml"
/>
<arg
name="behavior_path_planner_param_path"
value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml"
Expand Down
Loading

0 comments on commit e1218a6

Please sign in to comment.