Skip to content

Commit

Permalink
Update readme
Browse files Browse the repository at this point in the history 
update svg


readme updated


with test params
change sample rate


calculate accurate dt


test

Signed-off-by: Berkay <berkay@leodrive.ai>

fix trajectory size


update readme


change map loader params
Signed-off-by: Berkay <berkay@leodrive.ai>
  • Loading branch information
Berkay committed Jun 9, 2022
1 parent c784203 commit 0f48b40
Show file tree
Hide file tree
Showing 11 changed files with 5,079 additions and 104 deletions.
4 changes: 2 additions & 2 deletions launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,8 @@
input_delay: 0.24 # steering input delay time for delay compensation
vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s]
steer_lim_deg: 40.0 # steering angle limit [deg]
steer_rate_lim_dps: 5.0 # steering angle rate limit [deg/s]
acceleration_limit: 0.9 # acceleration limit for trajectory velocity modification [m/ss]
steer_rate_lim_dps: 20.0 # steering angle rate limit [deg/s]
acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss]
velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s]

# -- lowpass filter for noise reduction --
Expand Down
8 changes: 4 additions & 4 deletions launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,12 +59,12 @@
emergency_jerk: -3.0

# acceleration limit
max_acc: 0.9
min_acc: -0.9
max_acc: 3.0
min_acc: -5.0

# jerk limit
max_jerk: 0.6
min_jerk: -0.6
max_jerk: 2.0
min_jerk: -5.0

# pitch
use_trajectory_for_pitch_calculation: false
Expand Down
14 changes: 7 additions & 7 deletions launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,13 @@
# constraints param for normal driving
normal:
min_acc: -0.5 # min deceleration [m/ss]
max_acc: 0.5 # max acceleration [m/ss]
min_jerk: -0.4 # min jerk [m/sss]
max_jerk: 0.4 # max jerk [m/sss]
max_acc: 1.0 # max acceleration [m/ss]
min_jerk: -0.5 # min jerk [m/sss]
max_jerk: 1.0 # max jerk [m/sss]

# constraints to be observed
limit:
min_acc: -0.9 # min deceleration limit [m/ss]
max_acc: 0.9 # max acceleration limit [m/ss]
min_jerk: -0.6 # min jerk limit [m/sss]
max_jerk: 0.6 # max jerk limit [m/sss]
min_acc: -2.5 # min deceleration limit [m/ss]
max_acc: 1.0 # max acceleration limit [m/ss]
min_jerk: -1.5 # min jerk limit [m/sss]
max_jerk: 1.5 # max jerk limit [m/sss]
13 changes: 8 additions & 5 deletions ...fig/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,14 +8,14 @@
margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m]

# curve parameters
max_lateral_accel: 0.3 # max lateral acceleration limit [m/ss]
min_curve_velocity: 0.5 # min velocity at lateral acceleration limit [m/ss]
max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss]
min_curve_velocity: 0.1 # min velocity at lateral acceleration limit [m/ss]
decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit
decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit

# engage & replan parameters
replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s]
engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
engage_velocity: 0.1 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement)
engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
Expand Down Expand Up @@ -47,8 +47,11 @@
post_sparse_dt: 0.1 # resample time interval for sparse sampling [s]
post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m]

# vehicle model parameters
max_steering_angle_rate: 0.0872665 # steering angle rate [rad/s]
# steering angle rate limit parameters
max_steering_angle_rate: 0.349066 # steering angle rate [rad/s]
resample_ds: 0.5 # distance between trajectory points [m]
max_lookup_distance: 2.0 # maximum lookup distance [m]
min_lookup_distance: 1.0 # maximum lookup distance [m]

# system
over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point
8 changes: 4 additions & 4 deletions map/map_loader/config/lanelet2_map_loader.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/**:
ros__parameters:
lanelet2_map_projector_type: UTM # Options: MGRS, UTM
latitude: 40.81187906 # Latitude of map_origin, using in UTM
longitude: 29.35810110 # Longitude of map_origin, using in UTM
lanelet2_map_projector_type: MGRS # Options: MGRS, UTM
latitude: 40.816617984672746 # Latitude of map_origin, using in UTM
longitude: 29.360491808334285 # Longitude of map_origin, using in UTM

center_line_resolution: 5.0 # [m]
center_line_resolution: 5.0 # [m]
45 changes: 31 additions & 14 deletions planning/motion_velocity_smoother/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,13 @@ This function is used to approach near the obstacle or improve the accuracy of s
It applies the velocity limit to decelerate at the curve.
It calculate the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`.
The velocity limit is set as not to fall under `min_curve_velocity`.
It also fills the desired steering angle of curvature points with respect to following equation:

![desired steering angle calculation](./media/desired_steering_angle_eq.svg)

#### Apply steering rate limit

It applies the rate limit. If the (`steering_angle_rate` > `max_steering_angle_rate`), it decreases the velocity of the trajectory point to decrease `steering_angle_rate`.

#### Resample trajectory

Expand Down Expand Up @@ -96,19 +103,20 @@ After the optimization, a resampling called `post resampling` is performed befor

### Output

| Name | Type | Description |
| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- |
| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory |
| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] |
| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) |
| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) |
| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) |
| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) |
| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) |
| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) |
| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) |
| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) |
| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |
| Name | Type | Description |
| -------------------------------------------------- |-------------------------------------------|-----------------------------------------------------------------------------------------------------------|
| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory |
| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] |
| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) |
| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) |
| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) |
| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) |
| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) |
| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) |
| `~/debug/trajectory_steering_rate_limited` | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) |
| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) |
| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) |
| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |

## Parameters

Expand Down Expand Up @@ -187,6 +195,15 @@ After the optimization, a resampling called `post resampling` is performed befor
| `post_sparse_dt` | `double` | resample time interval for sparse sampling [s] | 0.1 |
| `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0 |

### Steering angle rate parameters

| Name | Type | Description | Default value |
|:--------------------------| :------- |:-----------------------------------------|:--------------|
| `max_steering_angle_rate` | `double` | Maximum steering tire angle rate [rad/s] | 0.349066 |
| `resample_ds` | `double` | Distance between trajectory points [m] | 0.5 |
| `max_lookup_distance` | `double` | Maximum lookup distance [m] | 2.0 |
| `min_lookup_distance` | `double` | Minimum lookup distance [m] | 1.0 |

### Weights for optimization

#### JerkFiltered
Expand Down Expand Up @@ -217,7 +234,7 @@ After the optimization, a resampling called `post resampling` is performed befor
### Others

| Name | Type | Description | Default value |
| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ |
|:------------------------------| :------- |:--------------------------------------------------------------------------------------------------| :------------ |
| `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389 |

<!-- Write parameters of this package.
Expand Down
10 changes: 5 additions & 5 deletions ...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,12 @@ class SmootherBase
double min_curve_velocity; // min velocity at curve [m/s]
double decel_distance_before_curve; // distance before slow down for lateral acc at a curve
double decel_distance_after_curve; // distance after slow down for lateral acc at a curve
double max_steering_angle_rate; // max steering angle rate [degree/s]
double max_steering_angle_rate; // max steering angle rate [degree/s]
double wheel_base; // wheel base [m]
double sample_ds; // distance between trajectory points [m]
double min_lookup_dist;
double max_lookup_dist;
double steering_limit_sample_rate;
resampling::ResampleParam resample_param;
};

Expand Down Expand Up @@ -80,10 +84,6 @@ class SmootherBase

protected:
BaseParam base_param_;
const double points_interval = 0.1; // constant interval distance of trajectory
// for lateral acceleration calculation and
// limit steering rate. [m]

};
} // namespace motion_velocity_smoother

Expand Down
Loading

0 comments on commit 0f48b40

Please sign in to comment.