diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp index 30eca6927db34..2fcde52ec7c81 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp @@ -44,6 +44,9 @@ class VelocityFactorInterface const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, const std::string & detail = ""); + void set( + const double & distance, const VelocityFactorStatus & status, const std::string & detail = ""); + private: VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; VelocityFactor velocity_factor_{}; diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp index e405cdb655c02..6a1ddff453bd0 100644 --- a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -36,6 +36,15 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } +void VelocityFactorInterface::set( + const double & distance, const VelocityFactorStatus & status, const std::string & detail) +{ + velocity_factor_.behavior = behavior_; + velocity_factor_.distance = static_cast(distance); + velocity_factor_.status = status; + velocity_factor_.detail = detail; +} + template void VelocityFactorInterface::set( const std::vector &, const Pose &, const Pose &, const VelocityFactorStatus, const std::string &); diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp index 9a33152a7dfdb..3346d4ce8104f 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp @@ -36,7 +36,7 @@ namespace autoware::trajectory::detail */ geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p); geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p); -geometry_msgs::msg::Point to_point(const Eigen::Ref & p); +geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p); geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p); geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p); geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p); diff --git a/common/autoware_trajectory/src/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp index 8cfb8029629cb..4c9649ef1f3ab 100644 --- a/common/autoware_trajectory/src/detail/util.cpp +++ b/common/autoware_trajectory/src/detail/util.cpp @@ -30,7 +30,7 @@ geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p) return p.position; } -geometry_msgs::msg::Point to_point(const Eigen::Ref & p) +geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p) { geometry_msgs::msg::Point point; point.x = p(0); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index 464a5d0f09bf7..68a8d919512b1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -23,6 +23,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler + autoware_trajectory eigen geometry_msgs pluginlib diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp index 29295f41cdf04..573a260138679 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "scene.hpp" -#include -#include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index 4dcd0309ce526..ffdcea16b45b5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" #include #include @@ -57,7 +58,7 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } for (const auto & stop_line : traffic_sign_reg_elem->refLines()) { - stop_lines_with_lane_id.push_back(std::make_pair(stop_line, lane_id)); + stop_lines_with_lane_id.emplace_back(stop_line, lane_id); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index 2a008ce1700ab..b83a4f94e9a1f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -15,11 +15,11 @@ #ifndef MANAGER_HPP_ #define MANAGER_HPP_ +#include "autoware/behavior_velocity_planner_common/plugin_interface.hpp" +#include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp" +#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "scene.hpp" -#include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 0500d22d425cd..6a4b85cd6926c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -14,60 +14,66 @@ #include "scene.hpp" -#include -#include -#include -#include +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/trajectory/path_point_with_lane_id.hpp" -#include -#include +#include namespace autoware::behavior_velocity_planner { -namespace bg = boost::geometry; 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, + const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line, + const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), - stop_line_(stop_line), + stop_line_(std::move(stop_line)), state_(State::APPROACH), + planner_param_(planner_param), debug_data_() { velocity_factor_.init(PlanningBehavior::STOP_SIGN); - planner_param_ = planner_param; } bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - universe_utils::ScopedTimeTrack st( - std::string(__func__) + " (lane_id:=" + std::to_string(module_id_) + ")", *getTimeKeeper()); + auto trajectory = + trajectory::Trajectory::Builder{}.build( + path->points); + + if (!trajectory) { + return true; + } + debug_data_ = DebugData(); - if (path->points.empty()) return true; - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.base_link2front = base_link2front; - first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points); + first_stop_path_point_distance_ = trajectory->length(); *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); - time_keeper_->start_track("createTargetPoint"); - // Calculate stop pose and insert index - const auto stop_point = arc_lane_utils::createTargetPoint( - *path, stop_line, planner_param_.stop_margin, - planner_data_->vehicle_info_.max_longitudinal_offset_m); - time_keeper_->end_track("createTargetPoint"); + // Calculate intersection with stop line + const auto trajectory_stop_line_intersection = + trajectory->crossed(stop_line.front(), stop_line.back()); + // If no collision found, do nothing - if (!stop_point) { + if (!trajectory_stop_line_intersection) { RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); return true; } - const auto stop_point_idx = stop_point->first; - auto stop_pose = stop_point->second; + const double stop_point_s = + *trajectory_stop_line_intersection - + (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin + + if (stop_point_s < 0.0) { + return true; + } + + const auto stop_pose = trajectory->compute(stop_point_s); /** * @brief : calculate signed arc length consider stop margin from stop line @@ -75,35 +81,26 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop * |----------------------------| * s---ego----------x--|--------g */ - time_keeper_->start_track( - "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength"); - const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( - path->points, stop_pose.position, stop_point_idx); - const size_t current_seg_idx = findEgoSegmentIndex(path->points); - const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( - path->points, planner_data_->current_odometry->pose.position, current_seg_idx, - stop_pose.position, stop_line_seg_idx); - time_keeper_->end_track( - "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength"); + const double ego_on_trajectory_s = + trajectory->closest(planner_data_->current_odometry->pose.position); + const double signed_arc_dist_to_stop_point = stop_point_s - ego_on_trajectory_s; + switch (state_) { case State::APPROACH: { // Insert stop pose - planning_utils::insertStopPoint(stop_pose.position, stop_line_seg_idx, *path); + trajectory->longitudinal_velocity_mps.range(stop_point_s, trajectory->length()).set(0.0); - // Update first stop index - first_stop_path_point_distance_ = autoware::motion_utils::calcSignedArcLength( - path->points, 0, stop_pose.position, stop_line_seg_idx); - debug_data_.stop_pose = stop_pose; + // Update first stop path point distance + first_stop_path_point_distance_ = stop_point_s; + debug_data_.stop_pose = stop_pose.point.pose; // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_pose; + stop_factor.stop_pose = stop_pose.point.pose; stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, - VelocityFactor::APPROACHING); + velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::APPROACHING); } // Move to stopped state if stopped @@ -125,34 +122,22 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop } case State::STOPPED: { - // Change state after vehicle departure - const auto stopped_pose = autoware::motion_utils::calcLongitudinalOffsetPose( - path->points, planner_data_->current_odometry->pose.position, 0.0); - - if (!stopped_pose) { - break; - } - - SegmentIndexWithPose ego_pos_on_path; - ego_pos_on_path.pose = stopped_pose.value(); - ego_pos_on_path.index = findEgoSegmentIndex(path->points); - // Insert stop pose - planning_utils::insertStopPoint(ego_pos_on_path.pose.position, ego_pos_on_path.index, *path); - - debug_data_.stop_pose = stop_pose; + trajectory->longitudinal_velocity_mps.range(ego_on_trajectory_s, trajectory->length()) + .set(0.0); + const auto ego_pos_on_path = trajectory->compute(ego_on_trajectory_s).point.pose; + debug_data_.stop_pose = ego_pos_on_path; // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = ego_pos_on_path.pose; + stop_factor.stop_pose = ego_pos_on_path; stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::STOPPED); } - const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); + const double elapsed_time = (clock_->now() - *stopped_time_).seconds(); if (planner_param_.stop_duration_sec < elapsed_time) { RCLCPP_INFO(logger_, "STOPPED -> START"); @@ -175,6 +160,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop } } + path->points = trajectory->restore(); + return true; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 78aec89adb063..cb48aabe57c1a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -15,23 +15,20 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ +#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" +#include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" + #include -#include -#include #include #include #define EIGEN_MPL2_ONLY #include #include -#include -#include -#include -#include #include #include -#include namespace autoware::behavior_velocity_planner { @@ -42,24 +39,6 @@ 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; @@ -77,10 +56,9 @@ class StopLineModule : public SceneModuleInterface bool show_stop_line_collision_check; }; -public: StopLineModule( - const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line, + const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;