From a4b97d113fec8ff4d9a53ab0276797bf8e89ffc3 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 25 Jul 2022 10:37:46 +0900 Subject: [PATCH 1/8] fix(behavior_velocity_planner): use alias Signed-off-by: satoshi-ota --- .../scene_module/stop_line/manager.hpp | 8 +++-- .../include/scene_module/stop_line/scene.hpp | 23 ++++++++------- .../src/scene_module/stop_line/manager.cpp | 12 +++----- .../src/scene_module/stop_line/scene.cpp | 29 +++++++------------ 4 files changed, 33 insertions(+), 39 deletions(-) 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 b2e2145966345..65271c2a9a422 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 @@ -26,6 +26,9 @@ namespace behavior_velocity_planner { + +using autoware_auto_planning_msgs::msg::PathWithLaneId; + class StopLineModuleManager : public SceneModuleManagerInterface { public: @@ -35,10 +38,11 @@ class StopLineModuleManager : public SceneModuleManagerInterface private: StopLineModule::PlannerParam planner_param_; - 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 2fd33bc91920e..3b2d76f343670 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 @@ -34,6 +34,11 @@ 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; @@ -79,9 +84,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; @@ -91,21 +94,19 @@ class StopLineModule : public SceneModuleInterface 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 PathWithLaneId & path, const LineString2d & stop_line, const SearchRangeIndex & search_index); boost::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPoint2d & collision); + const PathWithLaneId & path, const StopLineModule::SegmentIndexWithPoint2d & collision); boost::optional calcStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const 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); + PathWithLaneId insertStopPose( + const PathWithLaneId & path, + const StopLineModule::SegmentIndexWithPose & insert_index_with_pose, StopReason * stop_reason); lanelet::ConstLineString3d stop_line_; int64_t lane_id_; 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 2d6d5acb5d96d..27dbd004bf428 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 @@ -48,8 +48,7 @@ TrafficSignsWithLaneId getTrafficSignRegElemsOnPath( } 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::vector stop_lines_with_lane_id; @@ -71,8 +70,7 @@ std::vector getStopLinesWithLaneIdOnPath( } std::set 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; @@ -97,8 +95,7 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".use_initialization_stop_line_state", false); } -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())) { @@ -113,8 +110,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 cee419d4f99f5..1fb6af4e629b1 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 @@ -32,8 +32,7 @@ double calcYawFromPoints( } geometry_msgs::msg::Pose calcInterpolatedPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithOffset & offset_segment) + const PathWithLaneId & path, const StopLineModule::SegmentIndexWithOffset & offset_segment) { // Get segment points const auto & p_front = path.points.at(offset_segment.index).point.pose.position; @@ -66,8 +65,7 @@ geometry_msgs::msg::Pose calcInterpolatedPose( } 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; @@ -104,7 +102,7 @@ StopLineModule::StopLineModule( } boost::optional StopLineModule::findCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const PathWithLaneId & path, const LineString2d & stop_line, const SearchRangeIndex & search_index) { const size_t min_search_index = std::max(static_cast(0), search_index.min_idx); @@ -133,8 +131,7 @@ boost::optional StopLineModule::findCol } boost::optional StopLineModule::findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPoint2d & collision) + const 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; @@ -146,7 +143,7 @@ boost::optional StopLineModule::findOffs } boost::optional StopLineModule::calcStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const PathWithLaneId & path, const boost::optional & offset_segment) { // If no stop point found due to out of range, use front point on path @@ -158,10 +155,9 @@ boost::optional StopLineModule::calcStopPo offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; } -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) +PathWithLaneId StopLineModule::insertStopPose( + const PathWithLaneId & path, const StopLineModule::SegmentIndexWithPose & stop_pose_with_index, + StopReason * stop_reason) { auto modified_path = path; @@ -180,7 +176,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose( // Get stop point and stop factor { - tier4_planning_msgs::msg::StopFactor stop_factor; + 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); @@ -189,17 +185,14 @@ autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose( return modified_path; } -bool StopLineModule::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) +bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, 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); + *stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE); const LineString2d stop_line = planning_utils::extendLine( stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); From bd533666ebbf9736069c58e7b68e519082cc1aaf Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 25 Jul 2022 15:12:12 +0900 Subject: [PATCH 2/8] fix(behavior_velocity_planner): add hold stop margin Signed-off-by: satoshi-ota --- .../include/scene_module/stop_line/scene.hpp | 8 +- .../src/scene_module/stop_line/scene.cpp | 146 ++++++++++-------- 2 files changed, 83 insertions(+), 71 deletions(-) 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 3b2d76f343670..2ebfe1e086520 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 @@ -104,12 +105,13 @@ class StopLineModule : public SceneModuleInterface const PathWithLaneId & path, const boost::optional & offset_segment); - PathWithLaneId insertStopPose( - const PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPose & insert_index_with_pose, StopReason * stop_reason); + void insertStopPoint(const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const; + + std::shared_ptr stopped_time_; lanelet::ConstLineString3d stop_line_; int64_t lane_id_; + State state_; // Parameter 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 1fb6af4e629b1..2e254ca78540e 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,7 +21,12 @@ namespace behavior_velocity_planner { + namespace bg = boost::geometry; +using motion_utils::calcSignedArcLength; +using motion_utils::insertTargetPoint; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getPose; namespace { @@ -155,50 +160,16 @@ boost::optional StopLineModule::calcStopPo offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; } -PathWithLaneId StopLineModule::insertStopPose( - const PathWithLaneId & path, const StopLineModule::SegmentIndexWithPose & stop_pose_with_index, - 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 - { - 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); - } - - return modified_path; -} - bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, 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_ = DebugData(); debug_data_.base_link2front = base_link2front; - first_stop_path_point_index_ = static_cast(path->points.size()) - 1; *stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE); 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); const SearchRangeIndex dst_search_range = planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_); @@ -209,8 +180,6 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop if (!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); // Find offset segment const auto offset_segment = findOffsetSegment(*path, *collision); @@ -218,53 +187,94 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop // Calculate stop pose and insert index const auto stop_pose_with_index = calcStopPose(*path, offset_segment); - 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"); + const auto stop_point = getPoint(stop_pose_with_index.get().pose); + const auto signed_arc_dist_to_stop_point = + calcSignedArcLength(path->points, current_position, stop_point); + + switch (state_) { + case State::APPROACH: { + if (!stop_pose_with_index) { + break; } + // Insert stop pose + insertStopPoint(stop_point, *path); + + debug_data_.stop_pose = stop_pose_with_index.get().pose; + + // Move to stopped state if stopped + if ( + signed_arc_dist_to_stop_point < planner_param_.stop_check_dist && + planner_data_->isVehicleStopped()) { + RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + + state_ = State::STOPPED; + stopped_time_ = std::make_shared(clock_->now()); + + 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; } - } else if (state_ == State::STOPPED) { - // Change state after vehicle departure - if (!planner_data_->isVehicleStopped()) { - RCLCPP_INFO(logger_, "STOPPED -> START"); - state_ = State::START; + + case State::STOPPED: { + // Change state after vehicle departure + insertStopPoint(current_position, *path); + + debug_data_.stop_pose = stop_pose_with_index.get().pose; + + 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::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_.stop_check_dist) { + RCLCPP_INFO(logger_, "START -> APPROACH"); + state_ = State::APPROACH; + } } + + break; } + + default: + RCLCPP_ERROR(logger_, "Unknown state."); } return true; } +void StopLineModule::insertStopPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const +{ + 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; + } +} + geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( const lanelet::ConstLineString3d & stop_line) { From bbe5d6eea2255500662a89ac8b87e7b14fc028c9 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 26 Jul 2022 08:46:21 +0900 Subject: [PATCH 3/8] fix(behavior_velocity_planner): use motion_utils Signed-off-by: satoshi-ota --- .../include/scene_module/stop_line/scene.hpp | 25 +-- .../src/scene_module/stop_line/scene.cpp | 144 +++--------------- 2 files changed, 27 insertions(+), 142 deletions(-) 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 2ebfe1e086520..0e2055e5ed6e0 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 @@ -47,24 +47,12 @@ class StopLineModule : public SceneModuleInterface 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; @@ -92,33 +80,26 @@ class StopLineModule : public SceneModuleInterface private: int64_t module_id_; - geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); - boost::optional findCollision( const PathWithLaneId & path, const LineString2d & stop_line, const SearchRangeIndex & search_index); - boost::optional findOffsetSegment( - const PathWithLaneId & path, const StopLineModule::SegmentIndexWithPoint2d & collision); - - boost::optional calcStopPose( - const PathWithLaneId & path, - const boost::optional & offset_segment); - void insertStopPoint(const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const; std::shared_ptr stopped_time_; lanelet::ConstLineString3d stop_line_; + int64_t lane_id_; + // State machine State state_; // Parameter PlannerParam planner_param_; // Debug - DebugData debug_data_; + mutable DebugData debug_data_; }; } // namespace behavior_velocity_planner 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 2e254ca78540e..67422e97bd89e 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 @@ -23,76 +23,13 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +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 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 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}; - } - } - - // No enough path length - return {}; -} - -} // namespace - StopLineModule::StopLineModule( const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line, const PlannerParam & planner_param, const rclcpp::Logger logger, @@ -135,57 +72,41 @@ boost::optional StopLineModule::findCol return {}; } -boost::optional StopLineModule::findOffsetSegment( - const 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; - - const auto & p_back = to_bg2d(path.points.at(collision.index + 1).point.pose.position); - - return findBackwardOffsetSegment( - path, collision.index + 1, base_backward_length + bg::distance(p_back, collision.point)); -} - -boost::optional StopLineModule::calcStopPose( - const 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}; - } - - return StopLineModule::SegmentIndexWithPose{ - offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; -} - bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + 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 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 SearchRangeIndex dst_search_range = planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_); // Find collision - const auto collision = findCollision(*path, stop_line, dst_search_range); + const auto collision = findCollision(ego_path, stop_line, dst_search_range); // If no collision found, do nothing if (!collision) { return true; } - // Find offset segment - const auto offset_segment = findOffsetSegment(*path, *collision); + const auto p_stop_line = + createPoint(collision.get().point.x(), collision.get().point.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); + + if (!stop_pose) { + return false; + } - // Calculate stop pose and insert index - const auto stop_pose_with_index = calcStopPose(*path, offset_segment); + 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 @@ -193,21 +114,14 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop * |----------------------------| * s---ego----------x--|--------g */ - const auto stop_point = getPoint(stop_pose_with_index.get().pose); const auto signed_arc_dist_to_stop_point = - calcSignedArcLength(path->points, current_position, stop_point); + calcSignedArcLength(ego_path.points, ego_pos, stop_pose.get().position); switch (state_) { case State::APPROACH: { - if (!stop_pose_with_index) { - break; - } - // Insert stop pose - insertStopPoint(stop_point, *path); + insertStopPoint(stop_pose.get().position, *path); + planning_utils::appendStopReason(stop_factor, stop_reason); - debug_data_.stop_pose = stop_pose_with_index.get().pose; - - // Move to stopped state if stopped if ( signed_arc_dist_to_stop_point < planner_param_.stop_check_dist && planner_data_->isVehicleStopped()) { @@ -226,10 +140,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop } case State::STOPPED: { - // Change state after vehicle departure - insertStopPoint(current_position, *path); - - debug_data_.stop_pose = stop_pose_with_index.get().pose; + insertStopPoint(ego_pos, *path); + planning_utils::appendStopReason(stop_factor, stop_reason); const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); @@ -273,15 +185,7 @@ void StopLineModule::insertStopPoint( for (size_t i = insert_idx.get(); i < path.points.size(); ++i) { path.points.at(i).point.longitudinal_velocity_mps = 0.0; } -} -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; + debug_data_.stop_pose = getPose(path.points.at(insert_idx.get())); } } // namespace behavior_velocity_planner From 90631363bca6d9a6bf41b7b31acd783d393e7dcc Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 26 Jul 2022 14:58:37 +0900 Subject: [PATCH 4/8] fix(bahevior_velocity_planner): clean up Signed-off-by: satoshi-ota --- .../include/scene_module/stop_line/scene.hpp | 2 +- .../src/scene_module/stop_line/debug.cpp | 106 ++++++++---------- 2 files changed, 48 insertions(+), 60 deletions(-) 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 0e2055e5ed6e0..cbe1432ad9826 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 @@ -99,7 +99,7 @@ class StopLineModule : public SceneModuleInterface PlannerParam planner_param_; // Debug - mutable DebugData debug_data_; + DebugData debug_data_; }; } // namespace behavior_velocity_planner 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 3c4a6307507b6..7b4b095978cb7 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 @@ -12,86 +12,74 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else #include +#endif namespace behavior_velocity_planner { -namespace -{ -using DebugData = StopLineModule::DebugData; -visualization_msgs::msg::MarkerArray createMarkers( - const DebugData & debug_data, const int64_t module_id) +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() { - visualization_msgs::msg::MarkerArray msg; - tf2::Transform tf_base_link2front( - tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0)); + MarkerArray msg; - // Stop VirtualWall - if (debug_data.stop_pose) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "stop_virtual_wall"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - tf2::Transform tf_map2base_link; - tf2::fromMsg(*debug_data.stop_pose, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - tf2::toMsg(tf_map2front, marker.pose); - marker.pose.position.z += 1.0; - marker.scale.x = 0.1; - marker.scale.y = 5.0; - marker.scale.z = 2.0; - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; + const auto now = this->clock_->now(); + // Search Segments + { + auto marker = createDefaultMarker( + "map", now, "search_segments", module_id_, Marker::SPHERE_LIST, + createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + for (const auto & e : debug_data_.search_segments) { + marker.points.push_back(createPoint(e.front().x(), e.front().y(), 0.0)); + marker.points.push_back(createPoint(e.back().x(), e.back().y(), 0.0)); + } msg.markers.push_back(marker); } - // Factor Text - if (debug_data.stop_pose) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "factor_text"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::msg::Marker::ADD; - tf2::Transform tf_map2base_link; - tf2::fromMsg(*debug_data.stop_pose, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - tf2::toMsg(tf_map2front, marker.pose); - marker.pose.position.z += 2.0; - marker.scale.x = 0.0; - marker.scale.y = 0.0; - marker.scale.z = 1.0; - marker.color.a = 0.999; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.text = "stop line"; + // Search stopline + { + 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; + 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)); msg.markers.push_back(marker); } return msg; } -} // namespace - -visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() +MarkerArray StopLineModule::createVirtualWallMarkerArray() { - visualization_msgs::msg::MarkerArray debug_marker_array; + MarkerArray wall_marker; + + if (!debug_data_.stop_pose) { + return wall_marker; + } + + const auto now = this->clock_->now(); - appendMarkerArray( - createMarkers(debug_data_, module_id_), this->clock_->now(), &debug_marker_array); + 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 debug_marker_array; + return wall_marker; } } // namespace behavior_velocity_planner From f33302a9fda7504941c9266d7174a961832c0679 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 26 Jul 2022 15:52:33 +0900 Subject: [PATCH 5/8] fix(behavior_velocity_planner): use common function Signed-off-by: satoshi-ota --- .../config/stop_line.param.yaml | 2 +- .../include/scene_module/stop_line/scene.hpp | 15 +-- .../src/scene_module/stop_line/debug.cpp | 19 +--- .../src/scene_module/stop_line/manager.cpp | 2 +- .../src/scene_module/stop_line/scene.cpp | 101 ++++++++++-------- 5 files changed, 67 insertions(+), 72 deletions(-) diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml index 468a225fde471..0118452b72b85 100644 --- a/planning/behavior_velocity_planner/config/stop_line.param.yaml +++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml @@ -2,6 +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 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 cbe1432ad9826..4b809908ccebe 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 @@ -27,7 +27,6 @@ #include #include #include -#include #include #include @@ -42,17 +41,9 @@ using tier4_planning_msgs::msg::StopReason; class StopLineModule : public SceneModuleInterface { - using StopLineWithLaneId = std::pair; - public: enum class State { APPROACH, STOPPED, START }; - struct SegmentIndexWithPoint2d - { - size_t index; - Point2d point; - }; - struct DebugData { double base_link2front; @@ -62,8 +53,8 @@ class StopLineModule : public SceneModuleInterface struct PlannerParam { double stop_margin; - double stop_check_dist; double stop_duration_sec; + double hold_stop_margin_distance; bool use_initialization_stop_line_state; }; @@ -80,10 +71,6 @@ class StopLineModule : public SceneModuleInterface private: int64_t module_id_; - boost::optional findCollision( - const PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index); - void insertStopPoint(const geometry_msgs::msg::Point & stop_point, PathWithLaneId & path) const; std::shared_ptr stopped_time_; 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 7b4b095978cb7..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 @@ -39,18 +39,6 @@ MarkerArray StopLineModule::createDebugMarkerArray() MarkerArray msg; const auto now = this->clock_->now(); - // Search Segments - { - auto marker = createDefaultMarker( - "map", now, "search_segments", module_id_, Marker::SPHERE_LIST, - createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 0.0, 1.0, 0.999)); - - for (const auto & e : debug_data_.search_segments) { - marker.points.push_back(createPoint(e.front().x(), e.front().y(), 0.0)); - marker.points.push_back(createPoint(e.back().x(), e.back().y(), 0.0)); - } - msg.markers.push_back(marker); - } // Search stopline { @@ -59,8 +47,11 @@ MarkerArray StopLineModule::createDebugMarkerArray() createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 0.0, 1.0, 0.999)); const auto line = debug_data_.search_stopline; - 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)); + 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)); + } + msg.markers.push_back(marker); } 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 27dbd004bf428..0a0adb71ba833 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 @@ -89,7 +89,7 @@ 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.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance", 2.0); p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0); p.use_initialization_stop_line_state = node.declare_parameter(ns + ".use_initialization_stop_line_state", false); 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 67422e97bd89e..e8168b6bf081e 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 @@ -23,6 +23,8 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Line = bg::model::linestring; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::insertTargetPoint; @@ -30,6 +32,53 @@ 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()) +{ + 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; + } + } + + if (found_max_num) { + break; + } + } + + 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( const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line, const PlannerParam & planner_param, const rclcpp::Logger logger, @@ -43,35 +92,6 @@ StopLineModule::StopLineModule( planner_param_ = planner_param; } -boost::optional StopLineModule::findCollision( - const PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index) -{ - 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 (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; - - // 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); - - // Ignore if no collision found - if (collision_points.empty()) { - continue; - } - - // Select first collision - const auto & collision_point = collision_points.at(0); - - return StopLineModule::SegmentIndexWithPoint2d{i, collision_point}; - } - - return {}; -} - bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -82,21 +102,18 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop const auto ego_path = *path; const auto & ego_pos = planner_data_->current_pose.pose.position; - const LineString2d stop_line = planning_utils::extendLine( + const auto stop_line = planning_utils::extendLine( stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); - const SearchRangeIndex dst_search_range = - planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_); - // Find collision - const auto collision = findCollision(ego_path, stop_line, dst_search_range); + debug_data_.search_stopline = stop_line; + + const auto intersects = getLinestringIntersects(ego_path, stop_line, ego_pos, 1); - // If no collision found, do nothing - if (!collision) { - return true; + if (intersects.empty()) { + return false; } - const auto p_stop_line = - createPoint(collision.get().point.x(), collision.get().point.y(), ego_pos.z); + 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); @@ -123,14 +140,14 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop planning_utils::appendStopReason(stop_factor, stop_reason); if ( - signed_arc_dist_to_stop_point < planner_param_.stop_check_dist && + signed_arc_dist_to_stop_point < planner_param_.hold_stop_margin_distance && planner_data_->isVehicleStopped()) { RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); state_ = State::STOPPED; stopped_time_ = std::make_shared(clock_->now()); - if (signed_arc_dist_to_stop_point < -planner_param_.stop_check_dist) { + 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"); } @@ -156,7 +173,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop 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_.stop_check_dist) { + if (signed_arc_dist_to_stop_point > planner_param_.hold_stop_margin_distance) { RCLCPP_INFO(logger_, "START -> APPROACH"); state_ = State::APPROACH; } From 5a4e14f3b60307e8d3974b6370d272538fc0e817 Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Thu, 22 Jun 2023 20:49:19 +0900 Subject: [PATCH 6/8] fix: replace motion_utils to tier4_autoware_utils Signed-off-by: Yuma Nihei --- .../src/scene_module/stop_line/debug.cpp | 4 ++-- .../src/scene_module/stop_line/scene.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) 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..a0d8bb5a74a0a 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 @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -25,7 +25,7 @@ namespace behavior_velocity_planner { -using motion_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; 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 e8168b6bf081e..c4bda389a5980 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 @@ -25,9 +25,9 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; using Line = bg::model::linestring; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::insertTargetPoint; +using tier4_autoware_utils::calcLongitudinalOffsetPose; +using tier4_autoware_utils::calcSignedArcLength; +using tier4_autoware_utils::insertTargetPoint; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; From 3608a7739047b369b590efafa653daa54a1d9ce6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 22 Jun 2023 11:51:38 +0000 Subject: [PATCH 7/8] ci(pre-commit): autofix --- .../src/scene_module/stop_line/debug.cpp | 4 ++-- .../src/scene_module/stop_line/scene.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 a0d8bb5a74a0a..cd5ee9d1f8da8 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 @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -25,12 +25,12 @@ namespace behavior_velocity_planner { -using tier4_autoware_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 tier4_autoware_utils::createStopVirtualWallMarker; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; 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 c4bda389a5980..83d946821f396 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 @@ -27,10 +27,10 @@ using Point = bg::model::d2::point_xy; using Line = bg::model::linestring; using tier4_autoware_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcSignedArcLength; -using tier4_autoware_utils::insertTargetPoint; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; +using tier4_autoware_utils::insertTargetPoint; namespace { From ed942fcf51ac278ce3339684f9aca82ec697de27 Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Thu, 22 Jun 2023 21:12:53 +0900 Subject: [PATCH 8/8] feat: add search_stopline --- .../include/scene_module/stop_line/scene.hpp | 1 + 1 file changed, 1 insertion(+) 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 4b809908ccebe..21a2a7cb18120 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 @@ -48,6 +48,7 @@ class StopLineModule : public SceneModuleInterface { double base_link2front; boost::optional stop_pose; + LineString2d search_stopline; }; struct PlannerParam