diff --git a/planning/behavior_velocity_planner/config/detection_area.param.yaml b/planning/behavior_velocity_planner/config/detection_area.param.yaml index 9174045bf0150..63ca5fc7d96a2 100644 --- a/planning/behavior_velocity_planner/config/detection_area.param.yaml +++ b/planning/behavior_velocity_planner/config/detection_area.param.yaml @@ -6,3 +6,4 @@ dead_line_margin: 5.0 use_pass_judge_line: false state_clear_time: 2.0 + hold_stop_margin_distance: 0.0 diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index ae0718c2eaece..95bbfb0faff05 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -36,6 +36,9 @@ namespace behavior_velocity_planner using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; class DetectionAreaModule : public SceneModuleInterface { @@ -58,6 +61,7 @@ class DetectionAreaModule : public SceneModuleInterface double dead_line_margin; bool use_pass_judge_line; double state_clear_time; + double hold_stop_margin_distance; }; public: @@ -66,9 +70,7 @@ class DetectionAreaModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) override; + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; @@ -81,19 +83,16 @@ class DetectionAreaModule : public SceneModuleInterface bool canClearStopState() const; bool isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + const PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose) const; bool hasEnoughBrakingDistance( const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const PathIndexWithPose & stop_point) const; + void insertStopPoint(const geometry_msgs::msg::Pose & stop_pose, PathWithLaneId & path) const; boost::optional createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const double margin) const; + const PathWithLaneId & path, const LineString2d & stop_line, const double margin) const; // Key Feature const lanelet::autoware::DetectionArea & detection_area_reg_elem_; diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index 8698ee51f3391..f9bee714d7aff 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -37,6 +37,8 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) planner_param_.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 5.0); planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false); planner_param_.state_clear_time = node.declare_parameter(ns + ".state_clear_time", 2.0); + planner_param_.hold_stop_margin_distance = + node.declare_parameter(ns + ".hold_stop_margin_distance", 0.0); } void DetectionAreaModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 61e32190a077d..6d230638d30ee 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -30,6 +30,10 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestSegmentIndex; +using motion_utils::insertTargetPoint; namespace { @@ -74,7 +78,7 @@ boost::optional getNearestCollisionPoint( } boost::optional findCollisionSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line) + const PathWithLaneId & path, const LineString2d & stop_line) { for (size_t i = 0; i < path.points.size() - 1; ++i) { const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point @@ -92,8 +96,7 @@ boost::optional findCollisionSegment( } boost::optional findForwardOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, - const double offset_length) + const PathWithLaneId & path, const size_t base_idx, const double offset_length) { double sum_length = 0.0; for (size_t i = base_idx; i < path.points.size() - 1; ++i) { @@ -115,8 +118,7 @@ boost::optional findForwardOffsetSegment( } boost::optional findBackwardOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, - const double offset_length) + const PathWithLaneId & path, const size_t base_idx, const double offset_length) { double sum_length = 0.0; const auto start = static_cast(base_idx) - 1; @@ -138,8 +140,8 @@ boost::optional findBackwardOffsetSegment( } boost::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const PathIndexWithPoint2d & collision_segment, const double offset_length) + const PathWithLaneId & path, const PathIndexWithPoint2d & collision_segment, + const double offset_length) { const size_t & collision_idx = collision_segment.first; const Point2d & collision_point = collision_segment.second; @@ -156,8 +158,7 @@ boost::optional findOffsetSegment( } geometry_msgs::msg::Pose calcTargetPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const PathIndexWithOffset & offset_segment) + const PathWithLaneId & path, const PathIndexWithOffset & offset_segment) { const size_t offset_idx = offset_segment.first; const double remain_offset_length = offset_segment.second; @@ -208,9 +209,7 @@ LineString2d DetectionAreaModule::getStopLineGeometry2d() const stop_line[0], stop_line[1], planner_data_->stop_line_extend_length); } -bool DetectionAreaModule::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) +bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { // Store original path const auto original_path = *path; @@ -218,8 +217,7 @@ bool DetectionAreaModule::modifyPathVelocity( // Reset data debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - *stop_reason = - planning_utils::initializeStopReason(tier4_planning_msgs::msg::StopReason::DETECTION_AREA); + *stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA); // Find obstacles in detection area const auto obstacle_points = getObstaclePoints(); @@ -240,10 +238,24 @@ bool DetectionAreaModule::modifyPathVelocity( return true; } - const auto & stop_pose = stop_point->second; + const auto is_stopped = planner_data_->isVehicleStopped(0.0); + + auto stop_pose = stop_point->second; + const auto stop_dist = calcSignedArcLength(path->points, self_pose.position, stop_pose.position); - setDistance( - motion_utils::calcSignedArcLength(path->points, self_pose.position, stop_pose.position)); + // Don't re-approach when the ego stops closer to the stop point than hold_stop_margin_distance + if (is_stopped && stop_dist < planner_param_.hold_stop_margin_distance) { + const auto ego_pos_on_path = + calcLongitudinalOffsetPose(original_path.points, self_pose.position, 0.0); + + if (!ego_pos_on_path) { + return false; + } + + stop_pose = ego_pos_on_path.get(); + } + + setDistance(stop_dist); // Check state setSafe(canClearStopState()); @@ -272,14 +284,14 @@ bool DetectionAreaModule::modifyPathVelocity( } // Ignore objects detected after stop_line if not in STOP state - if (state_ != State::STOP && isOverLine(original_path, self_pose, stop_pose)) { + if (state_ != State::STOP && isOverLine(original_path, self_pose, stop_point->second)) { setSafe(true); return true; } // Ignore objects if braking distance is not enough if (planner_param_.use_pass_judge_line) { - if (state_ != State::STOP && !hasEnoughBrakingDistance(self_pose, stop_pose)) { + if (state_ != State::STOP && !hasEnoughBrakingDistance(self_pose, stop_point->second)) { RCLCPP_WARN_THROTTLE( logger_, *clock_, std::chrono::milliseconds(1000).count(), "[detection_area] vehicle is over stop border"); @@ -290,14 +302,14 @@ bool DetectionAreaModule::modifyPathVelocity( // Insert stop point state_ = State::STOP; - *path = insertStopPoint(original_path, *stop_point); + insertStopPoint(stop_pose, *path); // For virtual wall - debug_data_.stop_poses.push_back(stop_pose); + debug_data_.stop_poses.push_back(stop_point->second); // Create StopReason { - tier4_planning_msgs::msg::StopFactor stop_factor; + StopFactor stop_factor{}; stop_factor.stop_pose = stop_point->second; stop_factor.stop_factor_points = obstacle_points; planning_utils::appendStopReason(stop_factor, stop_reason); @@ -310,7 +322,7 @@ bool DetectionAreaModule::modifyPathVelocity( if ( !first_stop_path_point_index_ || static_cast(insert_idx) < first_stop_path_point_index_) { - debug_data_.first_stop_pose = stop_pose; + debug_data_.first_stop_pose = stop_point->second; first_stop_path_point_index_ = static_cast(insert_idx); } } @@ -360,10 +372,10 @@ bool DetectionAreaModule::canClearStopState() const } bool DetectionAreaModule::isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const + const PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose) const { - return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) < 0; + return calcSignedArcLength(path.points, self_pose.position, line_pose.position) < 0; } bool DetectionAreaModule::hasEnoughBrakingDistance( @@ -379,29 +391,23 @@ bool DetectionAreaModule::hasEnoughBrakingDistance( return calcSignedDistance(self_pose, line_pose.position) > pass_judge_line_distance; } -autoware_auto_planning_msgs::msg::PathWithLaneId DetectionAreaModule::insertStopPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const PathIndexWithPose & stop_point) const +void DetectionAreaModule::insertStopPoint( + const geometry_msgs::msg::Pose & stop_pose, PathWithLaneId & path) const { - auto output_path = path; + const size_t base_idx = findNearestSegmentIndex(path.points, stop_pose.position); + const auto insert_idx = insertTargetPoint(base_idx, stop_pose.position, path.points); - size_t insert_idx = static_cast(stop_point.first + 1); - const auto stop_pose = stop_point.second; - - // To PathPointWithLaneId - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; - stop_point_with_lane_id = output_path.points.at(insert_idx); - stop_point_with_lane_id.point.pose = stop_pose; - stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + if (!insert_idx) { + return; + } - // Insert stop point or replace with zero velocity - planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx); - return output_path; + for (size_t i = insert_idx.get(); i < path.points.size(); ++i) { + path.points.at(i).point.longitudinal_velocity_mps = 0.0; + } } boost::optional DetectionAreaModule::createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const double margin) const + const PathWithLaneId & path, const LineString2d & stop_line, const double margin) const { // Find collision segment const auto collision_segment = findCollisionSegment(path, stop_line);