Skip to content

Commit

Permalink
bag fix by muramatsu 0316
Browse files Browse the repository at this point in the history
  • Loading branch information
@YusukeMuramatsu-gt
YusukeMuramatsu-gt committed Mar 16, 2023
1 parent cdff97d commit 96bd2bc
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 14 deletions.
7 changes: 4 additions & 3 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 @@ -8,7 +8,8 @@
expand_stop_range: 0.0 # margin of vehicle footprint [m]
expand_stop_range_l: 0.0 # margin of vehicle left footprint [m]
expand_stop_range_r: 0.0 # margin of vehicle right footprint [m]
mirror_width: 0.4 # mirror width [m]
mirror_width_l: 0.4 # mirror width [m]
mirror_width_r: 0.4 # mirror width [m]

slow_down_planner:
# slow down planner parameters
Expand All @@ -22,8 +23,8 @@
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.000023 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai.
speed_thresh_high: 1.38 # [m/s]
speed_thresh_low : 0.0 # [m/s]
speed_thresh_high: 3.00 # [m/s]
speed_thresh_low : 2.77 # [m/s]
yaw_rate_thresh: 100.0 # [rad/s] <= need tuning parameter 100=disable
curvature_stop: True # set "True", if curvature is high and searched vehicle, become stop mode.
curvature_thresh: 0.002 # [1/m]
Expand Down
7 changes: 4 additions & 3 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 @@ -8,7 +8,8 @@
expand_stop_range: 0.0 # margin of vehicle footprint [m]
expand_stop_range_l: 0.0 # margin of vehicle left footprint [m]
expand_stop_range_r: 0.0 # margin of vehicle right footprint [m]
mirror_width: 0.4 # mirror width [m]
mirror_width_l: 0.4 # mirror width [m]
mirror_width_r: 0.4 # mirror width [m]

slow_down_planner:
# slow down planner parameters
Expand All @@ -22,8 +23,8 @@
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.000023 #[-] set bigger, deceleration become bigger. for isuzu tyokusen suretigai.
speed_thresh_high: 1.38 # [m/s]
speed_thresh_low : 0.0 # [m/s]
speed_thresh_high: 3.00 # [m/s]
speed_thresh_low : 2.77 # [m/s]
yaw_rate_thresh: 100.0 # [rad/s] <= need tuning parameter 100=disable
curvature_stop: True # set "True", if curvature is high and searched vehicle, become stop mode.
curvature_thresh: 0.002 # [1/m]
Expand Down
3 changes: 2 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 @@ -113,7 +113,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node
double expand_stop_range; // default
double expand_stop_range_l; // for isuzu proj
double expand_stop_range_r; // for isuzu proj
double mirror_width; // for isuzu proj
double mirror_width_l; // for isuzu proj
double mirror_width_r; // for isuzu proj
double extend_distance; // trajectory extend_distance [m]
double step_length; // step length for pointcloud search range [m]
double stop_search_radius; // search radius for obstacle point cloud [m]
Expand Down
22 changes: 15 additions & 7 deletions planning/obstacle_stop_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -473,7 +473,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
p.expand_stop_range = declare_parameter(ns + "expand_stop_range", 0.0); // default
p.expand_stop_range_l = declare_parameter(ns + "expand_stop_range_l", 0.0); // for isuzu proj
p.expand_stop_range_r = declare_parameter(ns + "expand_stop_range_r", 0.0); // for isuzu proj
p.mirror_width = declare_parameter(ns + "mirror_width", 0.0); // for isuzu proj
p.mirror_width_l = declare_parameter(ns + "mirror_width_l", 0.0); // for isuzu proj
p.mirror_width_r = declare_parameter(ns + "mirror_width_r", 0.0); // for isuzu proj
p.extend_distance = declare_parameter(ns + "extend_distance", 0.0);
p.step_length = declare_parameter(ns + "step_length", 1.0);
p.stop_margin += i.max_longitudinal_offset_m;
Expand All @@ -487,7 +488,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
// common param
p.normal_min_jerk = declare_parameter("normal.min_jerk", -0.3);
p.normal_min_acc = declare_parameter("normal.min_acc", -1.0);
p.limit_min_jerk = declare_parameter("limit.min_jerk", -1.5);
p.limit_min_jerk = declare_parameter("limit.min_jerk", -1.5);
p.limit_min_acc = declare_parameter("limit.min_acc", -2.5);
// slow down planner specific parameters
p.forward_margin = declare_parameter(ns + "forward_margin", 5.0);
Expand All @@ -508,7 +509,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
p.consider_constraints = declare_parameter(ns + "consider_constraints", false);
p.forward_margin_min = declare_parameter(ns + "forward_margin_min", 1.0);
p.forward_margin_span = declare_parameter(ns + "forward_margin_span", -0.1);
p.slow_down_min_jerk = declare_parameter(ns + "jerk_min_slow_down", -0.3);
p.slow_down_min_jerk = declare_parameter(ns + "jerk_min_slow_down", -0.3);
p.jerk_start = declare_parameter(ns + "jerk_start", -0.1);
p.jerk_span = declare_parameter(ns + "jerk_span", -0.01);
p.slow_down_vel = declare_parameter(ns + "slow_down_vel", 1.39);
Expand Down Expand Up @@ -866,8 +867,8 @@ void ObstacleStopPlannerNode::searchObstacle(
decimate_trajectory.at(i + 1).pose,
one_step_move_vehicle_polygon,
vehicle_info,
stop_param.expand_stop_range_l + stop_param.mirror_width,
stop_param.expand_stop_range_r + stop_param.mirror_width);
stop_param.expand_stop_range_l + stop_param.mirror_width_l,
stop_param.expand_stop_range_r + stop_param.mirror_width_r);
}

// create one step polygon for vehicle
Expand Down Expand Up @@ -955,6 +956,7 @@ void ObstacleStopPlannerNode::insertVelocity(

// calc curvature of planned path
std::vector<double> curvature_list = calcTrajectoryCurvature(slow_down_param_.curvature_smoothing_num, output);
bool dist_flag = true;

if (planner_data.slow_down_require) {
// insert slow down point
Expand All @@ -971,6 +973,12 @@ void ObstacleStopPlannerNode::insertVelocity(
const auto vehicle_idx = std::min(planner_data.trajectory_trim_index, traj_end_idx);
const auto dist_baselink_to_obstacle =
calcSignedArcLength(output, vehicle_idx, index_with_dist_remain.get().first);

if (dist_baselink_to_obstacle - index_with_dist_remain.get().second > 0){
dist_flag = true;
}else{
dist_flag = false;
}

debug_ptr_->setDebugValues(
DebugValues::TYPE::OBSTACLE_DISTANCE,
Expand Down Expand Up @@ -1035,8 +1043,8 @@ void ObstacleStopPlannerNode::insertVelocity(
const auto reach_slow_down_end_point = isInFrontOfTargetPoint(planner_data.current_pose, p_end);
const auto is_in_slow_down_section = reach_slow_down_start_point && !reach_slow_down_end_point;
const auto index_with_dist_remain = findNearestFrontIndex(0, output, p_end);

if (is_in_slow_down_section && index_with_dist_remain) {
if (is_in_slow_down_section && index_with_dist_remain && dist_flag) {
const auto end_insert_point_with_idx = getBackwardInsertPointFromBasePoint(
index_with_dist_remain.get().first, output, -index_with_dist_remain.get().second);

Expand Down

0 comments on commit 96bd2bc

Please sign in to comment.