From 96bd2bca2f4bf40adbe41ef9632d7ede8333d33d Mon Sep 17 00:00:00 2001 From: mura-yusuke Date: Thu, 16 Mar 2023 11:54:56 +0900 Subject: [PATCH] bag fix by muramatsu 0316 --- .../obstacle_stop_planner.param.yaml | 7 +++--- .../config/obstacle_stop_planner.param.yaml | 7 +++--- .../include/obstacle_stop_planner/node.hpp | 3 ++- planning/obstacle_stop_planner/src/node.cpp | 22 +++++++++++++------ 4 files changed, 25 insertions(+), 14 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml index 23a163fe50330..0d97cadae87e6 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml @@ -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 @@ -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] diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index 23a163fe50330..0d97cadae87e6 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -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 @@ -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] diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 8bbf6b1d4609a..f03142041b96a 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -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] diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 15270737b392c..72bae69fd768c 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -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; @@ -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); @@ -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); @@ -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 @@ -955,6 +956,7 @@ void ObstacleStopPlannerNode::insertVelocity( // calc curvature of planned path std::vector curvature_list = calcTrajectoryCurvature(slow_down_param_.curvature_smoothing_num, output); + bool dist_flag = true; if (planner_data.slow_down_require) { // insert slow down point @@ -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, @@ -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);