From 113ef4962a4f3298ea361c552f35d5a227ef3cc0 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 26 Jul 2022 18:06:47 +0900 Subject: [PATCH 1/3] feat(obstacle_stop_planner): add hold stop margin distance Signed-off-by: satoshi-ota --- .../include/obstacle_stop_planner/node.hpp | 13 ++++---- planning/obstacle_stop_planner/src/node.cpp | 31 +++++++++++++++++-- 2 files changed, 36 insertions(+), 8 deletions(-) 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 ccb46ae6d962a..7ef762dff3ccb 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 e5288a0d8d985..c5df3cde9348e 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); } { @@ -788,7 +793,30 @@ void ObstacleStopPlannerNode::insertVelocity( 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); + } } } @@ -940,7 +968,6 @@ void ObstacleStopPlannerNode::insertStopPoint( } stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose); - debug_ptr_->pushPose(p_insert.pose, PoseType::Stop); } StopPoint ObstacleStopPlannerNode::searchInsertPoint( From a8d4d5a8119f1a510e8da1124cec0284c4496c97 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 29 Jul 2022 08:56:58 +0900 Subject: [PATCH 2/3] feat(obstacle_stop_planner): output distance info Signed-off-by: satoshi-ota --- .../include/obstacle_stop_planner/debug_marker.hpp | 11 ++++++----- planning/obstacle_stop_planner/src/node.cpp | 14 ++++++++++++-- 2 files changed, 18 insertions(+), 7 deletions(-) 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/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index c5df3cde9348e..4bf15ee916262 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -778,6 +778,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; @@ -790,6 +792,14 @@ 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); @@ -836,8 +846,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, From 2233b26b772fda016b53f7242344f674a488c3c3 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 29 Jul 2022 13:06:03 +0900 Subject: [PATCH 3/3] feat(obstacle_stop_planner): add param into config Signed-off-by: satoshi-ota --- .../config/obstacle_stop_planner.param.yaml | 1 + 1 file changed, 1 insertion(+) 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