From cf4beb4bf5d64cf0301a5576e5f27f0d2ecd80ef Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 3 Aug 2022 12:08:29 +0900 Subject: [PATCH] feat(obstacle_stop_planner): add hold stop margin distance (#1434) * feat(obstacle_stop_planner): add hold stop margin distance Signed-off-by: satoshi-ota * feat(obstacle_stop_planner): output distance info Signed-off-by: satoshi-ota * feat(obstacle_stop_planner): add param into config Signed-off-by: satoshi-ota --- .../config/obstacle_stop_planner.param.yaml | 1 + .../obstacle_stop_planner/debug_marker.hpp | 11 ++--- .../include/obstacle_stop_planner/node.hpp | 13 +++--- planning/obstacle_stop_planner/src/node.cpp | 45 +++++++++++++++++-- 4 files changed, 55 insertions(+), 15 deletions(-) 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 c4be8d7c35789..7cb827feb0227 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -7,6 +7,7 @@ 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: # slow down planner parameters diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp index 3b996db24f66a..6477066bd8baf 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -51,11 +51,12 @@ class DebugValues CURRENT_VEL = 0, CURRENT_ACC = 1, CURRENT_FORWARD_MARGIN = 2, - OBSTACLE_DISTANCE = 3, - FLAG_FIND_COLLISION_OBSTACLE = 4, - FLAG_FIND_SLOW_DOWN_OBSTACLE = 5, - FLAG_ADAPTIVE_CRUISE = 6, - FLAG_EXTERNAL = 7, + SLOWDOWN_OBSTACLE_DISTANCE = 3, + COLLISION_OBSTACLE_DISTANCE = 4, + FLAG_FIND_COLLISION_OBSTACLE = 5, + FLAG_FIND_SLOW_DOWN_OBSTACLE = 6, + FLAG_ADAPTIVE_CRUISE = 7, + FLAG_EXTERNAL = 8, SIZE }; 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 43edfc7b80f98..5d61c8ddf7a68 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -108,12 +108,13 @@ class ObstacleStopPlannerNode : public rclcpp::Node struct StopParam { - double stop_margin; // stop margin distance from obstacle on the path [m] - double min_behavior_stop_margin; // margin distance, any other stop point is inserted [m] - double expand_stop_range; // margin of vehicle footprint [m] - 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] + double stop_margin; // stop margin distance from obstacle on the path [m] + double min_behavior_stop_margin; // margin distance, any other stop point is inserted [m] + double expand_stop_range; // margin of vehicle footprint [m] + 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] + double hold_stop_margin_distance; // keep stopping if the ego is in this margin [m] }; struct SlowDownParam diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index f41784aa489f7..e85dcdbb50d34 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -42,11 +42,15 @@ namespace motion_planning { +using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; +using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcAzimuthAngle; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getPose; using tier4_autoware_utils::getRPY; namespace @@ -470,6 +474,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.stop_search_radius = p.step_length + std::hypot(i.vehicle_width_m / 2.0 + p.expand_stop_range, i.vehicle_length_m / 2.0); + p.hold_stop_margin_distance = declare_parameter(ns + "hold_stop_margin_distance", 0.0); } { @@ -775,6 +780,8 @@ void ObstacleStopPlannerNode::insertVelocity( const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, const StopParam & stop_param) { + const auto & base_link2front = vehicle_info.max_longitudinal_offset_m; + if (planner_data.stop_require) { // insert stop point const auto traj_end_idx = output.size() - 1; @@ -787,10 +794,41 @@ void ObstacleStopPlannerNode::insertVelocity( planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); if (index_with_dist_remain) { + 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); + + debug_ptr_->setDebugValues( + DebugValues::TYPE::COLLISION_OBSTACLE_DISTANCE, + dist_baselink_to_obstacle + index_with_dist_remain.get().second - base_link2front); + const auto stop_point = searchInsertPoint( index_with_dist_remain.get().first, output, index_with_dist_remain.get().second, stop_param); - insertStopPoint(stop_point, output, planner_data.stop_reason_diag); + + const auto & ego_pos = planner_data.current_pose.position; + const auto stop_point_distance = + calcSignedArcLength(output, ego_pos, getPoint(stop_point.point)); + const auto is_stopped = current_vel < 0.01; + + 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.point.pose = ego_pos_on_path.get(); + + insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag); + + debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); + } + + } else { + insertStopPoint(stop_point, output, planner_data.stop_reason_diag); + + debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); + } } } @@ -810,8 +848,8 @@ void ObstacleStopPlannerNode::insertVelocity( calcSignedArcLength(output, vehicle_idx, index_with_dist_remain.get().first); debug_ptr_->setDebugValues( - DebugValues::TYPE::OBSTACLE_DISTANCE, - dist_baselink_to_obstacle + index_with_dist_remain.get().second); + DebugValues::TYPE::SLOWDOWN_OBSTACLE_DISTANCE, + dist_baselink_to_obstacle + index_with_dist_remain.get().second - base_link2front); const auto slow_down_section = createSlowDownSection( index_with_dist_remain.get().first, output, planner_data.lateral_deviation, index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, @@ -942,7 +980,6 @@ void ObstacleStopPlannerNode::insertStopPoint( } stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose); - debug_ptr_->pushPose(p_insert.pose, PoseType::Stop); } StopPoint ObstacleStopPlannerNode::searchInsertPoint(