From b7df1dce9d17efdcb4d95daee605f122e5972862 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 22 Aug 2022 16:57:30 +0900 Subject: [PATCH] revert(behavior_velocity_planner): keep stopping in hold stop margin distance (#1433) (#1645) Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../config/stop_line.param.yaml | 4 +- .../scene_module/stop_line/manager.hpp | 12 +- .../include/scene_module/stop_line/scene.hpp | 57 ++- .../src/scene_module/stop_line/debug.cpp | 89 +++-- .../src/scene_module/stop_line/manager.cpp | 17 +- .../src/scene_module/stop_line/scene.cpp | 349 +++++++++++------- 6 files changed, 353 insertions(+), 175 deletions(-) diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml index 0118452b72b85..936bfc76cc98d 100644 --- a/planning/behavior_velocity_planner/config/stop_line.param.yaml +++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml @@ -2,6 +2,8 @@ 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 4947594aa2d72..68ad08594a70f 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,8 +29,6 @@ namespace behavior_velocity_planner { - -using autoware_auto_planning_msgs::msg::PathWithLaneId; using StopLineWithLaneId = std::pair; class StopLineModuleManager : public SceneModuleManagerInterface @@ -44,15 +42,17 @@ class StopLineModuleManager : public SceneModuleManagerInterface StopLineModule::PlannerParam planner_param_; std::vector getStopLinesWithLaneIdOnPath( - const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map); std::set getStopLineIdSetOnPath( - const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map); - void launchNewModules(const PathWithLaneId & path) override; + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const PathWithLaneId & path) override; + const autoware_auto_planning_msgs::msg::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 c39f8629ab403..d109398e33d56 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,7 +15,6 @@ #ifndef SCENE_MODULE__STOP_LINE__SCENE_HPP_ #define SCENE_MODULE__STOP_LINE__SCENE_HPP_ -#include #include #include #include @@ -27,6 +26,7 @@ #include #include #include +#include #include #include @@ -34,29 +34,46 @@ 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: @@ -65,7 +82,9 @@ class StopLineModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; @@ -73,13 +92,27 @@ class StopLineModule : public SceneModuleInterface private: int64_t module_id_; - std::shared_ptr stopped_time_; + geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); - lanelet::ConstLineString3d stop_line_; + boost::optional findCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const SearchRangeIndex & search_index); - int64_t lane_id_; + boost::optional findOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const StopLineModule::SegmentIndexWithPoint2d & collision); - // State machine + 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); + + lanelet::ConstLineString3d stop_line_; + int64_t lane_id_; 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 880264b6eef09..1b847cf1e73f1 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,53 +24,90 @@ 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; -MarkerArray StopLineModule::createDebugMarkerArray() +namespace { - MarkerArray msg; +using DebugData = StopLineModule::DebugData; - const auto now = this->clock_->now(); +visualization_msgs::msg::MarkerArray createStopLineCollisionCheck( + const DebugData & debug_data, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; - // Search stopline + // Search Segments { - 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)); + 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); + } + + // 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)); + 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; } -MarkerArray StopLineModule::createVirtualWallMarkerArray() +} // namespace + +visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() { - MarkerArray wall_marker; + 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; +} +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 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); - + 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); + } 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 e0dff1107d1bd..aaf85b5db1f5e 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,14 +29,18 @@ 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 PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) { std::vector stop_lines_with_lane_id; @@ -58,7 +62,8 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } std::set StopLineModuleManager::getStopLineIdSetOnPath( - const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) { std::set stop_line_id_set; @@ -69,7 +74,8 @@ std::set StopLineModuleManager::getStopLineIdSetOnPath( return stop_line_id_set; } -void StopLineModuleManager::launchNewModules(const PathWithLaneId & path) +void StopLineModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { @@ -84,7 +90,8 @@ void StopLineModuleManager::launchNewModules(const PathWithLaneId & path) } std::function &)> -StopLineModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) +StopLineModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::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 2889b81e0971f..3cd08f1f5b6e2 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,63 +21,73 @@ 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 { -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()) +double calcYawFromPoints( + const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back) { - 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; - } - } + return std::atan2(p_back.y - p_front.y, p_back.x - p_front.x); +} - if (found_max_num) { - break; +geometry_msgs::msg::Pose calcInterpolatedPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const StopLineModule::SegmentIndexWithOffset & offset_segment) +{ + // 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; +} + +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}; } } - 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; + // No enough path length + return {}; } + } // namespace StopLineModule::StopLineModule( @@ -93,110 +103,199 @@ StopLineModule::StopLineModule( planner_param_ = planner_param; } -bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +boost::optional StopLineModule::findCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const SearchRangeIndex & search_index) { - 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); - - const auto ego_path = *path; - const auto & ego_pos = planner_data_->current_pose.pose.position; - - const auto stop_line = planning_utils::extendLine( - stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + 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); + } + } - 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 auto intersects = getLinestringIntersects(ego_path, stop_line, ego_pos, 1); + // 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); - if (intersects.empty()) { - return false; - } + // Ignore if no collision found + if (collision_points.empty()) { + continue; + } - 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); + // Select first collision + const auto & collision_point = collision_points.at(0); - if (!stop_pose) { - return false; + return StopLineModule::SegmentIndexWithPoint2d{i, collision_point}; } - StopFactor stop_factor; - stop_factor.stop_pose = stop_pose.get(); - stop_factor.stop_factor_points.push_back(p_stop_line); - - /** - * @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); + return {}; +} - switch (state_) { - case State::APPROACH: { - planning_utils::insertStopPoint(stop_pose.get().position, *path); - planning_utils::appendStopReason(stop_factor, stop_reason); +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; - debug_data_.stop_pose = stop_pose.get(); + const auto & p_back = to_bg2d(path.points.at(collision.index + 1).point.pose.position); - if ( - signed_arc_dist_to_stop_point < planner_param_.hold_stop_margin_distance && - planner_data_->isVehicleStopped()) { - RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + return findBackwardOffsetSegment( + path, collision.index + 1, base_backward_length + bg::distance(p_back, collision.point)); +} - state_ = State::STOPPED; - stopped_time_ = std::make_shared(clock_->now()); +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}; + } - 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"); - } - } + return StopLineModule::SegmentIndexWithPose{ + offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; +} - break; - } +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); + } - case State::STOPPED: { - const auto ego_pos_on_path = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, 0.0); + return modified_path; +} - if (!ego_pos_on_path) { - break; - } +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); - planning_utils::insertStopPoint(ego_pos_on_path.get(), *path); - planning_utils::appendStopReason(stop_factor, stop_reason); + 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); - debug_data_.stop_pose = stop_pose.get(); + // Find offset segment + const auto offset_segment = findOffsetSegment(*path, *collision); - const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); + // Calculate stop pose and insert index + const auto stop_pose_with_index = calcStopPose(*path, offset_segment); - if (planner_param_.stop_duration_sec < elapsed_time) { - RCLCPP_INFO(logger_, "STOPPED -> START"); - state_ = State::START; + 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"); } - - break; } - - 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; - } + } 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; } - - break; } - - default: - RCLCPP_ERROR(logger_, "Unknown state."); } return true; } + +geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( + const lanelet::ConstLineString3d & stop_line) +{ + 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; +} } // namespace behavior_velocity_planner