diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml index 936bfc76cc98d..0118452b72b85 100644 --- a/planning/behavior_velocity_planner/config/stop_line.param.yaml +++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml @@ -2,8 +2,6 @@ ros__parameters: stop_line: stop_margin: 0.0 - stop_check_dist: 2.0 stop_duration_sec: 1.0 + hold_stop_margin_distance: 2.0 use_initialization_stop_line_state: true - debug: - show_stopline_collision_check: false # [-] whether to show stopline collision diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp index 68ad08594a70f..4947594aa2d72 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp @@ -29,6 +29,8 @@ namespace behavior_velocity_planner { + +using autoware_auto_planning_msgs::msg::PathWithLaneId; using StopLineWithLaneId = std::pair; class StopLineModuleManager : public SceneModuleManagerInterface @@ -42,17 +44,15 @@ class StopLineModuleManager : public SceneModuleManagerInterface StopLineModule::PlannerParam planner_param_; std::vector getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map); + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); std::set getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map); + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const PathWithLaneId & path) override; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp index d109398e33d56..096410385d00e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp @@ -15,6 +15,7 @@ #ifndef SCENE_MODULE__STOP_LINE__SCENE_HPP_ #define SCENE_MODULE__STOP_LINE__SCENE_HPP_ +#include #include #include #include @@ -26,7 +27,6 @@ #include #include #include -#include #include #include @@ -34,46 +34,29 @@ namespace behavior_velocity_planner { + +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; + class StopLineModule : public SceneModuleInterface { - using StopLineWithLaneId = std::pair; - public: enum class State { APPROACH, STOPPED, START }; - struct SegmentIndexWithPose - { - size_t index; - geometry_msgs::msg::Pose pose; - }; - - struct SegmentIndexWithPoint2d - { - size_t index; - Point2d point; - }; - - struct SegmentIndexWithOffset - { - size_t index; - double offset; - }; - struct DebugData { double base_link2front; boost::optional stop_pose; - std::vector search_segments; LineString2d search_stopline; }; struct PlannerParam { double stop_margin; - double stop_check_dist; double stop_duration_sec; + double hold_stop_margin_distance; bool use_initialization_stop_line_state; - bool show_stopline_collision_check; }; public: @@ -82,9 +65,7 @@ class StopLineModule : 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; @@ -92,27 +73,15 @@ class StopLineModule : public SceneModuleInterface private: int64_t module_id_; - geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); - - boost::optional findCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index); + void insertStopPoint(const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const; - boost::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPoint2d & collision); - - boost::optional calcStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const boost::optional & offset_segment); - - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPose & insert_index_with_pose, - tier4_planning_msgs::msg::StopReason * stop_reason); + std::shared_ptr stopped_time_; lanelet::ConstLineString3d stop_line_; + int64_t lane_id_; + + // State machine State state_; // Parameter diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp index 1b847cf1e73f1..880264b6eef09 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp @@ -24,90 +24,53 @@ namespace behavior_velocity_planner { + +using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; -namespace -{ -using DebugData = StopLineModule::DebugData; - -visualization_msgs::msg::MarkerArray createStopLineCollisionCheck( - const DebugData & debug_data, const int64_t module_id) +MarkerArray StopLineModule::createDebugMarkerArray() { - visualization_msgs::msg::MarkerArray msg; + MarkerArray msg; - // Search Segments - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "search_segments"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - for (const auto & e : debug_data.search_segments) { - marker.points.push_back( - geometry_msgs::build().x(e.at(0).x()).y(e.at(0).y()).z(0.0)); - marker.points.push_back( - geometry_msgs::build().x(e.at(1).x()).y(e.at(1).y()).z(0.0)); - } - marker.scale = createMarkerScale(0.1, 0.1, 0.1); - marker.color = createMarkerColor(0.0, 0.0, 1.0, 0.999); - msg.markers.push_back(marker); - } + const auto now = this->clock_->now(); // Search stopline { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "search_stopline"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - const auto p0 = debug_data.search_stopline.at(0); - marker.points.push_back( - geometry_msgs::build().x(p0.x()).y(p0.y()).z(0.0)); - const auto p1 = debug_data.search_stopline.at(1); - marker.points.push_back( - geometry_msgs::build().x(p1.x()).y(p1.y()).z(0.0)); + auto marker = createDefaultMarker( + "map", now, "search_stopline", module_id_, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + const auto line = debug_data_.search_stopline; + if (!line.empty()) { + marker.points.push_back(createPoint(line.front().x(), line.front().y(), 0.0)); + marker.points.push_back(createPoint(line.back().x(), line.back().y(), 0.0)); + } - marker.scale = createMarkerScale(0.1, 0.1, 0.1); - marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.999); msg.markers.push_back(marker); } return msg; } -} // namespace - -visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() +MarkerArray StopLineModule::createVirtualWallMarkerArray() { - visualization_msgs::msg::MarkerArray debug_marker_array; - if (planner_param_.show_stopline_collision_check) { - appendMarkerArray( - createStopLineCollisionCheck(debug_data_, module_id_), &debug_marker_array, - this->clock_->now()); - } - return debug_marker_array; -} + MarkerArray wall_marker; -visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArray() -{ - const auto now = this->clock_->now(); - visualization_msgs::msg::MarkerArray wall_marker; if (!debug_data_.stop_pose) { return wall_marker; } - const auto p_front = tier4_autoware_utils::calcOffsetPose( - *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); - if (state_ == State::APPROACH) { - appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p_front, "stopline", now, module_id_), &wall_marker, - now); - } + + const auto now = this->clock_->now(); + + const auto p = calcOffsetPose(*debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); + appendMarkerArray(createStopVirtualWallMarker(p, "stopline", now, module_id_), &wall_marker); + return wall_marker; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp index aaf85b5db1f5e..e0dff1107d1bd 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp @@ -29,18 +29,14 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) const std::string ns(getModuleName()); auto & p = planner_param_; p.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0); - p.stop_check_dist = node.declare_parameter(ns + ".stop_check_dist", 2.0); p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0); + p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance", 2.0); p.use_initialization_stop_line_state = node.declare_parameter(ns + ".use_initialization_stop_line_state", false); - // debug - p.show_stopline_collision_check = - node.declare_parameter(ns + ".debug.show_stopline_collision_check", false); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::vector stop_lines_with_lane_id; @@ -62,8 +58,7 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } std::set StopLineModuleManager::getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::set stop_line_id_set; @@ -74,8 +69,7 @@ std::set StopLineModuleManager::getStopLineIdSetOnPath( return stop_line_id_set; } -void StopLineModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void StopLineModuleManager::launchNewModules(const PathWithLaneId & path) { for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { @@ -90,8 +84,7 @@ void StopLineModuleManager::launchNewModules( } std::function &)> -StopLineModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +StopLineModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index 3cd08f1f5b6e2..57cfb42cff38c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -21,73 +21,63 @@ namespace behavior_velocity_planner { + namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Line = bg::model::linestring; +using motion_utils::calcLongitudinalOffsetPoint; +using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcSignedArcLength; +using motion_utils::insertTargetPoint; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getPose; namespace { -double calcYawFromPoints( - const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back) -{ - return std::atan2(p_back.y - p_front.y, p_back.x - p_front.x); -} - -geometry_msgs::msg::Pose calcInterpolatedPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithOffset & offset_segment) +std::vector getLinestringIntersects( + const PathWithLaneId & ego_path, const Line & linestring, + const geometry_msgs::msg::Point & ego_pos, + const size_t max_num = std::numeric_limits::max()) { - // Get segment points - const auto & p_front = path.points.at(offset_segment.index).point.pose.position; - const auto & p_back = path.points.at(offset_segment.index + 1).point.pose.position; - - // To Eigen point - const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); - const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); - - // Calculate interpolation ratio - const auto interpolate_ratio = offset_segment.offset / (p_eigen_back - p_eigen_front).norm(); - - // Add offset to front point - const auto interpolated_point_2d = - p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); - const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); - - // Calculate orientation so that X-axis would be along the trajectory - tf2::Quaternion quat; - quat.setRPY(0, 0, calcYawFromPoints(p_front, p_back)); - - // To Pose - geometry_msgs::msg::Pose interpolated_pose; - interpolated_pose.position.x = interpolated_point_2d.x(); - interpolated_pose.position.y = interpolated_point_2d.y(); - interpolated_pose.position.z = interpolated_z; - interpolated_pose.orientation = tf2::toMsg(quat); - - return interpolated_pose; -} + std::vector intersects{}; + + bool found_max_num = false; + for (size_t i = 0; i < ego_path.points.size() - 1; ++i) { + const auto & p_back = ego_path.points.at(i).point.pose.position; + const auto & p_front = ego_path.points.at(i + 1).point.pose.position; + const Line segment{{p_back.x, p_back.y}, {p_front.x, p_front.y}}; + + std::vector tmp_intersects{}; + bg::intersection(segment, linestring, tmp_intersects); + + for (const auto & p : tmp_intersects) { + intersects.push_back(p); + if (intersects.size() == max_num) { + found_max_num = true; + break; + } + } -boost::optional findBackwardOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, - const double offset_length) -{ - double sum_length = 0.0; - const auto start = static_cast(base_idx) - 1; - for (std::int32_t i = start; i >= 0; --i) { - const auto p_front = to_bg2d(path.points.at(i).point.pose.position); - const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position); - - sum_length += bg::distance(p_front, p_back); - - // If it's over offset point, return front index and remain offset length - if (sum_length >= offset_length) { - const auto k = static_cast(i); - return StopLineModule::SegmentIndexWithOffset{k, sum_length - offset_length}; + if (found_max_num) { + break; } } - // No enough path length - return {}; -} + const auto compare = [&](const Point & p1, const Point & p2) { + const auto dist_l1 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + + const auto dist_l2 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + + return dist_l1 < dist_l2; + }; + + std::sort(intersects.begin(), intersects.end(), compare); + return intersects; +} } // namespace StopLineModule::StopLineModule( @@ -103,199 +93,125 @@ StopLineModule::StopLineModule( planner_param_ = planner_param; } -boost::optional StopLineModule::findCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index) +bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - const size_t min_search_index = std::max(static_cast(0), search_index.min_idx); - const size_t max_search_index = std::min(search_index.max_idx, path.points.size() - 1); - - // for creating debug marker - if (planner_param_.show_stopline_collision_check) { - debug_data_.search_stopline = stop_line; - for (size_t i = min_search_index; i < max_search_index; ++i) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - debug_data_.search_segments.push_back(path_segment); - } - } + const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + debug_data_ = DebugData(); + debug_data_.base_link2front = base_link2front; + *stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE); - for (size_t i = min_search_index; i < max_search_index; ++i) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; + const auto ego_path = *path; + const auto & ego_pos = planner_data_->current_pose.pose.position; - // Find intersection - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(stop_line, path_segment, collision_points); + const auto stop_line = planning_utils::extendLine( + stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); - // Ignore if no collision found - if (collision_points.empty()) { - continue; - } + debug_data_.search_stopline = stop_line; - // Select first collision - const auto & collision_point = collision_points.at(0); + const auto intersects = getLinestringIntersects(ego_path, stop_line, ego_pos, 1); - return StopLineModule::SegmentIndexWithPoint2d{i, collision_point}; + if (intersects.empty()) { + return false; } - return {}; -} + const auto p_stop_line = createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z); + const auto margin = planner_param_.stop_margin + base_link2front; + const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop_line, -margin); -boost::optional StopLineModule::findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPoint2d & collision) -{ - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto base_backward_length = planner_param_.stop_margin + base_link2front; + if (!stop_pose) { + return false; + } - const auto & p_back = to_bg2d(path.points.at(collision.index + 1).point.pose.position); + StopFactor stop_factor; + stop_factor.stop_pose = stop_pose.get(); + stop_factor.stop_factor_points.push_back(p_stop_line); - return findBackwardOffsetSegment( - path, collision.index + 1, base_backward_length + bg::distance(p_back, collision.point)); -} + /** + * @brief : calculate signed arc length consider stop margin from stop line + * + * |----------------------------| + * s---ego----------x--|--------g + */ + const auto signed_arc_dist_to_stop_point = + calcSignedArcLength(ego_path.points, ego_pos, stop_pose.get().position); -boost::optional StopLineModule::calcStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const boost::optional & offset_segment) -{ - // If no stop point found due to out of range, use front point on path - if (!offset_segment) { - return StopLineModule::SegmentIndexWithPose{0, path.points.front().point.pose}; - } + switch (state_) { + case State::APPROACH: { + insertStopPoint(stop_pose.get().position, *path); + planning_utils::appendStopReason(stop_factor, stop_reason); - return StopLineModule::SegmentIndexWithPose{ - offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; -} + debug_data_.stop_pose = stop_pose.get(); -autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPose & stop_pose_with_index, - tier4_planning_msgs::msg::StopReason * stop_reason) -{ - auto modified_path = path; - - // Insert stop pose to between segment start and end - size_t insert_index = static_cast(stop_pose_with_index.index + 1); - auto stop_point_with_lane_id = modified_path.points.at(insert_index); - stop_point_with_lane_id.point.pose = stop_pose_with_index.pose; - stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - - // Update first stop index - first_stop_path_point_index_ = static_cast(insert_index); - debug_data_.stop_pose = stop_point_with_lane_id.point.pose; - - // Insert stop point or replace with zero velocity - planning_utils::insertVelocity(modified_path, stop_point_with_lane_id, 0.0, insert_index); - - // Get stop point and stop factor - { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_point_with_lane_id.point.pose; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); - } + if ( + signed_arc_dist_to_stop_point < planner_param_.hold_stop_margin_distance && + planner_data_->isVehicleStopped()) { + RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); - return modified_path; -} + state_ = State::STOPPED; + stopped_time_ = std::make_shared(clock_->now()); -bool StopLineModule::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) -{ - debug_data_ = DebugData(); - if (path->points.empty()) return true; - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - debug_data_.base_link2front = base_link2front; - first_stop_path_point_index_ = static_cast(path->points.size()) - 1; - *stop_reason = - planning_utils::initializeStopReason(tier4_planning_msgs::msg::StopReason::STOP_LINE); + if (signed_arc_dist_to_stop_point < -planner_param_.hold_stop_margin_distance) { + RCLCPP_ERROR( + logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED"); + } + } - const LineString2d stop_line = planning_utils::extendLine( - stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); - const auto & current_position = planner_data_->current_pose.pose.position; - const PointWithSearchRangeIndex src_point_with_search_range_index = - planning_utils::findFirstNearSearchRangeIndex(path->points, current_position); - SearchRangeIndex dst_search_range = - planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_); - - // extend following and previous search range to avoid no collision - if (dst_search_range.max_idx < path->points.size() - 1) dst_search_range.max_idx++; - if (dst_search_range.min_idx > 0) dst_search_range.min_idx--; - - // Find collision - const auto collision = findCollision(*path, stop_line, dst_search_range); - - // If no collision found, do nothing - if (!collision) { - RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); - return true; - } - const double center_line_z = (stop_line_[0].z() + stop_line_[1].z()) / 2.0; - const auto stop_line_position = planning_utils::toRosPoint(collision->point, center_line_z); + break; + } - // Find offset segment - const auto offset_segment = findOffsetSegment(*path, *collision); + case State::STOPPED: { + const auto ego_pos_on_path = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, 0.0); - // Calculate stop pose and insert index - const auto stop_pose_with_index = calcStopPose(*path, offset_segment); + if (!ego_pos_on_path) { + break; + } - const PointWithSearchRangeIndex dst_point_with_search_range_index = { - stop_line_position, dst_search_range}; - const double stop_line_margin = base_link2front + planner_param_.stop_margin; - /** - * @brief : calculate signed arc length consider stop margin from stop line - * - * |----------------------------| - * s---ego----------x--|--------g - */ - const double signed_arc_dist_to_stop_point = - planning_utils::calcSignedArcLengthWithSearchIndex( - path->points, src_point_with_search_range_index, dst_point_with_search_range_index) - - stop_line_margin; - if (state_ == State::APPROACH) { - // Insert stop pose - *path = insertStopPose(*path, *stop_pose_with_index, stop_reason); - - // Move to stopped state if stopped - if ( - signed_arc_dist_to_stop_point < planner_param_.stop_check_dist && - planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { - RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); - state_ = State::STOPPED; - if (signed_arc_dist_to_stop_point < -planner_param_.stop_check_dist) { - RCLCPP_ERROR( - logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED"); + insertStopPoint(ego_pos_on_path.get(), *path); + planning_utils::appendStopReason(stop_factor, stop_reason); + + debug_data_.stop_pose = stop_pose.get(); + + const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); + + if (planner_param_.stop_duration_sec < elapsed_time) { + RCLCPP_INFO(logger_, "STOPPED -> START"); + state_ = State::START; } + + break; } - } else if (state_ == State::STOPPED) { - // Change state after vehicle departure - if (!planner_data_->isVehicleStopped()) { - RCLCPP_INFO(logger_, "STOPPED -> START"); - state_ = State::START; - } - } else if (state_ == State::START) { - // Initialize if vehicle is far from stop_line - if (planner_param_.use_initialization_stop_line_state) { - if (signed_arc_dist_to_stop_point > planner_param_.stop_check_dist) { - RCLCPP_INFO(logger_, "START -> APPROACH"); - state_ = State::APPROACH; + + case State::START: { + // Initialize if vehicle is far from stop_line + if (planner_param_.use_initialization_stop_line_state) { + if (signed_arc_dist_to_stop_point > planner_param_.hold_stop_margin_distance) { + RCLCPP_INFO(logger_, "START -> APPROACH"); + state_ = State::APPROACH; + } } + + break; } + + default: + RCLCPP_ERROR(logger_, "Unknown state."); } return true; } -geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( - const lanelet::ConstLineString3d & stop_line) +void StopLineModule::insertStopPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const { - geometry_msgs::msg::Point center_point; - center_point.x = (stop_line[0].x() + stop_line[1].x()) / 2.0; - center_point.y = (stop_line[0].y() + stop_line[1].y()) / 2.0; - center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; - return center_point; + const size_t base_idx = findNearestSegmentIndex(path.points, stop_point); + const auto insert_idx = insertTargetPoint(base_idx, stop_point, path.points); + + if (!insert_idx) { + return; + } + + for (size_t i = insert_idx.get(); i < path.points.size(); ++i) { + path.points.at(i).point.longitudinal_velocity_mps = 0.0; + } } } // namespace behavior_velocity_planner