Skip to content

Commit

Permalink
for slow down 0127
Browse files Browse the repository at this point in the history
  • Loading branch information
@YusukeMuramatsu-gt
YusukeMuramatsu-gt committed Jan 27, 2023
1 parent 06d9749 commit cdff97d
Show file tree
Hide file tree
Showing 4 changed files with 43 additions and 34 deletions.
13 changes: 6 additions & 7 deletions ...nning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@

slow_down_planner:
# slow down planner parameters
forward_margin: 250.0 #150.0 # margin distance from slow down point to vehicle front [m]
forward_margin: 250.0 # margin distance from slow down point to vehicle front [m]
backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m]
expand_slow_down_range: 1.9 #1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
expand_slow_down_range: 1.9 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
expand_slow_down_range_l: 0.4 # offset from vehicle left edge for expanding the search area of the surrounding point cloud [m]
expand_slow_down_range_r: 1.9 # offset from vehicle right edge for expanding the search area of the surrounding point cloud [m]
time_margin: 10 # if detect obstacle for more than time_margin, slow down start (integer)
max_slow_down_vel: 4.16 #5.55 #4.16 #15km/h # max slow down velocity [m/s]
min_slow_down_vel: 4.16 #5.55 #2.77 #10km/h # min slow down velocity [m/s]
max_slow_down_vel: 4.16 #15km/h # max slow down velocity [m/s]
min_slow_down_vel: 4.16 #15km/h # min slow down velocity [m/s]
max_deceleration: 2.0 # for obstacle collision checker
passing_param: 0.00010 #0.00015 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai.
passing_param: 0.000023 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai.
speed_thresh_high: 1.38 # [m/s]
speed_thresh_low : 0.0 # [m/s]
yaw_rate_thresh: 100.0 # [rad/s] <= need tuning parameter 100=disable
Expand All @@ -30,8 +30,7 @@
curvature_smoothing_num: 50 # [-]
curve_forward_margin: 120.0 #150.0 # margin distance from slow down point to vehicle front in high curvature[m]
curve_backward_margin: 5.0 # margin distance from slow down point to vehicle rear in high curvature[m]
curve_min_slow_down_vel: 0.5 # min slow down velocity in high curvature[m/s] x > 0
passing_param_for_curve: 0.0003 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai in high curvature.
passing_param_for_curve: 0.00020 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai in high curvature.


# slow down constraint parameters
Expand Down
13 changes: 6 additions & 7 deletions planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@

slow_down_planner:
# slow down planner parameters
forward_margin: 250.0 #150.0 # margin distance from slow down point to vehicle front [m]
forward_margin: 250.0 # margin distance from slow down point to vehicle front [m]
backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m]
expand_slow_down_range: 1.9 #1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
expand_slow_down_range: 1.9 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
expand_slow_down_range_l: 0.4 # offset from vehicle left edge for expanding the search area of the surrounding point cloud [m]
expand_slow_down_range_r: 1.9 # offset from vehicle right edge for expanding the search area of the surrounding point cloud [m]
time_margin: 10 # if detect obstacle for more than time_margin, slow down start (integer)
max_slow_down_vel: 4.16 #5.55 #4.16 #15km/h # max slow down velocity [m/s]
min_slow_down_vel: 4.16 #5.55 #2.77 #10km/h # min slow down velocity [m/s]
max_slow_down_vel: 4.16 #15km/h # max slow down velocity [m/s]
min_slow_down_vel: 4.16 #15km/h # min slow down velocity [m/s]
max_deceleration: 2.0 # for obstacle collision checker
passing_param: 0.00010 #0.00015 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai.
passing_param: 0.000023 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai.
speed_thresh_high: 1.38 # [m/s]
speed_thresh_low : 0.0 # [m/s]
yaw_rate_thresh: 100.0 # [rad/s] <= need tuning parameter 100=disable
Expand All @@ -30,8 +30,7 @@
curvature_smoothing_num: 50 # [-]
curve_forward_margin: 120.0 #150.0 # margin distance from slow down point to vehicle front in high curvature[m]
curve_backward_margin: 5.0 # margin distance from slow down point to vehicle rear in high curvature[m]
curve_min_slow_down_vel: 0.5 # min slow down velocity in high curvature[m/s] x > 0
passing_param_for_curve: 0.0003 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai in high curvature.
passing_param_for_curve: 0.00020 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai in high curvature.


# slow down constraint parameters
Expand Down
1 change: 0 additions & 1 deletion planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node
// check complete deceleration [m/ss]
double slow_down_search_radius; // search radius for slow down obstacle point cloud [m]
double passing_param; //for isuzu
double curve_min_slow_down_vel; //for isuzu
double passing_param_for_curve; //for isuzu
double curve_forward_margin; //for isuzu
double curve_backward_margin; //for isuzu
Expand Down
50 changes: 31 additions & 19 deletions planning/obstacle_stop_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -500,7 +500,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
p.min_slow_down_vel = declare_parameter(ns + "min_slow_down_vel", 2.0);
p.max_deceleration = declare_parameter(ns + "max_deceleration", 2.0); // for obstacle collision checker
p.passing_param = declare_parameter(ns + "passing_param", 0.00015); // for isuzu proj
p.curve_min_slow_down_vel = declare_parameter(ns + "curve_min_slow_down_vel", 0.5); // for isuzu proj
p.passing_param_for_curve = declare_parameter(ns + "passing_param_for_curve", 0.0004); // for isuzu proj
p.curve_forward_margin = declare_parameter(ns + "curve_forward_margin", 10.0); // for isuzu proj
p.curve_backward_margin = declare_parameter(ns + "curve_backward_margin", 5.0); // for isuzu proj
Expand Down Expand Up @@ -1005,7 +1004,7 @@ void ObstacleStopPlannerNode::insertVelocity(
index_with_dist_remain.get().second,
stop_param);
if (stop_point_on_curv.index <= output.size()) {

const auto slow_down_section = createSlowDownSectionforCurve(
index_with_dist_remain.get().first,
output,
Expand Down Expand Up @@ -1227,20 +1226,28 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection(
const auto update_backward_margin_from_vehicle = slow_down_param_.backward_margin + dist_remain;

// get current velocity and distance to the oncoming car
const double current_velocity = current_velocity_ptr_->twist.twist.linear.x; //for isuzu by muramatsu
const double acc_constrained_vel = std::max(0.0, current_velocity + (dist_baselink_to_obstacle - slow_down_param_.forward_margin)* slow_down_param_.passing_param *current_velocity); //for isuzu by muramatsu

const double current_velocity = current_velocity_ptr_->twist.twist.linear.x;

const auto slow_down_threshold = current_velocity / dist_baselink_to_obstacle;
double acc_constrained_vel = 0.0;

// for isuzu proj
// ref : https://tier4.github.io/autoware.iv/tree/main/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/
const double rule_based_target_velocity =
slow_down_param_.min_slow_down_vel + (slow_down_param_.max_slow_down_vel - slow_down_param_.min_slow_down_vel) *
std::max(lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
std::max(lateral_deviation - vehicle_info.vehicle_width_m / 2.0, 0.0) /
slow_down_param_.expand_slow_down_range;


if(slow_down_threshold > 0.20){ // for sudden appear case
acc_constrained_vel = std::max(0.0, current_velocity + (dist_baselink_to_obstacle - slow_down_param_.forward_margin)*
slow_down_param_.passing_param *current_velocity );
}else{
acc_constrained_vel = std::max(0.0, current_velocity + (dist_baselink_to_obstacle - slow_down_param_.forward_margin)*
slow_down_param_.passing_param *current_velocity / (slow_down_threshold * 9.2));
}
// use `ros2 run rqt_plot rqt_plot /control/command/control_cmd/longitudinal/acceleration` to check acceleration behavior.
// const auto velocity = rule_based_target_velocity; // turn off acc limit
const auto velocity = std::max(rule_based_target_velocity, acc_constrained_vel); // turn on acc limit //for isuzu by muramatsu

const auto velocity = std::max(rule_based_target_velocity, acc_constrained_vel);

// RCLCPP_WARN(get_logger(), "[for isuzu] velocity:%f ",velocity);

Expand Down Expand Up @@ -1289,16 +1296,21 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSectionforCurve( //for i
const auto update_backward_margin_from_vehicle = slow_down_param_.curve_backward_margin + dist_remain;

// get current velocity and distance to the oncoming car
const double current_velocity = current_velocity_ptr_->twist.twist.linear.x; //for isuzu by muramatsu
const double acc_constrained_vel = std::max(0.0, current_velocity + (dist_baselink_to_obstacle - slow_down_param_.forward_margin)* slow_down_param_.passing_param_for_curve *current_velocity); //for isuzu by muramatsu


// for isuzu proj
const double rule_based_target_velocity = slow_down_param_.curve_min_slow_down_vel;

const auto velocity = std::max(rule_based_target_velocity, acc_constrained_vel); // turn on acc limit //for isuzu by muramatsu

// RCLCPP_WARN(get_logger(), "[for isuzu] velocity:%f ",velocity);
const double current_velocity = current_velocity_ptr_->twist.twist.linear.x;

const auto slow_down_threshold = current_velocity / dist_baselink_to_obstacle;
double acc_constrained_vel = 0.0;

if(current_velocity < 1.5){ // for stopping
acc_constrained_vel = std::max(0.0, current_velocity - 0.2);
}else if(slow_down_threshold > 0.20){ // for sudden appear case
acc_constrained_vel = std::max(0.0, current_velocity + (dist_baselink_to_obstacle - slow_down_param_.curve_forward_margin)*
slow_down_param_.passing_param_for_curve *current_velocity );
}else{
acc_constrained_vel = std::max(0.0, current_velocity + (dist_baselink_to_obstacle - slow_down_param_.curve_forward_margin)*
slow_down_param_.passing_param_for_curve *current_velocity / (slow_down_threshold * 7.2));
}
const auto velocity = acc_constrained_vel;

return createSlowDownSectionFromMargin(
idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle,
Expand Down

0 comments on commit cdff97d

Please sign in to comment.