diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 70d228506adfd..bbe5ab3d85401 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -48,13 +48,12 @@ When the deceleration section is inserted, the start point of the section is ins ### Common Parameter -| Parameter | Type | Description | -| ----------------------- | ------ | ---------------------------------------------------------------------------------------- | -| `enable_slow_down` | bool | enable slow down planner [-] | -| `max_velocity` | double | max velocity [m/s] | -| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | -| `lowpass_gain` | double | low pass gain for calculating acceleration [-] | -| `max_yaw_deviation_deg` | double | # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes) | +| Parameter | Type | Description | +| ------------------- | ------ | ---------------------------------------------------------------------------------------- | +| `enable_slow_down` | bool | enable slow down planner [-] | +| `max_velocity` | double | max velocity [m/s] | +| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | +| `lowpass_gain` | double | low pass gain for calculating acceleration [-] | ### Obstacle Stop Planner 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 ff0281bf574eb..6fe30b216118e 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,6 @@ step_length: 1.0 # step length for pointcloud search range [m] extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance expand_stop_range: 0.0 # margin of vehicle footprint [m] - max_yaw_deviation_deg: 90.0 # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes) hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] slow_down_planner: 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 08647aae98826..7d63be90db608 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -98,12 +98,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node struct NodeParam { - bool enable_slow_down; // set True, slow down for obstacle beside the path - double max_velocity; // max velocity [m/s] - double lowpass_gain; // smoothing calculated current acceleration [-] - double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] - double max_yaw_deviation_rad; // maximum ego yaw deviation from trajectory [rad] (measures - // against overlapping lanes) + bool enable_slow_down; // set True, slow down for obstacle beside the path + double max_velocity; // max velocity [m/s] + double lowpass_gain; // smoothing calculated current acceleration [-] + double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] + double ego_nearest_dist_threshold; // dist threshold for ego's nearest index + double ego_nearest_yaw_threshold; // yaw threshold for ego's nearest index }; struct StopParam diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 4f0f180d8c4b5..f79d09aab3242 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -43,9 +43,10 @@ namespace motion_planning { using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using tier4_autoware_utils::calcAzimuthAngle; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; @@ -457,8 +458,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.hunting_threshold = declare_parameter("hunting_threshold", 0.5); p.lowpass_gain = declare_parameter("lowpass_gain", 0.9); lpf_acc_ = std::make_shared(p.lowpass_gain); - const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0); - p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad(max_yaw_deviation_deg); + p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); } { @@ -817,17 +818,39 @@ void ObstacleStopPlannerNode::insertVelocity( index_with_dist_remain.get().first, output, index_with_dist_remain.get().second, stop_param); - const auto & ego_pos = planner_data.current_pose.position; - const auto stop_point_distance = - calcSignedArcLength(output, ego_pos, getPoint(stop_point.point)); + const auto & ego_pose = planner_data.current_pose; + const size_t ego_seg_idx = findFirstNearestIndexWithSoftConstraints( + output, ego_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + + const double stop_point_distance = [&]() { + if (output.size() < 2) { + return 0.0; + } + + size_t stop_seg_idx = 0; + const double lon_offset = + calcLongitudinalOffsetToSegment(output, stop_point.index, getPoint(stop_point.point)); + if (lon_offset < 0) { + stop_seg_idx = std::max(static_cast(0), stop_point.index - 1); + } else { + stop_seg_idx = std::min(output.size() - 2, stop_point.index); + } + + return calcSignedArcLength( + output, ego_pose.position, ego_seg_idx, getPoint(stop_point.point), stop_seg_idx); + }(); const auto is_stopped = current_vel < 0.01; + const auto & ego_pos = planner_data.current_pose.position; if (stop_point_distance < stop_param_.hold_stop_margin_distance && is_stopped) { const auto ego_pos_on_path = calcLongitudinalOffsetPose(output, ego_pos, 0.0); if (ego_pos_on_path) { StopPoint current_stop_pos{}; - current_stop_pos.index = findNearestSegmentIndex(output, ego_pos); + current_stop_pos.index = findFirstNearestSegmentIndexWithSoftConstraints( + output, ego_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); current_stop_pos.point.pose = ego_pos_on_path.get(); insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag); @@ -1316,14 +1339,10 @@ TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( { TrajectoryPoints output{}; - size_t min_distance_index = 0; - const auto nearest_index = - motion_utils::findNearestIndex(input, self_pose, 10.0, node_param_.max_yaw_deviation_rad); - if (!nearest_index) { - min_distance_index = motion_utils::findNearestIndex(input, self_pose.position); - } else { - min_distance_index = nearest_index.value(); - } + const size_t min_distance_index = findFirstNearestIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + for (size_t i = min_distance_index; i < input.size(); ++i) { output.push_back(input.at(i)); }