diff --git a/build_depends.repos b/build_depends.repos index 2313a9be487a6..96bb9a7c14bed 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -32,7 +32,7 @@ repositories: core/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: 1.3.0 + version: 1.5.0 # universe universe/external/tier4_autoware_msgs: type: git diff --git a/common/autoware_motion_utils/README.md b/common/autoware_motion_utils/README.md index 993ec7a3ea7c3..8d80ae92188f3 100644 --- a/common/autoware_motion_utils/README.md +++ b/common/autoware_motion_utils/README.md @@ -47,7 +47,7 @@ The second function finds the nearest index in the lane whose id is `lane_id`. ```cpp size_t findNearestIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); ``` diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp index 19328179932c4..9f8e214e6f1ee 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -113,8 +113,8 @@ std::vector resamplePoseVector( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -136,10 +136,11 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, - const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, - const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, + const double resample_interval, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, + const bool resample_input_path_stop_point = true); /** * @brief A resampling function for a path. Note that in a default setting, position xy are diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp index d4f88c17c4d70..a00b1d274c809 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp @@ -15,11 +15,11 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#include "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include "autoware_planning_msgs/msg/detail/path__struct.hpp" #include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" -#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include @@ -58,7 +58,7 @@ autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input template <> inline autoware_planning_msgs::msg::Path convertToPath( - const tier4_planning_msgs::msg::PathWithLaneId & input) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input) { autoware_planning_msgs::msg::Path output{}; output.header = input.header; @@ -80,7 +80,7 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) template <> inline TrajectoryPoints convertToTrajectoryPoints( - const tier4_planning_msgs::msg::PathWithLaneId & input) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input) { TrajectoryPoints output{}; for (const auto & p : input.points) { @@ -95,19 +95,20 @@ inline TrajectoryPoints convertToTrajectoryPoints( } template -tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input) +autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + [[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); } template <> -inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( +inline autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( const TrajectoryPoints & input) { - tier4_planning_msgs::msg::PathWithLaneId output{}; + autoware_internal_planning_msgs::msg::PathWithLaneId output{}; for (const auto & p : input) { - tier4_planning_msgs::msg::PathPointWithLaneId pp; + autoware_internal_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose = p.pose; pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; output.points.emplace_back(pp); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp index 5272478cccd78..74805dbfde455 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp @@ -17,8 +17,8 @@ #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -51,8 +51,8 @@ autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( * twist information * @return resampled path(poses) */ -tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const tier4_planning_msgs::msg::PathWithLaneId & path, +autoware_internal_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp index 0d875e636bad5..d23c0851537e7 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -23,23 +23,23 @@ namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, - const int64_t lane_id); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id); size_t findNearestSegmentIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, - const int64_t lane_id); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id); // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle // center follow the input path // @param [in] path with position to be followed by the center of the vehicle // @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle // center follow the input path NOTE: rear_to_cog is supposed to be positive -tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); } // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 0ce42fbe1080f..558a4a94336da 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -23,9 +23,9 @@ #include #include +#include #include #include -#include #include #include @@ -58,8 +58,9 @@ void validateNonEmpty(const T & points) extern template void validateNonEmpty>( const std::vector &); -extern template void validateNonEmpty>( - const std::vector &); +extern template void +validateNonEmpty>( + const std::vector &); extern template void validateNonEmpty>( const std::vector &); @@ -116,8 +117,8 @@ extern template std::optional isDrivingForward>( const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional isDrivingForward>( const std::vector &); @@ -151,8 +152,8 @@ extern template std::optional isDrivingForwardWithTwist>( const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -196,9 +197,9 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) extern template std::vector removeOverlapPoints>( const std::vector & points, const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); extern template std::vector removeOverlapPoints>( @@ -311,8 +312,9 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin extern template size_t findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); -extern template size_t findNearestIndex>( - const std::vector & points, +extern template size_t +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t findNearestIndex>( const std::vector & points, @@ -379,8 +381,8 @@ findNearestIndex>( const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -461,10 +463,11 @@ extern template double calcLongitudinalOffsetToSegment>( const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double calcLongitudinalOffsetToSegment< + std::vector>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false); extern template double calcLongitudinalOffsetToSegment>( const std::vector & points, const size_t seg_idx, @@ -503,8 +506,8 @@ extern template size_t findNearestSegmentIndex & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t findNearestSegmentIndex>( @@ -555,8 +558,8 @@ findNearestSegmentIndex>( const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -629,8 +632,8 @@ extern template double calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); extern template double calcLateralOffset>( @@ -691,8 +694,8 @@ extern template double calcLateralOffset & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double calcLateralOffset>( const std::vector & points, @@ -733,9 +736,9 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); extern template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, @@ -782,10 +785,10 @@ extern template std::vector calcSignedArcLengthPartialSum>( const std::vector & points, const size_t src_idx, const size_t dst_idx); -extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); +extern template std::vector calcSignedArcLengthPartialSum< + std::vector>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); extern template std::vector calcSignedArcLengthPartialSum>( const std::vector & points, const size_t src_idx, @@ -825,8 +828,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -861,9 +864,9 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, @@ -908,8 +911,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double calcSignedArcLength>( @@ -936,8 +939,9 @@ double calcArcLength(const T & points) extern template double calcArcLength>( const std::vector & points); -extern template double calcArcLength>( - const std::vector & points); +extern template double +calcArcLength>( + const std::vector & points); extern template double calcArcLength>( const std::vector & points); @@ -974,8 +978,8 @@ extern template std::vector calcCurvature>( const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector calcCurvature>( const std::vector & points); @@ -1032,8 +1036,9 @@ extern template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); extern template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); +calcCurvatureAndSegmentLength< + std::vector>( + const std::vector & points); extern template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); @@ -1146,9 +1151,9 @@ calcLongitudinalOffsetPoint> const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception = false); +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, @@ -1191,8 +1196,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional calcLongitudinalOffsetPoint>( @@ -1292,10 +1297,10 @@ calcLongitudinalOffsetPose>( const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction = true, - const bool throw_exception = false); +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, @@ -1339,8 +1344,8 @@ calcLongitudinalOffsetPose>( const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional @@ -1441,9 +1446,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1496,9 +1501,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1569,9 +1574,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1623,9 +1628,9 @@ insertTargetPoint>( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional @@ -1674,9 +1679,9 @@ insertStopPoint>( std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional insertStopPoint>( @@ -1723,9 +1728,9 @@ insertStopPoint>( std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional insertStopPoint>( @@ -1779,9 +1784,9 @@ insertStopPoint>( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional @@ -1906,8 +1911,9 @@ void insertOrientation(T & points, const bool is_driving_forward) extern template void insertOrientation>( std::vector & points, const bool is_driving_forward); -extern template void insertOrientation>( - std::vector & points, +extern template void +insertOrientation>( + std::vector & points, const bool is_driving_forward); extern template void insertOrientation>( std::vector & points, @@ -1974,8 +1980,8 @@ extern template double calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double @@ -2013,8 +2019,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -2050,9 +2056,9 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, @@ -2153,8 +2159,8 @@ findFirstNearestIndexWithSoftConstraints::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2209,8 +2215,8 @@ extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2272,9 +2278,9 @@ calcDistanceToForwardStopPoint & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +extern template std::optional calcDistanceToForwardStopPoint< + std::vector>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -2312,9 +2318,9 @@ cropForwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); extern template std::vector @@ -2352,9 +2358,9 @@ cropBackwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); extern template std::vector @@ -2394,9 +2400,9 @@ cropPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); extern template std::vector @@ -2460,8 +2466,9 @@ double calcYawDeviation( extern template double calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double calcYawDeviation>( - const std::vector & points, +extern template double +calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); extern template double calcYawDeviation>( const std::vector & points, @@ -2495,8 +2502,9 @@ extern template bool isTargetPointFront & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); -extern template bool isTargetPointFront>( - const std::vector & points, +extern template bool +isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); extern template bool isTargetPointFront>( diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 75dc56de5466d..0a5eae0db4fbb 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -22,6 +22,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_internal_planning_msgs autoware_interpolation autoware_planning_msgs autoware_universe_utils @@ -32,7 +33,6 @@ rclcpp tf2 tf2_geometry_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 9e736444495fa..333f5bd11a421 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -186,8 +186,8 @@ std::vector resamplePoseVector( return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -340,7 +340,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( return input_path; } - tier4_planning_msgs::msg::PathWithLaneId resampled_path; + autoware_internal_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; @@ -359,9 +359,9 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( return resampled_path; } -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, - const bool use_akima_spline_for_xy, const bool use_lerp_for_z, +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { // validate arguments diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index 0ae9d44683f62..2c0698232a16e 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -17,10 +17,10 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::motion_utils { diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index 55c7360a25c41..973df4758f96b 100644 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -25,7 +25,7 @@ namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -56,8 +56,8 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, - const int64_t lane_id) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) { const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); if (opt_range) { @@ -66,7 +66,7 @@ size_t findNearestIndexFromLaneId( validateNonEmpty(path.points); - const auto sub_points = std::vector{ + const auto sub_points = std::vector{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; validateNonEmpty(sub_points); @@ -77,8 +77,8 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, - const int64_t lane_id) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) { const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); @@ -99,8 +99,8 @@ size_t findNearestSegmentIndexFromLaneId( } // NOTE: rear_to_cog is supposed to be positive -tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation) { auto cog_path = path; diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 35e39fda75a69..61014d95c7588 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -24,8 +24,9 @@ namespace autoware::motion_utils // template void validateNonEmpty>( const std::vector &); -template void validateNonEmpty>( - const std::vector &); +template void +validateNonEmpty>( + const std::vector &); template void validateNonEmpty>( const std::vector &); @@ -33,8 +34,8 @@ template void validateNonEmpty isDrivingForward>( const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); template std::optional isDrivingForward>( const std::vector &); @@ -44,8 +45,8 @@ template std::optional isDrivingForwardWithTwist>( const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -54,9 +55,9 @@ isDrivingForwardWithTwist removeOverlapPoints>( const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); template std::vector removeOverlapPoints>( @@ -83,8 +84,9 @@ searchZeroVelocityIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, +template size_t +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); template size_t findNearestIndex>( const std::vector & points, @@ -96,8 +98,8 @@ findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional findNearestIndex>( @@ -109,10 +111,10 @@ template double calcLongitudinalOffsetToSegment>( const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double calcLongitudinalOffsetToSegment< + std::vector>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double calcLongitudinalOffsetToSegment>( const std::vector & points, const size_t seg_idx, @@ -122,8 +124,9 @@ calcLongitudinalOffsetToSegment>( const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestSegmentIndex>( - const std::vector & points, +template size_t +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); template size_t findNearestSegmentIndex>( const std::vector & points, @@ -135,8 +138,8 @@ findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional findNearestSegmentIndex>( @@ -147,8 +150,9 @@ findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); template double calcLateralOffset>( const std::vector & points, @@ -158,8 +162,9 @@ template double calcLateralOffset>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double calcLateralOffset>( const std::vector & points, @@ -169,9 +174,10 @@ template double calcLateralOffset>( const std::vector & points, const size_t src_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, const size_t dst_idx); @@ -181,10 +187,10 @@ template std::vector calcSignedArcLengthPartialSum>( const std::vector & points, const size_t src_idx, const size_t dst_idx); -template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); +template std::vector calcSignedArcLengthPartialSum< + std::vector>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); template std::vector calcSignedArcLengthPartialSum>( const std::vector & points, const size_t src_idx, @@ -194,8 +200,9 @@ calcSignedArcLengthPartialSum>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, +template double +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); template double calcSignedArcLength>( const std::vector &, @@ -205,9 +212,10 @@ template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); @@ -216,8 +224,9 @@ template double calcSignedArcLength>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, +template double +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); template double calcSignedArcLength>( const std::vector & points, @@ -226,8 +235,9 @@ template double calcSignedArcLength>( const std::vector & points); -template double calcArcLength>( - const std::vector & points); +template double +calcArcLength>( + const std::vector & points); template double calcArcLength>( const std::vector & points); @@ -235,8 +245,8 @@ template double calcArcLength calcCurvature>( const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); template std::vector calcCurvature>( const std::vector & points); @@ -245,9 +255,9 @@ calcCurvature>( template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); +template std::vector>> calcCurvatureAndSegmentLength< + std::vector>( + const std::vector & points); template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); @@ -264,9 +274,9 @@ calcLongitudinalOffsetPoint> const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception); +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception); template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, @@ -278,8 +288,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional calcLongitudinalOffsetPoint>( @@ -293,9 +303,9 @@ calcLongitudinalOffsetPose>( const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction, +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional calcLongitudinalOffsetPose>( @@ -310,8 +320,8 @@ calcLongitudinalOffsetPose>( const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional @@ -326,9 +336,9 @@ insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -342,9 +352,9 @@ insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -358,9 +368,9 @@ insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -375,10 +385,10 @@ insertTargetPoint>( std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, - const double max_yaw, const double overlap_threshold); + std::vector & points, + const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, @@ -391,9 +401,9 @@ template std::optional insertStopPoint & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional insertStopPoint>( @@ -407,9 +417,9 @@ template std::optional insertStopPoint & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional insertStopPoint>( @@ -423,9 +433,9 @@ template std::optional insertStopPoint & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional insertStopPoint>( @@ -450,8 +460,9 @@ insertDecelPoint>( // template void insertOrientation>( std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void +insertOrientation>( + std::vector & points, const bool is_driving_forward); template void insertOrientation>( std::vector & points, @@ -462,8 +473,9 @@ template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); template double calcSignedArcLength>( @@ -475,8 +487,9 @@ template double calcSignedArcLength>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); template double calcSignedArcLength>( const std::vector & points, @@ -486,9 +499,10 @@ template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); @@ -499,8 +513,8 @@ findFirstNearestIndexWithSoftConstraints & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints>( @@ -513,8 +527,8 @@ template size_t findFirstNearestSegmentIndexWithSoftConstraints< const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< std::vector>( @@ -533,9 +547,9 @@ cropForwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); template std::vector @@ -550,9 +564,9 @@ cropBackwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); template std::vector @@ -567,9 +581,9 @@ cropPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); template std::vector @@ -582,8 +596,9 @@ cropPoints>( template double calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, +template double +calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); template double calcYawDeviation>( const std::vector & points, @@ -594,8 +609,9 @@ template bool isTargetPointFront & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool isTargetPointFront>( - const std::vector & points, +template bool +isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); template bool isTargetPointFront>( diff --git a/common/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp index ef97dfa410d2d..72ddd043fbddf 100644 --- a/common/autoware_motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -31,12 +31,12 @@ namespace using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::transformPoint; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -316,7 +316,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Output key is not same as input { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -422,7 +422,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Duplicated points in the original path { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(11); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -456,7 +456,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) { // Input path size is not enough for interpolation { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -489,7 +489,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -523,7 +523,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength is longer than input path { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -562,7 +562,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) using autoware::motion_utils::resamplePath; { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -670,7 +670,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // change initial orientation { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -794,7 +794,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp x, y { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -871,7 +871,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Slerp z { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -950,7 +950,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp v { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( diff --git a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp index b4b60ff403048..ec878cfe8cbc2 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp @@ -30,10 +30,10 @@ namespace using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::transformPoint; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -351,7 +351,7 @@ TEST(Interpolation, interpolate_path_for_path) using autoware::motion_utils::calcInterpolatedPoint; { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -515,7 +515,7 @@ TEST(Interpolation, interpolate_path_for_path) // Duplicated Points { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 62e4ac74cb639..e607c12397fe0 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -23,8 +23,8 @@ namespace { using autoware::universe_utils::createPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -55,7 +55,7 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { using autoware::motion_utils::getPathIndexRangeWithLaneId; - using tier4_planning_msgs::msg::PathWithLaneId; + using autoware_internal_planning_msgs::msg::PathWithLaneId; // Usual cases { diff --git a/common/autoware_test_utils/config/sample_topic_snapshot.yaml b/common/autoware_test_utils/config/sample_topic_snapshot.yaml index 2ff746b84c3ac..8122d6e5f4a4d 100644 --- a/common/autoware_test_utils/config/sample_topic_snapshot.yaml +++ b/common/autoware_test_utils/config/sample_topic_snapshot.yaml @@ -28,5 +28,5 @@ fields: # type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects # topic: /perception/object_recognition/tracking/objects # - name: path_with_lane_id - # type: PathWithLaneId # tier4_planning_msgs::msg::PathWithLaneId + # type: PathWithLaneId # autoware_internal_planning_msgs::msg::PathWithLaneId # topic: /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index b98af2133f175..70be3880fe7e4 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include #include @@ -30,8 +32,6 @@ #include #include #include -#include -#include #include #include @@ -48,14 +48,14 @@ namespace autoware::test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 8375d3e731683..e4cba1f1a82a5 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -29,7 +30,6 @@ #include #include #include -#include #include @@ -56,6 +56,8 @@ using autoware_perception_msgs::msg::TrafficLightElement; using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_perception_msgs::msg::TrafficLightGroupArray; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; @@ -72,8 +74,6 @@ using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistWithCovariance; using nav_msgs::msg::Odometry; using std_msgs::msg::Header; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; /** diff --git a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp index dbd3dd6638c95..9186785ba6145 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -244,7 +244,7 @@ struct PathWithLaneIdConfig * @param [in] config_opt if null, only the path points & quiver are plotted with "k" color. */ inline void plot_autoware_object( - const tier4_planning_msgs::msg::PathWithLaneId & path, autoware::pyplot::Axes & axes, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, autoware::pyplot::Axes & axes, const std::optional & config_opt = std::nullopt) { py::dict kwargs{}; diff --git a/common/autoware_test_utils/src/topic_snapshot_saver.cpp b/common/autoware_test_utils/src/topic_snapshot_saver.cpp index bd9af2d5672b8..70d716de72e21 100644 --- a/common/autoware_test_utils/src/topic_snapshot_saver.cpp +++ b/common/autoware_test_utils/src/topic_snapshot_saver.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include @@ -48,7 +48,7 @@ using MessageType = std::variant< autoware_planning_msgs::msg::LaneletRoute, // 4 autoware_perception_msgs::msg::TrafficLightGroupArray, // 5 autoware_perception_msgs::msg::TrackedObjects, // 6 - tier4_planning_msgs::msg::PathWithLaneId // 7 + autoware_internal_planning_msgs::msg::PathWithLaneId // 7 >; std::optional get_topic_index(const std::string & name) diff --git a/common/autoware_test_utils/test/test_mock_data_parser.cpp b/common/autoware_test_utils/test/test_mock_data_parser.cpp index 036f16c827713..b256c819db6aa 100644 --- a/common/autoware_test_utils/test/test_mock_data_parser.cpp +++ b/common/autoware_test_utils/test/test_mock_data_parser.cpp @@ -18,13 +18,13 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "autoware_test_utils/mock_data_parser.hpp" -#include +#include #include namespace autoware::test_utils { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; // Example YAML structure as a string for testing const char g_complete_yaml[] = R"( diff --git a/common/autoware_trajectory/examples/example_find_intervals.cpp b/common/autoware_trajectory/examples/example_find_intervals.cpp new file mode 100644 index 0000000000000..69d0ed2a38350 --- /dev/null +++ b/common/autoware_trajectory/examples/example_find_intervals.cpp @@ -0,0 +1,109 @@ +// Copyright 2025 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/path_point_with_lane_id.hpp" +#include "autoware/trajectory/utils/find_intervals.hpp" + +#include + +#include + +using Trajectory = + autoware::trajectory::Trajectory; + +autoware_internal::msg::PathPointWithLaneId path_point_with_lane_id( + double x, double y, uint8_t lane_id) +{ + autoware_internal::msg::PathPointWithLaneId point; + point.point.pose.position.x = x; + point.point.pose.position.y = y; + point.lane_ids.emplace_back(lane_id); + return point; +} + +int main() +{ + pybind11::scoped_interpreter guard{}; + auto plt = matplotlibcpp17::pyplot::import(); + + std::vector points{ + path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0), + path_point_with_lane_id(0.93, 1.41, 0), path_point_with_lane_id(1.26, 1.71, 0), + path_point_with_lane_id(1.62, 1.90, 0), path_point_with_lane_id(1.96, 1.98, 0), + path_point_with_lane_id(2.48, 1.96, 1), path_point_with_lane_id(3.02, 1.87, 1), + path_point_with_lane_id(3.56, 1.82, 1), path_point_with_lane_id(4.14, 2.02, 1), + path_point_with_lane_id(4.56, 2.36, 1), path_point_with_lane_id(4.89, 2.72, 1), + path_point_with_lane_id(5.27, 3.15, 1), path_point_with_lane_id(5.71, 3.69, 1), + path_point_with_lane_id(6.09, 4.02, 0), path_point_with_lane_id(6.54, 4.16, 0), + path_point_with_lane_id(6.79, 3.92, 0), path_point_with_lane_id(7.11, 3.60, 0), + path_point_with_lane_id(7.42, 3.01, 0)}; + + auto trajectory = Trajectory::Builder{}.build(points); + + if (!trajectory) { + return 1; + } + + const auto intervals = autoware::trajectory::find_intervals( + *trajectory, [](const autoware_internal::msg::PathPointWithLaneId & point) { + return point.lane_ids[0] == 1; + }); + + std::vector x_id0; + std::vector y_id0; + std::vector x_id1; + std::vector y_id1; + std::vector x_all; + std::vector y_all; + + for (const auto & point : points) { + if (point.lane_ids[0] == 0) { + x_id0.push_back(point.point.pose.position.x); + y_id0.push_back(point.point.pose.position.y); + } else { + x_id1.push_back(point.point.pose.position.x); + y_id1.push_back(point.point.pose.position.y); + } + } + + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x_all.push_back(p.point.pose.position.x); + y_all.push_back(p.point.pose.position.y); + } + + plt.plot(Args(x_all, y_all), Kwargs("color"_a = "blue")); + plt.scatter(Args(x_id0, y_id0), Kwargs("color"_a = "blue", "label"_a = "lane_id = 0")); + plt.scatter(Args(x_id1, y_id1), Kwargs("color"_a = "red", "label"_a = "lane_id = 1")); + + std::vector x_cropped; + std::vector y_cropped; + + trajectory->crop(intervals[0].start, intervals[0].end - intervals[0].start); + + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x_cropped.push_back(p.point.pose.position.x); + y_cropped.push_back(p.point.pose.position.y); + } + + plt.plot( + Args(x_cropped, y_cropped), + Kwargs("color"_a = "red", "label"_a = "interval between lane_id = 1")); + + plt.legend(); + plt.show(); + + return 0; +} diff --git a/common/autoware_trajectory/examples/example_shift.cpp b/common/autoware_trajectory/examples/example_shift.cpp new file mode 100644 index 0000000000000..9d04696f78f53 --- /dev/null +++ b/common/autoware_trajectory/examples/example_shift.cpp @@ -0,0 +1,187 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/point.hpp" +#include "autoware/trajectory/utils/shift.hpp" + +#include + +#include +#include + +geometry_msgs::msg::Point point(double x, double y) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + return p; +} + +int main() +{ + pybind11::scoped_interpreter guard{}; + auto plt = matplotlibcpp17::pyplot::import(); + + geometry_msgs::msg::Point p; + (void)(p); + + std::vector points = { + point(0.49, 0.59), point(0.61, 1.22), point(0.86, 1.93), point(1.20, 2.56), point(1.51, 3.17), + point(1.85, 3.76), point(2.14, 4.26), point(2.60, 4.56), point(3.07, 4.55), point(3.61, 4.30), + point(3.95, 4.01), point(4.29, 3.68), point(4.90, 3.25), point(5.54, 3.10), point(6.24, 3.18), + point(6.88, 3.54), point(7.51, 4.25), point(7.85, 4.93), point(8.03, 5.73), point(8.16, 6.52), + point(8.31, 7.28), point(8.45, 7.93), point(8.68, 8.45), point(8.96, 8.96), point(9.32, 9.36)}; + + auto trajectory = + autoware::trajectory::Trajectory::Builder{}.build(points); + + if (!trajectory) { + return 1; + } + + std::cout << "length: " << trajectory->length() << std::endl; + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval; + shift_interval.end = -1.0; + shift_interval.lateral_offset = 0.5; + + auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval; + shift_interval.start = trajectory->length() / 4.0; + shift_interval.end = trajectory->length() * 3.0 / 4.0; + shift_interval.lateral_offset = 0.5; + auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval; + shift_interval.start = trajectory->length() * 3.0 / 4.0; + shift_interval.end = trajectory->length() / 4.0; + shift_interval.lateral_offset = 0.5; + auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval1; + shift_interval1.start = trajectory->length() / 4.0; + shift_interval1.end = trajectory->length() * 2.0 / 4.0; + shift_interval1.lateral_offset = 0.5; + + autoware::trajectory::ShiftInterval shift_interval2; + shift_interval2.start = trajectory->length() * 2.0 / 4.0; + shift_interval2.end = trajectory->length() * 3.0 / 4.0; + shift_interval2.lateral_offset = -0.5; + + auto shifted_trajectory = + autoware::trajectory::shift(*trajectory, {shift_interval1, shift_interval2}); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + return 0; +} diff --git a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp index fb1197e53ab67..0d7317f09789e 100644 --- a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp @@ -13,10 +13,15 @@ // limitations under the License. #include "autoware/trajectory/path_point.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "lanelet2_core/primitives/LineString.h" #include #include +#include + +#include #include #include @@ -69,25 +74,24 @@ int main() trajectory->align_orientation_with_trajectory_direction(); - geometry_msgs::msg::Point p1; - geometry_msgs::msg::Point p2; - p1.x = 7.5; - p1.y = 8.6; - p2.x = 10.2; - p2.y = 7.7; + lanelet::LineString2d line_string; + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 7.5, 8.6, 0.0)); + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.2, 7.7, 0.0)); - auto s = trajectory->crossed(p1, p2); - auto crossed = trajectory->compute(s.value()); + auto s = autoware::trajectory::crossed(*trajectory, line_string); + auto crossed = trajectory->compute(s.at(0)); plt.plot( - Args(std::vector{p1.x, p2.x}, std::vector{p1.y, p2.y}), + Args( + std::vector{line_string[0].x(), line_string[1].x()}, + std::vector{line_string[0].y(), line_string[1].y()}), Kwargs("color"_a = "purple")); plt.scatter( Args(crossed.pose.position.x, crossed.pose.position.y), Kwargs("label"_a = "Crossed on trajectory", "color"_a = "purple")); - trajectory->longitudinal_velocity_mps.range(s.value(), trajectory->length()).set(0.0); + trajectory->longitudinal_velocity_mps().range(s.at(0), trajectory->length()).set(0.0); std::vector x; std::vector y; diff --git a/common/autoware_trajectory/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_trajectory_point.cpp index 014f27ffc9a3a..3aead351085ee 100644 --- a/common/autoware_trajectory/examples/example_trajectory_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_point.cpp @@ -14,6 +14,9 @@ #include "autoware/trajectory/interpolator/cubic_spline.hpp" #include "autoware/trajectory/point.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "lanelet2_core/primitives/LineString.h" #include @@ -86,7 +89,8 @@ int main() p.x = 5.37; p.y = 6.0; - auto s = trajectory->closest(p); + double s = autoware::trajectory::closest(*trajectory, p); + auto closest = trajectory->compute(s); plt.scatter(Args(p.x, p.y), Kwargs("color"_a = "green")); @@ -98,18 +102,21 @@ int main() Kwargs("color"_a = "green")); } { - geometry_msgs::msg::Point p1; - geometry_msgs::msg::Point p2; - p1.x = 6.97; - p1.y = 6.36; - p2.x = 9.23; - p2.y = 5.92; - - auto s = trajectory->crossed(p1, p2); - auto crossed = trajectory->compute(s.value()); + lanelet::LineString2d line_string; + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 6.97, 6.36, 0.0)); + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 9.23, 5.92, 0.0)); + + auto s = autoware::trajectory::crossed(*trajectory, line_string); + if (s.empty()) { + std::cerr << "Failed to find a crossing point" << std::endl; + return 1; + } + auto crossed = trajectory->compute(s.at(0)); plt.plot( - Args(std::vector{p1.x, p2.x}, std::vector{p1.y, p2.y}), + Args( + std::vector{line_string[0].x(), line_string[1].x()}, + std::vector{line_string[0].y(), line_string[1].y()}), Kwargs("color"_a = "purple")); plt.scatter( diff --git a/common/autoware_trajectory/examples/example_trajectory_pose.cpp b/common/autoware_trajectory/examples/example_trajectory_pose.cpp index a6ca1310fdc42..baa26abedecc1 100644 --- a/common/autoware_trajectory/examples/example_trajectory_pose.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_pose.cpp @@ -45,7 +45,6 @@ int main() pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)}; using autoware::trajectory::Trajectory; - using autoware::trajectory::interpolator::CubicSpline; auto trajectory = Trajectory::Builder{}.build(poses); diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp similarity index 60% rename from common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp rename to common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp index 3346d4ce8104f..55981326c4db4 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp @@ -12,36 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ -#define AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ - -#include "lanelet2_core/primitives/Point.h" - -#include - -#include -#include -#include -#include +#ifndef AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ +#include #include #include namespace autoware::trajectory::detail { -/** - * @brief Convert various point types to geometry_msgs::msg::Point. - * @param p The input point to be converted. - * @return geometry_msgs::msg::Point The converted point. - */ -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::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); -geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p); - +inline namespace helpers +{ /** * @brief Merge multiple vectors into one, keeping only unique elements. * @tparam Vectors Variadic template parameter for vector types. @@ -65,11 +46,27 @@ std::vector merge_vectors(const Vectors &... vectors) return {unique_elements.begin(), unique_elements.end()}; } +/** + * @brief Ensures the vector has at least a specified number of points by linearly interpolating + * values. + * + * @param x Input vector of double values. + * @param min_points Minimum number of points required. + * @return A vector with at least `min_points` elements. + * + * @note If `x.size() >= min_points`, the input vector is returned as-is. + * + * @code + * std::vector input = {1.0, 4.0, 6.0}; + * auto result = fill_bases(input, 6); + * // result: {1.0, 2.0, 3.0, 4.0, 5.0, 6.0} + * @endcode + */ std::vector fill_bases(const std::vector & x, const size_t & min_points); std::vector crop_bases( const std::vector & x, const double & start, const double & end); - +} // namespace helpers } // namespace autoware::trajectory::detail -#endif // AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ +#endif // AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp index 1ab22d94605dc..06f1185620692 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -// clang-format off -#ifndef AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT -#define AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT -// clang-format on +#ifndef AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ +#include "autoware/trajectory/detail/logging.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" #include @@ -85,9 +84,11 @@ class InterpolatedArray */ InterpolatedArray & operator=(const InterpolatedArray & other) { - bases_ = other.bases_; - values_ = other.values_; - interpolator_ = other.interpolator_->clone(); + if (this != &other) { + bases_ = other.bases_; + values_ = other.values_; + interpolator_ = other.interpolator_->clone(); + } return *this; } @@ -152,9 +153,11 @@ class InterpolatedArray // Set the values in the specified range std::fill(values.begin() + start_index, values.begin() + end_index + 1, value); - parent_.interpolator_->build(bases, values); - - // return *this; + bool success = parent_.interpolator_->build(bases, values); + if (!success) { + throw std::runtime_error( + "Failed to build interpolator."); // This Exception should not be thrown. + } } }; @@ -168,9 +171,8 @@ class InterpolatedArray { if (start < this->start() || end > this->end()) { RCLCPP_WARN( - rclcpp::get_logger("InterpolatedArray"), - "The range [%f, %f] is out of the array range [%f, %f]", start, end, this->start(), - this->end()); + get_logger(), "The range [%f, %f] is out of the array range [%f, %f]", start, end, + this->start(), this->end()); start = std::max(start, this->start()); end = std::min(end, this->end()); } @@ -185,7 +187,11 @@ class InterpolatedArray InterpolatedArray & operator=(const T & value) { std::fill(values_.begin(), values_.end(), value); - interpolator_->build(bases_, values_); + bool success = interpolator_->build(bases_, values_); + if (!success) { + throw std::runtime_error( + "Failed to build interpolator."); // This Exception should not be thrown. + } return *this; } @@ -208,6 +214,4 @@ class InterpolatedArray } // namespace autoware::trajectory::detail -// clang-format off -#endif // AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT -// clang-format on +#endif // AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp new file mode 100644 index 0000000000000..2ad8d66a4c46d --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp @@ -0,0 +1,30 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__DETAIL__LOGGING_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__LOGGING_HPP_ + +#include +#include + +namespace autoware::trajectory +{ + +rclcpp::Logger & get_logger(); + +rclcpp::Clock & get_clock(); + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__DETAIL__LOGGING_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp new file mode 100644 index 0000000000000..f4150b7e37294 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ + +#include "lanelet2_core/primitives/Point.h" + +#include + +#include +#include +#include +#include + +namespace autoware::trajectory::detail +{ + +struct MutablePoint2d +{ + MutablePoint2d(double & x, double & y) : x(x), y(y) {} + double & x; + double & y; +}; +struct MutablePoint3d : MutablePoint2d +{ + MutablePoint3d(double & x, double & y, double & z) : MutablePoint2d{x, y}, z(z) {} + double & z; +}; + +struct ImmutablePoint2d +{ + ImmutablePoint2d(const double & x, const double & y) : x(x), y(y) {} + const double x; + const double y; +}; + +struct ImmutablePoint3d : ImmutablePoint2d +{ + ImmutablePoint3d(const double & x, const double & y, const double & z) + : ImmutablePoint2d{x, y}, z(z) + { + } + const double z; +}; + +/** + * @brief Convert various point types to geometry_msgs::msg::Point. + * @param p The input point to be converted. + * @return geometry_msgs::msg::Point The converted point. + */ +MutablePoint3d to_point(geometry_msgs::msg::Point & p); +MutablePoint3d to_point(geometry_msgs::msg::Pose & p); +MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p); +MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); +MutablePoint2d to_point(lanelet::BasicPoint2d & p); + +ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p); +ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p); +ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p); +ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); +ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p); + +} // namespace autoware::trajectory::detail + +#endif // AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/forward.hpp b/common/autoware_trajectory/include/autoware/trajectory/forward.hpp new file mode 100644 index 0000000000000..1bd9d1e4705e2 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/forward.hpp @@ -0,0 +1,26 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__FORWARD_HPP_ +#define AUTOWARE__TRAJECTORY__FORWARD_HPP_ + +namespace autoware::trajectory +{ + +template +class Trajectory; + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__FORWARD_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp index eeca7775a7ff3..df18c65e0520b 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp @@ -35,6 +35,7 @@ class InterpolatorCommonInterface { protected: std::vector bases_; ///< bases values for the interpolation. + bool is_built_{false}; ///< flag indicating whether the interpolator has been built. /** * @brief Get the start of the interpolation range. @@ -124,7 +125,7 @@ class InterpolatorCommonInterface * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - bool build(const std::vector & bases, const std::vector & values) + [[nodiscard]] bool build(const std::vector & bases, const std::vector & values) { if (bases.size() != values.size()) { return false; @@ -133,9 +134,17 @@ class InterpolatorCommonInterface return false; } build_impl(bases, values); + is_built_ = true; return true; } + /** + * @brief Check if the interpolator has been built. + * + * @return True if the interpolator has been built, false otherwise. + */ + [[nodiscard]] bool is_built() const { return is_built_; } + /** * @brief Get the minimum number of required points for the interpolator. * @@ -150,10 +159,15 @@ class InterpolatorCommonInterface * * @param s The point at which to compute the interpolated value. * @return The interpolated value. + * @throw std::runtime_error if the interpolator has not been built. */ [[nodiscard]] T compute(const double & s) const { - double clamped_s = validate_compute_input(s); + if (!is_built_) { + throw std::runtime_error( + "Interpolator has not been built."); // This Exception should not be thrown. + } + const double clamped_s = validate_compute_input(s); return compute_impl(clamped_s); } }; diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp index c9456ecf8a020..98e46e5467a89 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp @@ -72,7 +72,7 @@ struct InterpolatorMixin : public InterpolatorInterface [[nodiscard]] std::optional build(Args &&... args) { auto interpolator = InterpolatorType(std::forward(args)...); - bool success = interpolator.build(bases_, values_); + const bool success = interpolator.build(bases_, values_); if (!success) { return std::nullopt; } diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp index 86dbbe8cdbde4..1f207822e23a0 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp @@ -73,7 +73,7 @@ class InterpolatorInterface : public detail::InterpolatorCommonInterface */ [[nodiscard]] double compute_first_derivative(const double & s) const { - double clamped_s = this->validate_compute_input(s); + const double clamped_s = this->validate_compute_input(s); return compute_first_derivative_impl(clamped_s); } @@ -85,7 +85,7 @@ class InterpolatorInterface : public detail::InterpolatorCommonInterface */ [[nodiscard]] double compute_second_derivative(const double & s) const { - double clamped_s = this->validate_compute_input(s); + const double clamped_s = this->validate_compute_input(s); return compute_second_derivative_impl(clamped_s); } diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp index eb4ffd6f46207..d7295570b53b9 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp @@ -16,9 +16,10 @@ #define AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ #include "autoware/trajectory/detail/interpolated_array.hpp" -#include "autoware/trajectory/interpolator/stairstep.hpp" #include "autoware/trajectory/pose.hpp" +#include + #include #include #include @@ -32,11 +33,46 @@ class Trajectory using BaseClass = Trajectory; using PointType = autoware_planning_msgs::msg::PathPoint; + std::shared_ptr> + longitudinal_velocity_mps_; //!< Longitudinal velocity in m/s + std::shared_ptr> + lateral_velocity_mps_; //!< Lateral velocity in m/s + std::shared_ptr> + heading_rate_rps_; //!< Heading rate in rad/s}; + public: - detail::InterpolatedArray longitudinal_velocity_mps{ - nullptr}; //!< Longitudinal velocity in m/s - detail::InterpolatedArray lateral_velocity_mps{nullptr}; //!< Lateral velocity in m/s - detail::InterpolatedArray heading_rate_rps{nullptr}; //!< Heading rate in rad/s}; + Trajectory(); + ~Trajectory() override = default; + Trajectory(const Trajectory & rhs); + Trajectory(Trajectory && rhs) = default; + Trajectory & operator=(const Trajectory & rhs); + Trajectory & operator=(Trajectory && rhs) = default; + + [[nodiscard]] std::vector get_internal_bases() const override; + + detail::InterpolatedArray & longitudinal_velocity_mps() + { + return *longitudinal_velocity_mps_; + } + + detail::InterpolatedArray & lateral_velocity_mps() { return *lateral_velocity_mps_; } + + detail::InterpolatedArray & heading_rate_rps() { return *heading_rate_rps_; } + + [[nodiscard]] const detail::InterpolatedArray & longitudinal_velocity_mps() const + { + return *longitudinal_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & lateral_velocity_mps() const + { + return *lateral_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & heading_rate_rps() const + { + return *heading_rate_rps_; + } /** * @brief Build the trajectory from the points @@ -65,16 +101,7 @@ class Trajectory std::unique_ptr trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - set_xy_interpolator(); - set_z_interpolator(); - set_orientation_interpolator(); - set_longitudinal_velocity_interpolator>(); - set_lateral_velocity_interpolator>(); - set_heading_rate_interpolator>(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) @@ -105,7 +132,7 @@ class Trajectory template Builder & set_longitudinal_velocity_interpolator(Args &&... args) { - trajectory_->longitudinal_velocity_mps = detail::InterpolatedArray( + trajectory_->longitudinal_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -113,7 +140,7 @@ class Trajectory template Builder & set_lateral_velocity_interpolator(Args &&... args) { - trajectory_->lateral_velocity_mps = detail::InterpolatedArray( + trajectory_->lateral_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -121,7 +148,7 @@ class Trajectory template Builder & set_heading_rate_interpolator(Args &&... args) { - trajectory_->heading_rate_rps = detail::InterpolatedArray( + trajectory_->heading_rate_rps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp index 0ed92a10a7ea4..d70d8443a511d 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp @@ -16,9 +16,8 @@ #define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ #include "autoware/trajectory/path_point.hpp" -#include "autoware/trajectory/point.hpp" -#include +#include #include #include @@ -27,15 +26,29 @@ namespace autoware::trajectory { template <> -class Trajectory +class Trajectory : public Trajectory { using BaseClass = Trajectory; - using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; + using PointType = autoware_internal_planning_msgs::msg::PathPointWithLaneId; using LaneIdType = std::vector; + std::shared_ptr> lane_ids_; //!< Lane ID + public: - detail::InterpolatedArray lane_ids{nullptr}; //!< Lane ID + Trajectory(); + ~Trajectory() override = default; + Trajectory(const Trajectory & rhs) = default; + Trajectory(Trajectory && rhs) = default; + Trajectory & operator=(const Trajectory & rhs); + Trajectory & operator=(Trajectory && rhs) = default; + + detail::InterpolatedArray & lane_ids() { return *lane_ids_; } + + [[nodiscard]] const detail::InterpolatedArray & lane_ids() const + { + return *lane_ids_; + } /** * @brief Build the trajectory from the points @@ -44,6 +57,8 @@ class Trajectory */ bool build(const std::vector & points); + [[nodiscard]] std::vector get_internal_bases() const override; + /** * @brief Compute the point on the trajectory at a given s value * @param s Arc length @@ -64,17 +79,7 @@ class Trajectory std::unique_ptr trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - set_xy_interpolator(); - set_z_interpolator(); - set_orientation_interpolator(); - set_longitudinal_velocity_interpolator>(); - set_lateral_velocity_interpolator>(); - set_heading_rate_interpolator>(); - set_lane_ids_interpolator>(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) @@ -105,7 +110,7 @@ class Trajectory template Builder & set_longitudinal_velocity_interpolator(Args &&... args) { - trajectory_->longitudinal_velocity_mps = detail::InterpolatedArray( + trajectory_->longitudinal_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -113,7 +118,7 @@ class Trajectory template Builder & set_lateral_velocity_interpolator(Args &&... args) { - trajectory_->lateral_velocity_mps = detail::InterpolatedArray( + trajectory_->lateral_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -121,7 +126,7 @@ class Trajectory template Builder & set_heading_rate_interpolator(Args &&... args) { - trajectory_->heading_rate_rps = detail::InterpolatedArray( + trajectory_->heading_rate_rps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -129,7 +134,7 @@ class Trajectory template Builder & set_lane_ids_interpolator(Args &&... args) { - trajectory_->lane_ids = detail::InterpolatedArray( + trajectory_->lane_ids_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp index 3e763e2428f1b..af47c404f7351 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp @@ -15,18 +15,14 @@ #ifndef AUTOWARE__TRAJECTORY__POINT_HPP_ #define AUTOWARE__TRAJECTORY__POINT_HPP_ -#include "autoware/trajectory/detail/utils.hpp" -#include "autoware/trajectory/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/forward.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" -#include "autoware/trajectory/interpolator/linear.hpp" #include #include -#include #include -#include #include #include #include @@ -34,10 +30,6 @@ namespace autoware::trajectory { - -template -class Trajectory; - /** * @brief Trajectory class for geometry_msgs::msg::Point */ @@ -58,8 +50,6 @@ class Trajectory double start_{0.0}, end_{0.0}; //!< Start and end of the arc length of the trajectory - using ConstraintFunction = std::function; - /** * @brief Validate the arc length is within the trajectory * @param s Arc length @@ -67,6 +57,18 @@ class Trajectory [[nodiscard]] double clamp(const double & s, bool show_warning = false) const; public: + Trajectory(); + virtual ~Trajectory() = default; + Trajectory(const Trajectory & rhs); + Trajectory(Trajectory && rhs) = default; + Trajectory & operator=(const Trajectory & rhs); + Trajectory & operator=(Trajectory && rhs) = default; + + /** + * @brief Get the internal bases(arc lengths) of the trajectory + * @return Vector of bases(arc lengths) + */ + [[nodiscard]] virtual std::vector get_internal_bases() const; /** * @brief Get the length of the trajectory * @return Length of the trajectory @@ -108,137 +110,6 @@ class Trajectory */ [[nodiscard]] double curvature(double s) const; - /** - * @brief Find the closest point with constraint - * @tparam InputPointType Type of input point - * @param p Input point - * @param constraints Constraint function - * @return Optional arc length of the closest point - */ - template - [[nodiscard]] std::optional closest_with_constraint( - const InputPointType & p, const ConstraintFunction & constraints) const - { - using trajectory::detail::to_point; - Eigen::Vector2d point(to_point(p).x, to_point(p).y); - std::vector distances_from_segments; - std::vector lengths_from_start_points; - - auto axis = detail::crop_bases(bases_, start_, end_); - - for (size_t i = 1; i < axis.size(); ++i) { - Eigen::Vector2d p0; - Eigen::Vector2d p1; - p0 << x_interpolator_->compute(axis.at(i - 1)), y_interpolator_->compute(axis.at(i - 1)); - p1 << x_interpolator_->compute(axis.at(i)), y_interpolator_->compute(axis.at(i)); - Eigen::Vector2d v = p1 - p0; - Eigen::Vector2d w = point - p0; - double c1 = w.dot(v); - double c2 = v.dot(v); - double length_from_start_point = NAN; - double distance_from_segment = NAN; - if (c1 <= 0) { - length_from_start_point = axis.at(i - 1); - distance_from_segment = (point - p0).norm(); - } else if (c2 <= c1) { - length_from_start_point = axis.at(i); - distance_from_segment = (point - p1).norm(); - } else { - length_from_start_point = axis.at(i - 1) + c1 / c2 * (p1 - p0).norm(); - distance_from_segment = (point - (p0 + (c1 / c2) * v)).norm(); - } - if (constraints(length_from_start_point)) { - distances_from_segments.push_back(distance_from_segment); - lengths_from_start_points.push_back(length_from_start_point); - } - } - if (distances_from_segments.empty()) { - return std::nullopt; - } - - auto min_it = std::min_element(distances_from_segments.begin(), distances_from_segments.end()); - - return lengths_from_start_points[std::distance(distances_from_segments.begin(), min_it)] - - start_; - } - - /** - * @brief Find the closest point - * @tparam InputPointType Type of input point - * @param p Input point - * @return Arc length of the closest point - */ - template - [[nodiscard]] double closest(const InputPointType & p) const - { - auto s = closest_with_constraint(p, [](const double &) { return true; }); - return *s; - } - /** - * @brief Find the crossing point with constraint - * @tparam InputPointType Type of input point - * @param start Start point - * @param end End point - * @param constraints Constraint function - * @return Optional arc length of the crossing point - */ - template - [[nodiscard]] std::optional crossed_with_constraint( - const InputPointType & start, const InputPointType & end, - const ConstraintFunction & constraints) const - { - using trajectory::detail::to_point; - Eigen::Vector2d line_start(to_point(start).x, to_point(start).y); - Eigen::Vector2d line_end(to_point(end).x, to_point(end).y); - Eigen::Vector2d line_dir = line_end - line_start; - - auto axis = detail::crop_bases(bases_, start_, end_); - - for (size_t i = 1; i < axis.size(); ++i) { - Eigen::Vector2d p0; - Eigen::Vector2d p1; - p0 << x_interpolator_->compute(axis.at(i - 1)), y_interpolator_->compute(axis.at(i - 1)); - p1 << x_interpolator_->compute(axis.at(i)), y_interpolator_->compute(axis.at(i)); - - Eigen::Vector2d segment_dir = p1 - p0; - - double det = segment_dir.x() * line_dir.y() - segment_dir.y() * line_dir.x(); - - if (std::abs(det) < 1e-10) { - continue; - } - - Eigen::Vector2d p0_to_line_start = line_start - p0; - - double t = (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; - double u = - (p0_to_line_start.x() * segment_dir.y() - p0_to_line_start.y() * segment_dir.x()) / det; - - if (t >= 0.0 && t <= 1.0 && u >= 0.0 && u <= 1.0) { - double intersection_s = axis.at(i - 1) + t * (axis.at(i) - axis.at(i - 1)); - if (constraints(intersection_s)) { - return intersection_s - start_; - } - } - } - - return std::nullopt; - } - - /** - * @brief Find the crossing point - * @tparam InputPointType Type of input point - * @param start Start point - * @param end End point - * @return Optional arc length of the crossing point - */ - template - [[nodiscard]] std::optional crossed( - const InputPointType & start, const InputPointType & end) const - { - return crossed_with_constraint(start, end, [](const double &) { return true; }); - } - /** * @brief Restore the trajectory points * @param min_points Minimum number of points @@ -254,13 +125,7 @@ class Trajectory std::unique_ptr trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - // Default interpolators - set_xy_interpolator(); - set_z_interpolator(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp index 8acc7b48dc8e4..911db11bf8dfc 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__TRAJECTORY__POSE_HPP_ #define AUTOWARE__TRAJECTORY__POSE_HPP_ -#include "autoware/trajectory/interpolator/cubic_spline.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" -#include "autoware/trajectory/interpolator/spherical_linear.hpp" #include "autoware/trajectory/point.hpp" +#include #include #include @@ -43,8 +42,21 @@ class Trajectory : public Trajectory & points); + /** + * @brief Get the internal bases(arc lengths) of the trajectory + * @return Vector of bases(arc lengths) + */ + [[nodiscard]] std::vector get_internal_bases() const override; + /** * @brief Compute the pose on the trajectory at a given s value * @param s Arc length @@ -69,13 +81,7 @@ class Trajectory : public Trajectory trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - set_xy_interpolator(); - set_z_interpolator(); - set_orientation_interpolator(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp new file mode 100644 index 0000000000000..842e743271318 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp @@ -0,0 +1,100 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ + +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +#include +#include +#include + +namespace autoware::trajectory +{ +namespace detail::impl +{ +/** + * @brief Internal implementation to find the closest point on a trajectory to a given point with + * constraints. + * @param trajectory_compute A function that computes a 2D point on the trajectory for a given + * parameter `s`. + * @param bases A vector of double values representing the sequence of bases for the trajectory. + * @param point The 2D point to which the closest point on the trajectory is to be found. + * @param constraint A function that evaluates whether a given parameter `s` satisfies the + * constraint. + * @return An optional double value representing the parameter `s` of the closest point on the + * trajectory that satisfies the constraint, or `std::nullopt` if no such point exists. + */ +std::optional closest_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, const Eigen::Vector2d & point, + const std::function & constraint); +} // namespace detail::impl + +/** + * @brief Finds the closest point on a trajectory to a given point where the given constraint is + * satisfied. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam ArgPointType The type of the input point. + * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. + * @param trajectory The trajectory to evaluate. + * @param point The point to which the closest point on the trajectory is to be found. + * @param constraint The constraint to apply to each point in the trajectory. + * @return An optional double value representing the parameter `s` of the closest point on the + * trajectory that satisfies the constraint, or `std::nullopt` if no such point exists. + */ +template +std::optional closest_with_constraint( + const trajectory::Trajectory & trajectory, const ArgPointType & point, + const Constraint & constraint) +{ + using autoware::trajectory::detail::to_point; + + return detail::impl::closest_with_constraint_impl( + [&trajectory](const double & s) { + TrajectoryPointType point = trajectory.compute(s); + Eigen::Vector2d result; + result << to_point(point).x, to_point(point).y; + return result; + }, + trajectory.get_internal_bases(), {to_point(point).x, to_point(point).y}, + [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); +} + +/** + * @brief Finds the closest point on a trajectory to a given point. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam ArgPointType The type of the input point. + * @param trajectory The trajectory to evaluate. + * @param point The point to which the closest point on the trajectory is to be found. + * @return The parameter `s` of the closest point on the trajectory. + */ +template +double closest( + const trajectory::Trajectory & trajectory, const ArgPointType & point) +{ + std::optional s = + *closest_with_constraint(trajectory, point, [](const TrajectoryPointType &) { return true; }); + if (!s) { + throw std::runtime_error("No closest point found."); // This Exception should not be thrown. + } + return *s; +} +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp new file mode 100644 index 0000000000000..1ce75c67d8128 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp @@ -0,0 +1,33 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__CROP_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__CROP_HPP_ + +#include "autoware/trajectory/forward.hpp" + +namespace autoware::trajectory +{ + +template +trajectory::Trajectory crop( + trajectory::Trajectory trajectory, const double & start, const double & end) +{ + trajectory.crop(start, end); + return trajectory; +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__CROP_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp new file mode 100644 index 0000000000000..cb3406218800e --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp @@ -0,0 +1,118 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +#include +#include + +namespace autoware::trajectory +{ +namespace detail::impl +{ + +/** + * @brief Internal implementation to find intersections between a trajectory and a linestring with + * constraints. + * @param trajectory_compute A function that computes a 2D point on the trajectory for a given + * parameter `s`. + * @param bases A vector of double values representing the sequence of bases for the trajectory. + * @param linestring A vector of pairs representing the linestring as a sequence of 2D line + * segments. + * @param constraint A function that evaluates whether a given parameter `s` satisfies the + * constraint. + * @return A vector of double values representing the parameters `s` where the trajectory intersects + * the linestring and satisfies the constraint. + */ +std::vector crossed_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, // + const std::vector> & linestring, + const std::function & constraint); +} // namespace detail::impl + +/** + * @brief Finds intersections between a trajectory and a linestring where the given constraint is + * satisfied. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam LineStringType The type of the linestring. + * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. + * @param trajectory The trajectory to evaluate. + * @param linestring The linestring to intersect with the trajectory. + * @param constraint The constraint to apply to each point in the trajectory. + * @return A vector of double values representing the parameters `s` where the trajectory intersects + * the linestring and satisfies the constraint. + */ +template +[[nodiscard]] std::vector crossed_with_constraint( + const trajectory::Trajectory & trajectory, const LineStringType & linestring, + const Constraint & constraint) +{ + using autoware::trajectory::detail::to_point; + + std::function trajectory_compute = + [&trajectory](const double & s) { + TrajectoryPointType point = trajectory.compute(s); + Eigen::Vector2d result; + result << to_point(point).x, to_point(point).y; + return result; + }; + + std::vector> linestring_eigen; + + if (linestring.end() - linestring.begin() < 2) { + return {}; + } + + auto point_it = linestring.begin(); + auto point_it_next = linestring.begin() + 1; + + for (; point_it_next != linestring.end(); ++point_it, ++point_it_next) { + Eigen::Vector2d start; + Eigen::Vector2d end; + start << point_it->x(), point_it->y(); + end << point_it_next->x(), point_it_next->y(); + linestring_eigen.emplace_back(start, end); + } + + return detail::impl::crossed_with_constraint_impl( + trajectory_compute, trajectory.get_internal_bases(), linestring_eigen, + [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); +} + +/** + * @brief Finds intersections between a trajectory and a linestring. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam LineStringType The type of the linestring. + * @param trajectory The trajectory to evaluate. + * @param linestring The linestring to intersect with the trajectory. + * @return A vector of double values representing the parameters `s` where the trajectory intersects + * the linestring. + */ +template +[[nodiscard]] std::vector crossed( + const trajectory::Trajectory & trajectory, const LineStringType & linestring) +{ + return crossed_with_constraint( + trajectory, linestring, [](const TrajectoryPointType &) { return true; }); +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp new file mode 100644 index 0000000000000..ad816d0cd7917 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp @@ -0,0 +1,76 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ + +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +#include +#include +namespace autoware::trajectory +{ + +/** + * @struct Interval + * @brief Represents an interval with a start and end value. + */ +struct Interval +{ + double start; ///< Start value of the interval. + double end; ///< End value of the interval. +}; + +namespace detail::impl +{ + +/** + * @brief Internal implementation to find intervals in a sequence of bases that satisfy a + * constraint. + * @param bases A vector of double values representing the sequence of bases. + * @param constraint A function that evaluates whether a given base satisfies the constraint. + * @return A vector of Interval objects representing the intervals where the constraint is + * satisfied. + */ +std::vector find_intervals_impl( + const std::vector & bases, const std::function & constraint); + +} // namespace detail::impl + +/** + * @brief Finds intervals in a trajectory where the given constraint is satisfied. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. + * @param trajectory The trajectory to evaluate. + * @param constraint The constraint to apply to each point in the trajectory. + * @return A vector of Interval objects representing the intervals where the constraint is + * satisfied. + */ +template +std::vector find_intervals( + const Trajectory & trajectory, const Constraint & constraint) +{ + using autoware::trajectory::detail::to_point; + + return detail::impl::find_intervals_impl( + trajectory.get_internal_bases(), + [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp new file mode 100644 index 0000000000000..1fa7f2401d31d --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp @@ -0,0 +1,119 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ + +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +namespace autoware::trajectory +{ + +/** + * @struct ShiftInterval + * @brief Represents an interval for shifting a trajectory. + */ +struct ShiftInterval +{ + double start{0.0}; ///< Start position of the shift interval. + double end{0.0}; ///< End position of the shift interval. + double lateral_offset{0.0}; ///< Length of the shift to be applied. +}; + +/** + * @struct ShiftParameters + * @brief Represents parameters for shifting a trajectory. + */ +struct ShiftParameters +{ + double velocity{0.0}; ///< Velocity parameter for the shift. + double longitudinal_acc{0.0}; ///< Longitudinal acceleration parameter for the shift. + double lateral_acc_limit{-1.0}; ///< Lateral acceleration limit for the shift. +}; + +namespace detail::impl +{ + +/** + * @brief Internal implementation to apply a shift to a trajectory. + * @param bases A vector of double values representing the sequence of bases for the trajectory. + * @param shift_lengths A pointer to a vector of double values representing the shift lengths to be + * applied. + * @param shift_interval The interval over which the shift is applied. + * @param shift_parameters The parameters for the shift. + */ +void shift_impl( + const std::vector & bases, std::vector * shift_lengths, + const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters); + +} // namespace detail::impl + +/** + * @brief Shifts a trajectory based on the provided shift intervals and parameters. + * @tparam PointType The type of points in the trajectory. + * @param reference_trajectory The reference trajectory to be shifted. + * @param shift_intervals A vector of ShiftInterval objects representing the intervals for shifting. + * @param shift_parameters The parameters for the shift. + * @return The shifted trajectory. + */ +template +trajectory::Trajectory shift( + const trajectory::Trajectory & reference_trajectory, + const std::vector & shift_intervals, const ShiftParameters & shift_parameters = {}) +{ + auto bases = reference_trajectory.get_internal_bases(); + std::vector shift_lengths(bases.size(), 0.0); + for (const auto & shift_interval : shift_intervals) { + detail::impl::shift_impl(bases, &shift_lengths, shift_interval, shift_parameters); + } + // Apply shift. + std::vector shifted_points; + for (size_t i = 0; i < bases.size(); ++i) { + shifted_points.emplace_back(reference_trajectory.compute(bases.at(i))); + double azimuth = reference_trajectory.azimuth(bases.at(i)); + const double shift_length = shift_lengths.at(i); + detail::to_point(shifted_points.back()).x += std::sin(azimuth) * shift_length; + detail::to_point(shifted_points.back()).y -= std::cos(azimuth) * shift_length; + } + auto shifted_trajectory = reference_trajectory; + const bool valid = shifted_trajectory.build(shifted_points); + if (!valid) { + throw std::runtime_error( + "Failed to build shifted trajectory"); // This Exception should not be thrown. + } + return shifted_trajectory; +} + +/** + * @brief Shifts a trajectory based on a single shift interval and parameters. + * @tparam PointType The type of points in the trajectory. + * @param reference_trajectory The reference trajectory to be shifted. + * @param shift_interval The interval for shifting. + * @param shift_parameters The parameters for the shift. + * @return The shifted trajectory. + */ +template +trajectory::Trajectory shift( + const trajectory::Trajectory & reference_trajectory, + const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters = {}) +{ + return shift(reference_trajectory, std::vector{shift_interval}, shift_parameters); +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml index 230e83bbc3550..8ce9cedd40d8f 100644 --- a/common/autoware_trajectory/package.xml +++ b/common/autoware_trajectory/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_planning_msgs autoware_planning_msgs geometry_msgs lanelet2_core diff --git a/common/autoware_trajectory/src/detail/logging.cpp b/common/autoware_trajectory/src/detail/logging.cpp new file mode 100644 index 0000000000000..3656a38fcc50a --- /dev/null +++ b/common/autoware_trajectory/src/detail/logging.cpp @@ -0,0 +1,32 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/detail/logging.hpp" + +namespace autoware::trajectory +{ + +rclcpp::Logger & get_logger() +{ + static rclcpp::Logger logger = rclcpp::get_logger("autoware_trajectory"); + return logger; +} + +rclcpp::Clock & get_clock() +{ + static rclcpp::Clock clock(RCL_ROS_TIME); + return clock; +} + +} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/src/detail/types.cpp b/common/autoware_trajectory/src/detail/types.cpp new file mode 100644 index 0000000000000..9c3174f608a1a --- /dev/null +++ b/common/autoware_trajectory/src/detail/types.cpp @@ -0,0 +1,68 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/detail/types.hpp" + +namespace autoware::trajectory::detail +{ +MutablePoint3d to_point(geometry_msgs::msg::Point & p) +{ + return {p.x, p.y, p.z}; +} + +MutablePoint3d to_point(geometry_msgs::msg::Pose & p) +{ + return {p.position.x, p.position.y, p.position.z}; +} + +MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + +MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) +{ + return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; +} + +MutablePoint2d to_point(lanelet::BasicPoint2d & p) +{ + return {p.x(), p.y()}; +} + +ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p) +{ + return {p.x, p.y, p.z}; +} + +ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p) +{ + return {p.position.x, p.position.y, p.position.z}; +} + +ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + +ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) +{ + return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; +} + +ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p) +{ + return {p.x(), p.y()}; +} +} // namespace autoware::trajectory::detail diff --git a/common/autoware_trajectory/src/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp index 4c9649ef1f3ab..58b54cb5ef76b 100644 --- a/common/autoware_trajectory/src/detail/util.cpp +++ b/common/autoware_trajectory/src/detail/util.cpp @@ -12,64 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/helpers.hpp" #include #include namespace autoware::trajectory::detail { - -geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p) -{ - return p; -} - -geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p) -{ - return p.position; -} - -geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p) -{ - geometry_msgs::msg::Point point; - point.x = p(0); - point.y = p(1); - return point; -} - -geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p) -{ - geometry_msgs::msg::Point point; - point.x = p.pose.position.x; - point.y = p.pose.position.y; - return point; -} - -geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p) -{ - geometry_msgs::msg::Point point; - point.x = p.point.pose.position.x; - point.y = p.point.pose.position.y; - return point; -} - -geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p) +inline namespace helpers { - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - return point; -} - -geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - return point; -} - std::vector fill_bases(const std::vector & x, const size_t & min_points) { const auto original_size = x.size(); @@ -115,7 +66,7 @@ std::vector crop_bases( } // Copy all points within the range [start, end] - for (double i : x) { + for (const double i : x) { if (i >= start && i <= end) { result.push_back(i); } @@ -128,5 +79,5 @@ std::vector crop_bases( return result; } - +} // namespace helpers } // namespace autoware::trajectory::detail diff --git a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp index ed3ba62031185..f255135ae898b 100644 --- a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp +++ b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp @@ -41,8 +41,8 @@ geometry_msgs::msg::Quaternion SphericalLinear::compute_impl(const double & s) c const double t = (s - x0) / (x1 - x0); // Convert quaternions to Eigen vectors for calculation - Eigen::Quaterniond q0(y0.w, y0.x, y0.y, y0.z); - Eigen::Quaterniond q1(y1.w, y1.x, y1.y, y1.z); + const Eigen::Quaterniond q0(y0.w, y0.x, y0.y, y0.z); + const Eigen::Quaterniond q1(y1.w, y1.x, y1.y, y1.z); // Perform Slerp Eigen::Quaterniond q_slerp = q0.slerp(t, q1); diff --git a/common/autoware_trajectory/src/path_point.cpp b/common/autoware_trajectory/src/path_point.cpp index 0806bcead53f3..d264d1d054d87 100644 --- a/common/autoware_trajectory/src/path_point.cpp +++ b/common/autoware_trajectory/src/path_point.cpp @@ -14,10 +14,15 @@ #include "autoware/trajectory/path_point.hpp" -#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/forward.hpp" +#include "autoware/trajectory/interpolator/stairstep.hpp" +#include "autoware/trajectory/pose.hpp" #include +#include #include namespace autoware::trajectory @@ -25,6 +30,37 @@ namespace autoware::trajectory using PointType = autoware_planning_msgs::msg::PathPoint; +Trajectory::Trajectory() +: longitudinal_velocity_mps_(std::make_shared>( + std::make_shared>())), + lateral_velocity_mps_(std::make_shared>( + std::make_shared>())), + heading_rate_rps_(std::make_shared>( + std::make_shared>())) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: BaseClass(rhs), + longitudinal_velocity_mps_( + std::make_shared>(*rhs.longitudinal_velocity_mps_)), + lateral_velocity_mps_( + std::make_shared>(*rhs.lateral_velocity_mps_)), + heading_rate_rps_(std::make_shared>(*rhs.heading_rate_rps_)) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + *longitudinal_velocity_mps_ = *rhs.longitudinal_velocity_mps_; + *lateral_velocity_mps_ = *rhs.lateral_velocity_mps_; + *heading_rate_rps_ = *rhs.heading_rate_rps_; + } + return *this; +} + bool Trajectory::build(const std::vector & points) { std::vector poses; @@ -42,49 +78,52 @@ bool Trajectory::build(const std::vector & points) bool is_valid = true; is_valid &= Trajectory::build(poses); - is_valid &= this->longitudinal_velocity_mps.build(bases_, longitudinal_velocity_mps_values); - is_valid &= this->lateral_velocity_mps.build(bases_, lateral_velocity_mps_values); - is_valid &= this->heading_rate_rps.build(bases_, heading_rate_rps_values); + is_valid &= this->longitudinal_velocity_mps().build(bases_, longitudinal_velocity_mps_values); + is_valid &= this->lateral_velocity_mps().build(bases_, lateral_velocity_mps_values); + is_valid &= this->heading_rate_rps().build(bases_, heading_rate_rps_values); return is_valid; } +std::vector Trajectory::get_internal_bases() const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()), + get_bases(this->heading_rate_rps())); + + bases = detail::crop_bases(bases, start_, end_); + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + PointType Trajectory::compute(double s) const { PointType result; result.pose = Trajectory::compute(s); s = clamp(s); - result.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); - result.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); - result.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); + result.longitudinal_velocity_mps = + static_cast(this->longitudinal_velocity_mps().compute(s)); + result.lateral_velocity_mps = static_cast(this->lateral_velocity_mps().compute(s)); + result.heading_rate_rps = static_cast(this->heading_rate_rps().compute(s)); return result; } std::vector Trajectory::restore(const size_t & min_points) const { - auto get_bases = [](const auto & interpolated_array) { - auto [bases, values] = interpolated_array.get_data(); - return bases; - }; - - auto bases = detail::merge_vectors( - bases_, get_bases(this->longitudinal_velocity_mps), get_bases(this->lateral_velocity_mps), - get_bases(this->heading_rate_rps)); - - bases = detail::crop_bases(bases, start_, end_); + std::vector bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.pose = Trajectory::compute(s - start_); - p.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); - p.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); - p.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); - points.emplace_back(p); + points.emplace_back(compute(s)); } - return points; } diff --git a/common/autoware_trajectory/src/path_point_with_lane_id.cpp b/common/autoware_trajectory/src/path_point_with_lane_id.cpp index c3d7441fef405..96d3cfd91a027 100644 --- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp +++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp @@ -14,14 +14,31 @@ #include "autoware/trajectory/path_point_with_lane_id.hpp" -#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/interpolator/stairstep.hpp" +#include #include namespace autoware::trajectory { -using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; +using PointType = autoware_internal_planning_msgs::msg::PathPointWithLaneId; + +Trajectory::Trajectory() +: lane_ids_(std::make_shared>( + std::make_shared>())) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + lane_ids_ = std::make_shared>(this->lane_ids()); + } + return *this; +} bool Trajectory::build(const std::vector & points) { @@ -34,40 +51,46 @@ bool Trajectory::build(const std::vector & points) } bool is_valid = true; is_valid &= BaseClass::build(path_points); - is_valid &= lane_ids.build(bases_, lane_ids_values); + is_valid &= lane_ids().build(bases_, lane_ids_values); return is_valid; } +std::vector Trajectory::get_internal_bases() const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()), + get_bases(this->heading_rate_rps()), get_bases(this->lane_ids())); + + bases = detail::crop_bases(bases, start_, end_); + + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + PointType Trajectory::compute(double s) const { PointType result; result.point = BaseClass::compute(s); s = clamp(s); - result.lane_ids = lane_ids.compute(s); + result.lane_ids = lane_ids().compute(s); return result; } std::vector Trajectory::restore(const size_t & min_points) const { - auto get_bases = [](const auto & interpolated_array) { - auto [bases, values] = interpolated_array.get_data(); - return bases; - }; - - auto bases = detail::merge_vectors( - bases_, get_bases(this->longitudinal_velocity_mps), get_bases(this->lateral_velocity_mps), - get_bases(this->heading_rate_rps), get_bases(this->lane_ids)); - - bases = detail::crop_bases(bases, start_, end_); + auto bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.point = BaseClass::compute(s - start_); - p.lane_ids = lane_ids.compute(s); - points.emplace_back(p); + points.emplace_back(compute(s)); } return points; } diff --git a/common/autoware_trajectory/src/point.cpp b/common/autoware_trajectory/src/point.cpp index 40c6c357e9ba5..405bc7e7a2733 100644 --- a/common/autoware_trajectory/src/point.cpp +++ b/common/autoware_trajectory/src/point.cpp @@ -14,7 +14,9 @@ #include "autoware/trajectory/point.hpp" -#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/interpolator/linear.hpp" #include #include @@ -22,6 +24,7 @@ #include #include #include +#include #include namespace autoware::trajectory @@ -29,20 +32,51 @@ namespace autoware::trajectory using PointType = geometry_msgs::msg::Point; +Trajectory::Trajectory() +: x_interpolator_(std::make_shared()), + y_interpolator_(std::make_shared()), + z_interpolator_(std::make_shared()) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: x_interpolator_(rhs.x_interpolator_->clone()), + y_interpolator_(rhs.y_interpolator_->clone()), + z_interpolator_(rhs.z_interpolator_->clone()), + bases_(rhs.bases_), + start_(rhs.start_), + end_(rhs.end_) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + x_interpolator_ = rhs.x_interpolator_->clone(); + y_interpolator_ = rhs.y_interpolator_->clone(); + z_interpolator_ = rhs.z_interpolator_->clone(); + bases_ = rhs.bases_; + start_ = rhs.start_; + end_ = rhs.end_; + } + return *this; +} + bool Trajectory::build(const std::vector & points) { std::vector xs; std::vector ys; std::vector zs; + bases_.clear(); bases_.emplace_back(0.0); xs.emplace_back(points[0].x); ys.emplace_back(points[0].y); zs.emplace_back(points[0].z); for (size_t i = 1; i < points.size(); ++i) { - Eigen::Vector2d p0(points[i - 1].x, points[i - 1].y); - Eigen::Vector2d p1(points[i].x, points[i].y); + const Eigen::Vector2d p0(points[i - 1].x, points[i - 1].y); + const Eigen::Vector2d p1(points[i].x, points[i].y); bases_.emplace_back(bases_.back() + (p1 - p0).norm()); xs.emplace_back(points[i].x); ys.emplace_back(points[i].y); @@ -70,6 +104,14 @@ double Trajectory::clamp(const double & s, bool show_warning) const return std::clamp(s, 0.0, length()) + start_; } +std::vector Trajectory::get_internal_bases() const +{ + auto bases = detail::crop_bases(bases_, start_, end_); + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + double Trajectory::length() const { return end_ - start_; @@ -88,40 +130,36 @@ PointType Trajectory::compute(double s) const double Trajectory::azimuth(double s) const { s = clamp(s, true); - double dx = x_interpolator_->compute_first_derivative(s); - double dy = y_interpolator_->compute_first_derivative(s); + const double dx = x_interpolator_->compute_first_derivative(s); + const double dy = y_interpolator_->compute_first_derivative(s); return std::atan2(dy, dx); } double Trajectory::elevation(double s) const { s = clamp(s, true); - double dz = z_interpolator_->compute_first_derivative(s); + const double dz = z_interpolator_->compute_first_derivative(s); return std::atan2(dz, 1.0); } double Trajectory::curvature(double s) const { s = clamp(s, true); - double dx = x_interpolator_->compute_first_derivative(s); - double ddx = x_interpolator_->compute_second_derivative(s); - double dy = y_interpolator_->compute_first_derivative(s); - double ddy = y_interpolator_->compute_second_derivative(s); + const double dx = x_interpolator_->compute_first_derivative(s); + const double ddx = x_interpolator_->compute_second_derivative(s); + const double dy = y_interpolator_->compute_first_derivative(s); + const double ddy = y_interpolator_->compute_second_derivative(s); return std::abs(dx * ddy - dy * ddx) / std::pow(dx * dx + dy * dy, 1.5); } std::vector Trajectory::restore(const size_t & min_points) const { - auto bases = detail::crop_bases(bases_, start_, end_); + auto bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.x = x_interpolator_->compute(s); - p.y = y_interpolator_->compute(s); - p.z = z_interpolator_->compute(s); - points.emplace_back(p); + points.emplace_back(compute(s)); } return points; } diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index 3906ee17fa2eb..0dc3287769aea 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -14,6 +14,9 @@ #include "autoware/trajectory/pose.hpp" +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/interpolator/spherical_linear.hpp" + #include #include @@ -21,9 +24,27 @@ namespace autoware::trajectory { - using PointType = geometry_msgs::msg::Pose; +Trajectory::Trajectory() +: orientation_interpolator_(std::make_shared()) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: BaseClass(rhs), orientation_interpolator_(rhs.orientation_interpolator_->clone()) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + orientation_interpolator_ = rhs.orientation_interpolator_->clone(); + } + return *this; +} + bool Trajectory::build(const std::vector & points) { std::vector path_points; @@ -41,6 +62,14 @@ bool Trajectory::build(const std::vector & points) return is_valid; } +std::vector Trajectory::get_internal_bases() const +{ + auto bases = detail::crop_bases(bases_, start_, end_); + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + PointType Trajectory::compute(double s) const { PointType result; @@ -54,24 +83,26 @@ void Trajectory::align_orientation_with_trajectory_direction() { std::vector aligned_orientations; for (const auto & s : bases_) { - double azimuth = this->azimuth(s); - double elevation = this->elevation(s); - geometry_msgs::msg::Quaternion current_orientation = orientation_interpolator_->compute(s); + const double azimuth = this->azimuth(s); + const double elevation = this->elevation(s); + const geometry_msgs::msg::Quaternion current_orientation = + orientation_interpolator_->compute(s); tf2::Quaternion current_orientation_tf2( current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w); current_orientation_tf2.normalize(); - tf2::Vector3 x_axis(1.0, 0.0, 0.0); - tf2::Vector3 current_x_axis = tf2::quatRotate(current_orientation_tf2, x_axis); + const tf2::Vector3 x_axis(1.0, 0.0, 0.0); + const tf2::Vector3 current_x_axis = tf2::quatRotate(current_orientation_tf2, x_axis); - tf2::Vector3 desired_x_axis( + const tf2::Vector3 desired_x_axis( std::cos(elevation) * std::cos(azimuth), std::cos(elevation) * std::sin(azimuth), std::sin(elevation)); - tf2::Vector3 rotation_axis = current_x_axis.cross(desired_x_axis).normalized(); - double dot_product = current_x_axis.dot(desired_x_axis); - double rotation_angle = std::acos(dot_product); + const tf2::Vector3 rotation_axis = current_x_axis.cross(desired_x_axis).normalized(); + const double dot_product = current_x_axis.dot(desired_x_axis); + const double rotation_angle = std::acos(dot_product); - tf2::Quaternion delta_q(rotation_axis, rotation_angle); - tf2::Quaternion aligned_orientation_tf2 = (delta_q * current_orientation_tf2).normalized(); + const tf2::Quaternion delta_q(rotation_axis, rotation_angle); + const tf2::Quaternion aligned_orientation_tf2 = + (delta_q * current_orientation_tf2).normalized(); geometry_msgs::msg::Quaternion aligned_orientation; aligned_orientation.x = aligned_orientation_tf2.x(); @@ -81,20 +112,21 @@ void Trajectory::align_orientation_with_trajectory_direction() aligned_orientations.emplace_back(aligned_orientation); } - orientation_interpolator_->build(bases_, aligned_orientations); + const bool success = orientation_interpolator_->build(bases_, aligned_orientations); + if (!success) { + throw std::runtime_error( + "Failed to build orientation interpolator."); // This Exception should not be thrown. + } } std::vector Trajectory::restore(const size_t & min_points) const { - auto bases = detail::crop_bases(bases_, start_, end_); + auto bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.position = BaseClass::compute(s - start_); - p.orientation = orientation_interpolator_->compute(s); - points.emplace_back(p); + points.emplace_back(compute(s)); } return points; } diff --git a/common/autoware_trajectory/src/utils/closest.cpp b/common/autoware_trajectory/src/utils/closest.cpp new file mode 100644 index 0000000000000..76537f7ac122f --- /dev/null +++ b/common/autoware_trajectory/src/utils/closest.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/closest.hpp" + +#include +#include + +namespace autoware::trajectory::detail::impl +{ +std::optional closest_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, const Eigen::Vector2d & point, + const std::function & constraint) +{ + using trajectory::detail::to_point; + std::vector distances_from_segments; + std::vector lengths_from_start_points; + + for (size_t i = 1; i < bases.size(); ++i) { + const Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); + const Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); + + const Eigen::Vector2d v = p1 - p0; + const Eigen::Vector2d w = point - p0; + const double c1 = w.dot(v); + const double c2 = v.dot(v); + double length_from_start_point = NAN; + double distance_from_segment = NAN; + if (c1 <= 0) { + length_from_start_point = bases.at(i - 1); + distance_from_segment = (point - p0).norm(); + } else if (c2 <= c1) { + length_from_start_point = bases.at(i); + distance_from_segment = (point - p1).norm(); + } else { + length_from_start_point = bases.at(i - 1) + c1 / c2 * (p1 - p0).norm(); + distance_from_segment = (point - (p0 + (c1 / c2) * v)).norm(); + } + if (constraint(length_from_start_point)) { + distances_from_segments.push_back(distance_from_segment); + lengths_from_start_points.push_back(length_from_start_point); + } + } + if (distances_from_segments.empty()) { + return std::nullopt; + } + + auto min_it = std::min_element(distances_from_segments.begin(), distances_from_segments.end()); + + return lengths_from_start_points[std::distance(distances_from_segments.begin(), min_it)]; +} +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/crossed.cpp b/common/autoware_trajectory/src/utils/crossed.cpp new file mode 100644 index 0000000000000..f0052ee9e6164 --- /dev/null +++ b/common/autoware_trajectory/src/utils/crossed.cpp @@ -0,0 +1,86 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/crossed.hpp" + +#include +#include + +namespace autoware::trajectory::detail::impl +{ + +std::optional crossed_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, // + const Eigen::Vector2d & line_start, const Eigen::Vector2d & line_end, + const std::function & constraint) +{ + Eigen::Vector2d line_dir = line_end - line_start; + + for (size_t i = 1; i < bases.size(); ++i) { + const Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); + const Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); + + Eigen::Vector2d segment_dir = p1 - p0; + + const double det = segment_dir.x() * line_dir.y() - segment_dir.y() * line_dir.x(); + + if (std::abs(det) < 1e-10) { + continue; + } + + Eigen::Vector2d p0_to_line_start = line_start - p0; + + const double t = + (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; + const double u = + (p0_to_line_start.x() * segment_dir.y() - p0_to_line_start.y() * segment_dir.x()) / det; + + if (t >= 0.0 && t <= 1.0 && u >= 0.0 && u <= 1.0) { + double intersection = bases.at(i - 1) + t * (bases.at(i) - bases.at(i - 1)); + if (constraint(intersection)) { + return intersection; + } + } + } + + return std::nullopt; +} + +std::vector crossed_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, // + const std::vector> & linestring, + const std::function & constraint) +{ + using trajectory::detail::to_point; + + std::vector intersections; + + for (const auto & line : linestring) { + const Eigen::Vector2d & line_start = line.first; + const Eigen::Vector2d & line_end = line.second; + + std::optional intersection = + crossed_with_constraint_impl(trajectory_compute, bases, line_start, line_end, constraint); + + if (intersection) { + intersections.push_back(*intersection); + } + } + + return intersections; +} + +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/find_intervals.cpp b/common/autoware_trajectory/src/utils/find_intervals.cpp new file mode 100644 index 0000000000000..4899f07f9cccd --- /dev/null +++ b/common/autoware_trajectory/src/utils/find_intervals.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/find_intervals.hpp" + +#include +#include +#include + +namespace autoware::trajectory::detail::impl +{ + +std::vector find_intervals_impl( + const std::vector & bases, const std::function & constraint) +{ + std::vector intervals; + + std::optional start = std::nullopt; + for (size_t i = 0; i < bases.size(); ++i) { + if (!start && constraint(bases.at(i))) { + start = bases.at(i); // Start a new interval + } else if (start && (!constraint(bases.at(i)) || i == bases.size() - 1)) { + // End the current interval if the constraint fails or it's the last element + intervals.emplace_back(Interval{start.value(), bases.at(i - !constraint(bases.at(i)))}); + start = std::nullopt; // Reset the start + } + } + return intervals; +} + +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/shift.cpp b/common/autoware_trajectory/src/utils/shift.cpp new file mode 100644 index 0000000000000..f12e8df11f1f0 --- /dev/null +++ b/common/autoware_trajectory/src/utils/shift.cpp @@ -0,0 +1,280 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/shift.hpp" + +#include "autoware/trajectory/detail/logging.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" + +#include +#include +#include +#include + +namespace autoware::trajectory::detail::impl +{ + +// This function calculates base longitudinal and lateral lengths +// when acceleration limit is not considered (simple division approach). +std::pair, std::vector> get_base_lengths_without_accel_limit( + const double arc_length, const double shift_length) +{ + // Alias for clarity + const double total_arc_length = arc_length; + const double total_shift_length = shift_length; + + // Prepare base longitudinal positions + const std::vector base_longitudinal = { + 0.0, 0.25 * total_arc_length, 0.75 * total_arc_length, total_arc_length}; + + // Prepare base lateral positions + const std::vector base_lateral = { + 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; + + return {base_longitudinal, base_lateral}; +} + +// This function calculates base longitudinal and lateral lengths +// when acceleration limit is not considered, but velocity and acceleration are known. +std::pair, std::vector> get_base_lengths_without_accel_limit( + const double arc_length, const double shift_length, const double velocity, + const double longitudinal_acc, const double total_time) +{ + // Aliases for clarity + const double total_arc_length = arc_length; + const double total_shift_length = shift_length; + const double v0 = velocity; // initial velocity + const double a = longitudinal_acc; // longitudinal acceleration + const double t = total_time / 4.0; // quarter of total time + + // Calculate first segment in longitudinal direction + // s1 = v0 * t + 1/2 * a * t^2 (but capped by total_arc_length) + const double segment_s1 = std::min(v0 * t + 0.5 * a * t * t, total_arc_length); + + // Velocity at the end of first segment + const double v1 = v0 + a * t; + + // Calculate second segment in longitudinal direction + // s2 = s1 + 2 * v1 * t + 2 * a * t^2 (but capped by total_arc_length) + const double segment_s2 = std::min(segment_s1 + 2.0 * v1 * t + 2.0 * a * t * t, total_arc_length); + + // Prepare base longitudinal positions + const std::vector base_longitudinal = {0.0, segment_s1, segment_s2, total_arc_length}; + + // Prepare base lateral positions (simple division approach as original) + const std::vector base_lateral = { + 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; + + return {base_longitudinal, base_lateral}; +} +std::pair, std::vector> calc_base_lengths( + const double & arc_length, const double & shift_length, const ShiftParameters & shift_parameters) +{ + // Threshold for treating acceleration as zero + const double acc_threshold = 1.0e-4; + + // Extract initial velocity and clamp negative acceleration to zero + const double initial_vel = std::abs(shift_parameters.velocity); + const double used_lon_acc = + (shift_parameters.longitudinal_acc > acc_threshold) ? shift_parameters.longitudinal_acc : 0.0; + + // If there is no need to consider acceleration limit + if (initial_vel < 1.0e-5 && used_lon_acc < acc_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Velocity is effectively zero. " + "No lateral acceleration limit will be applied."); + return get_base_lengths_without_accel_limit(arc_length, shift_length); + } + + // Prepare main parameters + const double target_arclength = arc_length; + const double target_shift_abs = std::abs(shift_length); + + // Calculate total time (total_time) to travel 'target_arclength' + // If used_lon_acc is valid (> 0), use time from kinematic formula. Otherwise, use s/v + const double total_time = + (used_lon_acc > acc_threshold) + ? (-initial_vel + + std::sqrt(initial_vel * initial_vel + 2.0 * used_lon_acc * target_arclength)) / + used_lon_acc + : (target_arclength / initial_vel); + + // Calculate the maximum lateral acceleration if we do not add further constraints + const double max_lateral_acc = 8.0 * target_shift_abs / (total_time * total_time); + + // If the max_lateral_acc is already below the limit, no need to reduce it + if (max_lateral_acc < shift_parameters.lateral_acc_limit) { + RCLCPP_DEBUG_THROTTLE( + get_logger(), get_clock(), 3000, "No need to consider lateral acc limit. max: %f, limit: %f", + max_lateral_acc, shift_parameters.lateral_acc_limit); + return get_base_lengths_without_accel_limit( + target_arclength, shift_length, initial_vel, used_lon_acc, total_time); + } + + // Compute intermediate times (jerk_time / accel_time) and lateral jerk + const double jerk_time = + total_time / 2.0 - 2.0 * target_shift_abs / (shift_parameters.lateral_acc_limit * total_time); + const double accel_time = + 4.0 * target_shift_abs / (shift_parameters.lateral_acc_limit * total_time) - total_time / 2.0; + const double lateral_jerk = + (2.0 * shift_parameters.lateral_acc_limit * shift_parameters.lateral_acc_limit * total_time) / + (shift_parameters.lateral_acc_limit * total_time * total_time - 4.0 * target_shift_abs); + + // If computed times or jerk are invalid (negative or too small), skip the acc limit + if (jerk_time < 0.0 || accel_time < 0.0 || lateral_jerk < 0.0 || (jerk_time / total_time) < 0.1) { + RCLCPP_WARN_THROTTLE( + get_logger(), get_clock(), 3000, + "The specified lateral acceleration limit appears too restrictive. " + "No feasible jerk_time or accel_time was found. " + "Possible reasons: (1) shift_length is too large, (2) lateral_acc_limit is too low, " + "or (3) system kinematics are outside valid range.\n" + "Details:\n" + " - jerk_time: %.4f\n" + " - accel_time: %.4f\n" + " - lateral_jerk: %.4f\n" + " - computed_lateral_acc: %.4f\n" + " - lateral_acc_limit: %.4f\n\n" + "Suggestions:\n" + " - Increase the lateral_acc_limit.\n" + " - Decrease shift_length if possible.\n" + " - Re-check velocity and acceleration settings for consistency.", + jerk_time, accel_time, lateral_jerk, max_lateral_acc, shift_parameters.lateral_acc_limit); + return get_base_lengths_without_accel_limit(target_arclength, shift_length); + } + + // Precompute powers for jerk_time and accel_time + const double jerk_time3 = jerk_time * jerk_time * jerk_time; + const double accel_time2_jerk = accel_time * accel_time * jerk_time; + const double accel_time_jerk2 = accel_time * jerk_time * jerk_time; + + // ------------------------------------------------------ + // Calculate longitudinal base points + // ------------------------------------------------------ + // Segment s1 + const double segment_s1 = std::min( + jerk_time * initial_vel + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + const double v1 = initial_vel + used_lon_acc * jerk_time; + + // Segment s2 + const double segment_s2 = std::min( + segment_s1 + accel_time * v1 + 0.5 * used_lon_acc * accel_time * accel_time, target_arclength); + const double v2 = v1 + used_lon_acc * accel_time; + + // Segment s3 = s4 + const double segment_s3 = std::min( + segment_s2 + jerk_time * v2 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + const double v3 = v2 + used_lon_acc * jerk_time; + + // Segment s5 + const double segment_s5 = std::min( + segment_s3 + jerk_time * v3 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + const double v5 = v3 + used_lon_acc * jerk_time; + + // Segment s6 + const double segment_s6 = std::min( + segment_s5 + accel_time * v5 + 0.5 * used_lon_acc * accel_time * accel_time, target_arclength); + const double v6 = v5 + used_lon_acc * accel_time; + + // Segment s7 + const double segment_s7 = std::min( + segment_s6 + jerk_time * v6 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + + // ------------------------------------------------------ + // Calculate lateral base points + // ------------------------------------------------------ + // sign determines the direction of shift + const double shift_sign = (shift_length > 0.0) ? 1.0 : -1.0; + + // Shift amounts at each segment + const double shift_l1 = shift_sign * (1.0 / 6.0 * lateral_jerk * jerk_time3); + const double shift_l2 = + shift_sign * (1.0 / 6.0 * lateral_jerk * jerk_time3 + 0.5 * lateral_jerk * accel_time_jerk2 + + 0.5 * lateral_jerk * accel_time2_jerk); + const double shift_l3 = + shift_sign * (lateral_jerk * jerk_time3 + 1.5 * lateral_jerk * accel_time_jerk2 + + 0.5 * lateral_jerk * accel_time2_jerk); // = shift_l4 + const double shift_l5 = + shift_sign * (11.0 / 6.0 * lateral_jerk * jerk_time3 + 2.5 * lateral_jerk * accel_time_jerk2 + + 0.5 * lateral_jerk * accel_time2_jerk); + const double shift_l6 = + shift_sign * (11.0 / 6.0 * lateral_jerk * jerk_time3 + 3.0 * lateral_jerk * accel_time_jerk2 + + lateral_jerk * accel_time2_jerk); + const double shift_l7 = + shift_sign * (2.0 * lateral_jerk * jerk_time3 + 3.0 * lateral_jerk * accel_time_jerk2 + + lateral_jerk * accel_time2_jerk); + + // Construct the output vectors + const std::vector base_lon = {0.0, segment_s1, segment_s2, segment_s3, + segment_s5, segment_s6, segment_s7}; + const std::vector base_lat = {0.0, shift_l1, shift_l2, shift_l3, + shift_l5, shift_l6, shift_l7}; + + return {base_lon, base_lat}; +} + +void shift_impl( + const std::vector & bases, std::vector * shift_lengths, + const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters) +{ + // lateral shift + if (shift_interval.end <= 0.0) { + for (size_t i = 0; i < bases.size(); ++i) { + shift_lengths->at(i) += shift_interval.lateral_offset; + } + return; + } + + const double shift_arc_length = std::abs(shift_interval.end - shift_interval.start); + const bool shift_direction = shift_interval.end > shift_interval.start; + // Calculate base lengths + auto [base_lon, base_lat] = calc_base_lengths( + shift_arc_length, // + shift_interval.lateral_offset, // + shift_parameters); + + auto cubic_spline = + interpolator::CubicSpline::Builder{}.set_bases(base_lon).set_values(base_lat).build(); + + if (!cubic_spline) { + throw std::runtime_error( + "Failed to build cubic spline for shift calculation."); // This Exception should not be + // thrown. + } + for (size_t i = 0; i < bases.size(); ++i) { + // Calculate the shift length at the current base + const double s = bases.at(i); + if (shift_direction) { + if (s < shift_interval.start) { + continue; + } + if (s <= shift_interval.end) { + shift_lengths->at(i) += cubic_spline->compute(s - shift_interval.start); + } else { + shift_lengths->at(i) += cubic_spline->compute(shift_arc_length); + } + } else { + if (s > shift_interval.start) { + continue; + } + if (s >= shift_interval.end) { + shift_lengths->at(i) += cubic_spline->compute(shift_interval.start - s); + } else { + shift_lengths->at(i) += cubic_spline->compute(shift_arc_length); + } + } + } +} + +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/test/test_helpers.cpp b/common/autoware_trajectory/test/test_helpers.cpp new file mode 100644 index 0000000000000..c5ab9ae6b4758 --- /dev/null +++ b/common/autoware_trajectory/test/test_helpers.cpp @@ -0,0 +1,55 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/detail/helpers.hpp" + +#include + +#include + +TEST(TestHelpers, fill_bases) +{ + using autoware::trajectory::detail::fill_bases; + + std::vector x = {0.0, 1.0, 2.0, 3.0}; + size_t min_points = 9; + std::vector expected = {0.0, 1.0 / 3, 2.0 / 3, 1.0, 4.0 / 3, 5.0 / 3, 2.0, 2.5, 3.0}; + + auto result = fill_bases(x, min_points); + + EXPECT_EQ(result.size(), min_points); + + for (size_t i = 0; i < min_points; ++i) { + EXPECT_NEAR(result[i], expected[i], 1e-6); + } +} + +TEST(TestHelpers, crop_bases) +{ + using autoware::trajectory::detail::crop_bases; + + std::vector x = {0.0, 1.0, 2.0, 3.0, 4.0}; + double start = 1.5; + double end = 3.5; + + std::vector expected = {1.5, 2.0, 3.0, 3.5}; + + auto result = crop_bases(x, start, end); + + EXPECT_EQ(result.size(), expected.size()); + + for (size_t i = 0; i < expected.size(); ++i) { + EXPECT_NEAR(result[i], expected[i], 1e-6); + } +} diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp index 6f6d52107b348..bdbf5eb60f524 100644 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -12,18 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "autoware/trajectory/path_point_with_lane_id.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "autoware/trajectory/utils/find_intervals.hpp" +#include "lanelet2_core/primitives/LineString.h" #include -#include #include -using Trajectory = autoware::trajectory::Trajectory; +using Trajectory = + autoware::trajectory::Trajectory; -tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( +autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( double x, double y, uint8_t lane_id) { - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point; point.point.pose.position.x = x; point.point.pose.position.y = y; point.lane_ids.emplace_back(lane_id); @@ -33,13 +37,13 @@ tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( TEST(TrajectoryCreatorTest, create) { { - std::vector points{ + std::vector points{ path_point_with_lane_id(0.00, 0.00, 0)}; auto trajectory = Trajectory::Builder{}.build(points); ASSERT_TRUE(!trajectory); } { - std::vector points{ + std::vector points{ path_point_with_lane_id(0.00, 0.00, 0), path_point_with_lane_id(0.81, 1.68, 0), path_point_with_lane_id(1.65, 2.98, 0), path_point_with_lane_id(3.30, 4.01, 1)}; auto trajectory = Trajectory::Builder{}.build(points); @@ -54,7 +58,7 @@ class TrajectoryTest : public ::testing::Test void SetUp() override { - std::vector points{ + std::vector points{ path_point_with_lane_id(0.00, 0.00, 0), path_point_with_lane_id(0.81, 1.68, 0), path_point_with_lane_id(1.65, 2.98, 0), path_point_with_lane_id(3.30, 4.01, 1), path_point_with_lane_id(4.70, 4.52, 1), path_point_with_lane_id(6.49, 5.20, 1), @@ -70,7 +74,8 @@ TEST_F(TrajectoryTest, compute) { double length = trajectory->length(); - trajectory->longitudinal_velocity_mps.range(trajectory->length() / 3.0, trajectory->length()) + trajectory->longitudinal_velocity_mps() + .range(trajectory->length() / 3.0, trajectory->length()) .set(10.0); auto point = trajectory->compute(length / 2.0); @@ -85,8 +90,8 @@ TEST_F(TrajectoryTest, compute) TEST_F(TrajectoryTest, manipulate_velocity) { - trajectory->longitudinal_velocity_mps = 10.0; - trajectory->longitudinal_velocity_mps + trajectory->longitudinal_velocity_mps() = 10.0; + trajectory->longitudinal_velocity_mps() .range(trajectory->length() / 3, 2.0 * trajectory->length() / 3) .set(5.0); auto point1 = trajectory->compute(0.0); @@ -115,43 +120,22 @@ TEST_F(TrajectoryTest, curvature) TEST_F(TrajectoryTest, restore) { using autoware::trajectory::Trajectory; - trajectory->longitudinal_velocity_mps.range(4.0, trajectory->length()).set(5.0); - { - auto points = static_cast &>(*trajectory).restore(0); - EXPECT_EQ(10, points.size()); - } - - { - auto points = static_cast &>(*trajectory).restore(0); - EXPECT_EQ(10, points.size()); - } - - { - auto points = - static_cast &>(*trajectory).restore(0); - EXPECT_EQ(11, points.size()); - } - - { - auto points = trajectory->restore(0); - EXPECT_EQ(11, points.size()); - } + trajectory->longitudinal_velocity_mps().range(4.0, trajectory->length()).set(5.0); + auto points = trajectory->restore(0); + EXPECT_EQ(11, points.size()); } TEST_F(TrajectoryTest, crossed) { - geometry_msgs::msg::Pose pose1; - pose1.position.x = 0.0; - pose1.position.y = 10.0; - geometry_msgs::msg::Pose pose2; - pose2.position.x = 10.0; - pose2.position.y = 0.0; - - auto crossed_point = trajectory->crossed(pose1, pose2); - EXPECT_TRUE(crossed_point.has_value()); - - EXPECT_LT(0.0, *crossed_point); - EXPECT_LT(*crossed_point, trajectory->length()); + lanelet::LineString2d line_string; + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 10.0, 0.0)); + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, 0.0, 0.0)); + + auto crossed_point = autoware::trajectory::crossed(*trajectory, line_string); + ASSERT_EQ(crossed_point.size(), 1); + + EXPECT_LT(0.0, crossed_point.at(0)); + EXPECT_LT(crossed_point.at(0), trajectory->length()); } TEST_F(TrajectoryTest, closest) @@ -160,9 +144,7 @@ TEST_F(TrajectoryTest, closest) pose.position.x = 5.0; pose.position.y = 5.0; - std::cerr << "Closest: " << trajectory->closest(pose) << std::endl; - - auto closest_pose = trajectory->compute(trajectory->closest(pose)); + auto closest_pose = trajectory->compute(autoware::trajectory::closest(*trajectory, pose)); double distance = std::hypot( closest_pose.point.pose.position.x - pose.position.x, @@ -193,3 +175,15 @@ TEST_F(TrajectoryTest, crop) EXPECT_EQ(end_point_expect.point.pose.position.y, end_point_actual.point.pose.position.y); EXPECT_EQ(end_point_expect.lane_ids[0], end_point_actual.lane_ids[0]); } + +TEST_F(TrajectoryTest, find_interval) +{ + auto intervals = autoware::trajectory::find_intervals( + *trajectory, [](const autoware_internal_planning_msgs::msg::PathPointWithLaneId & point) { + return point.lane_ids[0] == 1; + }); + EXPECT_EQ(intervals.size(), 1); + EXPECT_LT(0, intervals[0].start); + EXPECT_LT(intervals[0].start, intervals[0].end); + EXPECT_NEAR(intervals[0].end, trajectory->length(), 0.1); +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp index e6d0363846b20..b854c0608846d 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp @@ -27,6 +27,7 @@ #define EIGEN_MPL2_ONLY #include +#include #include #include #include @@ -38,7 +39,6 @@ #include #include #include -#include #include @@ -135,7 +135,8 @@ inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::Pat } template <> -inline geometry_msgs::msg::Point getPoint(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Point getPoint( + const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose.position; } @@ -172,7 +173,8 @@ inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::PathP } template <> -inline geometry_msgs::msg::Pose getPose(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Pose getPose( + const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose; } @@ -197,7 +199,8 @@ inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::PathPoi } template <> -inline double getLongitudinalVelocity(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +inline double getLongitudinalVelocity( + const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } @@ -236,7 +239,8 @@ inline void setPose( template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, tier4_planning_msgs::msg::PathPointWithLaneId & p) + const geometry_msgs::msg::Pose & pose, + autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { p.point.pose = pose; } @@ -279,7 +283,7 @@ inline void setLongitudinalVelocity( template <> inline void setLongitudinalVelocity( - const float velocity, tier4_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } diff --git a/common/autoware_universe_utils/package.xml b/common/autoware_universe_utils/package.xml index 2415ce6e4a8c1..0cc702f1abfa2 100644 --- a/common/autoware_universe_utils/package.xml +++ b/common/autoware_universe_utils/package.xml @@ -14,6 +14,7 @@ autoware_internal_debug_msgs autoware_internal_msgs + autoware_internal_planning_msgs autoware_perception_msgs autoware_planning_msgs autoware_vehicle_msgs diff --git a/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index 4ed1c5497c6ae..a0ac700f93a30 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -24,7 +24,7 @@ TEST(geometry, getPoint_PathWithLaneId) const double y_ans = 2.0; const double z_ans = 3.0; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -46,7 +46,7 @@ TEST(geometry, getPose_PathWithLaneId) const double q_z_ans = 0.3; const double q_w_ans = 0.4; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; p.point.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -96,7 +96,7 @@ TEST(geometry, setPose_PathWithLaneId) p.orientation.z = q_z_ans; p.orientation.w = q_w_ans; - tier4_planning_msgs::msg::PathPointWithLaneId p_out{}; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans); @@ -113,7 +113,7 @@ TEST(geometry, setLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - tier4_planning_msgs::msg::PathPointWithLaneId p{}; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.point.longitudinal_velocity_mps, velocity); } diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 101338551cbf3..99aa8623de6c7 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -21,11 +21,11 @@ #include #include +#include #include #include #include #include -#include #include #include @@ -46,7 +46,7 @@ namespace autoware::lane_departure_checker { using autoware::universe_utils::Segment2d; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; typedef boost::geometry::index::rtree> SegmentRtree; class LaneDepartureChecker diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp index bde404603749b..1f0a0ed6ff666 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp @@ -19,10 +19,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -35,9 +35,9 @@ namespace autoware::lane_departure_checker::utils using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::MultiPoint2d; using autoware::universe_utils::PoseDeviation; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; /** diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp index e17e2bc697272..6f5e9a2c3d444 100644 --- a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp +++ b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp @@ -22,10 +22,10 @@ #include using autoware::universe_utils::LinearRing2d; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::PoseWithCovariance; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; namespace diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 06f188a1fb63d..258fb773c5537 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -28,11 +28,11 @@ #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include +#include #include #include #include #include -#include #include #include @@ -55,7 +55,7 @@ using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; /** * @brief Node for control evaluation diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 2d5177d4252d8..4e39c8e43d4ab 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,6 +2,9 @@ + + + @@ -9,6 +12,21 @@ + + + + + @@ -141,6 +161,9 @@ + + + diff --git a/planning/.pages b/planning/.pages index 7444815cdd28a..735e2b2c907c0 100644 --- a/planning/.pages +++ b/planning/.pages @@ -63,6 +63,9 @@ nav: - 'Motion Velocity Planner': - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/ - 'Available Modules': + - 'Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/ + - 'Obstacle Slow Down': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/ + - 'Obstacle Cruise': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/ - 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/ - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/ - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/ diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 8bf30757aafb7..020a184224a02 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::freespace_planner::FreespacePlannerNode; @@ -29,10 +30,8 @@ using autoware::planning_test_manager::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - test_manager->setRouteInputTopicName("freespace_planner/input/route"); - test_manager->setTrajectorySubscriber("freespace_planner/output/trajectory"); - test_manager->setOdometryTopicName("freespace_planner/input/odometry"); - test_manager->setInitialPoseTopicName("freespace_planner/input/odometry"); + test_manager->subscribeOutput( + "freespace_planner/output/trajectory"); return test_manager; } @@ -55,10 +54,16 @@ void publishMandatoryTopics( rclcpp::Node::SharedPtr test_target_node) { // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishOdometry(test_target_node, "freespace_planner/input/odometry"); - test_manager->publishOccupancyGrid(test_target_node, "freespace_planner/input/occupancy_grid"); - test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario"); + test_manager->publishInput( + test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map")); + test_manager->publishInput( + test_target_node, "freespace_planner/input/odometry", autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "freespace_planner/input/occupancy_grid", + autoware::test_utils::makeCostMapMsg()); + test_manager->publishInput( + test_target_node, "freespace_planner/input/scenario", + autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::PARKING)); } // the following tests are disable because they randomly fail @@ -70,12 +75,16 @@ TEST(PlanningModuleInterfaceTest, DISABLED_testPlanningInterfaceWithVariousTraje auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "freespace_planner/input/route"; + // test with normal route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -87,11 +96,16 @@ TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "freespace_planner/input/route"; + const std::string input_odometry_topic = "freespace_planner/input/odometry"; + // test for normal route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index 2235704a3a070..db9fef1cb7598 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::motion_planning::ObstacleCruisePlannerNode; @@ -31,12 +32,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: obstacle_cruise_planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory"); - - // set obstacle_cruise_planners input topic name(this topic is changed to test node): - test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory"); - - test_manager->setOdometryTopicName("obstacle_cruise_planner/input/odometry"); + test_manager->subscribeOutput( + "obstacle_cruise_planner/output/trajectory"); return test_manager; } @@ -63,9 +60,15 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "obstacle_cruise_planner/input/odometry"); - test_manager->publishPredictedObjects(test_target_node, "obstacle_cruise_planner/input/objects"); - test_manager->publishAcceleration(test_target_node, "obstacle_cruise_planner/input/acceleration"); + test_manager->publishInput( + test_target_node, "obstacle_cruise_planner/input/odometry", + autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "obstacle_cruise_planner/input/objects", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "obstacle_cruise_planner/input/acceleration", + geometry_msgs::msg::AccelWithCovarianceStamped{}); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -76,12 +79,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -94,12 +101,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "obstacle_cruise_planner/input/trajectory"; + const std::string input_odometry_topic = "obstacle_cruise_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp index 9da8409c6c2ae..2654be0937d35 100644 --- a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp +++ b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp @@ -23,6 +23,7 @@ #include #include +#include #include using autoware::motion_planning::ObstacleStopPlannerNode; @@ -32,9 +33,8 @@ using tier4_planning_msgs::msg::ExpandStopRange; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory"); - test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory"); - test_manager->setOdometryTopicName("obstacle_stop_planner/input/odometry"); + test_manager->subscribeOutput( + "obstacle_stop_planner/output/trajectory"); return test_manager; } @@ -65,21 +65,21 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "obstacle_stop_planner/input/odometry"); - test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud"); - test_manager->publishAcceleration(test_target_node, "obstacle_stop_planner/input/acceleration"); - test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects"); -} - -void publishExpandStopRange( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - auto test_node = test_manager->getTestNode(); - const auto expand_stop_range = test_manager->getTestNode()->create_publisher( - "obstacle_stop_planner/input/expand_stop_range", 1); - expand_stop_range->publish(ExpandStopRange{}); - autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); + test_manager->publishInput( + test_target_node, "obstacle_stop_planner/input/odometry", autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "obstacle_stop_planner/input/pointcloud", + sensor_msgs::msg::PointCloud2{}.set__header( + std_msgs::msg::Header{}.set__frame_id("base_link"))); + test_manager->publishInput( + test_target_node, "obstacle_stop_planner/input/acceleration", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "obstacle_stop_planner/input/objects", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "obstacle_stop_planner/input/expand_stop_range", + tier4_planning_msgs::msg::ExpandStopRange{}); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -90,15 +90,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); - publishExpandStopRange(test_manager, test_target_node); + + const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory"; // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - test_manager->testWithAbnormalTrajectory(test_target_node); + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic); rclcpp::shutdown(); } @@ -110,14 +112,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); - publishExpandStopRange(test_manager, test_target_node); + + const std::string input_trajectory_topic = "obstacle_stop_planner/input/trajectory"; + const std::string input_odometry_topic = "obstacle_stop_planner/input/odometry"; // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 144810b545dbe..2cda65a923da8 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -28,7 +28,6 @@ rclcpp_components std_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index e310fecf15d8c..164da3173b655 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -47,21 +48,24 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) auto test_target_node = std::make_shared(node_options); // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry"); + test_manager->publishInput( + test_target_node, "path_optimizer/input/odometry", autoware::test_utils::makeOdometry()); // set subscriber with topic name: path_optimizer → test_node_ - test_manager->setTrajectorySubscriber("path_optimizer/output/path"); + test_manager->subscribeOutput( + "path_optimizer/output/path"); - // set path_optimizer's input topic name(this topic is changed to test node) - test_manager->setPathInputTopicName("path_optimizer/input/path"); + const std::string input_trajectory_topic = "path_optimizer/input/path"; // test with normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPath(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with trajectory with empty/one point/overlapping point - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPath(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } diff --git a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp index 7402541c727ff..edf03556f2c21 100644 --- a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp +++ b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -46,22 +47,27 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) std::make_shared(node_options); // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "autoware_path_smoother/input/odometry"); + test_manager->publishInput( + test_target_node, "autoware_path_smoother/input/odometry", + autoware::test_utils::makeOdometry()); // set subscriber with topic name - test_manager->setTrajectorySubscriber("autoware_path_smoother/output/traj"); - test_manager->setPathSubscriber("autoware_path_smoother/output/path"); + test_manager->subscribeOutput( + "autoware_path_smoother/output/traj"); + test_manager->subscribeOutput( + "autoware_path_smoother/output/path"); - // set input topic name (this topic is changed to test node) - test_manager->setPathInputTopicName("autoware_path_smoother/input/path"); + const std::string input_path_topic = "autoware_path_smoother/input/path"; // test with normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPath(test_target_node, input_path_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with trajectory with empty/one point/overlapping point - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPath(test_target_node, input_path_topic)); rclcpp::shutdown(); } diff --git a/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp index a2325b197853d..05c57d8739d47 100644 --- a/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp +++ b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp @@ -18,11 +18,11 @@ #include #include +#include #include #include #include #include -#include #include #include #include @@ -209,10 +209,11 @@ class PlanningFactorInterface std::vector factors_; }; -extern template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, - const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, - const std::string &); +extern template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); extern template void PlanningFactorInterface::add( const std::vector &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, @@ -222,10 +223,11 @@ extern template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, - const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, - const double, const std::string &); +extern template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, + const double, const double, const std::string &); extern template void PlanningFactorInterface::add( const std::vector &, const Pose &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, diff --git a/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp index 6578776dddc4f..de9a8b760198c 100644 --- a/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp +++ b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp @@ -19,10 +19,11 @@ namespace autoware::planning_factor_interface { -template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, - const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, - const std::string &); +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); template void PlanningFactorInterface::add( const std::vector &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, @@ -32,10 +33,11 @@ template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, - const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, - const double, const std::string &); +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, + const double, const double, const std::string &); template void PlanningFactorInterface::add( const std::vector &, const Pose &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index cf45154893af7..127cb11d8e101 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -32,29 +32,10 @@ #include #include +#include #include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -63,190 +44,77 @@ #include #include #include +#include namespace autoware::planning_test_manager { -using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_map_msgs::msg::LaneletMapBin; -using autoware_perception_msgs::msg::PredictedObjects; -using autoware_perception_msgs::msg::TrafficLightGroupArray; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Path; -using autoware_planning_msgs::msg::Trajectory; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::PoseStamped; -using geometry_msgs::msg::Quaternion; -using geometry_msgs::msg::TransformStamped; -using nav_msgs::msg::OccupancyGrid; -using nav_msgs::msg::Odometry; -using sensor_msgs::msg::PointCloud2; -using tf2_msgs::msg::TFMessage; -using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::Scenario; -using tier4_planning_msgs::msg::VelocityLimit; - -enum class ModuleName { - UNKNOWN = 0, - START_PLANNER, -}; - class PlanningInterfaceTestManager { public: PlanningInterfaceTestManager(); - void publishOdometry( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0); - - void publishInitialPose( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0, - ModuleName module_name = ModuleName::UNKNOWN); - - void publishMaxVelocity(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishPointCloud(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAcceleration(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishPredictedObjects(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishOccupancyGrid(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishCostMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishLaneDrivingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishParkingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishParkingState(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishTrajectory(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishInitialPoseTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); - - void setTrajectoryInputTopicName(std::string topic_name); - void setParkingTrajectoryInputTopicName(std::string topic_name); - void setLaneDrivingTrajectoryInputTopicName(std::string topic_name); - void setRouteInputTopicName(std::string topic_name); - void setPathInputTopicName(std::string topic_name); - void setPathWithLaneIdTopicName(std::string topic_name); - void setPathTopicName(std::string topic_name); - - void setTrajectorySubscriber(std::string topic_name); - void setScenarioSubscriber(std::string topic_name); - void setPathWithLaneIdSubscriber(std::string topic_name); - void setRouteSubscriber(std::string topic_name); - void setPathSubscriber(std::string topic_name); - - void setInitialPoseTopicName(std::string topic_name); - void setOdometryTopicName(std::string topic_name); - - void testWithNominalTrajectory(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalTrajectory(rclcpp::Node::SharedPtr target_node); - - void testWithNominalRoute(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node); - - void testWithBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, ModuleName module_name = ModuleName::UNKNOWN); - - void testWithNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalPathWithLaneId(rclcpp::Node::SharedPtr target_node); - - void testWithNominalPath(rclcpp::Node::SharedPtr target_node); - void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node); - - // for invalid ego poses, contains some tests inside. - void testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - void testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - void testPathWithLaneIdWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - void testTrajectoryWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); - - // ego vehicle is located far from trajectory - void testOffTrackFromRoute(rclcpp::Node::SharedPtr target_node); - void testOffTrackFromPath(rclcpp::Node::SharedPtr target_node); - void testOffTrackFromPathWithLaneId(rclcpp::Node::SharedPtr target_node); - void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node); - - int getReceivedTopicNum(); - rclcpp::Node::SharedPtr getTestNode() const; + template + void publishInput( + const rclcpp::Node::SharedPtr target_node, const std::string & topic_name, const InputT & input, + const int repeat_count = 3) const + { + autoware::test_utils::publishToTargetNode( + test_node_, target_node, topic_name, {}, input, repeat_count); + } -private: - // Publisher (necessary for node running) - rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr initial_pose_pub_; - rclcpp::Publisher::SharedPtr max_velocity_pub_; - rclcpp::Publisher::SharedPtr point_cloud_pub_; - rclcpp::Publisher::SharedPtr acceleration_pub_; - rclcpp::Publisher::SharedPtr predicted_objects_pub_; - rclcpp::Publisher::SharedPtr occupancy_grid_pub_; - rclcpp::Publisher::SharedPtr cost_map_pub_; - rclcpp::Publisher::SharedPtr map_pub_; - rclcpp::Publisher::SharedPtr scenario_pub_; - rclcpp::Publisher::SharedPtr parking_scenario_pub_; - rclcpp::Publisher::SharedPtr lane_driving_scenario_pub_; - rclcpp::Publisher::SharedPtr parking_state_pub_; - rclcpp::Publisher::SharedPtr trajectory_pub_; - rclcpp::Publisher::SharedPtr route_pub_; - rclcpp::Publisher::SharedPtr TF_pub_; - rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; - rclcpp::Publisher::SharedPtr operation_mode_state_pub_; - rclcpp::Publisher::SharedPtr traffic_signals_pub_; + template + void subscribeOutput(const std::string & topic_name) + { + const auto qos = []() { + if constexpr (std::is_same_v) { + return rclcpp::QoS{1}; + } + return rclcpp::QoS{10}; + }(); + + test_output_subs_.push_back(test_node_->create_subscription( + topic_name, qos, [this](const typename OutputT::ConstSharedPtr) { received_topic_num_++; })); + } + + void testWithNormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithNormalRoute(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithBehaviorNormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithNormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithNormalPath(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithOffTrackInitialPoses( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void testWithOffTrackOdometry( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name); + + void resetReceivedTopicNum() { received_topic_num_ = 0; } + + size_t getReceivedTopicNum() const { return received_topic_num_; } + + rclcpp::Node::SharedPtr getTestNode() const { return test_node_; } + +private: // Subscriber - rclcpp::Subscription::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr route_sub_; - rclcpp::Subscription::SharedPtr scenario_sub_; - rclcpp::Subscription::SharedPtr path_with_lane_id_sub_; - rclcpp::Subscription::SharedPtr path_sub_; - - // Publisher for testing(trajectory) - rclcpp::Publisher::SharedPtr normal_trajectory_pub_; - rclcpp::Publisher::SharedPtr abnormal_trajectory_pub_; - - // Publisher for testing(route) - rclcpp::Publisher::SharedPtr normal_route_pub_; - rclcpp::Publisher::SharedPtr abnormal_route_pub_; - - // Publisher for testing(route) - rclcpp::Publisher::SharedPtr behavior_normal_route_pub_; - - // Publisher for testing(PathWithLaneId) - rclcpp::Publisher::SharedPtr normal_path_with_lane_id_pub_; - rclcpp::Publisher::SharedPtr abnormal_path_with_lane_id_pub_; - - // Publisher for testing(Path) - rclcpp::Publisher::SharedPtr normal_path_pub_; - rclcpp::Publisher::SharedPtr abnormal_path_pub_; - - std::string input_trajectory_name_ = ""; - std::string input_parking_trajectory_name_ = ""; - std::string input_lane_driving_trajectory_name_ = ""; - std::string input_route_name_ = ""; - std::string input_path_name_ = ""; - std::string input_path_with_lane_id_name_ = ""; - std::string input_initial_pose_name_ = ""; // for the map based pose - std::string input_odometry_name_ = ""; + std::vector test_output_subs_; // Node rclcpp::Node::SharedPtr test_node_; - std::string map_frame_ = "map"; - size_t count_{0}; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - void publishNominalTrajectory(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbnormalTrajectory( - rclcpp::Node::SharedPtr target_node, const Trajectory & abnormal_trajectory); - - void publishNominalRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbnormalRoute( - rclcpp::Node::SharedPtr target_node, const LaneletRoute & abnormal_route); - - void publishBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name, - ModuleName module_name = ModuleName::UNKNOWN); - void publishNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); - - void publishNominalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishAbnormalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); + size_t received_topic_num_ = 0; }; // class PlanningInterfaceTestManager } // namespace autoware::planning_test_manager diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index f07efd0cda116..f0e143fa0aea3 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -12,446 +12,114 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" -#include -#include -#include -#include +#include + +#include +#include +#include #include #include #include -#include namespace autoware::planning_test_manager { +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; PlanningInterfaceTestManager::PlanningInterfaceTestManager() { test_node_ = std::make_shared("planning_interface_test_node"); - tf_buffer_ = std::make_shared(test_node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); -} - -void PlanningInterfaceTestManager::publishOdometry( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, odom_pub_, autoware::test_utils::makeOdometry(shift)); -} - -void PlanningInterfaceTestManager::publishMaxVelocity( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, max_velocity_pub_, VelocityLimit{}); -} - -void PlanningInterfaceTestManager::publishPointCloud( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - PointCloud2 point_cloud_msg{}; - point_cloud_msg.header.frame_id = "base_link"; - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, point_cloud_pub_, point_cloud_msg); } -void PlanningInterfaceTestManager::publishAcceleration( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, acceleration_pub_, AccelWithCovarianceStamped{}); + publishInput( + target_node, topic_name, autoware::test_utils::generateTrajectory(10, 1.0), 5); } -void PlanningInterfaceTestManager::publishPredictedObjects( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithAbnormalTrajectory( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, predicted_objects_pub_, PredictedObjects{}); + publishInput(target_node, topic_name, Trajectory{}, 5); + publishInput( + target_node, topic_name, autoware::test_utils::generateTrajectory(1, 0.0), 5); + publishInput( + target_node, topic_name, + autoware::test_utils::generateTrajectory(10, 0.0, 0.0, 0.0, 0.0, 1), 5); } -void PlanningInterfaceTestManager::publishOccupancyGrid( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, occupancy_grid_pub_, - autoware::test_utils::makeCostMapMsg()); + publishInput(target_node, topic_name, autoware::test_utils::makeNormalRoute(), 5); } -void PlanningInterfaceTestManager::publishCostMap( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithAbnormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, cost_map_pub_, autoware::test_utils::makeCostMapMsg()); + publishInput(target_node, topic_name, LaneletRoute{}, 5); } -void PlanningInterfaceTestManager::publishMap( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithBehaviorNormalRoute( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, map_pub_, autoware::test_utils::makeMapBinMsg()); -} - -void PlanningInterfaceTestManager::publishLaneDrivingScenario( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, lane_driving_scenario_pub_, - autoware::test_utils::makeScenarioMsg(Scenario::LANEDRIVING)); -} - -void PlanningInterfaceTestManager::publishParkingScenario( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, parking_scenario_pub_, - autoware::test_utils::makeScenarioMsg(Scenario::PARKING)); -} - -void PlanningInterfaceTestManager::publishInitialPose( - rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift, - ModuleName module_name) -{ - if (module_name == ModuleName::START_PLANNER) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, initial_pose_pub_, - autoware_planning_test_manager::utils::makeInitialPoseFromLaneId(10291)); - } else { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, initial_pose_pub_, - autoware::test_utils::makeInitialPose(shift)); - } + publishInput(target_node, topic_name, autoware::test_utils::makeBehaviorNormalRoute(), 5); } -void PlanningInterfaceTestManager::publishParkingState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, parking_state_pub_, std_msgs::msg::Bool{}); -} - -void PlanningInterfaceTestManager::publishTrajectory( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, trajectory_pub_, Trajectory{}); -} - -void PlanningInterfaceTestManager::publishRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, route_pub_, autoware::test_utils::makeNormalRoute()); -} - -void PlanningInterfaceTestManager::publishTF( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, TF_pub_, - autoware::test_utils::makeTFMsg(target_node, "base_link", "map")); -} - -void PlanningInterfaceTestManager::publishOperationModeState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, operation_mode_state_pub_, OperationModeState{}); -} - -void PlanningInterfaceTestManager::publishTrafficSignals( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); -} - -void PlanningInterfaceTestManager::publishInitialPoseTF( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, initial_pose_tf_pub_, - autoware::test_utils::makeTFMsg(target_node, "odom", "base_link")); -} - -void PlanningInterfaceTestManager::setTrajectoryInputTopicName(std::string topic_name) -{ - input_trajectory_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setParkingTrajectoryInputTopicName(std::string topic_name) -{ - input_parking_trajectory_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setLaneDrivingTrajectoryInputTopicName(std::string topic_name) -{ - input_lane_driving_trajectory_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setRouteInputTopicName(std::string topic_name) -{ - input_route_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setPathInputTopicName(std::string topic_name) -{ - input_path_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setPathWithLaneIdTopicName(std::string topic_name) -{ - input_path_with_lane_id_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setInitialPoseTopicName(std::string topic_name) -{ - input_initial_pose_name_ = topic_name; -} - -void PlanningInterfaceTestManager::setOdometryTopicName(std::string topic_name) -{ - input_odometry_name_ = topic_name; -} - -void PlanningInterfaceTestManager::publishNominalTrajectory( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_trajectory_pub_, - autoware::test_utils::generateTrajectory(10, 1.0), 5); -} - -void PlanningInterfaceTestManager::publishAbnormalTrajectory( - rclcpp::Node::SharedPtr target_node, const Trajectory & abnormal_trajectory) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, input_trajectory_name_, abnormal_trajectory_pub_, abnormal_trajectory); -} - -void PlanningInterfaceTestManager::publishNominalRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_route_pub_, autoware::test_utils::makeNormalRoute(), - 5); -} - -void PlanningInterfaceTestManager::publishBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, std::string topic_name, ModuleName module_name) -{ - if (module_name == ModuleName::START_PLANNER) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, behavior_normal_route_pub_, - autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId(10291, 10333), 5); - } else { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, behavior_normal_route_pub_, - autoware::test_utils::makeBehaviorNormalRoute(), 5); - } -} - -void PlanningInterfaceTestManager::publishAbnormalRoute( - rclcpp::Node::SharedPtr target_node, const LaneletRoute & abnormal_route) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, input_route_name_, abnormal_route_pub_, abnormal_route, 5); -} - -void PlanningInterfaceTestManager::publishNominalPathWithLaneId( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { try { const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_path_with_lane_id_pub_, path, 5); + publishInput(target_node, topic_name, path, 5); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } } -void PlanningInterfaceTestManager::publishAbNominalPathWithLaneId( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithAbnormalPathWithLaneId( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, abnormal_path_with_lane_id_pub_, PathWithLaneId{}, 5); + publishInput(target_node, topic_name, PathWithLaneId{}, 5); } -void PlanningInterfaceTestManager::publishNominalPath( - rclcpp::Node::SharedPtr target_node, std::string topic_name) +void PlanningInterfaceTestManager::testWithNormalPath( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { try { const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, normal_path_pub_, - autoware::motion_utils::convertToPath(path), 5); + publishInput( + target_node, topic_name, autoware::motion_utils::convertToPath(path), 5); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } } -void PlanningInterfaceTestManager::publishAbnormalPath( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, abnormal_path_pub_, Path{}, 5); -} - -void PlanningInterfaceTestManager::setTrajectorySubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, traj_sub_, count_); -} - -void PlanningInterfaceTestManager::setRouteSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, route_sub_, count_); -} -void PlanningInterfaceTestManager::setScenarioSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, scenario_sub_, count_); -} - -void PlanningInterfaceTestManager::setPathWithLaneIdSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, path_with_lane_id_sub_, count_); -} - -void PlanningInterfaceTestManager::setPathSubscriber(std::string topic_name) -{ - autoware::test_utils::setSubscriber(test_node_, topic_name, path_sub_, count_); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithNominalTrajectory(rclcpp::Node::SharedPtr target_node) -{ - publishNominalTrajectory(target_node, input_trajectory_name_); -} - -// check to see if target node is dead. -void PlanningInterfaceTestManager::testWithAbnormalTrajectory(rclcpp::Node::SharedPtr target_node) -{ - publishAbnormalTrajectory(target_node, Trajectory{}); - publishAbnormalTrajectory( - target_node, autoware::test_utils::generateTrajectory(1, 0.0)); - publishAbnormalTrajectory( - target_node, autoware::test_utils::generateTrajectory(10, 0.0, 0.0, 0.0, 0.0, 1)); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithNominalRoute(rclcpp::Node::SharedPtr target_node) -{ - publishNominalRoute(target_node, input_route_name_); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithBehaviorNominalRoute( - rclcpp::Node::SharedPtr target_node, ModuleName module_name) -{ - publishBehaviorNominalRoute(target_node, input_route_name_, module_name); -} - -// check to see if target node is dead. -void PlanningInterfaceTestManager::testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node) -{ - publishAbnormalRoute(target_node, LaneletRoute{}); -} - -// test for normal working -void PlanningInterfaceTestManager::testWithNominalPathWithLaneId( - rclcpp::Node::SharedPtr target_node) -{ - publishNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); -} - -// check to see if target node is dead. -void PlanningInterfaceTestManager::testWithAbnormalPathWithLaneId( - rclcpp::Node::SharedPtr target_node) -{ - publishAbNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromRoute(target_node); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromPath(target_node); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testPathWithLaneIdWithInvalidEgoPose( - rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromPathWithLaneId(target_node); -} - -// put all abnormal ego pose related tests in this functions to run all tests with one line code -void PlanningInterfaceTestManager::testTrajectoryWithInvalidEgoPose( - rclcpp::Node::SharedPtr target_node) -{ - testOffTrackFromTrajectory(target_node); -} - -void PlanningInterfaceTestManager::testOffTrackFromRoute(rclcpp::Node::SharedPtr target_node) -{ - publishBehaviorNominalRoute(target_node, input_route_name_); - - const std::vector deviation_from_route = {0.0, 1.0, 10.0, 100.0}; - for (const auto & deviation : deviation_from_route) { - publishInitialPose(target_node, input_initial_pose_name_, deviation); - } -} - -void PlanningInterfaceTestManager::testOffTrackFromPath(rclcpp::Node::SharedPtr target_node) +void PlanningInterfaceTestManager::testWithAbnormalPath( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - // write me - (void)target_node; + publishInput(target_node, topic_name, Path{}, 5); } -void PlanningInterfaceTestManager::testOffTrackFromPathWithLaneId( - rclcpp::Node::SharedPtr target_node) +void PlanningInterfaceTestManager::testWithOffTrackInitialPoses( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - publishNominalPathWithLaneId(target_node, input_path_with_lane_id_name_); - - const std::vector deviation_from_path = {0.0, 1.0, 10.0, 100.0}; - for (const auto & deviation : deviation_from_path) { - publishOdometry(target_node, input_odometry_name_, deviation); + for (const auto & deviation : {0.0, 1.0, 10.0, 100.0}) { + publishInput(target_node, topic_name, autoware::test_utils::makeInitialPose(deviation), 5); } } -void PlanningInterfaceTestManager::testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node) +void PlanningInterfaceTestManager::testWithOffTrackOdometry( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name) { - publishNominalTrajectory(target_node, input_trajectory_name_); - - const std::vector deviation_from_traj = {0.0, 1.0, 10.0, 100.0}; - for (const auto & deviation : deviation_from_traj) { - publishOdometry(target_node, input_odometry_name_, deviation); + for (const auto & deviation : {0.0, 1.0, 10.0, 100.0}) { + publishInput(target_node, topic_name, autoware::test_utils::makeOdometry(deviation), 5); } } - -void PlanningInterfaceTestManager::testWithNominalPath(rclcpp::Node::SharedPtr target_node) -{ - publishNominalPath(target_node, input_path_name_); -} - -void PlanningInterfaceTestManager::testWithAbnormalPath(rclcpp::Node::SharedPtr target_node) -{ - publishAbnormalPath(target_node, input_path_name_); -} - -int PlanningInterfaceTestManager::getReceivedTopicNum() -{ - return count_; -} - -rclcpp::Node::SharedPtr PlanningInterfaceTestManager::getTestNode() const -{ - return test_node_; -} - } // namespace autoware::planning_test_manager diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp index 2cf03e04311f2..e52b74571282d 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::planning_test_manager::PlanningInterfaceTestManager; @@ -31,12 +32,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: planning_validator → test_node_ - test_manager->setTrajectorySubscriber("planning_validator/output/trajectory"); - - // set planning_validator's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("planning_validator/input/trajectory"); - - test_manager->setOdometryTopicName("planning_validator/input/kinematics"); + test_manager->subscribeOutput( + "planning_validator/output/trajectory"); return test_manager; } @@ -60,7 +57,8 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "planning_validator/input/kinematics"); + test_manager->publishInput( + test_target_node, "planning_validator/input/kinematics", autoware::test_utils::makeOdometry()); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -70,12 +68,15 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "planning_validator/input/trajectory"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); } TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) @@ -85,10 +86,13 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "planning_validator/input/trajectory"; + const std::string input_odometry_topic = "planning_validator/input/kinematics"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); } diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 6202be28e7e87..9c48083d42116 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -17,11 +17,11 @@ #include +#include #include #include #include #include -#include #include #include @@ -37,13 +37,13 @@ namespace autoware::route_handler { +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using std_msgs::msg::Header; -using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; using RouteSections = std::vector; diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 89930fd4a23a9..ba82437bd2d2b 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -23,9 +23,9 @@ #include #include +#include #include #include -#include #include #include @@ -52,12 +52,12 @@ namespace { using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternionFromYaw; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; using lanelet::utils::to2D; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; bool exists(const std::vector & primitives, const int64_t & id) { diff --git a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index ef9af48967a11..0256e7463b5a7 100644 --- a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -22,7 +22,9 @@ #include #include +#include #include + namespace autoware::scenario_selector { using autoware::planning_test_manager::PlanningInterfaceTestManager; @@ -32,9 +34,7 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: scenario_selector → test_node_ - test_manager->setScenarioSubscriber("output/scenario"); - - test_manager->setOdometryTopicName("input/odometry"); + test_manager->subscribeOutput("output/scenario"); return test_manager; } @@ -58,12 +58,18 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "input/odometry"); - test_manager->publishParkingState(test_target_node, "is_parking_completed"); - test_manager->publishTrajectory(test_target_node, "input/parking/trajectory"); - test_manager->publishMap(test_target_node, "input/lanelet_map"); - test_manager->publishRoute(test_target_node, "input/route"); - test_manager->publishOperationModeState(test_target_node, "input/operation_mode_state"); + test_manager->publishInput( + test_target_node, "input/odometry", autoware::test_utils::makeOdometry()); + test_manager->publishInput(test_target_node, "is_parking_completed", std_msgs::msg::Bool{}); + test_manager->publishInput( + test_target_node, "input/parking/trajectory", autoware_planning_msgs::msg::Trajectory{}); + test_manager->publishInput( + test_target_node, "input/lanelet_map", autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "input/route", autoware::test_utils::makeNormalRoute()); + test_manager->publishInput( + test_target_node, "input/operation_mode_state", + autoware_adapi_v1_msgs::msg::OperationModeState{}); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryLaneDrivingMode) @@ -74,15 +80,15 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryLaneDrivingMode publishMandatoryTopics(test_manager, test_target_node); - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/lane_driving/trajectory"); + const std::string input_trajectory_topic = "input/lane_driving/trajectory"; // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -95,15 +101,15 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryParkingMode) publishMandatoryTopics(test_manager, test_target_node); - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); + const std::string input_trajectory_topic = "input/parking/trajectory"; // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -115,14 +121,14 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/lane_driving/trajectory"); + const std::string input_trajectory_topic = "input/lane_driving/trajectory"; + const std::string input_odometry_topic = "input/odometry"; // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index b590af1b3ee0a..d8a424095fcc8 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -21,6 +21,7 @@ #include #include +#include #include using autoware::planning_test_manager::PlanningInterfaceTestManager; @@ -29,9 +30,8 @@ using autoware::velocity_smoother::VelocitySmootherNode; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - test_manager->setTrajectorySubscriber("velocity_smoother/output/trajectory"); - test_manager->setTrajectoryInputTopicName("velocity_smoother/input/trajectory"); - test_manager->setOdometryTopicName("/localization/kinematic_state"); + test_manager->subscribeOutput( + "velocity_smoother/output/trajectory"); return test_manager; } @@ -60,12 +60,17 @@ void publishMandatoryTopics( rclcpp::Node::SharedPtr test_target_node) { // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); - test_manager->publishMaxVelocity( - test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); - test_manager->publishOperationModeState( - test_target_node, "velocity_smoother/input/operation_mode_state"); - test_manager->publishAcceleration(test_target_node, "velocity_smoother/input/acceleration"); + test_manager->publishInput( + test_target_node, "/localization/kinematic_state", autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/external_velocity_limit_mps", + tier4_planning_msgs::msg::VelocityLimit{}); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/operation_mode_state", + autoware_adapi_v1_msgs::msg::OperationModeState{}); + test_manager->publishInput( + test_target_node, "velocity_smoother/input/acceleration", + geometry_msgs::msg::AccelWithCovarianceStamped{}); } TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) @@ -76,12 +81,15 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "velocity_smoother/output/trajectory"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -94,11 +102,14 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "velocity_smoother/output/trajectory"; + const std::string input_odometry_topic = "/localization/kinematic_state"; + // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW(test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 349d92377d091..c917171c2eb19 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -35,12 +35,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -54,13 +58,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 8987e7c0446ee..0f164ad05455e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -20,12 +20,12 @@ #include #include +#include #include #include #include #include #include -#include #include #include @@ -58,8 +58,8 @@ std::vector getAllKeys(const std::unordered_map & map) namespace autoware::behavior_path_planner { using autoware::universe_utils::Polygon2d; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedPath; -using tier4_planning_msgs::msg::PathWithLaneId; struct MinMaxValue { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 488475079613f..1b2593288ca34 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -32,12 +32,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -51,13 +55,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) {"autoware::behavior_path_planner::DynamicObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index 30b1938bb9427..c192fdb315c6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -28,7 +28,6 @@ autoware_universe_utils pluginlib rclcpp - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 506e43a94de1d..0f1ab11acf3c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -36,12 +36,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -56,13 +60,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index 6172fb75978cd..d4f6948de5a14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -38,7 +39,6 @@ #include #include #include -#include #include #include @@ -66,8 +66,8 @@ using autoware::behavior_path_planner::GoalPlannerParameters; using autoware::behavior_path_planner::PlannerData; using autoware::behavior_path_planner::PullOverPath; using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletRoute; -using tier4_planning_msgs::msg::PathWithLaneId; std::vector g_colors = { "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index f7fefa8ba9dc0..2fe4a0fa63dc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -38,7 +39,6 @@ #include #include #include -#include #include #include @@ -65,8 +65,8 @@ using autoware::behavior_path_planner::GoalPlannerParameters; using autoware::behavior_path_planner::PlannerData; using autoware::behavior_path_planner::PullOverPath; using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletRoute; -using tier4_planning_msgs::msg::PathWithLaneId; std::vector g_colors = { "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 3b57ab67b265d..5388ecd691bb0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 201b7c2d33bf6..a2978d8cd32b0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -17,14 +17,14 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" +#include #include -#include #include using autoware::universe_utils::LinearRing2d; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index f2cc7e2f4ef65..fb278fd65b7a3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -24,8 +24,8 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include -#include #include @@ -288,17 +288,7 @@ class GoalPlannerModule : public SceneModuleInterface getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished"); } - void updateModuleParams(const std::any & parameters) override - { - parameters_ = std::any_cast>(parameters); - if (parameters_->safety_check_params.enable_safety_check) { - ego_predicted_path_params_ = - std::make_shared(parameters_->ego_predicted_path_params); - objects_filtering_params_ = - std::make_shared(parameters_->objects_filtering_params); - safety_check_params_ = std::make_shared(parameters_->safety_check_params); - } - } + void updateModuleParams([[maybe_unused]] const std::any & parameters) override {} BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -308,7 +298,6 @@ class GoalPlannerModule : public SceneModuleInterface void updateData() override; void postProcess() override; - void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { @@ -316,61 +305,41 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - std::pair syncWithThreads(); + const GoalPlannerParameters parameters_; + const EgoPredictedPathParams & ego_predicted_path_params_ = parameters_.ego_predicted_path_params; + const ObjectsFilteringParams & objects_filtering_params_ = parameters_.objects_filtering_params; + const SafetyCheckParams safety_check_params_ = parameters_.safety_check_params; + + const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + + const bool left_side_parking_; bool trigger_thread_on_approach_{false}; + // pre-generate lane parking paths in a separate thread + rclcpp::TimerBase::SharedPtr lane_parking_timer_; + rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; + std::atomic is_lane_parking_cb_running_; // NOTE: never access to following variables except in updateData()!!! std::mutex lane_parking_mutex_; std::optional lane_parking_request_; LaneParkingResponse lane_parking_response_; - + // generate freespace parking paths in a separate thread + rclcpp::TimerBase::SharedPtr freespace_parking_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; + std::atomic is_freespace_parking_cb_running_; std::mutex freespace_parking_mutex_; std::optional freespace_parking_request_; FreespaceParkingResponse freespace_parking_response_; - /* - * state transitions and plan function used in each state - * - * +--------------------------+ - * | RUNNING | - * | planPullOverAsCandidate()| - * +------------+-------------+ - * | hasDecidedPath() - * 2 v - * +--------------------------+ - * | WAITING_APPROVAL | - * | planPullOverAsCandidate()| - * +------------+-------------+ - * | isActivated() - * 3 v - * +--------------------------+ - * | RUNNING | - * | planPullOverAsOutput() | - * +--------------------------+ - */ - - // The start_planner activates when it receives a new route, - // so there is no need to terminate the goal planner. - // If terminating it, it may switch to lane following and could generate an inappropriate path. - bool canTransitSuccessState() override { return false; } - bool canTransitFailureState() override { return false; } - - mutable StartGoalPlannerData goal_planner_data_; - - std::shared_ptr parameters_; - - mutable std::shared_ptr ego_predicted_path_params_; - mutable std::shared_ptr objects_filtering_params_; - mutable std::shared_ptr safety_check_params_; - - autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; - std::unique_ptr fixed_goal_planner_; // goal searcher std::shared_ptr goal_searcher_; GoalCandidates goal_candidates_{}; + bool use_bus_stop_area_{false}; + // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on // onFreespaceParkingTimer thread storage may point to while calculation. // onTimer/onFreespaceParkingTimer and their callees MUST NOT use this @@ -380,8 +349,6 @@ class GoalPlannerModule : public SceneModuleInterface std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; - autoware::universe_utils::LinearRing2d vehicle_footprint_; - std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() PathDecisionStateController path_decision_controller_{getLogger()}; @@ -393,24 +360,40 @@ class GoalPlannerModule : public SceneModuleInterface // ego may exceed the stop distance, so add a buffer const double stop_distance_buffer_{2.0}; - // for parking policy - bool left_side_parking_{true}; - - // pre-generate lane parking paths in a separate thread - rclcpp::TimerBase::SharedPtr lane_parking_timer_; - rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; - std::atomic is_lane_parking_cb_running_; - - // generate freespace parking paths in a separate thread - rclcpp::TimerBase::SharedPtr freespace_parking_timer_; - rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; - std::atomic is_freespace_parking_cb_running_; - // debug mutable GoalPlannerDebugData debug_data_; // goal seach - GoalCandidates generateGoalCandidates() const; + GoalCandidates generateGoalCandidates(const bool use_bus_stop_area) const; + + /* + * state transitions and plan function used in each state + * + * +--------------------------+ + * | RUNNING | + * | planPullOverAsCandidate()| + * +------------+-------------+ + * | hasDecidedPath() + * 2 v + * +--------------------------+ + * | WAITING_APPROVAL | + * | planPullOverAsCandidate()| + * +------------+-------------+ + * | isActivated() + * 3 v + * +--------------------------+ + * | RUNNING | + * | planPullOverAsOutput() | + * +--------------------------+ + */ + + // The start_planner activates when it receives a new route, + // so there is no need to terminate the goal planner. + // If terminating it, it may switch to lane following and could generate an inappropriate path. + bool canTransitSuccessState() override { return false; } + bool canTransitFailureState() override { return false; } + + std::pair syncWithThreads(); // stop or decelerate void deceleratePath(PullOverPath & pull_over_path) const; @@ -427,7 +410,7 @@ class GoalPlannerModule : public SceneModuleInterface double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status - bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); + bool hasFinishedCurrentPath(const PullOverContextData & ctx_data) const; double calcModuleRequestLength() const; void decideVelocity(PullOverPath & pull_over_path); void updateStatus(const BehaviorModuleOutput & output); @@ -480,9 +463,6 @@ class GoalPlannerModule : public SceneModuleInterface std::pair calcDistanceToPathChange( const PullOverContextData & context_data) const; - // safety check - void initializeSafetyCheckParameters(); - SafetyCheckParams createSafetyCheckParams() const; /* void updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, @@ -494,11 +474,8 @@ class GoalPlannerModule : public SceneModuleInterface */ std::pair isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params) const; + const std::optional & pull_over_path_opt, + const PathDecisionState & prev_data) const; // debug void setDebugData(const PullOverContextData & context_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index ac4ffe97d6f2d..19231ed26b5c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -30,7 +30,8 @@ class GoalSearcher : public GoalSearcherBase public: GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint); - GoalCandidates search(const std::shared_ptr & planner_data) override; + GoalCandidates search( + const std::shared_ptr & planner_data, const bool use_bus_stop_area) override; void update( GoalCandidates & goal_candidates, const std::shared_ptr occupancy_grid_map, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp index 23a51d1f8c86a..cef9981e3cb14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -47,7 +47,8 @@ class GoalSearcherBase virtual ~GoalSearcherBase() = default; MultiPolygon2d getAreaPolygons() const { return area_polygons_; } - virtual GoalCandidates search(const std::shared_ptr & planner_data) = 0; + virtual GoalCandidates search( + const std::shared_ptr & planner_data, const bool use_bus_stop_area) = 0; virtual void update( [[maybe_unused]] GoalCandidates & goal_candidates, [[maybe_unused]] const std::shared_ptr occupancy_grid_map, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp index 9b97f87c933a8..0955589d8857e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index 06ff47224dd44..a68cb9c2c84c9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index e12a759eb2137..553a6ddb7eecc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 7f404a4055b26..651d326061eb1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -18,16 +18,16 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" +#include #include -#include #include #include #include using autoware::universe_utils::LinearRing2d; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 994d8283fe36c..c8fe9c6d20e78 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index af20c5f6debe5..d00379c63258f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -33,9 +33,11 @@ class LaneParkingRequest public: LaneParkingRequest( const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output) + const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output, + const bool use_bus_stop_area) : vehicle_footprint_(vehicle_footprint), goal_candidates_(goal_candidates), + use_bus_stop_area_(use_bus_stop_area), upstream_module_output_(upstream_module_output) { } @@ -48,6 +50,7 @@ class LaneParkingRequest const autoware::universe_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; + const bool use_bus_stop_area_; const std::shared_ptr & get_planner_data() const { return planner_data_; } const ModuleStatus & get_current_status() const { return current_status_; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 7f375da295fb9..e6a345be6bddc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -20,11 +20,11 @@ #include +#include #include #include #include #include -#include #include #include @@ -36,11 +36,11 @@ namespace autoware::behavior_path_planner::goal_planner_utils { +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Shape = autoware_perception_msgs::msg::Shape; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index feb42cdee92b6..ee52ff5261f0a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -30,7 +30,6 @@ autoware_universe_utils pluginlib rclcpp - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 160bb33dc07de..b4b0cad48847a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -24,7 +24,7 @@ namespace autoware::behavior_path_planner { using Point2d = autoware::universe_utils::Point2d; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { @@ -91,7 +91,7 @@ PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the // refine_goal_search_radius_range is recommended bool is_valid_path{false}; - tier4_planning_msgs::msg::PathWithLaneId refined_path; + autoware_internal_planning_msgs::msg::PathWithLaneId refined_path; while (goal_search_radius >= 0 && !is_valid_path) { refined_path = utils::refinePathForGoal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 448429b9a3b0b..bf722d19744c9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -72,23 +72,26 @@ GoalPlannerModule::GoalPlannerModule( objects_of_interest_marker_interface_ptr_map, const std::shared_ptr planning_factor_interface) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT - parameters_{parameters}, + parameters_{*parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, + left_side_parking_{parameters_.parking_policy == ParkingPolicy::LEFT_SIDE}, is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false} { - occupancy_grid_map_ = std::make_shared(); + // TODO(soblin): remove safety_check_params.enable_safety_check because it does not make sense + if (!parameters_.safety_check_params.enable_safety_check) { + throw std::domain_error("goal_planner never works without safety check"); + } - left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; + occupancy_grid_map_ = std::make_shared(); // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); // set selected goal searcher // currently there is only one goal_searcher_type - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - vehicle_footprint_ = vehicle_info.createFootprint(); - goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_); + goal_searcher_ = std::make_shared(parameters_, vehicle_footprint_); // timer callback for generating lane parking candidate paths const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); @@ -98,13 +101,13 @@ GoalPlannerModule::GoalPlannerModule( &node, clock_, lane_parking_period_ns, [lane_parking_executor = std::make_unique( node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_, - is_lane_parking_cb_running_, getLogger(), *parameters_)]() { + is_lane_parking_cb_running_, getLogger(), parameters_)]() { lane_parking_executor->onTimer(); }, lane_parking_timer_cb_group_); // freespace parking - if (parameters_->enable_freespace_parking) { + if (parameters_.enable_freespace_parking) { auto freespace_planner = std::make_shared(node, *parameters); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = @@ -118,18 +121,6 @@ GoalPlannerModule::GoalPlannerModule( }, freespace_parking_timer_cb_group_); } - - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - } - - /** - * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called - * from the main thread only. - * If you want to see the processing time tree in console, uncomment the following line - */ - // time_keeper_->add_reporter(&std::cerr); } bool isOnModifiedGoal( @@ -328,6 +319,7 @@ void LaneParkingPlanner::onTimer() const auto & pull_over_path_opt = local_request.get_pull_over_path(); const auto & prev_data = local_request.get_prev_data(); const auto trigger_thread_on_approach = local_request.trigger_thread_on_approach(); + const auto use_bus_stop_area = local_request.use_bus_stop_area_; if (!trigger_thread_on_approach) { return; @@ -394,7 +386,7 @@ void LaneParkingPlanner::onTimer() std::vector path_candidates{}; std::optional closest_start_pose{}; std::optional> sorted_indices_opt{std::nullopt}; - if (parameters_.bus_stop_area.use_bus_stop_area && switch_bezier_) { + if (use_bus_stop_area && switch_bezier_) { bezier_planning_helper( local_planner_data, goal_candidates, upstream_module_output, current_lanes, closest_start_pose, path_candidates, sorted_indices_opt); @@ -667,7 +659,7 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(lane_parking_mutex_); if (!lane_parking_request_) { lane_parking_request_.emplace( - vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); + vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(), use_bus_stop_area_); } // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that // planner_data_ is not nullptr, so it is OK to copy as value @@ -698,14 +690,14 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(freespace_parking_mutex_); if (!freespace_parking_request_) { freespace_parking_request_.emplace( - *parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); + parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); } constexpr double stuck_time = 5.0; freespace_parking_request_.value().update( *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time, isStopped( odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time, - parameters_->th_stopped_velocity)); + parameters_.th_stopped_velocity)); // TODO(soblin): currently, ogm-based-collision-detector is updated to latest in // freespace_parking_request_.value().update, and it is shared with goal_planner_module. Next, // goal_planner_module update it and pass it to freespace_parking_request. @@ -722,7 +714,19 @@ void GoalPlannerModule::updateData() // update goal searcher and generate goal candidates if (goal_candidates_.empty()) { - goal_candidates_ = generateGoalCandidates(); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); + use_bus_stop_area_ = + parameters_.bus_stop_area.use_bus_stop_area && + std::any_of( + bus_stop_area_polygons.begin(), bus_stop_area_polygons.end(), [&](const auto & area) { + const auto & goal_position = planner_data_->route_handler->getOriginalGoalPose().position; + return boost::geometry::within( + universe_utils::Point2d{goal_position.x, goal_position.y}, area); + }); + goal_candidates_ = generateGoalCandidates(use_bus_stop_area_); } const lanelet::ConstLanelets current_lanes = @@ -736,7 +740,7 @@ void GoalPlannerModule::updateData() const double self_to_goal_arc_length = utils::getSignedDistance( planner_data_->self_odometry->pose.pose, planner_data_->route_handler->getOriginalGoalPose(), current_lanes); - if (self_to_goal_arc_length < parameters_->pull_over_prepare_length) { + if (self_to_goal_arc_length < parameters_.pull_over_prepare_length) { trigger_thread_on_approach_ = true; [[maybe_unused]] const auto send_only_request = syncWithThreads(); RCLCPP_INFO( @@ -770,21 +774,20 @@ void GoalPlannerModule::updateData() // save "old" state const auto prev_decision_state = path_decision_controller_.get_current_state(); - const auto [is_current_safe, collision_check_map] = isSafePath( - planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_, - ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); + const auto [is_current_safe, collision_check_map] = + isSafePath(planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state); debug_data_.collision_check = collision_check_map; // update to latest state // extract static and dynamic objects in extraction polygon for path collision check const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects( - *(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_, + *(planner_data_->dynamic_object), *(planner_data_->route_handler), parameters_, planner_data_->parameters.vehicle_width); const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_target_objects, parameters_->th_moving_object_velocity); + dynamic_target_objects, parameters_.th_moving_object_velocity); path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, - modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, + modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, parameters_, goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); @@ -793,16 +796,16 @@ void GoalPlannerModule::updateData() path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, dynamic_target_objects, prev_decision_state, isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity), + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, + parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } else { context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, dynamic_target_objects, prev_decision_state, isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity), + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, + parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } auto & ctx_data = context_data_.value(); @@ -828,15 +831,6 @@ void GoalPlannerModule::updateData() } } -void GoalPlannerModule::initializeSafetyCheckParameters() -{ - ego_predicted_path_params_ = - std::make_shared(parameters_->ego_predicted_path_params); - safety_check_params_ = std::make_shared(parameters_->safety_check_params); - objects_filtering_params_ = - std::make_shared(parameters_->objects_filtering_params); -} - void GoalPlannerModule::processOnExit() { resetPathCandidate(); @@ -879,7 +873,7 @@ bool GoalPlannerModule::isExecutionRequested() const utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() - : parameters_->pull_over_minimum_request_length; + : parameters_.pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -889,8 +883,8 @@ bool GoalPlannerModule::isExecutionRequested() const // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + *(route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); @@ -918,7 +912,7 @@ bool GoalPlannerModule::isExecutionReady() const // NOTE(soblin): at least in goal_planner, isExecutionReady is called via super::updateRTCStatus // from self::postProcess, so returning cached member variable like // path_decision_controller.get_current_state() is valid - if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { + if (isWaitingApproval()) { if (!path_decision_controller_.get_current_state().is_stable_safe) { RCLCPP_INFO_THROTTLE( getLogger(), *clock_, 5000, @@ -932,9 +926,9 @@ bool GoalPlannerModule::isExecutionReady() const double GoalPlannerModule::calcModuleRequestLength() const { const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (!min_stop_distance) { - return parameters_->pull_over_minimum_request_length; + return parameters_.pull_over_minimum_request_length; } // The module is requested at a distance such that the ego can stop for the pull over start point @@ -943,18 +937,18 @@ double GoalPlannerModule::calcModuleRequestLength() const // min_stop_distance is used as is, so scale is applied to provide a buffer. constexpr double scale_factor_for_buffer = 1.2; const double minimum_request_length = *min_stop_distance * scale_factor_for_buffer + - parameters_->backward_goal_search_length + + parameters_.backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); + return std::max(minimum_request_length, parameters_.pull_over_minimum_request_length); } bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data) { // return only before starting free space parking if (!isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity)) { + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, + parameters_.th_stopped_velocity)) { return false; } @@ -974,19 +968,18 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte const auto & path = lane_parking_path.full_path(); const auto & curvatures = lane_parking_path.full_path_curvatures(); if ( - parameters_->use_object_recognition && + parameters_.use_object_recognition && goal_planner_utils::checkObjectsCollision( path, curvatures, context_data.static_target_objects, context_data.dynamic_target_objects, - planner_data_->parameters, - parameters_->object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false, parameters_->maximum_deceleration, - parameters_->object_recognition_collision_check_max_extra_stopping_margin, + planner_data_->parameters, parameters_.object_recognition_collision_check_hard_margins.back(), + /*extract_static_objects=*/false, parameters_.maximum_deceleration, + parameters_.object_recognition_collision_check_max_extra_stopping_margin, debug_data_.ego_polygons_expanded)) { return false; } if ( - parameters_->use_occupancy_grid_for_path_collision_check && + parameters_.use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(path, occupancy_grid_map_)) { return false; } @@ -1002,14 +995,14 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return true; } -GoalCandidates GoalPlannerModule::generateGoalCandidates() const +GoalCandidates GoalPlannerModule::generateGoalCandidates(const bool use_bus_stop_area) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // calculate goal candidates const auto & route_handler = planner_data_->route_handler; if (utils::isAllowedGoalModification(route_handler)) { - return goal_searcher_->search(planner_data_); + return goal_searcher_->search(planner_data_, use_bus_stop_area); } // NOTE: @@ -1226,7 +1219,7 @@ std::optional GoalPlannerModule::selectPullOverPath( const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); const double backward_length = - parameters_->backward_goal_search_length + parameters_->decide_path_distance; + parameters_.backward_goal_search_length + parameters_.decide_path_distance; const auto & prev_module_output_path = getPreviousModuleOutput().path; // STEP1: Filter valid paths before sorting @@ -1265,7 +1258,7 @@ std::optional GoalPlannerModule::selectPullOverPath( const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data_->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), - goal_pose_length + parameters_->forward_goal_search_length); + goal_pose_length + parameters_.forward_goal_search_length); }(); sorted_path_indices.erase( @@ -1277,29 +1270,29 @@ std::optional GoalPlannerModule::selectPullOverPath( sorted_path_indices.end()); sortPullOverPaths( - planner_data_, *parameters_, pull_over_path_candidates, goal_candidates, + planner_data_, parameters_, pull_over_path_candidates, goal_candidates, context_data.static_target_objects, getLogger(), sorted_path_indices); // STEP3: Select the final pull over path by checking collision to make it as high priority as // possible const double collision_check_margin = - parameters_->object_recognition_collision_check_hard_margins.back(); + parameters_.object_recognition_collision_check_hard_margins.back(); for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const PathWithLaneId & parking_path = path.parking_path(); const auto & parking_path_curvatures = path.parking_path_curvatures(); if ( - parameters_->use_object_recognition && + parameters_.use_object_recognition && goal_planner_utils::checkObjectsCollision( parking_path, parking_path_curvatures, context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_->parameters, collision_check_margin, - true, parameters_->maximum_deceleration, - parameters_->object_recognition_collision_check_max_extra_stopping_margin, + true, parameters_.maximum_deceleration, + parameters_.object_recognition_collision_check_max_extra_stopping_margin, debug_data_.ego_polygons_expanded, true)) { continue; } if ( - parameters_->use_occupancy_grid_for_path_collision_check && + parameters_.use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } @@ -1313,12 +1306,11 @@ std::vector GoalPlannerModule::generateDrivableLanes() const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } @@ -1341,9 +1333,7 @@ void GoalPlannerModule::setOutput( } const auto & pull_over_path = selected_pull_over_path_with_velocity_opt.value(); - if ( - parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && - isActivated()) { + if (!context_data.is_stable_safe_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -1449,7 +1439,7 @@ void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) // partial_paths auto & first_path = pull_over_path.partial_paths().front(); const auto vel = - static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); + static_cast(std::max(current_vel, parameters_.pull_over_minimum_velocity)); for (auto & p : first_path.points) { p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } @@ -1538,7 +1528,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), - modified_goal_opt, last_path_update_time, *parameters_)) { + modified_goal_opt, last_path_update_time, parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ @@ -1708,11 +1698,6 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } -void GoalPlannerModule::setParameters(const std::shared_ptr & parameters) -{ - parameters_ = parameters; -} - PathWithLaneId GoalPlannerModule::generateStopPath( const PullOverContextData & context_data, const std::string & detail) const { @@ -1722,11 +1707,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath( const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - const double pull_over_velocity = parameters_->pull_over_velocity; + const double pull_over_velocity = parameters_.pull_over_velocity; const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); if (current_lanes.empty()) { @@ -1792,7 +1776,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -1815,8 +1799,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // if already passed the decel pose, set pull_over_velocity to stop_path. const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - pull_over_velocity); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, pull_over_velocity); for (auto & p : stop_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(stop_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1835,7 +1818,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath( // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (!min_stop_distance) { return path; } @@ -1898,7 +1881,7 @@ bool FreespaceParkingPlanner::isStuck( return false; } -bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) +bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1941,7 +1924,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d ctx_data.pull_over_path_opt.value().getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < - parameters_->th_arrived_distance; + parameters_.th_arrived_distance; } TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) @@ -1977,8 +1960,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & }; const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); if (current_lanes.empty()) { @@ -2035,7 +2017,7 @@ bool GoalPlannerModule::hasEnoughDistance( const bool is_separated_path = pull_over_path.partial_paths().size() > 1; const double distance_to_start = calcSignedArcLength( long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); - const double distance_to_restart = parameters_->decide_path_distance / 2; + const double distance_to_restart = parameters_.decide_path_distance / 2; const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; if (is_separated_path && is_stopped && distance_to_start < distance_to_restart) { @@ -2043,7 +2025,7 @@ bool GoalPlannerModule::hasEnoughDistance( } const auto current_to_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -2109,15 +2091,15 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const // if already passed the search start offset pose, set pull_over_velocity to first_path. const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - parameters_->pull_over_velocity); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, + parameters_.pull_over_velocity); for (auto & p : first_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { continue; } p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); + p.point.longitudinal_velocity_mps, static_cast(parameters_.pull_over_velocity)); } } @@ -2135,7 +2117,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -2154,7 +2136,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { utils::insertStopPoint(stop_point_length, path); @@ -2166,13 +2148,12 @@ void GoalPlannerModule::decelerateBeforeSearchStart( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const double pull_over_velocity = parameters_->pull_over_velocity; + const double pull_over_velocity = parameters_.pull_over_velocity; const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - pull_over_velocity); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -2271,22 +2252,9 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } -/* -void GoalPlannerModule::updateSafetyCheckTargetObjectsData( - const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, - const std::vector & ego_predicted_path) const -{ - goal_planner_data_.filtered_objects = filtered_objects; - goal_planner_data_.target_objects_on_lane = target_objects_on_lane; - goal_planner_data_.ego_predicted_path = ego_predicted_path; -} -*/ - static std::vector filterObjectsByWithinPolicy( const std::shared_ptr & objects, - const lanelet::ConstLanelets & target_lanes, - const std::shared_ptr< - autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams> & params) + const lanelet::ConstLanelets & target_lanes, const ObjectsFilteringParams & params) { // implanted part of behavior_path_planner::utils::path_safety_checker::filterObjects() and // createTargetObjectsOnLane() @@ -2296,8 +2264,8 @@ static std::vector filterOb return {}; } - const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold; - const auto & target_object_types = params->object_types_to_check; + const double ignore_object_velocity_threshold = params.ignore_object_velocity_threshold; + const auto & target_object_types = params.object_types_to_check; PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( *objects, ignore_object_velocity_threshold, true); @@ -2315,8 +2283,8 @@ static std::vector filterOb } } - const double safety_check_time_horizon = params->safety_check_time_horizon; - const double safety_check_time_resolution = params->safety_check_time_resolution; + const double safety_check_time_horizon = params.safety_check_time_horizon; + const double safety_check_time_resolution = params.safety_check_time_resolution; std::vector refined_filtered_objects; for (const auto & within_filtered_object : within_filtered_objects) { @@ -2328,11 +2296,7 @@ static std::vector filterOb std::pair GoalPlannerModule::isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params) const + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data) const { using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; using autoware::behavior_path_planner::utils::path_safety_checker:: @@ -2351,25 +2315,26 @@ std::pair GoalPlannerM const auto & dynamic_object = planner_data->dynamic_object; const auto & route_handler = planner_data->route_handler; const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data, parameters.backward_goal_search_length, parameters.forward_goal_search_length, + planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *route_handler, left_side_parking_, parameters.backward_goal_search_length, - parameters.forward_goal_search_length); + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); const std::pair terminal_velocity_and_accel = pull_over_path.getPairsTerminalVelocityAndAccel(); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - auto temp_param = std::make_shared(*ego_predicted_path_params); + auto temp_param = std::make_shared(ego_predicted_path_params_); utils::parking_departure::updatePathProperty(temp_param, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; const bool limit_to_max_velocity = true; const auto ego_seg_idx = planner_data->findEgoIndex(current_pull_over_path.points); const auto ego_predicted_path_from_current_pose = createPredictedPath( - ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity, - ego_seg_idx, is_object_front, limit_to_max_velocity); + std::make_shared(ego_predicted_path_params_), + current_pull_over_path.points, current_pose, current_velocity, ego_seg_idx, is_object_front, + limit_to_max_velocity); const auto ego_predicted_path = filterPredictedPathAfterTargetPose( ego_predicted_path_from_current_pose, pull_over_path.start_pose()); @@ -2413,35 +2378,35 @@ std::pair GoalPlannerM const auto expanded_pull_over_lanes_between_ego = goal_planner_utils::generateBetweenEgoAndExpandedPullOverLanes( pull_over_lanes, left_side_parking_, ego_pose_for_expand, - planner_data->parameters.vehicle_info, parameters.outer_road_detection_offset, - parameters.inner_road_detection_offset); + planner_data->parameters.vehicle_info, parameters_.outer_road_detection_offset, + parameters_.inner_road_detection_offset); const auto merged_expanded_pull_over_lanes = lanelet::utils::combineLaneletsShape(expanded_pull_over_lanes_between_ego); debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes; const auto filtered_objects = filterObjectsByWithinPolicy( - dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params); + dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params_); const double hysteresis_factor = - prev_data.is_stable_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; + prev_data.is_stable_safe ? 1.0 : parameters_.hysteresis_factor_expand_rate; const bool current_is_safe = std::invoke([&]() { - if (parameters.safety_check_params.method == "RSS") { + if (parameters_.safety_check_params.method == "RSS") { return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( current_pull_over_path, ego_predicted_path, filtered_objects, collision_check, - planner_data->parameters, safety_check_params->rss_params, - objects_filtering_params->use_all_predicted_path, hysteresis_factor, - safety_check_params->collision_check_yaw_diff_threshold); + planner_data->parameters, safety_check_params_.rss_params, + objects_filtering_params_.use_all_predicted_path, hysteresis_factor, + safety_check_params_.collision_check_yaw_diff_threshold); } - if (parameters.safety_check_params.method == "integral_predicted_polygon") { + if (parameters_.safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( ego_predicted_path, vehicle_info_, filtered_objects, - objects_filtering_params->check_all_predicted_path, - parameters.safety_check_params.integral_predicted_polygon_params, collision_check); + objects_filtering_params_.check_all_predicted_path, + parameters_.safety_check_params.integral_predicted_polygon_params, collision_check); } RCLCPP_ERROR( getLogger(), " [pull_over] invalid safety check method: %s", - parameters.safety_check_params.method.c_str()); + parameters_.safety_check_params.method.c_str()); throw std::domain_error("[pull_over] invalid safety check method"); }); @@ -2558,14 +2523,12 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } debug_marker_.markers.push_back(marker); - if (parameters_->safety_check_params.enable_safety_check) { - autoware::universe_utils::appendMarkerArray( - goal_planner_utils::createLaneletPolygonMarkerArray( - debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, - "expanded_pull_over_lane_between_ego", - autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), - &debug_marker_); - } + autoware::universe_utils::appendMarkerArray( + goal_planner_utils::createLaneletPolygonMarkerArray( + debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, + "expanded_pull_over_lane_between_ego", + autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), + &debug_marker_); // Visualize debug poses const auto & debug_poses = pull_over_path.debug_poses; @@ -2575,56 +2538,42 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } - // safety check - if (parameters_->safety_check_params.enable_safety_check) { - if (goal_planner_data_.ego_predicted_path.size() > 0) { - const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( - goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); - add(createPredictedPathMarkerArray( - ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9)); - } - if (goal_planner_data_.filtered_objects.objects.size() > 0) { - add(createObjectsMarkerArray( - goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); - } + auto collision_check = debug_data_.collision_check; + if (parameters_.safety_check_params.method == "RSS") { + add(showSafetyCheckInfo(collision_check, "object_debug_info")); + } + add(showPredictedPath(collision_check, "ego_predicted_path")); + add(showPolygon(collision_check, "ego_and_target_polygon_relation")); - auto collision_check = debug_data_.collision_check; - if (parameters_->safety_check_params.method == "RSS") { - add(showSafetyCheckInfo(collision_check, "object_debug_info")); - } - add(showPredictedPath(collision_check, "ego_predicted_path")); - add(showPolygon(collision_check, "ego_and_target_polygon_relation")); + // set objects of interest + for (const auto & [uuid, data] : collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } - // set objects of interest - for (const auto & [uuid, data] : collision_check) { - const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; - setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); - } + // TODO(Mamoru Sobue): it is not clear where ThreadSafeData::collision_check should be cleared + utils::parking_departure::initializeCollisionCheckDebugMap(collision_check); - // TODO(Mamoru Sobue): it is not clear where ThreadSafeData::collision_check should be cleared - utils::parking_departure::initializeCollisionCheckDebugMap(collision_check); + // visualize safety status maker + { + const auto & prev_data = context_data.prev_state_for_debug; + visualization_msgs::msg::MarkerArray marker_array{}; + const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "safety_status", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - // visualize safety status maker - { - const auto & prev_data = context_data.prev_state_for_debug; - visualization_msgs::msg::MarkerArray marker_array{}; - const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); - auto marker = createDefaultMarker( - header.frame_id, header.stamp, "safety_status", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - - marker.pose = planner_data_->self_odometry->pose.pose; - marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; - if (prev_data.safe_start_time) { - const double elapsed_time_from_safe_start = - (clock_->now() - prev_data.safe_start_time.value()).seconds(); - marker.text += - "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; - } - marker_array.markers.push_back(marker); - add(marker_array); + marker.pose = planner_data_->self_odometry->pose.pose; + marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; + if (prev_data.safe_start_time) { + const double elapsed_time_from_safe_start = + (clock_->now() - prev_data.safe_start_time.value()).seconds(); + marker.text += + "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; } + marker_array.markers.push_back(marker); + add(marker_array); } // Visualize planner type text @@ -2649,7 +2598,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) TODO(soblin): disable until thread safe design is done if (isStuck( context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_, - occupancy_grid_map_, *parameters_)) { + occupancy_grid_map_, parameters_)) { marker.text += " stuck"; } if (isStopped()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index b973cfd7ed549..6124955a39433 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -100,7 +100,8 @@ GoalSearcher::GoalSearcher( { } -GoalCandidates GoalSearcher::search(const std::shared_ptr & planner_data) +GoalCandidates GoalSearcher::search( + const std::shared_ptr & planner_data, const bool use_bus_stop_area) { GoalCandidates goal_candidates{}; @@ -118,7 +119,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const double forward_length = parameters_.forward_goal_search_length; const double backward_length = parameters_.backward_goal_search_length; const double margin_from_boundary = parameters_.margin_from_boundary; - const bool use_bus_stop_area = parameters_.bus_stop_area.use_bus_stop_area; + const double lateral_offset_interval = use_bus_stop_area ? parameters_.bus_stop_area.lateral_offset_interval : parameters_.lateral_offset_interval; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index a1ed5517edf25..e9df7f0a821b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -28,9 +28,9 @@ #include #include +#include #include #include -#include #include #include @@ -42,11 +42,11 @@ namespace autoware::behavior_path_planner using autoware::behavior_path_planner::PoseWithDetailOpt; using autoware::route_handler::Direction; using autoware::universe_utils::StopWatch; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using lane_change::PathSafetyStatus; -using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeBase { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 82b042d1135d2..b1b8f29b9c321 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -26,9 +26,9 @@ #include #include +#include #include #include -#include #include @@ -41,9 +41,9 @@ namespace autoware::behavior_path_planner { using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeInterface : public SceneModuleInterface { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index fec43d021bf8b..358bd367a8610 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -32,11 +32,11 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using lane_change::LanesPolygon; -using tier4_planning_msgs::msg::PathWithLaneId; using utils::path_safety_checker::ExtendedPredictedObjects; using utils::path_safety_checker::RSSparams; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 0fa2c6cc8dfbc..06d40548e4798 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -21,7 +21,7 @@ #include -#include +#include #include #include @@ -31,7 +31,7 @@ namespace autoware::behavior_path_planner::lane_change enum class PathType { ConstantJerk = 0, FrenetPlanner }; using autoware::behavior_path_planner::TurnSignalInfo; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; struct TrajectoryGroup { PathWithLaneId prepare; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 422c392cac462..4d37dba3b8658 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -27,10 +27,10 @@ #include #include +#include #include #include #include -#include #include @@ -49,6 +49,7 @@ using autoware::route_handler::Direction; using autoware::universe_utils::LineString2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; @@ -63,7 +64,6 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; -using tier4_planning_msgs::msg::PathWithLaneId; rclcpp::Logger get_logger(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index 40fd6fc4c572d..aab5f4c01e8ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -31,7 +31,6 @@ pluginlib range-v3 rclcpp - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index bf0af0d133dc7..465835ff5fbb4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -21,9 +21,9 @@ #include #include +#include #include #include -#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 9002270185847..bfe8eb0c29c7c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -73,13 +73,13 @@ using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::lane_change::PathType; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index f6b3e9eeaf715..393ca57bc3a67 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -36,12 +36,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -55,13 +59,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) "autoware::behavior_path_planner::LaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 5fb445788672c..6ac07240a622f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -42,11 +42,11 @@ using autoware::route_handler::RouteHandler; using autoware::test_utils::get_absolute_path_to_config; using autoware::test_utils::get_absolute_path_to_lanelet_map; using autoware::test_utils::get_absolute_path_to_route; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; class TestNormalLaneChange : public ::testing::Test { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md index 81cb945802595..dd51d7d33035e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md @@ -107,13 +107,13 @@ The Planner Manager's responsibilities include: ### Output -| Name | Type | Description | QoS Durability | -| :---------------------------- | :-------------------------------------------------- | :-------------------------------------------------------------------------------------------- | ----------------- | -| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | The path generated by modules | `volatile` | -| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | Turn indicators command | `volatile` | -| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Hazard lights command | `volatile` | -| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | Output modified goal commands | `transient_local` | -| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | The path the module is about to take. To be executed as soon as external approval is obtained | `volatile` | +| Name | Type | Description | QoS Durability | +| :---------------------------- | :----------------------------------------------------- | :-------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `autoware_internal_planning_msgs::msg::PathWithLaneId` | The path generated by modules | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | Turn indicators command | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Hazard lights command | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | Output modified goal commands | `transient_local` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | The path the module is about to take. To be executed as soon as external approval is obtained | `volatile` | ### Debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index d88c93712b1e3..bf87102134da7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -38,7 +39,6 @@ #include #include #include -#include #include #include #include @@ -54,6 +54,7 @@ namespace autoware::behavior_path_planner { using autoware::planning_factor_interface::PlanningFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -68,7 +69,6 @@ using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; -using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::Marker; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 741202590779c..a8ee2f90e5f09 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include @@ -42,7 +42,7 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::StopWatch; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = autoware::universe_utils::DebugPublisher; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 38c5aa0085fb9..0ac3f17fcb36a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -692,8 +692,9 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = autoware::motion_utils::convertToPath( - *path_candidate_ptr); + output = + autoware::motion_utils::convertToPath( + *path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp index 4930ead6d355d..26dc1589e679b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp @@ -26,33 +26,13 @@ namespace autoware::behavior_path_planner { -namespace -{ -using tier4_planning_msgs::msg::LateralOffset; - -void publishLateralOffset( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - auto test_node = test_manager->getTestNode(); - const auto pub_lateral_offset = test_manager->getTestNode()->create_publisher( - "behavior_path_planner/input/lateral_offset", 1); - pub_lateral_offset->publish(LateralOffset{}); - autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); -} -} // namespace - std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + test_manager->subscribeOutput( + "behavior_path_planner/output/path"); return test_manager; } @@ -103,16 +83,32 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - publishLateralOffset(test_manager, test_target_node); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/odometry", + autoware::test_utils::makeInitialPose()); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/perception", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/occupancy_grid_map", + autoware::test_utils::makeCostMapMsg()); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/scenario", + autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::LANEDRIVING)); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/vector_map", + autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/costmap", + autoware::test_utils::makeCostMapMsg()); + test_manager->publishInput( + test_target_node, "system/operation_mode/state", + autoware_adapi_v1_msgs::msg::OperationModeState{}); + test_manager->publishInput( + test_target_node, "behavior_path_planner/input/lateral_offset", + tier4_planning_msgs::msg::LateralOffset{}); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp index f1617cdd9ead0..0f035241c6bfd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp @@ -17,21 +17,21 @@ #endif // INPUT_HPP_ +#include "autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "tier4_planning_msgs/msg/path_point_with_lane_id.hpp" -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include namespace autoware::behavior_path_planner { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp index 0c933345e7647..4526f8a642408 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -34,12 +34,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -51,13 +55,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode({}, {}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 749d732aecd00..9ebf5bc1d3b41 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -39,7 +40,6 @@ #include #include #include -#include #include #include @@ -52,6 +52,7 @@ namespace autoware::behavior_path_planner { using autoware::route_handler::RouteHandler; using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightGroup; @@ -63,7 +64,6 @@ using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using tier4_planning_msgs::msg::LateralOffset; -using tier4_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; using lanelet::TrafficLight; using tier4_planning_msgs::msg::VelocityLimit; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index c5c0972972773..bc419e878b91f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -35,8 +35,8 @@ #include #include +#include #include -#include #include #include #include @@ -58,8 +58,8 @@ using autoware::planning_factor_interface::PlanningFactorInterface; using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::generateUUID; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; -using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::PlanningFactor; using tier4_planning_msgs::msg::SafetyFactorArray; using tier4_rtc_msgs::msg::State; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp index 85c2563c5d98c..8305f9268d23d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp @@ -20,10 +20,10 @@ #include +#include #include #include #include -#include #include #include @@ -41,6 +41,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::CollisionChec using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using autoware::universe_utils::Polygon2d; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Point; @@ -49,7 +50,6 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; -using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index 90cf533a8589d..067333edfcd11 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp @@ -24,10 +24,10 @@ #include #include +#include #include #include #include -#include #include @@ -43,12 +43,12 @@ namespace autoware::behavior_path_planner { using autoware::route_handler::RouteHandler; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::PathWithLaneId; const std::map g_signal_map = { {"left", TurnIndicatorsCommand::ENABLE_LEFT}, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index afe88ac04f302..93fa80519f183 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -17,10 +17,10 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include #include #include #include -#include #include @@ -29,12 +29,12 @@ namespace autoware::behavior_path_planner::drivable_area_expansion { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using autoware::universe_utils::LineString2d; using autoware::universe_utils::MultiLineString2d; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 434eb79189eaf..b632185367de4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -17,9 +17,9 @@ #include +#include #include #include -#include #include @@ -121,7 +121,8 @@ class OccupancyGridBasedCollisionDetector * @return true if a collision is detected, false if no collision is detected. */ [[nodiscard]] bool hasObstacleOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const bool check_out_of_range) const; /** * @brief Detects if a collision occurs at the specified base index in the occupancy grid map. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 9cc9bec8e963a..ab5371ac0dbb4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -20,10 +20,10 @@ #include +#include #include #include #include -#include #include @@ -33,10 +33,10 @@ namespace autoware::behavior_path_planner { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; struct ParallelParkingParameters { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 2ed9fdd9197df..88bd1e09af480 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -18,9 +18,9 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include -#include #include @@ -31,8 +31,8 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_perception_msgs::msg::PredictedObject; -using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters object based on velocity. @@ -86,9 +86,9 @@ bool is_vehicle(const ObjectClassification & classification); namespace autoware::behavior_path_planner::utils::path_safety_checker { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters objects based on object centroid position. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 7c16d7184c42e..f46aa8ad7dea3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -19,8 +19,8 @@ #include #include +#include #include -#include #include #include @@ -30,10 +30,10 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::generateUUID; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; struct ShiftLine diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp index 3e4f180035b20..0b7db4f103a93 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp @@ -21,10 +21,10 @@ #include #include +#include #include #include #include -#include #include @@ -35,11 +35,11 @@ namespace autoware::behavior_path_planner::utils { +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Path; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 7162f49b450e8..30f1ef6510cf9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include #include @@ -29,8 +31,6 @@ #include #include #include -#include -#include #include @@ -49,11 +49,11 @@ using autoware_perception_msgs::msg::PredictedPath; using autoware::route_handler::RouteHandler; using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::Polygon2d; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; static constexpr double eps = 0.01; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 33f46420ad537..640659ed6caab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -173,7 +173,8 @@ bool OccupancyGridBasedCollisionDetector::detectCollision( } bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const bool check_out_of_range) const { for (const auto & p : path.points) { const auto pose_local = global2local(costmap_, p.point.pose); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 789741153bf4a..bef35d5b7d187 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -39,10 +39,10 @@ using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::inverseTransformPoint; using autoware::universe_utils::normalizeRadian; using autoware::universe_utils::transformPose; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; -using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 9b99a5ef7b31d..e5735e4e9a14f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -41,7 +41,7 @@ namespace { double calcInterpolatedZ( - const tier4_planning_msgs::msg::PathWithLaneId & input, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( @@ -60,7 +60,7 @@ double calcInterpolatedZ( } double calcInterpolatedVelocity( - const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { const double seg_dist = autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); @@ -204,8 +204,9 @@ double calcLongitudinalDistanceFromEgoToObjects( } std::optional findIndexOutOfGoalSearchRange( - const std::vector & points, const Pose & goal, - const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) + const std::vector & points, + const Pose & goal, const int64_t goal_lane_id, + const double max_dist = std::numeric_limits::max()) { if (points.empty()) { return std::nullopt; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp index 8c732a972a212..d2827f1cf8cb4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -36,9 +36,9 @@ using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; using TwistWithCovariance = geometry_msgs::msg::TwistWithCovariance; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_perception_msgs::msg::PredictedPath; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp index b212cce551252..773087e60bc84 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp @@ -125,7 +125,7 @@ TEST_F(OccupancyGridBasedCollisionDetectorTest, detectCollision) TEST_F(OccupancyGridBasedCollisionDetectorTest, hasObstacleOnPath) { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; detector_.setMap(costmap_); // Condition: empty path @@ -135,7 +135,7 @@ TEST_F(OccupancyGridBasedCollisionDetectorTest, hasObstacleOnPath) size_t path_length = 10; path.points.reserve(path_length); for (size_t i = 0; i < path_length; i++) { - tier4_planning_msgs::msg::PathPointWithLaneId path_point; + autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point; path_point.point.pose = createPose(static_cast(i), 0.0, 0.0, 0.0, 0.0, 0.0); path.points.push_back(path_point); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp index cefa9151dd604..18da6d1565657 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp @@ -31,9 +31,9 @@ constexpr double epsilon = 1e-6; using autoware::behavior_path_planner::PlannerData; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using autoware::test_utils::generateTrajectory; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp index 562e8abfd432a..dfb69d430a314 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp @@ -17,14 +17,14 @@ #include -#include +#include #include #include #include -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; @@ -67,7 +67,7 @@ TEST(BehaviorPathPlanningPathUtilTest, getIdxByArclength) { using autoware::behavior_path_planner::utils::getIdxByArclength; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; // Condition: empty points EXPECT_ANY_THROW(getIdxByArclength(path, 5, 1.0)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index c5694db10f65e..9ba540b4a9792 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -20,10 +20,10 @@ #include #include +#include #include #include #include -#include #include @@ -47,9 +47,9 @@ using autoware::behavior_path_planner::utils::path_safety_checker::RSSparams; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; using autoware::universe_utils::Polygon2d; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; std::vector create_test_path() { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp index ab6076b328d14..845a04ceb5174 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp @@ -18,8 +18,8 @@ #include #include +#include #include -#include #include #include @@ -128,7 +128,7 @@ TEST(StaticDrivableArea, getOverlappedLaneletId) TEST(StaticDrivableArea, cutOverlappedLanes) { using autoware::behavior_path_planner::utils::cutOverlappedLanes; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; std::vector lanes; { // empty inputs const auto result = cutOverlappedLanes(path, lanes); @@ -258,8 +258,8 @@ TEST(StaticDrivableArea, generateDrivableLanes) TEST(StaticDrivableArea, generateDrivableArea_subfunction) { using autoware::behavior_path_planner::utils::generateDrivableArea; - tier4_planning_msgs::msg::PathWithLaneId path; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_internal_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; generateDrivableArea(path, 0.0, 0.0, true); EXPECT_TRUE(path.left_bound.empty()); EXPECT_TRUE(path.right_bound.empty()); @@ -450,7 +450,7 @@ TEST(DISABLED_StaticDrivableArea, generateDrivableArea) { using autoware::behavior_path_planner::PlannerData; using autoware::behavior_path_planner::utils::generateDrivableArea; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; std::vector lanes; bool enable_expanding_hatched_road_markings = true; bool enable_expanding_intersection_areas = true; @@ -474,7 +474,7 @@ TEST(DISABLED_StaticDrivableArea, generateDrivableArea) lanes = autoware::behavior_path_planner::utils::generateDrivableLanes({ll, shoulder_ll}); lanelet::BasicLineString2d path_ls; for (const auto & p : ll.centerline().basicLineString()) { - tier4_planning_msgs::msg::PathPointWithLaneId pp; + autoware_internal_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose.position.x = p.x(); pp.point.pose.position.y = p.y(); pp.point.pose.position.z = p.z(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp index a5d77b96292f9..300050211ff1e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp @@ -20,11 +20,11 @@ #include +#include #include #include #include #include -#include #include #include @@ -127,7 +127,7 @@ TEST_F(TrafficLightTest, calcDistanceToRedTrafficLight) using autoware::behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; { - const tier4_planning_msgs::msg::PathWithLaneId path; + const autoware_internal_planning_msgs::msg::PathWithLaneId path; const lanelet::ConstLanelets empty_lanelets; EXPECT_FALSE(calcDistanceToRedTrafficLight(empty_lanelets, path, planner_data_).has_value()); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index b6149e3b5f99c..fd17ca91ff269 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp @@ -28,12 +28,12 @@ using autoware::behavior_path_planner::TurnSignalDecider; using autoware::behavior_path_planner::TurnSignalInfo; using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternionFromYaw; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_planning_msgs::msg::PathPoint; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathPointWithLaneId; constexpr double nearest_dist_threshold = 5.0; constexpr double nearest_yaw_threshold = M_PI / 3.0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp index 13fec41092cc8..94ffaeba61711 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp @@ -27,11 +27,11 @@ #include using autoware::universe_utils::Point2d; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; using autoware::behavior_path_planner::PlannerData; using autoware_planning_msgs::msg::LaneletRoute; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index f84fe45d08ca4..6f8f3e9e94642 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -41,8 +41,8 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp index 4408645b26095..7ca21f08879fa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -34,8 +34,8 @@ #include namespace autoware::behavior_path_planner { +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; struct SoftConstraintsInputs diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index c8a5e47287477..8d8f8de9a5211 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -21,8 +21,8 @@ #include +#include #include -#include #include #include @@ -32,10 +32,10 @@ namespace autoware::behavior_path_planner { +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using tier4_planning_msgs::msg::LateralOffset; -using tier4_planning_msgs::msg::PathWithLaneId; class SideShiftModule : public SceneModuleInterface { diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp index 5a3e51c353c39..89fc8f0fb8b17 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp @@ -17,17 +17,17 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include #include #include #include -#include namespace autoware::behavior_path_planner { +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using tier4_planning_msgs::msg::PathWithLaneId; /** * @brief Sets the orientation (yaw angle) for all points in the path. diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index c248a40eb151a..1603f0c225ce2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -34,12 +34,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -52,13 +56,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) generateNode({"side_shift"}, {"autoware::behavior_path_planner::SideShiftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index e1a7ae455e374..9b2c8fa22a9d4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 5d36c996cca3a..bc8c6001ba067 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -22,7 +22,7 @@ #include -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp index 764499ea6dbcf..79bf7a5dddeae 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp @@ -17,14 +17,14 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include namespace autoware::behavior_path_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 0316d6a14a4f9..934234b3d4b4f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -22,8 +22,8 @@ #include "autoware/behavior_path_start_planner_module/util.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" +#include #include -#include #include #include @@ -32,8 +32,8 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::LinearRing2d; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; class PullOutPlannerBase { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 1000437bdea56..84337a0a037f4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -21,7 +21,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 3978738798386..640220a44dfcc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -32,7 +32,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp index 60c5ccedd93f5..dc3e36d72a9fb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp @@ -23,11 +23,11 @@ #include +#include #include #include #include #include -#include #include @@ -38,11 +38,11 @@ namespace autoware::behavior_path_planner::start_planner_utils { using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using autoware::route_handler::RouteHandler; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index c31707a413c6a..674f6a7f53981 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -28,7 +28,6 @@ autoware_universe_utils pluginlib rclcpp - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 293b2d9bc6f28..a89a154250d8b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -23,22 +23,22 @@ #include #include +#include #include #include #include #include -#include #include #include namespace autoware::behavior_path_planner { // auto msgs +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; -using tier4_planning_msgs::msg::PathWithLaneId; // ROS 2 general msgs using geometry_msgs::msg::Point; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index e9cb603b5d69c..6dd7138b4aa24 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -43,7 +43,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index c2531e5f2bb9f..4c36245e5b7d0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -32,12 +32,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, input_route_topic)); rclcpp::shutdown(); } @@ -51,13 +55,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_route_topic = "behavior_path_planner/input/route"; + const std::string input_odometry_topic = "behavior_path_planner/input/odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, input_route_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index 41330e459a524..1c3531b0a607f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -41,10 +41,11 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC private: PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 2d7b79357b602..b22b5aeca0842 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -105,15 +105,16 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path); // setSafe(), setDistance() void setRTCStatus( - const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + const BlindSpotDecision & decision, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); template void setRTCStatusByDecision( - const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + const Decision & decision, const autoware_internal_planning_msgs::msg::PathWithLaneId & path); // stop/GO void reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path); template void reactRTCApprovalByDecision( - const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path); + const Decision & decision, autoware_internal_planning_msgs::msg::PathWithLaneId * path); /** * @brief Generate a stop line and insert it into the path. @@ -126,10 +127,10 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC */ std::optional> generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * path) const; + autoware_internal_planning_msgs::msg::PathWithLaneId * path) const; std::optional isOverPassJudge( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const geometry_msgs::msg::Pose & stop_point_pose) const; double computeTimeToPassStopLine( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index e18d96709ef92..4931c94b1931f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -39,7 +39,7 @@ enum class TurnDirection { LEFT, RIGHT }; struct InterpolatedPathInfo { /** the interpolated path */ - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ @@ -49,8 +49,8 @@ struct InterpolatedPathInfo }; std::optional generateInterpolatedPathInfo( - const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, - rclcpp::Logger logger); + const lanelet::Id lane_id, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger); std::optional getFirstPointIntersectsLineByFootprint( const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, @@ -88,7 +88,7 @@ lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const double opposite_adjacent_extend_width); std::vector find_lane_ids_upto( - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id); lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, @@ -104,7 +104,7 @@ lanelet::ConstLanelets generateBlindSpotLanelets( * @return Blind spot polygons */ std::optional generateBlindSpotPolygons( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const double backward_detection_length); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index a978c074e0018..40be0866abc66 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -24,7 +24,7 @@ namespace autoware::behavior_velocity_planner */ template void BlindSpotModule::setRTCStatusByDecision( - const T &, [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + const T &, [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { static_assert("Unsupported type passed to setRTCStatus"); return; @@ -33,7 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision( template void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const T & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) + [[maybe_unused]] autoware_internal_planning_msgs::msg::PathWithLaneId * path) { static_assert("Unsupported type passed to reactRTCApprovalByDecision"); } @@ -44,7 +44,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -52,7 +52,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) + [[maybe_unused]] autoware_internal_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -63,7 +63,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -71,7 +71,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) + [[maybe_unused]] autoware_internal_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -81,7 +81,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Unsafe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) + const Unsafe & decision, const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { setSafe(false); const auto & current_pose = planner_data_->current_odometry->pose; @@ -92,7 +92,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) + const Unsafe & decision, autoware_internal_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; @@ -115,7 +115,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Safe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) + const Safe & decision, const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { setSafe(true); const auto & current_pose = planner_data_->current_odometry->pose; @@ -126,7 +126,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) + const Safe & decision, autoware_internal_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index ee6516f025709..76fb7dabc68a7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -37,7 +37,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) planner_param_ = PlannerParam::init(node, ns); } -void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void BlindSpotModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -69,7 +70,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 12943912f04cd..2bdbb249c20d9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -154,7 +154,8 @@ template VisitorSwitch(Ts...) -> VisitorSwitch; void BlindSpotModule::setRTCStatus( - const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) + const BlindSpotDecision & decision, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { std::visit( VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, @@ -183,7 +184,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) } static std::optional getDuplicatedPointIdx( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -198,7 +200,8 @@ static std::optional getDuplicatedPointIdx( } static std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, + autoware_internal_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -211,7 +214,8 @@ static std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) size_t insert_idx = closest_idx; - tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); + autoware_internal_planning_msgs::msg::PathPointWithLaneId inserted_point = + inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -230,7 +234,7 @@ static std::optional insertPointIndex( std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * path) const + autoware_internal_planning_msgs::msg::PathWithLaneId * path) const { // NOTE: this is optionally int for later subtraction const int margin_idx_dist = @@ -317,7 +321,7 @@ autoware_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWi } std::optional BlindSpotModule::isOverPassJudge( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const geometry_msgs::msg::Pose & stop_point_pose) const { const auto & current_pose = planner_data_->current_odometry->pose; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 8451661b2b71f..73255d0df69fb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -34,7 +34,7 @@ namespace autoware::behavior_velocity_planner namespace { static bool hasLaneIds( - const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) + const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) { for (const auto & pid : p.lane_ids) { if (pid == id) { @@ -45,7 +45,7 @@ static bool hasLaneIds( } static std::optional> findLaneIdInterval( - const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) + const autoware_internal_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) { bool found = false; size_t start = 0; @@ -73,8 +73,8 @@ static std::optional> findLaneIdInterval( } // namespace std::optional generateInterpolatedPathInfo( - const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, - rclcpp::Logger logger) + const lanelet::Id lane_id, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger) { constexpr double ds = 0.2; InterpolatedPathInfo interpolated_path_info; @@ -222,7 +222,7 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) } std::vector find_lane_ids_upto( - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id) { std::vector lane_ids; /* get lane ids until intersection */ @@ -330,7 +330,7 @@ lanelet::ConstLanelets generateBlindSpotLanelets( } std::optional generateBlindSpotPolygons( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const double backward_detection_length) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp index e0ceca4e12951..ab8876a012654 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp index 5c2a239e4142f..ea70a9b1e3d5c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp @@ -66,7 +66,7 @@ class TestWithAdjLaneData : public ::testing::Test dynamic_object = autoware::test_utils::create_const_shared_ptr( parse(config["dynamic_object"])); path_with_lane_id = - parse(config["path_with_lane_id"]); + parse(config["path_with_lane_id"]); // parameter auto node_options = rclcpp::NodeOptions{}; @@ -85,7 +85,7 @@ class TestWithAdjLaneData : public ::testing::Test std::shared_ptr self_odometry{}; std::shared_ptr dynamic_object{}; const lanelet::Id lane_id_{2200}; - tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware_internal_planning_msgs::msg::PathWithLaneId path_with_lane_id; autoware::behavior_velocity_planner::PlannerParam param; }; @@ -318,7 +318,7 @@ TEST_F(TestWithAdjLaneData, generateInterpolatedPathInfo) const auto & interpolated_path_info = interpolated_path_info_opt.value(); EXPECT_EQ(interpolated_path_info.lane_id_interval.has_value(), true); const auto [start, end] = interpolated_path_info.lane_id_interval.value(); - tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + autoware_internal_planning_msgs::msg::PathWithLaneId interpolated_path; for (auto i = start; i <= end; ++i) { interpolated_path.points.push_back(interpolated_path_info.path.points.at(i)); } @@ -377,7 +377,7 @@ class TestWithShoulderData : public ::testing::Test dynamic_object = autoware::test_utils::create_const_shared_ptr( parse(config["dynamic_object"])); path_with_lane_id = - parse(config["path_with_lane_id"]); + parse(config["path_with_lane_id"]); // parameter auto node_options = rclcpp::NodeOptions{}; @@ -396,7 +396,7 @@ class TestWithShoulderData : public ::testing::Test std::shared_ptr self_odometry{}; std::shared_ptr dynamic_object{}; const lanelet::Id lane_id_{1010}; - tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware_internal_planning_msgs::msg::PathWithLaneId path_with_lane_id; autoware::behavior_velocity_planner::PlannerParam param; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp index 137f21adadafa..70d49a46963cd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp @@ -34,14 +34,14 @@ #include #include -#include +#include #include namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; @@ -91,12 +91,14 @@ struct DebugData std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); std::optional> diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp index 091a427e14949..dc0241910306c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 8601119df8344..167917263318d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -52,12 +52,12 @@ namespace autoware::behavior_velocity_planner namespace bg = boost::geometry; using autoware::universe_utils::Polygon2d; using autoware::universe_utils::StopWatch; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; -using tier4_planning_msgs::msg::PathWithLaneId; namespace { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index 50de1b1a8e1ff..6951ead97ccea 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -56,7 +56,8 @@ using autoware::universe_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::vector> crosswalks; @@ -93,7 +94,8 @@ std::vector> getCrosswalksOnPath( std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::set crosswalk_id_set; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp index 7a875327d4dde..6246adb0facea 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index f231d7adc3326..2bdd1decac60f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -52,7 +52,7 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) } void DetectionAreaModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { for (const auto & detection_area_with_lane_id : planning_utils::getRegElemMapOnPath( @@ -72,7 +72,7 @@ void DetectionAreaModuleManager::launchNewModules( std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 4b34ac4a45298..0f26c8bb4bd61 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterface<> private: DetectionAreaModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 42cc0461cd59d..16df809e7ff62 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -36,7 +36,7 @@ namespace autoware::behavior_velocity_planner using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; class DetectionAreaModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp index f84d22debea8e..a359a3a64e416 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp index d8bada002e37b..44797629f220f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -15,7 +15,7 @@ #ifndef INTERPOLATED_PATH_INFO_HPP_ #define INTERPOLATED_PATH_INFO_HPP_ -#include +#include #include @@ -32,7 +32,7 @@ namespace autoware::behavior_velocity_planner struct InterpolatedPathInfo { /** the interpolated path */ - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index d6f7dfbac92d4..eeedb921b293d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -302,7 +302,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) } void IntersectionModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -356,7 +356,7 @@ void IntersectionModuleManager::launchNewModules( std::function &)> IntersectionModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -447,7 +447,7 @@ void IntersectionModuleManager::setActivation() } void IntersectionModuleManager::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -484,7 +484,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node } void MergeFromPrivateModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -553,7 +553,7 @@ void MergeFromPrivateModuleManager::launchNewModules( std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index c6f76d7c39640..e53194b688d87 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -24,8 +24,8 @@ #include #include +#include #include -#include #include #include @@ -46,10 +46,11 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -57,7 +58,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void sendRTC(const Time & stamp) override; void setActivation() override; /* called from SceneModuleInterface::updateSceneModuleInstances */ - void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr @@ -74,10 +76,10 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface<> private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index b4e1f313a6936..1ce7da981f516 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -485,7 +485,7 @@ VisitorSwitch(Ts...) -> VisitorSwitch; template void prepareRTCByDecisionResult( [[maybe_unused]] const T & result, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -496,7 +496,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const InternalError & result, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -506,7 +506,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const OverPassJudge & result, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -515,7 +515,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const StuckStop & result, const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -536,7 +536,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const YieldStuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const YieldStuckStop & result, const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -552,9 +552,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const NonOccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const NonOccludedCollisionStop & result, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -571,9 +571,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const FirstWaitBeforeOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const FirstWaitBeforeOcclusion & result, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); const auto closest_idx = result.closest_idx; @@ -590,9 +590,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const PeekingTowardOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const PeekingTowardOcclusion & result, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); const auto closest_idx = result.closest_idx; @@ -609,9 +609,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const OccludedAbsenceTrafficLight & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const OccludedAbsenceTrafficLight & result, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); const auto closest_idx = result.closest_idx; @@ -626,9 +626,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const OccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const OccludedCollisionStop & result, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -645,8 +645,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const Safe & result, const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; @@ -663,9 +664,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const FullyPrioritized & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const FullyPrioritized & result, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FullyPrioritized"); const auto closest_idx = result.closest_idx; @@ -680,7 +681,8 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const DecisionResult & decision_result, const tier4_planning_msgs::msg::PathWithLaneId & path) + const DecisionResult & decision_result, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -703,7 +705,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_occlusion_approved, [[maybe_unused]] const T & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_internal_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -718,7 +720,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_internal_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -732,7 +734,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_internal_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -744,7 +746,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -791,7 +793,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -823,7 +825,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -867,7 +869,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, + autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -919,7 +921,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, + autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -977,7 +979,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -1025,7 +1027,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -1079,7 +1081,7 @@ template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -1122,7 +1124,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_internal_planning_msgs::msg::PathWithLaneId * path, planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { @@ -1163,7 +1165,8 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path) + const DecisionResult & decision_result, + autoware_internal_planning_msgs::msg::PathWithLaneId * path) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1284,7 +1287,7 @@ void IntersectionModule::updateTrafficSignalObservation() } IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index afba73ad45922..19eb60c1e7361 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include @@ -514,13 +514,14 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); + const DecisionResult &, const autoware_internal_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path); + const DecisionResult & decision_result, + autoware_internal_planning_msgs::msg::PathWithLaneId * path); /** @}*/ private: @@ -567,7 +568,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * original_path) const; + autoware_internal_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets @@ -638,7 +639,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ std::optional isStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( @@ -667,7 +668,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ std::optional isYieldStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const InterpolatedPathInfo & interpolated_path_info, const IntersectionStopLines & intersection_stoplines) const; @@ -723,8 +724,8 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * intersection_stoplines.occlusion_stopline */ PassJudgeStatus isOverPassJudgeLinesStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, - const IntersectionStopLines & intersection_stoplines); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const bool is_occlusion_state, const IntersectionStopLines & intersection_stoplines); /** @} */ private: @@ -785,7 +786,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * situation */ std::string generateEgoRiskEvasiveDiagnosis( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, const std::vector>> & too_late_detect_objects, @@ -811,7 +812,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; /** @} */ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 5103cd6cc46e4..f727fe3d66951 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -603,7 +603,7 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( } std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, const std::vector< std::pair>> & @@ -813,7 +813,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 70e1d219c0d31..15960d2d08e94 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -72,7 +72,7 @@ lanelet::ConstLanelets getPrevLanelets( // end inclusive lanelet::ConstLanelet generatePathLanelet( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, const double interval) { lanelet::Points3d lefts; @@ -103,7 +103,7 @@ lanelet::ConstLanelet generatePathLanelet( } std::optional> getFirstPointInsidePolygons( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const std::vector & polygons, const bool search_forward = true) { @@ -355,7 +355,7 @@ std::optional IntersectionModule::generateIntersectionSto const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * original_path) const + autoware_internal_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; const double stopline_margin = planner_param_.common.default_stopline_margin; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 35f4e3b34dbee..2a605041c4fe4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -123,7 +123,7 @@ namespace autoware::behavior_velocity_planner namespace bg = boost::geometry; std::optional IntersectionModule::isStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const { const auto closest_idx = intersection_stoplines.closest_idx; @@ -308,7 +308,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & pa } std::optional IntersectionModule::isYieldStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const InterpolatedPathInfo & interpolated_path_info, const IntersectionStopLines & intersection_stoplines) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 4b9c0a9c5547e..1630acb501de4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -19,10 +19,10 @@ #include #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index 5c9b74a9ad3bd..22b4965235b95 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -47,7 +47,8 @@ namespace autoware::behavior_velocity_planner::util namespace bg = boost::geometry; static std::optional getDuplicatedPointIdx( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -62,7 +63,8 @@ static std::optional getDuplicatedPointIdx( } std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, + autoware_internal_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -75,7 +77,8 @@ std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) int insert_idx = closest_idx; - tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); + autoware_internal_planning_msgs::msg::PathPointWithLaneId inserted_point = + inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -93,7 +96,8 @@ std::optional insertPointIndex( } bool hasLaneIds( - const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) + const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p, + const std::set & ids) { for (const auto & pid : p.lane_ids) { if (ids.find(pid) != ids.end()) { @@ -104,7 +108,7 @@ bool hasLaneIds( } std::optional> findLaneIdsInterval( - const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) + const autoware_internal_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) { bool found = false; size_t start = 0; @@ -180,7 +184,7 @@ getFirstPointInsidePolygonsByFootprint( } std::optional getFirstPointInsidePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward) { @@ -328,7 +332,7 @@ mergeLaneletsByTopologicalSort( } bool isOverTargetIndex( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -357,7 +361,7 @@ std::optional getIntersectionArea( std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { InterpolatedPathInfo interpolated_path_info; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp index 0fac44f6cdf97..e8f48a901a641 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -38,14 +38,16 @@ namespace autoware::behavior_velocity_planner::util * @return if insertion was successful return the inserted point index */ std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, + autoware_internal_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( - const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); + const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p, + const std::set & ids); /** * @brief find the first contiguous interval of the path points that contains the specified lane ids @@ -53,7 +55,8 @@ bool hasLaneIds( * found, returns the pair (start-1, end) */ std::optional> findLaneIdsInterval( - const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); + const autoware_internal_planning_msgs::msg::PathWithLaneId & p, + const std::set & ids); /** * @brief return the index of the first point which is inside the given polygon @@ -61,7 +64,7 @@ std::optional> findLaneIdsInterval( * @param[in] search_forward flag for search direction */ std::optional getFirstPointInsidePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); @@ -73,7 +76,7 @@ std::optional getFirstPointInsidePolygon( * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); std::optional getIntersectionArea( @@ -84,7 +87,7 @@ std::optional getIntersectionArea( */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp index b515107e0ae8e..5ab57735af505 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 517568c93bd35..48a7b435b0e36 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -33,7 +33,7 @@ NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) } void NoDrivableLaneModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -58,7 +58,7 @@ void NoDrivableLaneModuleManager::launchNewModules( std::function &)> NoDrivableLaneModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp index bea068f5b9579..f32ff562de553 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoDrivableLaneModuleManager : public SceneModuleManagerInterface<> private: NoDrivableLaneModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; class NoDrivableLaneModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index 5490a37c7ec79..17df17f5ed84e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; class NoDrivableLaneModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp index 289b04a2ce96d..9abe09b60aef5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -15,7 +15,7 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include +#include #include #include @@ -33,8 +33,8 @@ namespace autoware::behavior_velocity_planner namespace bg = boost::geometry; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and no drivable lane polygon struct PathWithNoDrivableLanePolygonIntersection diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 55b6c0dffd1a4..0598bbb4f0d26 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -47,7 +47,7 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) } void NoStoppingAreaModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -71,7 +71,7 @@ void NoStoppingAreaModuleManager::launchNewModules( std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 523cbba291632..95ee6317817f5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,11 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: NoStoppingAreaModule::PlannerParam planner_param_{}; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 036b6e30509c4..58ea2a320f5bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -23,9 +23,9 @@ #include #include +#include #include #include -#include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp index 5ce56d756e919..ca9627bc1a057 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp @@ -48,13 +48,13 @@ bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & obje } void insert_stop_point( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) + autoware_internal_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) { auto insert_idx = stop_point.first + 1UL; const auto & stop_pose = stop_point.second; // To PathPointWithLaneId - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + autoware_internal_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(insert_idx); stop_point_with_lane_id.point.pose = stop_pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; @@ -64,7 +64,7 @@ void insert_stop_point( } std::optional generate_stop_line( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, const double stop_line_margin) { @@ -127,7 +127,8 @@ bool is_stoppable( } Polygon2d generate_ego_no_stopping_area_lane_polygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & ego_pose, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, rclcpp::Clock & clock) @@ -136,7 +137,7 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( double dist_from_start_sum = 0.0; constexpr double interpolation_interval = 0.5; bool is_in_area = false; - tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + autoware_internal_planning_msgs::msg::PathWithLaneId interpolated_path; if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger)) { return ego_area; } @@ -204,7 +205,7 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( } bool check_stop_lines_in_no_stopping_area( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, no_stopping_area::DebugData & debug_data) { const double stop_vel = std::numeric_limits::min(); @@ -245,7 +246,7 @@ bool check_stop_lines_in_no_stopping_area( } std::optional get_stop_line_geometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp index 73b835b4701dc..d545e1618fe71 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp @@ -21,8 +21,8 @@ #include #include +#include #include -#include #include @@ -85,7 +85,8 @@ bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & obje * @param stop_point stop line point on the lane */ void insert_stop_point( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const PathIndexWithPose & stop_point); /** * @brief generate stop line from no stopping area polygons @@ -99,7 +100,7 @@ void insert_stop_point( * @param stop_line_margin [m] margin to keep between the stop line and the no stopping areas **/ std::optional generate_stop_line( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, const double stop_line_margin); @@ -128,7 +129,8 @@ bool is_stoppable( * @return generated polygon */ Polygon2d generate_ego_no_stopping_area_lane_polygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & ego_pose, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, rclcpp::Clock & clock); @@ -141,7 +143,7 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( * @return true if exists */ bool check_stop_lines_in_no_stopping_area( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, DebugData & debug_data); /** @@ -156,7 +158,7 @@ bool check_stop_lines_in_no_stopping_area( * @return generated stop line */ std::optional get_stop_line_geometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp index 875e757cbfcec..714dbc102cc05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp index cc5bbd2903a52..d1747aafb4c78 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp @@ -20,10 +20,10 @@ #include #include +#include +#include #include #include -#include -#include #include #include @@ -44,12 +44,12 @@ bool point_in_polygon(const Point & p, const Polygon & poly) }) != poly.outer().end(); } -tier4_planning_msgs::msg::PathWithLaneId generate_straight_path( +autoware_internal_planning_msgs::msg::PathWithLaneId generate_straight_path( const size_t nb_points, const float velocity = 0.0, const double resolution = 1.0) { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; for (auto x = 0UL; x < nb_points; ++x) { - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = resolution * static_cast(x); p.point.pose.position.y = 0.0; p.point.longitudinal_velocity_mps = velocity; @@ -82,7 +82,7 @@ TEST(NoStoppingAreaTest, insertStopPoint) using autoware::behavior_velocity_planner::no_stopping_area::insert_stop_point; constexpr auto nb_points = 10; constexpr auto default_velocity = 5.0; - const tier4_planning_msgs::msg::PathWithLaneId base_path = + const autoware_internal_planning_msgs::msg::PathWithLaneId base_path = generate_straight_path(nb_points, default_velocity); autoware::behavior_velocity_planner::no_stopping_area::PathIndexWithPose stop_point; // stop exactly at a point, expect no new points but a 0 velocity at the stop point and after @@ -123,7 +123,8 @@ TEST(NoStoppingAreaTest, generateStopLine) { using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; constexpr auto nb_points = 10; - const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(nb_points); + const autoware_internal_planning_msgs::msg::PathWithLaneId path = + generate_straight_path(nb_points); lanelet::ConstPolygons3d no_stopping_areas; double ego_width = 0.0; double stop_line_margin = 0.0; @@ -228,7 +229,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) geometry_msgs::msg::Pose ego_pose; // ego at (0,0) ego_pose.position.x = 0.0; ego_pose.position.y = 0.0; - const tier4_planning_msgs::msg::PathWithLaneId path = + const autoware_internal_planning_msgs::msg::PathWithLaneId path = generate_straight_path(8); // ego path at x = 0, 1,..., 7 and y=0 double margin = 1.0; // desired margin added to the polygon after the end of the area double max_polygon_length = 10.0; // maximum length of the generated polygon @@ -307,7 +308,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) // cases where the polygon returned is empty // path is empty { - tier4_planning_msgs::msg::PathWithLaneId empty_path; + autoware_internal_planning_msgs::msg::PathWithLaneId empty_path; const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( empty_path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, logger, clock); @@ -356,7 +357,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) { using autoware::behavior_velocity_planner::no_stopping_area::check_stop_lines_in_no_stopping_area; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; autoware::universe_utils::Polygon2d poly; autoware::behavior_velocity_planner::no_stopping_area::DebugData debug_data; @@ -365,7 +366,7 @@ TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) constexpr auto nb_points = 10; constexpr auto non_stopped_velocity = 5.0; - const tier4_planning_msgs::msg::PathWithLaneId non_stopping_path = + const autoware_internal_planning_msgs::msg::PathWithLaneId non_stopping_path = generate_straight_path(nb_points, non_stopped_velocity); path = non_stopping_path; // set x=4 and x=5 to be stopping points @@ -396,7 +397,7 @@ TEST(NoStoppingAreaTest, getStopLineGeometry2d) { using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; using autoware::behavior_velocity_planner::no_stopping_area::get_stop_line_geometry2d; - const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(10); + const autoware_internal_planning_msgs::msg::PathWithLaneId path = generate_straight_path(10); lanelet::Polygon3d no_stopping_area; no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index d28cff55a7a8d..88d4a4397c73e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -117,7 +117,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } void OcclusionSpotModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -130,7 +130,7 @@ void OcclusionSpotModuleManager::launchNewModules( std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 08c1516dea67a..2bc6022d7ac4b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -23,11 +23,11 @@ #include #include +#include #include #include #include #include -#include #include #include @@ -53,10 +53,10 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface<> PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; class OcclusionSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index baf15cbebce81..f8174eb327acd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -95,8 +95,8 @@ bool buildDetectionAreaPolygon( } void calcSlowDownPointsForPossibleCollision( - const int closest_idx, const tier4_planning_msgs::msg::PathWithLaneId & path, const double offset, - std::vector & possible_collisions) + const int closest_idx, const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const double offset, std::vector & possible_collisions) { if (possible_collisions.empty()) { return; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 7bff65d59157d..354b52624127e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -23,11 +23,11 @@ #include #include +#include #include #include #include #include -#include #include #include @@ -48,6 +48,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -59,8 +61,6 @@ using lanelet::BasicLineString2d; using lanelet::BasicPoint2d; using lanelet::BasicPolygon2d; using lanelet::LaneletMapPtr; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 9def3b0938998..7fb146988fdfd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -17,7 +17,7 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index c744cdcb22b84..8642137b83831 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -22,11 +22,11 @@ #include #include +#include #include #include #include #include -#include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 8fee8ee783fa2..94e41a421fb53 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -29,9 +29,9 @@ using Point = geometry_msgs::msg::Point; using Vector3 = geometry_msgs::msg::Vector3; using DynamicObjects = autoware_perception_msgs::msg::PredictedObjects; using DynamicObject = autoware_perception_msgs::msg::PredictedObject; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; -using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { @@ -45,7 +45,8 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) std::vector possible_collisions; size_t num = 2000; // make a path with 2000 points from x=0 to x=4 - tier4_planning_msgs::msg::PathWithLaneId path = test::generatePath(0.0, 3.0, 4.0, 3.0, num); + autoware_internal_planning_msgs::msg::PathWithLaneId path = + test::generatePath(0.0, 3.0, 4.0, 3.0, num); // make 2000 possible collision from x=0 to x=10 test::generatePossibleCollisions(possible_collisions, 0.0, 3.0, 4.0, 3.0, num); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp index 39a887ab16476..8afc21842bc3a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -19,22 +19,22 @@ #include -#include -#include +#include +#include #include namespace test { -inline tier4_planning_msgs::msg::PathWithLaneId generatePath( +inline autoware_internal_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - tier4_planning_msgs::msg::PathWithLaneId path{}; + autoware_internal_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point{}; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md index 041e0b86e0d06..26d0e26f79a0f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md @@ -30,7 +30,7 @@ So for example, in order to stop at a stop line with the vehicles' front on the | Name | Type | Description | | ----------------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | tier4_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/path_with_lane_id` | autoware_internal_planning_msgs::msg::PathWithLaneId | path with lane_id | | `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | | `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | | `~input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 472538406c382..2f267131ef1be 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -24,13 +24,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #include @@ -58,7 +58,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // subscriber - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr trigger_sub_path_with_lane_id_; // polling subscribers @@ -92,7 +92,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node sub_external_velocity_limit_{ this, "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local()}; - void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); + void onTrigger( + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); void onParam(); @@ -133,7 +134,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function bool isDataReady(rclcpp::Clock clock); autoware_planning_msgs::msg::Path generatePath( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); std::unique_ptr logger_configure_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp index 9bd423ecfef21..42d014f9af9bc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp @@ -20,12 +20,12 @@ #include #include +#include #include #include #include #include #include -#include #include #include @@ -47,9 +47,9 @@ class BehaviorVelocityPlannerManager void removeScenePlugin(rclcpp::Node & node, const std::string & name); // cppcheck-suppress functionConst - tier4_planning_msgs::msg::PathWithLaneId planPathVelocity( + autoware_internal_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg); + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path_msg); private: pluginlib::ClassLoader plugin_loader_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 443bfebe2a3eb..d48151acf3b93 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -46,7 +46,7 @@ namespace { autoware_planning_msgs::msg::Path to_path( - const tier4_planning_msgs::msg::PathWithLaneId & path_with_id) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path_with_id) { autoware_planning_msgs::msg::Path path; for (const auto & path_point : path_with_id.points) { @@ -67,7 +67,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Trigger Subscriber trigger_sub_path_with_lane_id_ = - this->create_subscription( + this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); srv_load_plugin_ = create_service( @@ -297,7 +297,7 @@ bool BehaviorVelocityPlannerNode::isDataReady(rclcpp::Clock clock) } void BehaviorVelocityPlannerNode::onTrigger( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { std::unique_lock lk(mutex_); @@ -331,7 +331,7 @@ void BehaviorVelocityPlannerNode::onTrigger( } autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data) { autoware_planning_msgs::msg::Path output_path_msg; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 45ee83260d53a..4f0c673440088 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -73,11 +73,12 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( } } -tier4_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( +autoware_internal_planning_msgs::msg::PathWithLaneId +BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path_msg) { - tier4_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; + autoware_internal_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp index 8b9f39e97d22b..45ce88c8e92d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -29,14 +29,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: behavior_velocity_planner → test_node_ - test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); - - // set behavior_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setPathWithLaneIdTopicName( - "behavior_velocity_planner_node/input/path_with_lane_id"); - - test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->subscribeOutput( + "behavior_velocity_planner_node/output/path"); return test_manager; } @@ -99,21 +93,35 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); - test_manager->publishMaxVelocity( - test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); + test_manager->publishInput( + test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map")); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud", + sensor_msgs::msg::PointCloud2{}.set__header( + std_msgs::msg::Header{}.set__frame_id("base_link"))); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry", + autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/vector_map", + autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals", + autoware_perception_msgs::msg::TrafficLightGroupArray{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps", + tier4_planning_msgs::msg::VelocityLimit{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid", + autoware::test_utils::makeCostMapMsg()); } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp index 6e232318c1711..d62710f4daa24 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -48,13 +53,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp index 4867b14cbb806..ce6d828779060 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -30,10 +30,10 @@ class PluginInterface public: virtual ~PluginInterface() = default; virtual void init(rclcpp::Node & node) = 0; - virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) = 0; virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp index 44a00072a0be4..a4f2bc046783f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -28,13 +28,13 @@ class PluginWrapper : public PluginInterface { public: void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } - void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override + void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override { scene_manager_->plan(path); }; void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) override + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override { scene_manager_->updateSceneModuleInstances(planner_data, path); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 743fd0e31e387..37c1d07f62a40 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -28,8 +28,8 @@ #include #include +#include #include -#include #include #include @@ -54,8 +54,8 @@ using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; using autoware::universe_utils::StopWatch; using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; -using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest @@ -113,7 +113,7 @@ class SceneModuleInterface } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; }; template @@ -131,7 +131,7 @@ class SceneModuleManagerInterface is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); } if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( + pub_debug_path_ = node.create_publisher( std::string("~/debug/path_with_lane_id/") + module_name, 1); } pub_virtual_wall_ = node.create_publisher( @@ -153,7 +153,7 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { planner_data_ = planner_data; @@ -161,10 +161,13 @@ class SceneModuleManagerInterface deleteExpiredModules(path); } - virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } + virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) + { + modifyPathVelocity(path); + } protected: - virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) + virtual void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) { universe_utils::ScopedTimeTrack st( "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); @@ -188,7 +191,7 @@ class SceneModuleManagerInterface planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { - tier4_planning_msgs::msg::PathWithLaneId debug_path; + autoware_internal_planning_msgs::msg::PathWithLaneId debug_path; debug_path.header = path->header; debug_path.points = path->points; pub_debug_path_->publish(debug_path); @@ -198,12 +201,14 @@ class SceneModuleManagerInterface std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); } - virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual void launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) + virtual void deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -233,7 +238,7 @@ class SceneModuleManagerInterface } size_t findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -254,7 +259,8 @@ class SceneModuleManagerInterface rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; - rclcpp::Publisher::SharedPtr pub_debug_path_; + rclcpp::Publisher::SharedPtr + pub_debug_path_; std::shared_ptr processing_time_publisher_; @@ -267,14 +273,14 @@ class SceneModuleManagerInterface extern template SceneModuleManagerInterface::SceneModuleManagerInterface( rclcpp::Node & node, [[maybe_unused]] const char * module_name); extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; extern template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); extern template void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path); + autoware_internal_planning_msgs::msg::PathWithLaneId * path); extern template void SceneModuleManagerInterface::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); extern template void SceneModuleManagerInterface::registerModule( const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index fae13db2822e7..056952bd4fb9d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -156,7 +156,8 @@ std::optional findOffsetSegment( } std::optional findOffsetSegment( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset); template geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) @@ -191,7 +192,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse } std::optional createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const double margin, const double vehicle_offset); } // namespace arc_lane_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 696d4a41b7673..7598550220e05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -17,12 +17,12 @@ #include +#include #include #include #include #include #include -#include #include #include @@ -40,8 +40,8 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - tier4_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, point.pose.position.x, - point.pose.position.y, point.pose.position.z) + autoware_internal_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, + point.pose.position.x, point.pose.position.y, point.pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp index 5ac91cdaf1369..ba103e01f89ad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp @@ -17,10 +17,10 @@ #include +#include #include #include #include -#include #include #include @@ -33,7 +33,7 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPathMarkerArray( - const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createObjectsMarkerArray( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp index 2d4dab018fb18..770feb3e0ae9c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -17,14 +17,14 @@ #include +#include #include -#include namespace autoware::behavior_velocity_planner { bool splineInterpolate( - const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger); + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_internal_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger); autoware_planning_msgs::msg::Path interpolatePath( const autoware_planning_msgs::msg::Path & path, const double length, const double interval); autoware_planning_msgs::msg::Path filterLitterPathPoint( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 4dc355f0e6b1d..c82d3d5ed0a5e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -17,10 +17,10 @@ #include +#include #include #include #include -#include #include #include @@ -28,9 +28,9 @@ namespace autoware::behavior_velocity_planner { +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index fc4f852ef82f7..76743b819c041 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -17,10 +17,10 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include #include #include #include -#include #include #include @@ -67,14 +67,14 @@ using LineString2d = autoware::universe_utils::LineString2d; using Polygon2d = autoware::universe_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; namespace planning_utils { size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx); bool createDetectionAreaPolygons( @@ -105,7 +105,7 @@ inline int64_t bitShift(int64_t original_id) bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); @@ -218,8 +218,9 @@ std::set getLaneIdSetOnPath( const geometry_msgs::msg::Pose & current_pose); bool isOverLine( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, - const geometry_msgs::msg::Pose & line_pose, const double offset = 0.0); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double offset = 0.0); std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index 1849c05085ac4..b4e0457343df8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -38,7 +38,7 @@ SceneModuleInterface::SceneModuleInterface( } size_t SceneModuleInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -48,14 +48,14 @@ size_t SceneModuleInterface::findEgoSegmentIndex( template SceneModuleManagerInterface::SceneModuleManagerInterface( rclcpp::Node & node, [[maybe_unused]] const char * module_name); template size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); template void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path); + autoware_internal_planning_msgs::msg::PathWithLaneId * path); template void SceneModuleManagerInterface::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); template void SceneModuleManagerInterface::registerModule( const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 9b02b374b0a6c..525b4947d152b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -89,7 +89,8 @@ std::optional checkCollision( } std::optional findOffsetSegment( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset) { if (offset >= 0) { return findForwardOffsetSegment(path, index, offset); @@ -99,7 +100,7 @@ std::optional findOffsetSegment( } std::optional createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const double margin, const double vehicle_offset) { // Find collision segment diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index 2ddf62bb584ff..120faf15f6ad5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -54,7 +54,7 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( } visualization_msgs::msg::MarkerArray createPathMarkerArray( - const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index b8e6e28bae7c2..27f2e8f5554fb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -23,8 +23,8 @@ namespace autoware::behavior_velocity_planner { bool splineInterpolate( - const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_internal_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger) { if (input.points.size() < 2) { RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index cb9b58ec48924..c0d6eadaac66f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -20,8 +20,8 @@ #include #include +#include #include -#include #include @@ -39,10 +39,10 @@ namespace autoware::behavior_velocity_planner { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; @@ -58,9 +58,8 @@ bool smoothPath( const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; - auto trajectory = - autoware::motion_utils::convertToTrajectoryPoints( - in_path); + auto trajectory = autoware::motion_utils::convertToTrajectoryPoints< + autoware_internal_planning_msgs::msg::PathWithLaneId>(in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index a8f909201ac7f..1ae04b6cf9ef1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -42,7 +42,7 @@ namespace { size_t calcPointIndexFromSegmentIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) { const size_t prev_point_idx = seg_idx; @@ -102,7 +102,7 @@ using autoware::universe_utils::calcOffsetPose; using autoware_planning_msgs::msg::PathPoint; size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) { if (idx == 0) { @@ -300,7 +300,7 @@ bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return geometry_msgs::msg::Pose{}; @@ -593,8 +593,9 @@ std::vector getSubsequentLaneIdsSetOnPath( // TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, - const geometry_msgs::msg::Pose & line_pose, const double offset) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double offset) { return autoware::motion_utils::calcSignedArcLength( path.points, self_pose.position, line_pose.position) + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp index 5ed490ab755a9..283e54c01403b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp @@ -28,8 +28,8 @@ TEST(smoothPath, nominal) { using autoware::behavior_velocity_planner::smoothPath; - using tier4_planning_msgs::msg::PathPointWithLaneId; - using tier4_planning_msgs::msg::PathWithLaneId; + using autoware_internal_planning_msgs::msg::PathPointWithLaneId; + using autoware_internal_planning_msgs::msg::PathWithLaneId; rclcpp::init(0, nullptr); rclcpp::NodeOptions options; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp index 8c8ef575b5a9a..bc5abee0cae7f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp @@ -16,11 +16,11 @@ #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" +#include #include #include #include #include -#include #include @@ -63,7 +63,7 @@ TEST(PlanningUtilsTest, createDetectionAreaPolygons) // Input parameters Polygons2d da_polys; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; geometry_msgs::msg::Pose target_pose; size_t target_seg_idx = 0; autoware::behavior_velocity_planner::DetectionRange da_range; @@ -83,7 +83,7 @@ TEST(PlanningUtilsTest, createDetectionAreaPolygons) // Path with some points for (double i = 0.0; i < 3.0; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point; point.point.pose.position.x = i * 5.0; point.point.pose.position.y = 0.0; point.point.longitudinal_velocity_mps = 1.0; @@ -175,7 +175,7 @@ TEST(PlanningUtilsTest, insertDecelPoint) TEST(PlanningUtilsTest, insertVelocity) { auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); - tier4_planning_msgs::msg::PathPointWithLaneId path_point; + autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point; path_point.point.pose.position.x = 5.0; path_point.point.pose.position.y = 0.0; path_point.point.longitudinal_velocity_mps = 10.0; @@ -215,10 +215,10 @@ TEST(PlanningUtilsTest, insertStopPoint) // Test for getAheadPose TEST(PlanningUtilsTest, getAheadPose) { - tier4_planning_msgs::msg::PathWithLaneId path; - tier4_planning_msgs::msg::PathPointWithLaneId point1; - tier4_planning_msgs::msg::PathPointWithLaneId point2; - tier4_planning_msgs::msg::PathPointWithLaneId point3; + autoware_internal_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point1; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point2; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point3; point1.point.pose.position.x = 0.0; point2.point.pose.position.x = 5.0; point3.point.pose.position.x = 10.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp index 4dbf4c73fcd82..f61bec186e67f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp @@ -15,23 +15,23 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ +#include +#include #include -#include -#include #include namespace test { -inline tier4_planning_msgs::msg::PathWithLaneId generatePath( +inline autoware_internal_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - tier4_planning_msgs::msg::PathWithLaneId path{}; + autoware_internal_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point{}; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp index 0cf1e343fb646..84a9efdab8ba9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -21,8 +21,8 @@ #include #include +#include #include -#include #include #include @@ -42,8 +42,8 @@ namespace autoware::behavior_velocity_planner using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::getOrDeclareParameter; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; -using tier4_planning_msgs::msg::PathWithLaneId; using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; @@ -88,7 +88,7 @@ class SceneModuleManagerInterfaceWithRTC SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override; protected: RTCInterface rtc_interface_; @@ -123,7 +123,8 @@ class SceneModuleManagerInterfaceWithRTC void publishObjectsOfInterestMarker(); - void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) { @@ -143,13 +144,13 @@ class SceneModuleManagerInterfaceWithRTC extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; extern template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); extern template void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path); + autoware_internal_planning_msgs::msg::PathWithLaneId * path); extern template void SceneModuleManagerInterface::registerModule( const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp index 34c7a30e73acf..e26e7f22ef625 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -46,7 +46,8 @@ SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( { } -void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) +void SceneModuleManagerInterfaceWithRTC::plan( + autoware_internal_planning_msgs::msg::PathWithLaneId * path) { setActivation(); modifyPathVelocity(path); @@ -111,7 +112,7 @@ void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() } void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -132,12 +133,12 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( } template size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); template void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path); + autoware_internal_planning_msgs::msg::PathWithLaneId * path); template void SceneModuleManagerInterface::registerModule( const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 6b4050fe9bfbb..7d199e7c74409 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -45,6 +45,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; @@ -53,9 +55,7 @@ using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; using run_out_utils::PlannerParam; using run_out_utils::PredictedPath; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; -using PathPointsWithLaneId = std::vector; +using PathPointsWithLaneId = std::vector; /** * @brief base class for creating dynamic obstacles from multiple types of input diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4af7882461643..f9ddcf9b4c747 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -146,7 +146,8 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) setDynamicObstacleCreator(node, debug_ptr_); } -void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void RunOutModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return; @@ -163,7 +164,7 @@ void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathW std::function &)> RunOutModuleManager::getModuleExpiredFunction( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp index 068ed81015fb1..978ad56fa50eb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -37,10 +37,10 @@ class RunOutModuleManager : public SceneModuleManagerInterface<> std::shared_ptr debug_ptr_; std::unique_ptr dynamic_obstacle_creator_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp index e4e08d69a79f7..b1e68f68d2f87 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp @@ -24,7 +24,7 @@ namespace autoware::behavior_velocity_planner::run_out_utils * This function returns the point with the smallest (signed) longitudinal distance */ geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.hpp index 62591af27af11..3ede1b11a8c12 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,8 +15,8 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ +#include #include -#include #include #include @@ -31,7 +31,7 @@ namespace run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index a300e8f5f9284..08d78c3541c72 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -745,7 +745,7 @@ std::optional RunOutModule::calcStopPoint( bool RunOutModule::insertStopPoint( const std::optional stop_point, - tier4_planning_msgs::msg::PathWithLaneId & path) + autoware_internal_planning_msgs::msg::PathWithLaneId & path) { // no stop point if (!stop_point) { @@ -766,7 +766,7 @@ bool RunOutModule::insertStopPoint( } // to PathPointWithLaneId - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + autoware_internal_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(nearest_seg_idx); stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); @@ -894,7 +894,7 @@ void RunOutModule::insertApproachingVelocity( // to PathPointWithLaneId // use lane id of point behind inserted point - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + autoware_internal_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); stop_point_with_lane_id.point.pose = *stop_point; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 1879d724f98b3..18b47372db409 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -33,11 +33,11 @@ namespace autoware::behavior_velocity_planner { using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; class RunOutModule : public SceneModuleInterface @@ -127,7 +127,7 @@ class RunOutModule : public SceneModuleInterface bool insertStopPoint( const std::optional stop_point, - tier4_planning_msgs::msg::PathWithLaneId & path); + autoware_internal_planning_msgs::msg::PathWithLaneId & path); void insertVelocityForState( const std::optional & dynamic_obstacle, const PlannerData planner_data, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index c48e4f867fac0..755df2ed5ac82 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -40,12 +40,12 @@ using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; -using tier4_planning_msgs::msg::PathWithLaneId; -using PathPointsWithLaneId = std::vector; +using PathPointsWithLaneId = std::vector; struct CommonParam { double normal_min_jerk; // min jerk limit for mild stop [m/sss] diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp index e6a838c77fb37..f7051125eb480 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp @@ -28,10 +28,10 @@ #include #include +#include +#include #include #include -#include -#include #include #include @@ -51,9 +51,9 @@ using autoware_perception_msgs::msg::ObjectClassification; using geometry_msgs::msg::Point; using Polygons2d = std::vector; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; -using PathPointsWithLaneId = std::vector; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; using autoware::behavior_velocity_planner::applyVoxelGridFilter; using autoware::behavior_velocity_planner::createPredictedPath; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp index 75bf59751ed44..9e906c816b63c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp index eb8502f043b22..66f4a6c71f6fc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include @@ -31,9 +31,9 @@ #include using autoware::behavior_velocity_planner::run_out_utils::findLongitudinalNearestPoint; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; class TestPathUtils : public ::testing::Test { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp index d39436baa7dea..cbc94cb0e2486 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include @@ -37,9 +37,9 @@ using autoware::behavior_velocity_planner::DynamicObstacle; using autoware::behavior_velocity_planner::run_out_utils::StateMachine; using autoware::behavior_velocity_planner::run_out_utils::StateParam; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; class TestStateMachine : public ::testing::Test { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp index c523dd0a53a71..0eae81f186be1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp @@ -24,8 +24,8 @@ #include #include +#include #include -#include #include @@ -55,10 +55,10 @@ using autoware::behavior_velocity_planner::run_out_utils::PredictedPath; using autoware::behavior_velocity_planner::run_out_utils::toEnum; using autoware::behavior_velocity_planner::run_out_utils::trimPathFromSelfPose; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using geometry_msgs::msg::Point; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; class TestRunOutUtils : public ::testing::Test { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index a69e33e9e0391..7b54f34c35a07 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp @@ -53,7 +53,8 @@ SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) static_cast(getOrDeclareParameter(node, ns + ".max_speed")); } -void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void SpeedBumpModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { for (const auto & speed_bump_with_lane_id : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -70,7 +71,7 @@ void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa std::function &)> SpeedBumpModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto speed_bump_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp index f98db8b88b7a9..a3312c62d5b7e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface<> private: SpeedBumpModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; class SpeedBumpModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 5b908f90bf9c2..3ac4364dabb69 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; class SpeedBumpModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp index 72bad3db86f73..96a24322254ca 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp @@ -30,8 +30,8 @@ #include #include +#include #include -#include #include @@ -40,9 +40,9 @@ namespace autoware::behavior_velocity_planner namespace bg = boost::geometry; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point32; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and speed bump struct PathPolygonIntersectionStatus 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 e289779df34b6..da059840d36e7 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 @@ -40,7 +40,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) { std::vector stop_lines_with_lane_id; @@ -62,7 +63,8 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } std::set StopLineModuleManager::getStopLineIdSetOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) { std::set stop_line_id_set; @@ -73,7 +75,8 @@ std::set StopLineModuleManager::getStopLineIdSetOnPath( return stop_line_id_set; } -void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void StopLineModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { @@ -88,7 +91,7 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat std::function &)> StopLineModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_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/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index c746e2bf6a314..70ad0a6b8d0b9 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 @@ -21,7 +21,7 @@ #include -#include +#include #include #include @@ -44,17 +44,17 @@ class StopLineModuleManager : public SceneModuleManagerInterface<> StopLineModule::PlannerParam planner_param_; std::vector getStopLinesWithLaneIdOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); std::set getStopLineIdSetOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; class StopLineModulePlugin : public PluginWrapper 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 2115a0a7eca6e..5e132565a7a03 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 @@ -15,10 +15,12 @@ #include "scene.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" #include -#include +#include #include #include @@ -30,8 +32,8 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, - const std::shared_ptr + const std::shared_ptr & time_keeper, + const std::shared_ptr & planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), @@ -44,8 +46,8 @@ StopLineModule::StopLineModule( bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) { auto trajectory = - trajectory::Trajectory::Builder{}.build( - path->points); + trajectory::Trajectory::Builder{} + .build(path->points); if (!trajectory) { return true; @@ -58,7 +60,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) return true; } - trajectory->longitudinal_velocity_mps.range(*stop_point, trajectory->length()).set(0.0); + trajectory->longitudinal_velocity_mps().range(*stop_point, trajectory->length()).set(0.0); path->points = trajectory->restore(); @@ -83,7 +85,7 @@ std::pair> StopLineModule::getEgoAndStopPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, const State & state) const { - const double ego_s = trajectory.closest(ego_pose.position); + const double ego_s = autoware::trajectory::closest(trajectory, ego_pose); std::optional stop_point_s; switch (state) { @@ -94,16 +96,16 @@ std::pair> StopLineModule::getEgoAndStopPoint( // Calculate intersection with stop line const auto trajectory_stop_line_intersection = - trajectory.crossed(stop_line.front(), stop_line.back()); + autoware::trajectory::crossed(trajectory, stop_line); // If no collision found, do nothing - if (!trajectory_stop_line_intersection) { + if (trajectory_stop_line_intersection.size() == 0) { stop_point_s = std::nullopt; break; } stop_point_s = - *trajectory_stop_line_intersection - + trajectory_stop_line_intersection.at(0) - (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin if (*stop_point_s < 0.0) { 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 d2c999a377ab4..0948e0540cd81 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 @@ -40,7 +40,7 @@ class StopLineModule : public SceneModuleInterface public: using StopLineWithLaneId = std::pair; using Trajectory = - autoware::trajectory::Trajectory; + autoware::trajectory::Trajectory; enum class State { APPROACH, STOPPED, START }; struct DebugData @@ -64,13 +64,15 @@ class StopLineModule : public SceneModuleInterface * @param planner_param Planning parameters. * @param logger Logger for output messages. * @param clock Shared clock instance. + * @param time_keeper Time keeper for the module. + * @param planning_factor_interface Planning factor interface. */ StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, - const std::shared_ptr + const std::shared_ptr & time_keeper, + const std::shared_ptr & planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp index c6d6ff638cfbb..ac158a0e59084 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 2b176a64c597d..069122b6ed4c9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -28,9 +28,9 @@ using autoware::behavior_velocity_planner::StopLineModule; -tier4_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) +autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) { - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x; p.point.pose.position.y = y; return p; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md index 0507d69af1b4e..7b18771828a3e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md @@ -50,13 +50,13 @@ The managing of your modules is defined in manager.hpp and manager.cpp. The mana #### `launchNewModules()` Method -- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `autoware_internal_planning_msgs::msg::PathWithLaneId`. - It is responsible for launching new modules based on the provided path information (PathWithLaneId). The implementation of this method involves initializing and configuring modules specific to your behavior velocity planner by using the `TemplateModule` class. - In the provided source code, it initializes a `module_id` to 0 and checks if a module with the same ID is already registered. If not, it registers a new `TemplateModule` with the module ID. Note that each module managed by the `TemplateModuleManager` should have a unique ID. The template code registers a single module, so the `module_id` is set as 0 for simplicity. #### `getModuleExpiredFunction()` Method -- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `autoware_internal_planning_msgs::msg::PathWithLaneId`. - It returns a `std::function&)>`. This function is used by the behavior velocity planner to determine whether a particular module has expired or not based on the given path. - The implementation of this method is expected to return a function that can be used to check the expiration status of modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index a004f5b823138..541bfa823d93d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -38,7 +38,7 @@ TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) } void TemplateModuleManager::launchNewModules( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { @@ -50,7 +50,7 @@ void TemplateModuleManager::launchNewModules( std::function &)> TemplateModuleManager::getModuleExpiredFunction( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp index c5b9293fcdcc5..0eb1f820000d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -64,7 +64,7 @@ class TemplateModuleManager * * @param path The path with lane ID information to determine module launch. */ - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; /** * @brief Get a function to check module expiration. @@ -76,7 +76,7 @@ class TemplateModuleManager * @return A function for checking module expiration. */ std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; }; /** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index d204e589f0416..4f8442af4aeb2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -23,7 +23,7 @@ #include namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; class TemplateModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index 23746a61b6043..6aac965227efb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -7,3 +7,9 @@ yellow_lamp_period: 2.75 enable_pass_judge: true enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + + v2i: + use_rest_time: false + last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red + velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not + required_time_to_departure: 3.0 # prevent low speed pass diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 518d3275a28d3..16f6e85c422d6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -31,6 +31,7 @@ autoware_universe_utils eigen geometry_msgs + jpn_signal_v2i_msgs pluginlib rclcpp tf2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 1d1ee7fc996b2..d22f7426219fa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -17,14 +17,17 @@ #include #include #include +#include #include #include #include +#include #include #include #include +#include namespace autoware::behavior_velocity_planner { using autoware::universe_utils::getOrDeclareParameter; @@ -42,17 +45,34 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + planner_param_.v2i_use_rest_time = getOrDeclareParameter(node, ns + ".v2i.use_rest_time"); + planner_param_.v2i_last_time_allowed_to_pass = + getOrDeclareParameter(node, ns + ".v2i.last_time_allowed_to_pass"); + planner_param_.v2i_velocity_threshold = + getOrDeclareParameter(node, ns + ".v2i.velocity_threshold"); + planner_param_.v2i_required_time_to_departure = + getOrDeclareParameter(node, ns + ".v2i.required_time_to_departure"); pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); + + if (planner_param_.v2i_use_rest_time) { + RCLCPP_INFO(logger_, "V2I is enabled"); + v2i_subscriber_ = autoware::universe_utils::InterProcessPollingSubscriber< + jpn_signal_v2i_msgs::msg::TrafficLightInfo>:: + create_subscription(&node, "/v2i/external/v2i_traffic_light_info", 1); + } } -void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) +void TrafficLightModuleManager::modifyPathVelocity( + autoware_internal_planning_msgs::msg::PathWithLaneId * path) { visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; autoware_perception_msgs::msg::TrafficLightGroup tl_state; + if (planner_param_.v2i_use_rest_time) updateV2IRestTimeInfo(); + nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( @@ -83,7 +103,7 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat } void TrafficLightModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { for (const auto & traffic_light_reg_elem : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -103,6 +123,9 @@ void TrafficLightModuleManager::launchNewModules( registerModule(std::make_shared( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, logger_.get_child("traffic_light_module"), clock_, time_keeper_, + std::bind( + &TrafficLightModuleManager::getV2IRestTimeToRedSignal, this, + traffic_light_reg_elem.first->id()), planning_factor_interface_)); generateUUID(lane_id); updateRTCStatus( @@ -114,7 +137,7 @@ void TrafficLightModuleManager::launchNewModules( std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -163,6 +186,37 @@ bool TrafficLightModuleManager::hasSameTrafficLight( return false; } +void TrafficLightModuleManager::updateV2IRestTimeInfo() +{ + if (!v2i_subscriber_) { + return; + } + auto msg = v2i_subscriber_->takeData(); + if (!msg) { + return; + } + + std::vector traffic_light_ids; + traffic_light_id_to_rest_time_map_.clear(); + for (const auto & car_light : msg->car_lights) { + for (const auto & state : car_light.states) { + traffic_light_id_to_rest_time_map_[state.traffic_light_group_id] = { + msg->header.stamp, car_light.min_rest_time_to_red}; + traffic_light_ids.push_back(state.traffic_light_group_id); + } + } +} + +std::optional TrafficLightModuleManager::getV2IRestTimeToRedSignal( + const lanelet::Id & id) const +{ + const auto rest_time_to_red_signal = traffic_light_id_to_rest_time_map_.find(id); + if (rest_time_to_red_signal == traffic_light_id_to_rest_time_map_.end()) { + return std::nullopt; + } + return rest_time_to_red_signal->second; +} + } // namespace autoware::behavior_velocity_planner #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp index 5ac32d1107880..a16d52b57ac0b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp @@ -20,11 +20,14 @@ #include #include #include +#include #include -#include +#include +#include #include +#include #include #include @@ -40,12 +43,13 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC private: TrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; - void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override; bool isModuleRegisteredFromRegElement(const lanelet::Id & id, const size_t module_id) const; @@ -55,6 +59,16 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr element, const lanelet::TrafficLightConstPtr registered_element) const; + void updateV2IRestTimeInfo(); + + std::optional getV2IRestTimeToRedSignal( + const lanelet::Id & id) const; + + // V2I + autoware::universe_utils::InterProcessPollingSubscriber< + jpn_signal_v2i_msgs::msg::TrafficLightInfo>::SharedPtr v2i_subscriber_; + std::map traffic_light_id_to_rest_time_map_; + // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 3a8692e9c53dd..90cd2f79eb148 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -19,13 +19,17 @@ #include #include #include +#include +#include #include #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -45,6 +49,8 @@ TrafficLightModule::TrafficLightModule( lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, + const std::function(void)> & + get_rest_time_to_red_signal, const std::shared_ptr planning_factor_interface) : SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), @@ -53,7 +59,8 @@ TrafficLightModule::TrafficLightModule( lane_(lane), state_(State::APPROACH), debug_data_(), - is_prev_state_stop_(false) + is_prev_state_stop_(false), + get_rest_time_to_red_signal_(get_rest_time_to_red_signal) { planner_param_ = planner_param; } @@ -110,6 +117,16 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path) // Check if stop is coming. const bool is_stop_signal = isStopSignal(); + // Use V2I if available + if (planner_param_.v2i_use_rest_time && !is_stop_signal) { + bool is_v2i_handled = handleV2I(signed_arc_length_to_stop_point, [&]() { + *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second); + }); + if (is_v2i_handled) { + return true; + } + } + // Update stop signal received time if (is_stop_signal) { if (!stop_signal_received_time_ptr_) { @@ -186,6 +203,9 @@ void TrafficLightModule::updateTrafficSignal() TrafficSignalStamped signal; if (!findValidTrafficSignal(signal)) { // Don't stop if it never receives traffic light topic. + // Reset looking_tl_state + looking_tl_state_.elements.clear(); + looking_tl_state_.traffic_light_group_id = 0; return; } @@ -213,7 +233,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const delay_response_time); const bool distance_stoppable = pass_judge_line_distance < signed_arc_length; - const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; + const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 1.0; const bool stoppable = distance_stoppable || slow_velocity; const bool reachable = signed_arc_length < reachable_distance; @@ -274,11 +294,11 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const return false; } -tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( - const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, - const Eigen::Vector2d & target_point) +autoware_internal_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, + const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point) { - tier4_planning_msgs::msg::PathWithLaneId modified_path; + autoware_internal_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; // Create stop pose @@ -302,4 +322,44 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( return modified_path; } +bool TrafficLightModule::handleV2I( + const double & signed_arc_length_to_stop_point, const std::function & insert_stop_pose) +{ + std::optional rest_time_to_red_signal = + get_rest_time_to_red_signal_(); + + if (!rest_time_to_red_signal) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, + "Failed to get V2I rest time to red signal. traffic_light_lane_id: %ld", lane_id_); + return false; + } + + auto time_diff = (clock_->now() - rest_time_to_red_signal->stamp).seconds(); + if (time_diff > planner_param_.tl_state_timeout) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "V2I data is timeout. traffic_light_lane_id: %ld, time diff: %f", + lane_id_, time_diff); + return false; + } + + const double rest_time_allowed_to_go_ahead = + rest_time_to_red_signal->time_to_red - planner_param_.v2i_last_time_allowed_to_pass; + const double ego_v = planner_data_->current_velocity->twist.linear.x; + + const bool should_stop = + (ego_v >= planner_param_.v2i_velocity_threshold && + ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) || + (ego_v < planner_param_.v2i_velocity_threshold && + rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure); + + setSafe(!should_stop); + if (isActivated()) { + is_prev_state_stop_ = false; + return true; + } + insert_stop_pose(); + is_prev_state_stop_ = true; + return true; +} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 71f0855a4af6f..d4d210763dd3f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -15,6 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ +#include #include #include #include @@ -33,6 +34,13 @@ namespace autoware::behavior_velocity_planner { + +struct TrafficSignalTimeToRedStamped +{ + builtin_interfaces::msg::Time stamp; + double time_to_red{}; +}; + class TrafficLightModule : public SceneModuleInterfaceWithRTC { public: @@ -63,6 +71,11 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC double yellow_lamp_period; double stop_time_hysteresis; bool enable_pass_judge; + // V2I Parameter + bool v2i_use_rest_time; + double v2i_last_time_allowed_to_pass; + double v2i_velocity_threshold; + double v2i_required_time_to_departure; }; public: @@ -71,6 +84,8 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, + const std::function(void)> & + get_rest_time_to_red_signal, const std::shared_ptr planning_factor_interface); @@ -91,9 +106,9 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC private: bool isStopSignal(); - tier4_planning_msgs::msg::PathWithLaneId insertStopPose( - const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, - const Eigen::Vector2d & target_point); + autoware_internal_planning_msgs::msg::PathWithLaneId insertStopPose( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, + const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point); bool isPassthrough(const double & signed_arc_length) const; @@ -103,6 +118,15 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC void updateTrafficSignal(); + /** + * @brief Handle V2I Rest Time to Red Signal + * @param signed_arc_length_to_stop_point signed arc length to stop point + * @param output_path output path + * @return true if V2I is handled + */ + bool handleV2I( + const double & signed_arc_length_to_stop_point, const std::function & insert_stop_pose); + // Lane id const int64_t lane_id_; @@ -131,6 +155,9 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC // Traffic Light State TrafficSignal looking_tl_state_; + + // V2I + std::function(void)> get_rest_time_to_red_signal_; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp index 9b13123380ce1..1bff335fc0b34 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp @@ -59,8 +59,9 @@ auto findNearestCollisionPoint( } auto createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, - const double offset) -> std::optional> + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, + const LineString2d & stop_line, const double offset) + -> std::optional> { if (input.points.size() < 2) { return std::nullopt; @@ -124,7 +125,7 @@ auto createTargetPoint( } auto calcStopPointAndInsertIndex( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, const double & stop_line_extend_length) -> std::optional> { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp index ad4ed84cea116..01ff773f0d440 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp @@ -60,8 +60,9 @@ auto findNearestCollisionPoint( * point, return std::nullopt. */ auto createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, - const double offset) -> std::optional>; + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, + const LineString2d & stop_line, const double offset) + -> std::optional>; /** * @brief find intersection point between path and stop line and return the point. @@ -73,7 +74,7 @@ auto createTargetPoint( * point, return std::nullopt. */ auto calcStopPointAndInsertIndex( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, const double & stop_line_extend_length) -> std::optional>; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp index e24c2dab5dab9..7c261486c1270 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp index a5d09acc103e9..da427b2c3be1b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp @@ -20,8 +20,8 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternion; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index dade98dfc1133..46fdb3236c75d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -66,7 +66,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node } void VirtualTrafficLightModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { autoware::universe_utils::LineString2d ego_path_linestring; for (const auto & path_point : path.points) { @@ -102,7 +102,7 @@ void VirtualTrafficLightModuleManager::launchNewModules( std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -113,7 +113,7 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( } void VirtualTrafficLightModuleManager::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path) + autoware_internal_planning_msgs::msg::PathWithLaneId * path) { // NOTE: virtual traffic light specific implementation // Since the argument of modifyPathVelocity cannot be changed, the specific information diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index aecef0851d8ab..8e8fc1d08ecbc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -43,12 +43,12 @@ class VirtualTrafficLightModuleManager private: VirtualTrafficLightModule::PlannerParam planner_param_; - void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; autoware::universe_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index a42e3520ea3c1..ce90ff4b1f1af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -368,7 +368,7 @@ bool VirtualTrafficLightModule::hasRightOfWay( } void VirtualTrafficLightModule::insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) + autoware_internal_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, *map_data_.stop_line, end_line_idx); @@ -430,7 +430,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } void VirtualTrafficLightModule::insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) + autoware_internal_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, map_data_.end_lines, end_line_idx); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 24e77cfaea157..47f10f06ab940 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -62,7 +62,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface struct ModuleData { geometry_msgs::msg::Pose head_pose{}; - tier4_planning_msgs::msg::PathWithLaneId path{}; + autoware_internal_planning_msgs::msg::PathWithLaneId path{}; std::optional stop_head_pose_at_stop_line; std::optional stop_head_pose_at_end_line; }; @@ -131,10 +131,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool hasRightOfWay(const tier4_v2x_msgs::msg::VirtualTrafficLightState & state); void insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); + autoware_internal_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); void insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); + autoware_internal_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); }; } // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp index a1950bf768f4c..234781aab0d61 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp @@ -86,7 +86,7 @@ geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Poi return geom_p; } -void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path) +void insertStopVelocityFromStart(autoware_internal_planning_msgs::msg::PathWithLaneId * path) { for (auto & p : path->points) { p.point.longitudinal_velocity_mps = 0.0; @@ -95,7 +95,7 @@ void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, - tier4_planning_msgs::msg::PathWithLaneId * path) + autoware_internal_planning_msgs::msg::PathWithLaneId * path) { const auto collision_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( path->points, collision.index, collision.point); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp index ab11ca5c81391..35b9c8d9ef46a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -58,11 +58,11 @@ geometry_msgs::msg::Pose calcHeadPose( geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point3d & p); -void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path); +void insertStopVelocityFromStart(autoware_internal_planning_msgs::msg::PathWithLaneId * path); std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, - tier4_planning_msgs::msg::PathWithLaneId * path); + autoware_internal_planning_msgs::msg::PathWithLaneId * path); template std::optional findLastCollisionBeforeEndLine( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp index 0e22413c0c8ec..74c61696a0d58 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -53,12 +53,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); publishVirtualTrafficLightState(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -76,13 +81,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); publishVirtualTrafficLightState(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp index e6bfa0234951c..7d89c57b89b41 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp @@ -33,11 +33,11 @@ using autoware::behavior_velocity_planner::virtual_traffic_light::insertStopVelo using autoware::behavior_velocity_planner::virtual_traffic_light::SegmentIndexWithPoint; using autoware::behavior_velocity_planner::virtual_traffic_light::toAutowarePoints; -tier4_planning_msgs::msg::PathWithLaneId generateStraightPath() +autoware_internal_planning_msgs::msg::PathWithLaneId generateStraightPath() { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; for (size_t i = 0; i < 10; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point; point.point.pose.position.x = static_cast(i); point.point.pose.position.y = 0; point.point.pose.position.z = 0; @@ -168,8 +168,8 @@ TEST(VirtualTrafficLightTest, ConvertToGeomPoint) TEST(VirtualTrafficLightTest, InsertStopVelocityFromStart) { - tier4_planning_msgs::msg::PathWithLaneId path; - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_internal_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point; point.point.longitudinal_velocity_mps = 10.0; path.points.push_back(point); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp index b323d6d201795..49f14a744e1fb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface<> { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp index e0b7fa5c31965..fb8286874fe03 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp @@ -31,12 +31,17 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); rclcpp::shutdown(); } @@ -51,13 +56,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index c7a103e0269ca..15fcc52f0fd24 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -97,19 +97,21 @@ void DynamicObstacleStopModule::update_parameters(const std::vector & ego_trajectory_points, + [[maybe_unused]] const std::vector & + raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { VelocityPlanningResult result; debug_data_.reset_data(); - if (ego_trajectory_points.size() < 2) return result; + if (smoothed_trajectory_points.size() < 2) return result; autoware::universe_utils::StopWatch stopwatch; stopwatch.tic(); stopwatch.tic("preprocessing"); dynamic_obstacle_stop::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - ego_data.trajectory = ego_trajectory_points; + ego_data.trajectory = smoothed_trajectory_points; autoware::motion_utils::removeOverlapPoints(ego_data.trajectory); ego_data.first_trajectory_idx = autoware::motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position); @@ -164,7 +166,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( if (stop_pose) { result.stop_points.push_back(stop_pose->position); planning_factor_interface_->add( - ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, + smoothed_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, SafetyFactorArray{}); create_virtual_walls(); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp index a678e7657a6c3..aca86a82798ab 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -36,7 +36,8 @@ class DynamicObstacleStopModule : public PluginModuleInterface void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 0726fdba02de0..850255e0c5093 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -96,12 +96,12 @@ bool is_unavoidable( }; std::vector filter_predicted_objects( - const std::vector & objects, const EgoData & ego_data, + const std::vector> & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; for (const auto & object : objects) { - const auto & predicted_object = object.predicted_object; + const auto & predicted_object = object->predicted_object; const auto is_not_too_slow = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >= params.minimum_object_velocity; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index 33abb19074099..77b3d2b1ead45 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -76,7 +76,7 @@ bool is_unavoidable( /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects std::vector filter_predicted_objects( - const std::vector & objects, const EgoData & ego_data, + const std::vector> & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp index 8cf83f63f4d01..2e3af577a83f4 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -53,23 +53,23 @@ struct PlannerParam struct EgoData { - TrajectoryPoints trajectory; + TrajectoryPoints trajectory{}; size_t first_trajectory_idx{}; - double longitudinal_offset_to_first_trajectory_idx; // [m] - geometry_msgs::msg::Pose pose; - autoware::universe_utils::MultiPolygon2d trajectory_footprints; - Rtree rtree; - std::optional earliest_stop_pose; + double longitudinal_offset_to_first_trajectory_idx{}; // [m] + geometry_msgs::msg::Pose pose{}; + autoware::universe_utils::MultiPolygon2d trajectory_footprints{}; + Rtree rtree{}; + std::optional earliest_stop_pose{}; }; /// @brief debug data struct DebugData { - autoware::universe_utils::MultiPolygon2d obstacle_footprints; + autoware::universe_utils::MultiPolygon2d obstacle_footprints{}; size_t prev_dynamic_obstacles_nb{}; - autoware::universe_utils::MultiPolygon2d ego_footprints; + autoware::universe_utils::MultiPolygon2d ego_footprints{}; size_t prev_ego_footprints_nb{}; - std::optional stop_pose; + std::optional stop_pose{}; size_t prev_collisions_nb{}; double z{}; void reset_data() @@ -82,8 +82,8 @@ struct DebugData struct Collision { - geometry_msgs::msg::Point point; - std::string object_uuid; + geometry_msgs::msg::Point point{}; + std::string object_uuid{}; }; } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 419b5d9e46bd1..3ff4cf0c9972f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -205,7 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable) TEST(TestObjectFiltering, filterPredictedObjects) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; - std::vector objects; + std::vector> objects; autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; double hysteresis{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt new file mode 100644 index 0000000000000..f51e026428aec --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_obstacle_cruise_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md new file mode 100644 index 0000000000000..296bb6290e398 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md @@ -0,0 +1,118 @@ +# Obstacle Cruise + +## Role + +The `obstacle_cruise` module does the cruise planning against a dynamic obstacle in front of the ego. + +## Activation + +This module is activated if the launch parameter `launch_obstacle_cruise_module` is set to true. + +## Inner-workings / Algorithms + +### Obstacle Filtering + +The obstacles meeting the following condition are determined as obstacles for cruising. + +- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`. + +- The object type is for cruising according to `obstacle_filtering.object_type.*`. +- The object is not crossing the ego's trajectory (\*1). +- If the object is inside the trajectory. + - The object type is for inside cruising according to `obstacle_filtering.object_type.inside.*`. + - The object velocity is larger than `obstacle_filtering.obstacle_velocity_threshold_from_cruise`. +- If the object is outside the trajectory. + - The object type is for outside cruising according to `obstacle_filtering.object_type.outside.*`. + - The object velocity is larger than `obstacle_filtering.outside_obstacle.obstacle_velocity_threshold`. + - The highest confident predicted path collides with the ego's trajectory. + - Its collision's period is larger than `obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold`. + +#### NOTE + +##### \*1: Crossing obstacles + +Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold`. + +##### Yield for vehicles that might cut in into the ego's lane + +It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane. + +The obstacles meeting the following condition are determined as obstacles for yielding (cruising). + +- The object type is for cruising according to `obstacle_filtering.object_type.*` and it is moving with a speed greater than `obstacle_filtering.yield.stopped_obstacle_velocity_threshold`. +- The object is not crossing the ego's trajectory (\*1). +- There is another object of type `obstacle_filtering.object_type.*` stopped in front of the moving obstacle. +- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `obstacle_filtering.yield.max_lat_dist_between_obstacles` +- Both obstacles, moving and stopped, are within `obstacle_filtering.yield.lat_distance_threshold` and `obstacle_filtering.yield.lat_distance_threshold` + `obstacle_filtering.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively. + +If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle. + +### Cruise Planning + +The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition. +This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle. + +The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation. + +$$ +d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, +$$ + +assuming that $d_{rss}$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. +These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`. + +| Parameter | Type | Description | +| ------------------------------------------ | ------ | ----------------------------------------------------------------------------- | +| `cruise_planning.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] | +| `cruise_planning.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] | +| `cruise_planning.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] | + +The detailed formulation is as follows. + +$$ +\begin{align} +d_{error} & = d - d_{rss} \\ +d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\ +d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ +v_{pid} & = pid(d_{quad, normalized}) \\ +v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ +v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise}) +\end{align} +$$ + +| Variable | Description | +| ----------------- | --------------------------------------- | +| `d` | actual distance to obstacle | +| `d_{rss}` | ideal distance to obstacle based on RSS | +| `v_{min, cruise}` | `min_cruise_target_vel` | +| `w_{acc}` | `output_ratio_during_accel` | +| `lpf(val)` | apply low-pass filter to `val` | +| `pid(val)` | apply pid to `val` | + +#### Algorithm selection for cruise planner + +Currently, only a PID-based planner is supported. +Each planner will be explained in the following. + +| Parameter | Type | Description | +| --------------------------- | ------ | ------------------------------------------------------------ | +| `option.planning_algorithm` | string | cruise and stop planning algorithm, selected from "pid_base" | + +#### PID-based planner + +In order to keep the safe distance, the target velocity and acceleration is calculated and sent as an external velocity limit to the velocity smoothing package (`velocity_smoother` by default). +The target velocity and acceleration is respectively calculated with the PID controller according to the error between the reference safe distance and the actual distance. + +#### Optimization-based planner + +under construction + +## Debugging + +### Obstacle for cruise + +Orange sphere which is an obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic. + +Orange wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise/virtual_wall` topic. + +![cruise_visualization](./docs/cruise_visualization.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml new file mode 100644 index 0000000000000..214f7bb35a155 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml @@ -0,0 +1,120 @@ +/**: + ros__parameters: + obstacle_cruise: + option: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + + cruise_planning: + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] + min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] + min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] + safe_distance_margin : 5.0 # This is also used as a stop margin [m] + + pid_based_planner: + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic + + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 + + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false + + disable_target_acceleration: true + + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 + + lpf_normalized_error_cruise_dist_gain: 0.2 + + optimization_based_planner: + dense_resampling_time_interval: 0.2 + sparse_resampling_time_interval: 2.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + velocity_margin: 0.2 #[m/s] + + # Parameters for safe distance + t_dangerous: 0.5 + + # For initial Motion + replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] + engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) + engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # Weights for optimization + max_s_weight: 100.0 + max_v_weight: 1.0 + over_s_safety_weight: 1000000.0 + over_s_ideal_weight: 50.0 + over_v_weight: 500000.0 + over_a_weight: 5000.0 + over_j_weight: 10000.0 + + obstacle_filtering: + object_type: + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: false + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: false + pedestrian: false + + max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width + + # if crossing vehicle is determined as target obstacles or not + crossing_obstacle: + obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + obstacle_traj_angle_threshold : 0.523599 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + + outside_obstacle: + obstacle_velocity_threshold : 1.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 0.2 # time threshold to decide cut-in obstacle for cruise or stop [s] + max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego + max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s] + num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego + yield: + enable_yield: true + lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_obstacles_collision_time: 10.0 # how far the blocking obstacle + stopped_obstacle_velocity_threshold: 0.5 + + # hysteresis for cruise and stop + obstacle_velocity_threshold_from_cruise : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png new file mode 100644 index 0000000000000..acb094135e884 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml new file mode 100644 index 0000000000000..e0cbf2c193b7a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml @@ -0,0 +1,41 @@ + + + + autoware_motion_velocity_obstacle_cruise_module + 0.40.0 + obstacle cruise feature in motion_velocity_planner + + Takayuki Murooka + Yuki Takagi + Maxime Clement + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_motion_utils + autoware_motion_velocity_planner_common_universe + autoware_osqp_interface + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_signal_processing + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + libboost-dev + pluginlib + rclcpp + tf2 + tier4_metric_msgs + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml new file mode 100644 index 0000000000000..d841b9beba5c9 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/cruise_planner_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/cruise_planner_interface.hpp new file mode 100644 index 0000000000000..63f979813e78f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/cruise_planner_interface.hpp @@ -0,0 +1,104 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CRUISE_PLANNER_INTERFACE_HPP_ +#define CRUISE_PLANNER_INTERFACE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" +#include "parameters.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class CruisePlannerInterface +{ +public: + CruisePlannerInterface( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param) + { + clock_ = node.get_clock(); + logger_ = node.get_logger(); + common_param_ = common_param; + cruise_planning_param_ = cruise_planning_param; + } + + virtual ~CruisePlannerInterface() = default; + + virtual std::vector plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) = 0; + + virtual void update_parameters(const std::vector & parameters) = 0; + + virtual Float32MultiArrayStamped get_cruise_planning_debug_message( + [[maybe_unused]] const rclcpp::Time & current_time) const + { + return Float32MultiArrayStamped{}; + } + +protected: + rclcpp::Clock::SharedPtr clock_{}; + rclcpp::Logger logger_{rclcpp::get_logger("")}; + CommonParam common_param_; + CruisePlanningParam cruise_planning_param_; + + static double calc_distance_to_collisionPoint( + const std::vector & traj_points, + const std::shared_ptr planner_data, + const geometry_msgs::msg::Point & collision_point) + { + const double offset = planner_data->is_driving_forward + ? std::abs(planner_data->vehicle_info_.max_longitudinal_offset_m) + : std::abs(planner_data->vehicle_info_.min_longitudinal_offset_m); + + const size_t ego_segment_idx = + planner_data->find_segment_index(traj_points, planner_data->current_odometry.pose.pose); + + const size_t collision_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, collision_point); + + const auto dist_to_collision_point = autoware::motion_utils::calcSignedArcLength( + traj_points, planner_data->current_odometry.pose.pose.position, ego_segment_idx, + collision_point, collision_segment_idx); + + return dist_to_collision_point - offset; + } + + double calc_rss_distance( + const double ego_vel, const double obstacle_vel, const double margin = 0.0) const + { + const double rss_dist_with_margin = + ego_vel * cruise_planning_param_.idling_time + + std::pow(ego_vel, 2) * 0.5 / std::abs(cruise_planning_param_.min_ego_accel_for_rss) - + std::pow(obstacle_vel, 2) * 0.5 / std::abs(cruise_planning_param_.min_object_accel_for_rss) + + margin; + return rss_dist_with_margin; + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // CRUISE_PLANNER_INTERFACE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/metrics_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/metrics_manager.hpp new file mode 100644 index 0000000000000..11554121a0882 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/metrics_manager.hpp @@ -0,0 +1,60 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef METRICS_MANAGER_HPP_ +#define METRICS_MANAGER_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MetricsManager +{ +public: + void init() { metrics_.clear(); } + + void calculate_metrics(const std::string & module_name, const std::string & reason) + { + // Create status + { + // Decision + Metric decision_metric; + decision_metric.name = module_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics_.push_back(decision_metric); + } + } + + MetricArray create_metric_array(const rclcpp::Time & current_time) + { + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), metrics_.begin(), metrics_.end()); + return metrics_msg; + } + +private: + std::vector metrics_; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // METRICS_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp new file mode 100644 index 0000000000000..c1d02842de1bb --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp @@ -0,0 +1,826 @@ +// Copyright 2025 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_module.hpp" + +#include "autoware/universe_utils/ros/uuid_helper.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace +{ +double calc_diff_angle_against_trajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & target_pose) +{ + const size_t nearest_idx = + autoware::motion_utils::findNearestIndex(traj_points, target_pose.position); + const double traj_yaw = tf2::getYaw(traj_points.at(nearest_idx).pose.orientation); + + const double target_yaw = tf2::getYaw(target_pose.orientation); + + const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); + return diff_yaw; +} + +std::vector resample_highest_confidence_predicted_paths( + const std::vector & predicted_paths, const double time_interval, + const double time_horizon, const size_t num_paths) +{ + std::vector sorted_paths = predicted_paths; + + // Sort paths by descending confidence + std::sort( + sorted_paths.begin(), sorted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; }); + + std::vector selected_paths; + size_t path_count = 0; + + // Select paths that meet the confidence thresholds + for (const auto & path : sorted_paths) { + if (path_count < num_paths) { + selected_paths.push_back(path); + ++path_count; + } + } + + // Resample each selected path + std::vector resampled_paths; + for (const auto & path : selected_paths) { + if (path.path.size() < 2) { + continue; + } + resampled_paths.push_back( + autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + } + + return resampled_paths; +} + +VelocityLimitClearCommand create_velocity_limit_clear_command( + const rclcpp::Time & current_time, [[maybe_unused]] const std::string & module_name) +{ + VelocityLimitClearCommand msg; + msg.stamp = current_time; + msg.sender = "obstacle_cruise"; + msg.command = true; + return msg; +} + +Float64Stamped create_float64_stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} +} // namespace + +void ObstacleCruiseModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + // ros parameters + planning_algorithm_ = + getOrDeclareParameter(node, "obstacle_cruise.option.planning_algorithm"); + common_param_ = CommonParam(node); + cruise_planning_param_ = CruisePlanningParam(node); + obstacle_filtering_param_ = ObstacleFilteringParam(node); + + // common publisher + processing_time_publisher_ = + node.create_publisher("~/debug/obstacle_cruise/processing_time_ms", 1); + virtual_wall_publisher_ = + node.create_publisher("~/obstacle_cruise/virtual_walls", 1); + debug_publisher_ = node.create_publisher("~/obstacle_cruise/debug_markers", 1); + + // module publisher + metrics_pub_ = node.create_publisher("~/cruise/metrics", 10); + debug_cruise_planning_info_pub_ = + node.create_publisher("~/debug/cruise_planning_info", 1); + processing_time_detail_pub_ = node.create_publisher( + "~/debug/processing_time_detail_ms/obstacle_cruise", 1); + + // interface + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "obstacle_cruise"); + planning_factor_interface_ = + std::make_unique( + &node, "obstacle_cruise"); + + // time keeper + time_keeper_ = std::make_shared(processing_time_detail_pub_); + + // cruise planner + cruise_planner_ = create_cruise_planner(node); +} + +void ObstacleCruiseModule::update_parameters(const std::vector & parameters) +{ + cruise_planner_->update_parameters(parameters); +} + +VelocityPlanningResult ObstacleCruiseModule::plan( + const std::vector & raw_trajectory_points, + [[maybe_unused]] const std::vector & + smoothed_trajectory_points, + const std::shared_ptr planner_data) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. init variables + stop_watch_.tic(); + debug_data_ptr_ = std::make_shared(); + metrics_manager_.init(); + + // filter obstacles of predicted objects + const auto cruise_obstacles = filter_cruise_obstacle_for_predicted_object( + planner_data->current_odometry, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold, raw_trajectory_points, planner_data->objects, + rclcpp::Time(planner_data->predicted_objects_header.stamp), planner_data->is_driving_forward, + planner_data->vehicle_info_, planner_data->trajectory_polygon_collision_check); + + // plan cruise + VelocityPlanningResult result; + [[maybe_unused]] const auto cruise_traj_points = cruise_planner_->plan_cruise( + planner_data, raw_trajectory_points, cruise_obstacles, debug_data_ptr_, + planning_factor_interface_, result.velocity_limit); + metrics_manager_.calculate_metrics("PlannerInterface", "cruise"); + + // clear velocity limit if necessary + if (result.velocity_limit) { + need_to_clear_velocity_limit_ = true; + } else { + if (need_to_clear_velocity_limit_) { + // clear velocity limit + result.velocity_limit_clear_command = + create_velocity_limit_clear_command(clock_->now(), module_name_); + need_to_clear_velocity_limit_ = false; + } + } + + publish_debug_info(); + + return result; +} + +std::string ObstacleCruiseModule::get_module_name() const +{ + return module_name_; +} + +std::unique_ptr ObstacleCruiseModule::create_cruise_planner( + rclcpp::Node & node) const +{ + if (planning_algorithm_ == "pid_base") { + return std::make_unique(node, common_param_, cruise_planning_param_); + } else if (planning_algorithm_ == "optimization_base") { + return std::make_unique(node, common_param_, cruise_planning_param_); + } + throw std::logic_error("Designated algorithm is not supported."); +} + +std::vector ObstacleCruiseModule::filter_cruise_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & current_pose = odometry.pose.pose; + + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold, + p.decimate_trajectory_step_length, 0.0); + const auto decimated_traj_polys = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + debug_data_ptr_->decimated_traj_polys = decimated_traj_polys; + + // cruise + std::vector cruise_obstacles; + for (const auto & object : objects) { + // 1. rough filtering + // 1.1. Check if the obstacle is in front of the ego. + const double lon_dist_from_ego_to_obj = + object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + if (lon_dist_from_ego_to_obj < 0.0) { + continue; + } + + // 1.2. Check if the rough lateral distance is smaller than the threshold. + const double min_lat_dist_to_traj_poly = + utils::calc_possible_min_dist_from_obj_to_traj_poly(object, traj_points, vehicle_info); + if (obstacle_filtering_param_.max_lat_margin < min_lat_dist_to_traj_poly) { + continue; + } + + // 2. precise filtering for cruise + const auto cruise_obstacle = create_cruise_obstacle( + odometry, traj_points, decimated_traj_points, decimated_traj_polys, object, + predicted_objects_stamp, object->get_dist_to_traj_poly(decimated_traj_polys), + is_driving_forward, vehicle_info, trajectory_polygon_collision_check); + if (cruise_obstacle) { + cruise_obstacles.push_back(*cruise_obstacle); + continue; + } + + // 3. precise filtering for yield cruise + if (obstacle_filtering_param_.enable_yield) { + const auto yield_obstacles = find_yield_cruise_obstacles( + odometry, objects, predicted_objects_stamp, traj_points, vehicle_info); + if (yield_obstacles) { + for (const auto & y : yield_obstacles.value()) { + // Check if there is no member with the same UUID in cruise_obstacles + auto it = std::find_if( + cruise_obstacles.begin(), cruise_obstacles.end(), + [&y](const auto & c) { return y.uuid == c.uuid; }); + + // If no matching UUID found, insert yield obstacle into cruise_obstacles + if (it == cruise_obstacles.end()) { + cruise_obstacles.push_back(y); + } + } + } + } + } + prev_cruise_object_obstacles_ = cruise_obstacles; + + return cruise_obstacles; +} + +void ObstacleCruiseModule::publish_debug_info() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. debug marker + MarkerArray debug_marker; + + // 1.1. obstacles + std::vector stop_collision_points; + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) { + // obstacle + const auto obstacle_marker = utils::get_object_marker( + debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles", 1.0, 0.6, 0.1); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + for (size_t j = 0; j < debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.size(); + ++j) { + stop_collision_points.push_back( + debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.at(j).point); + } + } + + // 1.2. collision points + for (size_t i = 0; i < stop_collision_points.size(); ++i) { + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", i, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = stop_collision_points.at(i); + debug_marker.markers.push_back(collision_point_marker); + } + + // 1.3. intentionally ignored obstacles + for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) { + const auto marker = utils::get_object_marker( + debug_data_ptr_->intentionally_ignored_obstacles.at(i) + ->predicted_object.kinematics.initial_pose_with_covariance.pose, + i, "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); + debug_marker.markers.push_back(marker); + } + + // 1.4. detection area + auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { + for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { + const auto & current_point = decimated_traj_poly.outer().at(dp_idx); + const auto & next_point = + decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); + + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(decimated_traj_polys_marker); + + debug_publisher_->publish(debug_marker); + + // 2. virtual wall + virtual_wall_publisher_->publish(debug_data_ptr_->cruise_wall_marker); + + // 3. cruise planning info + const auto cruise_debug_msg = cruise_planner_->get_cruise_planning_debug_message(clock_->now()); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); + + // 4. objects of interest + objects_of_interest_marker_interface_->publishMarkerArray(); + + // 5. metrics + const auto metrics_msg = metrics_manager_.create_metric_array(clock_->now()); + metrics_pub_->publish(metrics_msg); + + // 6. processing time + processing_time_publisher_->publish(create_float64_stamped(clock_->now(), stop_watch_.toc())); + + // 7. planning factor + planning_factor_interface_->publish(); +} + +std::optional ObstacleCruiseModule::create_cruise_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + // NOTE: When driving backward, Stop will be planned instead of cruise. + // When the obstacle is crossing the ego's trajectory, cruise can be ignored. + if ( + !is_cruise_obstacle(object->predicted_object.classification.at(0).label) || + !is_driving_forward) { + return std::nullopt; + } + + if (object->get_lon_vel_relative_to_traj(traj_points) < 0.0) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore obstacle (%s) since it's driving in opposite direction.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if (obstacle_filtering_param_.max_lat_margin < dist_from_obj_poly_to_traj_poly) { + const auto time_to_traj = dist_from_obj_poly_to_traj_poly / + std::max(1e-6, object->get_lat_vel_relative_to_traj(traj_points)); + if (time_to_traj > obstacle_filtering_param_.max_lateral_time_margin) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore obstacle (%s) since it's far from trajectory.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + } + + if (is_obstacle_crossing(traj_points, object)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore obstacle (%s) since it's crossing the ego's trajectory..", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + const auto collision_points = [&]() -> std::optional> { + constexpr double epsilon = 1e-6; + if (dist_from_obj_poly_to_traj_poly < epsilon) { + // obstacle is inside the trajectory + return create_collision_points_for_inside_cruise_obstacle( + traj_points, decimated_traj_points, decimated_traj_polys, object, predicted_objects_stamp, + is_driving_forward, vehicle_info, trajectory_polygon_collision_check); + } + // obstacle is outside the trajectory + // If the ego is stopping, do not plan cruise for outside obstacles. Stop will be planned. + if (odometry.twist.twist.linear.x < 0.1) { + return std::nullopt; + } + return create_collision_points_for_outside_cruise_obstacle( + traj_points, decimated_traj_points, decimated_traj_polys, object, predicted_objects_stamp, + is_driving_forward, vehicle_info, trajectory_polygon_collision_check); + }(); + if (!collision_points) { + return std::nullopt; + } + + return CruiseObstacle{ + obj_uuid_str, + predicted_objects_stamp, + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->get_lon_vel_relative_to_traj(traj_points), + object->get_lat_vel_relative_to_traj(traj_points), + *collision_points}; +} + +std::optional> ObstacleCruiseModule::find_yield_cruise_obstacles( + const Odometry & odometry, const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points, + const VehicleInfo & vehicle_info) +{ + const auto & current_pose = odometry.pose.pose; + + if (objects.empty() || traj_points.empty()) return std::nullopt; + + std::vector> stopped_objects; + std::vector> moving_objects; + + std::for_each(objects.begin(), objects.end(), [&](const auto & o) { + const bool is_moving = + std::hypot( + o->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + o->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y) > + obstacle_filtering_param_.stopped_obstacle_velocity_threshold; + if (is_moving) { + const bool is_within_lat_dist_threshold = + o->get_dist_to_traj_lateral(traj_points) < + obstacle_filtering_param_.yield_lat_distance_threshold; + if (is_within_lat_dist_threshold) moving_objects.push_back(o); + return; + } + // lat threshold is larger for stopped obstacles + const bool is_within_lat_dist_threshold = + o->get_dist_to_traj_lateral(traj_points) < + obstacle_filtering_param_.yield_lat_distance_threshold + + obstacle_filtering_param_.max_lat_dist_between_obstacles; + if (is_within_lat_dist_threshold) stopped_objects.push_back(o); + return; + }); + + if (stopped_objects.empty() || moving_objects.empty()) return std::nullopt; + + std::sort(stopped_objects.begin(), stopped_objects.end(), [&](const auto & o1, const auto & o2) { + return o1->get_dist_from_ego_longitudinal(traj_points, current_pose.position) < + o2->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + }); + + std::sort(moving_objects.begin(), moving_objects.end(), [&](const auto & o1, const auto & o2) { + return o1->get_dist_from_ego_longitudinal(traj_points, current_pose.position) < + o2->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + }); + + std::vector yield_obstacles; + for (const auto & moving_object : moving_objects) { + for (const auto & stopped_object : stopped_objects) { + const bool is_moving_obs_behind_of_stopped_obs = + moving_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position) < + stopped_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + const bool is_moving_obs_ahead_of_ego_front = + moving_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position) > + vehicle_info.vehicle_length_m; + + if (!is_moving_obs_ahead_of_ego_front || !is_moving_obs_behind_of_stopped_obs) continue; + + const double lateral_distance_between_obstacles = std::abs( + moving_object->get_dist_to_traj_lateral(traj_points) - + stopped_object->get_dist_to_traj_lateral(traj_points)); + + const double longitudinal_distance_between_obstacles = std::abs( + moving_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position) - + stopped_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position)); + + const double moving_object_speed = std::hypot( + moving_object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + moving_object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); + + const bool are_obstacles_aligned = lateral_distance_between_obstacles < + obstacle_filtering_param_.max_lat_dist_between_obstacles; + const bool obstacles_collide_within_threshold_time = + longitudinal_distance_between_obstacles / moving_object_speed < + obstacle_filtering_param_.max_obstacles_collision_time; + if (are_obstacles_aligned && obstacles_collide_within_threshold_time) { + const auto yield_obstacle = + create_yield_cruise_obstacle(moving_object, predicted_objects_stamp, traj_points); + if (yield_obstacle) { + yield_obstacles.push_back(*yield_obstacle); + using autoware::objects_of_interest_marker_interface::ColorName; + objects_of_interest_marker_interface_->insertObjectData( + stopped_object->predicted_object.kinematics.initial_pose_with_covariance.pose, + stopped_object->predicted_object.shape, ColorName::RED); + } + } + } + } + if (yield_obstacles.empty()) return std::nullopt; + return yield_obstacles; +} + +std::optional> +ObstacleCruiseModule::create_collision_points_for_inside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + // check label + if (!is_inside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore inside obstacle (%s) since its type is not designated.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + { // consider hysteresis + // const bool is_prev_obstacle_stop = get_obstacle_from_uuid(prev_stop_obstacles_, + // obstacle.uuid).has_value(); + const bool is_prev_obstacle_cruise = + utils::get_obstacle_from_uuid(prev_cruise_object_obstacles_, obj_uuid_str).has_value(); + + if (is_prev_obstacle_cruise) { + if ( + object->get_lon_vel_relative_to_traj(traj_points) < + obstacle_filtering_param_.obstacle_velocity_threshold_from_cruise) { + return std::nullopt; + } + // NOTE: else is keeping cruise + } else { + // If previous obstacle is stop or does not exist. + if ( + object->get_lon_vel_relative_to_traj(traj_points) < + obstacle_filtering_param_.obstacle_velocity_threshold_to_cruise) { + return std::nullopt; + } + // NOTE: else is cruise from stop + } + } + + // Get highest confidence predicted path + constexpr double prediction_resampling_time_interval = 0.1; + constexpr double prediction_resampling_time_horizon = 10.0; + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, prediction_resampling_time_horizon, 1); + + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } + + // calculate nearest collision point + std::vector collision_index; + const auto collision_points = polygon_utils::get_collision_points( + decimated_traj_points, decimated_traj_polys, predicted_objects_stamp, + resampled_predicted_paths.front(), object->predicted_object.shape, clock_->now(), + is_driving_forward, collision_index, + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape) + + trajectory_polygon_collision_check.decimate_trajectory_step_length + + std::hypot( + vehicle_info.vehicle_length_m, + vehicle_info.vehicle_width_m * 0.5 + obstacle_filtering_param_.max_lat_margin)); + return collision_points; +} + +std::optional> +ObstacleCruiseModule::create_collision_points_for_outside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + // check label + if (!is_outside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since its type is not designated.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if ( + std::hypot( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y) < + obstacle_filtering_param_.outside_obstacle_velocity_threshold) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since the obstacle velocity is low.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + // Get the highest confidence predicted paths + constexpr double prediction_resampling_time_interval = 0.1; + constexpr double prediction_resampling_time_horizon = 10.0; + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, prediction_resampling_time_horizon, + obstacle_filtering_param_.num_of_predicted_paths_for_outside_cruise_obstacle); + + // calculate collision condition for cruise + std::vector collision_index; + const auto get_collision_points = [&]() -> std::vector { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_points = polygon_utils::get_collision_points( + decimated_traj_points, decimated_traj_polys, predicted_objects_stamp, predicted_path, + object->predicted_object.shape, clock_->now(), is_driving_forward, collision_index, + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape) + + trajectory_polygon_collision_check.decimate_trajectory_step_length + + std::hypot( + vehicle_info.vehicle_length_m, + vehicle_info.vehicle_width_m * 0.5 + obstacle_filtering_param_.max_lat_margin), + obstacle_filtering_param_.max_prediction_time_for_collision_check); + if (!collision_points.empty()) { + return collision_points; + } + } + return {}; + }; + + const auto collision_points = get_collision_points(); + + if (collision_points.empty()) { + // Ignore vehicle obstacles outside the trajectory without collision + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since there are no collision points.", + obj_uuid_str.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + const double overlap_time = + (rclcpp::Time(collision_points.back().stamp) - rclcpp::Time(collision_points.front().stamp)) + .seconds(); + if (overlap_time < obstacle_filtering_param_.ego_obstacle_overlap_time_threshold) { + // Ignore vehicle obstacles outside the trajectory, whose predicted path + // overlaps the ego trajectory in a certain time. + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since it will not collide with the ego.", + obj_uuid_str.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + // Ignore obstacles behind the ego vehicle. + // Note: Only using isFrontObstacle(), behind obstacles cannot be filtered + // properly when the trajectory is crossing or overlapping. + const size_t first_collision_index = collision_index.front(); + if (!is_front_collide_obstacle(traj_points, object, first_collision_index)) { + return std::nullopt; + } + return collision_points; +} + +std::optional ObstacleCruiseModule::create_yield_cruise_obstacle( + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points) +{ + if (traj_points.empty()) return std::nullopt; + // check label + + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + if (!is_outside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore yield obstacle (%s) since its type is not designated.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if ( + std::hypot( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y) < + obstacle_filtering_param_.outside_obstacle_velocity_threshold) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore yield obstacle (%s) since the obstacle velocity is low.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if (is_obstacle_crossing(traj_points, object)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore yield obstacle (%s) since it's crossing the ego's trajectory..", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + const auto collision_points = [&]() -> std::optional> { + const auto obstacle_idx = autoware::motion_utils::findNearestIndex( + traj_points, object->predicted_object.kinematics.initial_pose_with_covariance.pose); + if (!obstacle_idx) return std::nullopt; + const auto collision_traj_point = traj_points.at(obstacle_idx.value()); + const auto object_time = clock_->now() + traj_points.at(obstacle_idx.value()).time_from_start; + + polygon_utils::PointWithStamp collision_traj_point_with_stamp; + collision_traj_point_with_stamp.stamp = object_time; + collision_traj_point_with_stamp.point.x = collision_traj_point.pose.position.x; + collision_traj_point_with_stamp.point.y = collision_traj_point.pose.position.y; + collision_traj_point_with_stamp.point.z = collision_traj_point.pose.position.z; + std::vector collision_points_vector{ + collision_traj_point_with_stamp}; + return collision_points_vector; + }(); + + if (!collision_points) return std::nullopt; + // check if obstacle is driving on the opposite direction + if (object->get_lon_vel_relative_to_traj(traj_points) < 0.0) return std::nullopt; + return CruiseObstacle{ + obj_uuid_str, + predicted_objects_stamp, + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->get_lon_vel_relative_to_traj(traj_points), + object->get_lat_vel_relative_to_traj(traj_points), + collision_points.value(), + true}; +} + +bool ObstacleCruiseModule::is_inside_cruise_obstacle(const uint8_t label) const +{ + const auto & types = obstacle_filtering_param_.inside_object_types; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruiseModule::is_outside_cruise_obstacle(const uint8_t label) const +{ + const auto & types = obstacle_filtering_param_.outside_object_types; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruiseModule::is_cruise_obstacle(const uint8_t label) const +{ + return is_inside_cruise_obstacle(label) || is_outside_cruise_obstacle(label); +} + +bool ObstacleCruiseModule::is_front_collide_obstacle( + const std::vector & traj_points, + const std::shared_ptr object, const size_t first_collision_idx) const +{ + const auto obstacle_idx = autoware::motion_utils::findNearestIndex( + traj_points, object->predicted_object.kinematics.initial_pose_with_covariance.pose.position); + + const double obstacle_to_col_points_distance = + autoware::motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); + const double object_possible_max_dist_from_center = + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape); + + // If the obstacle is far in front of the collision point, the obstacle is behind the ego. + return obstacle_to_col_points_distance > -object_possible_max_dist_from_center; +} + +bool ObstacleCruiseModule::is_obstacle_crossing( + const std::vector & traj_points, + const std::shared_ptr object) const +{ + const double diff_angle = calc_diff_angle_against_trajectory( + traj_points, object->predicted_object.kinematics.initial_pose_with_covariance.pose); + + // NOTE: Currently predicted objects does not have orientation availability even + // though sometimes orientation is not available. + const bool is_obstacle_crossing_trajectory = + obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold < std::abs(diff_angle) && + obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold < M_PI - std::abs(diff_angle); + if (!is_obstacle_crossing_trajectory) { + return false; + } + + // Only obstacles crossing the ego's trajectory with high speed are considered. + return true; +} +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::ObstacleCruiseModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp new file mode 100644 index 0000000000000..d9e65c2a3b7db --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp @@ -0,0 +1,137 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_MODULE_HPP_ +#define OBSTACLE_CRUISE_MODULE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "cruise_planner_interface.hpp" +#include "metrics_manager.hpp" +#include "optimization_based_planner/optimization_based_planner.hpp" +#include "parameters.hpp" +#include "pid_based_planner/pid_based_planner.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class ObstacleCruiseModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + VelocityPlanningResult plan( + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override; + +private: + std::string module_name_; + rclcpp::Clock::SharedPtr clock_{}; + + // ros parameters + std::string planning_algorithm_; + CommonParam common_param_; + CruisePlanningParam cruise_planning_param_; + ObstacleFilteringParam obstacle_filtering_param_; + + // common publisher + rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; + + // module publisher + rclcpp::Publisher::SharedPtr metrics_pub_; + std::unique_ptr + objects_of_interest_marker_interface_; + rclcpp::Publisher::SharedPtr + processing_time_detail_pub_; + + // cruise planner + std::unique_ptr cruise_planner_; + + // internal variables + autoware::universe_utils::StopWatch stop_watch_; + std::vector prev_cruise_object_obstacles_; + mutable std::shared_ptr debug_data_ptr_; + bool need_to_clear_velocity_limit_{false}; + MetricsManager metrics_manager_; + mutable std::shared_ptr time_keeper_; + + std::unique_ptr create_cruise_planner(rclcpp::Node & node) const; + std::vector filter_cruise_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check); + void publish_debug_info(); + std::optional create_cruise_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional> find_yield_cruise_obstacles( + const Odometry & odometry, const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points, + const VehicleInfo & vehicle_info); + std::optional> + create_collision_points_for_inside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional> + create_collision_points_for_outside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional create_yield_cruise_obstacle( + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points); + bool is_inside_cruise_obstacle(const uint8_t label) const; + bool is_outside_cruise_obstacle(const uint8_t label) const; + bool is_cruise_obstacle(const uint8_t label) const; + bool is_front_collide_obstacle( + const std::vector & traj_points, + const std::shared_ptr object, const size_t first_collision_idx) const; + bool is_obstacle_crossing( + const std::vector & traj_points, + const std::shared_ptr object) const; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OBSTACLE_CRUISE_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp new file mode 100644 index 0000000000000..c2489bbf3039f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp @@ -0,0 +1,754 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "optimization_based_planner.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace autoware::motion_velocity_planner +{ +constexpr double ZERO_VEL_THRESHOLD = 0.01; +constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; + +OptimizationBasedPlanner::OptimizationBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param) +: CruisePlannerInterface(node, common_param, cruise_planning_param) +{ + smoothed_traj_sub_ = node.create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; }); + + // parameter + dense_resampling_time_interval_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.dense_resampling_time_interval"); + sparse_resampling_time_interval_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.sparse_resampling_time_interval"); + dense_time_horizon_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.dense_time_horizon"); + max_time_horizon_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.max_time_horizon"); + + t_dangerous_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.t_dangerous"); + velocity_margin_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.velocity_margin"); + + replan_vel_deviation_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.replan_vel_deviation"); + engage_velocity_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.engage_velocity"); + engage_acceleration_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.engage_acceleration"); + engage_exit_ratio_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.engage_exit_ratio"); + stop_dist_to_prohibit_engage_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.stop_dist_to_prohibit_engage"); + + const double max_s_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.max_s_weight"); + const double max_v_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.max_v_weight"); + const double over_s_safety_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_s_safety_weight"); + const double over_s_ideal_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_s_ideal_weight"); + const double over_v_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_v_weight"); + const double over_a_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_a_weight"); + const double over_j_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_j_weight"); + + // velocity optimizer + velocity_optimizer_ptr_ = std::make_shared( + max_s_weight, max_v_weight, over_s_safety_weight, over_s_ideal_weight, over_v_weight, + over_a_weight, over_j_weight); + + // publisher + optimized_sv_pub_ = node.create_publisher("~/optimized_sv_trajectory", 1); + optimized_st_graph_pub_ = node.create_publisher("~/optimized_st_graph", 1); + boundary_pub_ = node.create_publisher("~/boundary", 1); + debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); +} + +std::vector OptimizationBasedPlanner::plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, + [[maybe_unused]] std::shared_ptr debug_data_ptr, + [[maybe_unused]] std::unique_ptr & + planning_factor_interface, + [[maybe_unused]] std::optional & velocity_limit) +{ + // Create Time Vector defined by resampling time interval + const std::vector time_vec = createTimeVector(); + if (time_vec.size() < 2) { + RCLCPP_ERROR( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Resolution size is not enough"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Get the nearest point on the trajectory + const size_t closest_idx = + planner_data->find_segment_index(stop_traj_points, planner_data->current_odometry.pose.pose); + + // Compute maximum velocity + double v_max = 0.0; + for (const auto & point : stop_traj_points) { + v_max = std::max(v_max, static_cast(point.longitudinal_velocity_mps)); + } + + // Get Current Velocity + double v0; + double a0; + std::tie(v0, a0) = calcInitialMotion(planner_data, stop_traj_points, prev_output_); + a0 = std::min(common_param_.max_accel, std::max(a0, common_param_.min_accel)); + + // Check trajectory size + if (stop_traj_points.size() - closest_idx <= 2) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "The number of points on the trajectory is too small or failed to calculate front offset"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Check if reached goal + if (checkHasReachedGoal(planner_data, stop_traj_points)) { + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Get S Boundaries from the obstacle + const auto s_boundaries = getSBoundaries(planner_data, stop_traj_points, obstacles, time_vec); + if (!s_boundaries) { + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Optimization + VelocityOptimizer::OptimizationData data; + data.time_vec = time_vec; + data.s0 = 0.0; + data.a0 = a0; + data.v_max = v_max; + data.a_max = common_param_.max_accel; + data.a_min = common_param_.min_accel; + data.j_max = common_param_.max_jerk; + data.j_min = common_param_.min_jerk; + data.limit_a_max = common_param_.limit_max_accel; + data.limit_a_min = common_param_.limit_min_accel; + data.limit_j_max = common_param_.limit_max_jerk; + data.limit_j_min = common_param_.limit_min_jerk; + data.t_dangerous = t_dangerous_; + data.idling_time = cruise_planning_param_.idling_time; + data.s_boundary = *s_boundaries; + data.v0 = v0; + RCLCPP_DEBUG(rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "v0 %f", v0); + + const auto optimized_result = velocity_optimizer_ptr_->optimize(data); + + // Publish Debug trajectories + const double traj_front_to_vehicle_offset = + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx); + publishDebugTrajectory( + planner_data, traj_front_to_vehicle_offset, time_vec, *s_boundaries, optimized_result); + + // Transformation from t to s + const auto processed_result = + processOptimizedResult(data.v0, optimized_result, traj_front_to_vehicle_offset); + if (!processed_result) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Processed Result is empty"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + const auto & opt_position = processed_result->s; + const auto & opt_velocity = processed_result->v; + + // Check Size + if (opt_position.size() == 1 && opt_velocity.front() < ZERO_VEL_THRESHOLD) { + auto output = stop_traj_points; + output.at(closest_idx).longitudinal_velocity_mps = data.v0; + for (size_t i = closest_idx + 1; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + } + prev_output_ = output; + return output; + } else if (opt_position.size() == 1) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Optimized Trajectory is too small"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Get Zero Velocity Position + double closest_stop_dist = std::numeric_limits::max(); + for (size_t i = 0; i < opt_velocity.size(); ++i) { + if (opt_velocity.at(i) < ZERO_VEL_THRESHOLD) { + closest_stop_dist = std::min(closest_stop_dist, opt_position.at(i)); + break; + } + } + const auto traj_stop_dist = + autoware::motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx); + if (traj_stop_dist) { + closest_stop_dist = std::min(*traj_stop_dist + traj_front_to_vehicle_offset, closest_stop_dist); + } + + // Resample Optimum Velocity + size_t break_id = stop_traj_points.size(); + std::vector resampled_opt_position; + for (size_t i = closest_idx; i < stop_traj_points.size(); ++i) { + const double query_s = std::max( + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); + if (query_s > opt_position.back()) { + break_id = i; + break; + } + resampled_opt_position.push_back(query_s); + } + // resample optimum velocity for original each position + auto resampled_opt_velocity = + autoware::interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); + for (size_t i = break_id; i < stop_traj_points.size(); ++i) { + resampled_opt_velocity.push_back(stop_traj_points.at(i).longitudinal_velocity_mps); + } + + // Create Output Data + std::vector output = stop_traj_points; + for (size_t i = 0; i < closest_idx; ++i) { + output.at(i).longitudinal_velocity_mps = data.v0; + } + for (size_t i = closest_idx; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = + resampled_opt_velocity.at(i - closest_idx) + velocity_margin_; + } + output.back().longitudinal_velocity_mps = 0.0; // terminal velocity is zero + + // Insert Closest Stop Point + autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); + + prev_output_ = output; + return output; +} + +std::vector OptimizationBasedPlanner::createTimeVector() +{ + std::vector time_vec; + for (double t = 0.0; t < dense_time_horizon_; t += dense_resampling_time_interval_) { + time_vec.push_back(t); + } + if (dense_time_horizon_ - time_vec.back() < 1e-3) { + time_vec.back() = dense_time_horizon_; + } else { + time_vec.push_back(dense_time_horizon_); + } + + for (double t = dense_time_horizon_ + sparse_resampling_time_interval_; t < max_time_horizon_; + t += sparse_resampling_time_interval_) { + time_vec.push_back(t); + } + if (max_time_horizon_ - time_vec.back() < 1e-3) { + time_vec.back() = max_time_horizon_; + } else { + time_vec.push_back(max_time_horizon_); + } + + return time_vec; +} + +// v0, a0 +std::tuple OptimizationBasedPlanner::calcInitialMotion( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & prev_traj_points) +{ + const auto & ego_vel = planner_data->current_odometry.twist.twist.linear.x; + const auto & ego_pose = planner_data->current_odometry.pose.pose; + const auto & input_traj_points = stop_traj_points; + const double vehicle_speed{std::abs(ego_vel)}; + const auto current_closest_point = autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(input_traj_points), + planner_data->current_odometry.pose.pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)}; + + double initial_vel{}; + double initial_acc{}; + + // first time + if (prev_traj_points.empty()) { + initial_vel = vehicle_speed; + initial_acc = 0.0; + return std::make_tuple(initial_vel, initial_acc); + } + + TrajectoryPoint prev_output_closest_point; + if (smoothed_trajectory_ptr_) { + prev_output_closest_point = autoware::motion_utils::calcInterpolatedPoint( + *smoothed_trajectory_ptr_, ego_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + } else { + prev_output_closest_point = autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(prev_traj_points), ego_pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); + } + + // when velocity tracking deviation is large + const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps}; + const double vel_error{vehicle_speed - std::abs(desired_vel)}; + if (std::abs(vel_error) > replan_vel_deviation_) { + initial_vel = vehicle_speed; // use current vehicle speed + initial_acc = prev_output_closest_point.acceleration_mps2; + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : Large deviation error for speed control. Use current speed for " + "initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f", + desired_vel, vehicle_speed, vel_error, replan_vel_deviation_); + return std::make_tuple(initial_vel, initial_acc); + } + + // if current vehicle velocity is low && base_desired speed is high, + // use engage_velocity for engage vehicle + const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_; + if (vehicle_speed < engage_vel_thr) { + if (target_vel >= engage_velocity_) { + const auto stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( + input_traj_points, ego_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + if (!stop_dist.has_value()) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " + "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f)", + vehicle_speed, target_vel, engage_velocity_, engage_vel_thr); + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else if (stop_dist.value() > stop_dist_to_prohibit_engage_) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " + "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", + vehicle_speed, target_vel, engage_velocity_, engage_vel_thr, stop_dist.value()); + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist.value()); + } + } else if (target_vel > 0.0) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), clock, 3000, + "calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ", + target_vel, engage_velocity_); + } + } + + // normal update: use closest in prev_output + initial_vel = prev_output_closest_point.longitudinal_velocity_mps; + initial_acc = prev_output_closest_point.acceleration_mps2; + return std::make_tuple(initial_vel, initial_acc); +} + +bool OptimizationBasedPlanner::checkHasReachedGoal( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points) +{ + // If goal is close and current velocity is low, we don't optimize trajectory + const auto closest_stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( + stop_traj_points, planner_data->current_odometry.pose.pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); + if ( + closest_stop_dist && *closest_stop_dist < 0.5 && + planner_data->current_odometry.twist.twist.linear.x < 0.6) { + return true; + } + + return false; +} + +std::optional OptimizationBasedPlanner::getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, const std::vector & time_vec) +{ + if (stop_traj_points.empty()) { + return std::nullopt; + } + + const auto traj_length = calcTrajectoryLengthFromCurrentPose( + stop_traj_points, planner_data->current_odometry.pose.pose, planner_data); + if (!traj_length) { + return {}; + } + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = *traj_length; + } + + double min_slow_down_point_length = std::numeric_limits::max(); + std::optional min_slow_down_idx = {}; + for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) { + const auto & obj = obstacles.at(o_idx); + if (obj.collision_points.empty()) { + continue; + } + + // Step1 Get S boundary from the obstacle + const auto obj_s_boundaries = + getSBoundaries(planner_data, stop_traj_points, obj, time_vec, *traj_length); + if (!obj_s_boundaries) { + continue; + } + + // Step2 update s boundaries + for (size_t i = 0; i < obj_s_boundaries->size(); ++i) { + if (obj_s_boundaries->at(i).max_s < s_boundaries.at(i).max_s) { + s_boundaries.at(i) = obj_s_boundaries->at(i); + } + } + + // Step3 search nearest obstacle to follow for rviz marker + const double obj_vel = std::abs(obj.velocity); + const double rss_dist = + calc_rss_distance(planner_data->current_odometry.twist.twist.linear.x, obj_vel); + + const auto & safe_distance_margin = cruise_planning_param_.safe_distance_margin; + const double ego_obj_length = autoware::motion_utils::calcSignedArcLength( + stop_traj_points, planner_data->current_odometry.pose.pose.position, + obj.collision_points.front().point); + const double slow_down_point_length = ego_obj_length - (rss_dist + safe_distance_margin); + + if (slow_down_point_length < min_slow_down_point_length) { + min_slow_down_point_length = slow_down_point_length; + min_slow_down_idx = o_idx; + } + } + + // Publish wall marker for slowing down or stopping + if (min_slow_down_idx) { + const auto & current_time = clock_->now(); + + const auto marker_pose = autoware::motion_utils::calcLongitudinalOffsetPose( + stop_traj_points, planner_data->current_odometry.pose.pose.position, + min_slow_down_point_length); + + if (marker_pose) { + MarkerArray wall_msg; + + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + marker_pose.value(), "obstacle to follow", current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &wall_msg); + + // publish rviz marker + debug_wall_marker_pub_->publish(wall_msg); + } + } + + return s_boundaries; +} + +std::optional OptimizationBasedPlanner::getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, const CruiseObstacle & object, + const std::vector & time_vec, const double traj_length) +{ + if (object.collision_points.empty()) { + return {}; + } + + const bool onEgoTrajectory = + checkOnTrajectory(planner_data, stop_traj_points, object.collision_points.front()); + const auto & safe_distance_margin = cruise_planning_param_.safe_distance_margin; + + // If the object is on the current ego trajectory, + // we assume the object travels along ego trajectory + if (onEgoTrajectory) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "On Trajectory Object"); + + return getSBoundariesForOnTrajectoryObject( + stop_traj_points, planner_data, time_vec, safe_distance_margin, object, traj_length); + } + + // Ignore low velocity objects that are not on the trajectory + return getSBoundariesForOffTrajectoryObject( + stop_traj_points, planner_data, time_vec, safe_distance_margin, object, traj_length); +} + +std::optional OptimizationBasedPlanner::getSBoundariesForOnTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) const +{ + const double & min_object_accel_for_rss = cruise_planning_param_.min_object_accel_for_rss; + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = traj_length; + } + + const double v_obj = std::abs(object.velocity); + + const double dist_to_collision_point = calc_distance_to_collisionPoint( + traj_points, planner_data, object.collision_points.front().point); + + double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); + const double current_v_obj = v_obj; + const double initial_s_upper_bound = + current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + s_boundaries.front().max_s = std::clamp(initial_s_upper_bound, 0.0, s_boundaries.front().max_s); + s_boundaries.front().is_object = true; + for (size_t i = 1; i < time_vec.size(); ++i) { + const double dt = time_vec.at(i) - time_vec.at(i - 1); + current_s_obj = current_s_obj + current_v_obj * dt; + + const double s_upper_bound = + current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + s_boundaries.at(i).max_s = std::clamp(s_upper_bound, 0.0, s_boundaries.at(i).max_s); + s_boundaries.at(i).is_object = true; + } + + return s_boundaries; +} + +std::optional OptimizationBasedPlanner::getSBoundariesForOffTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) +{ + const auto & current_time = clock_->now(); + const double & min_object_accel_for_rss = cruise_planning_param_.min_object_accel_for_rss; + + const double v_obj = std::abs(object.velocity); + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = traj_length; + } + + for (const auto & collision_point : object.collision_points) { + const double object_time = (rclcpp::Time(collision_point.stamp) - current_time).seconds(); + if (object_time < 0) { + // Ignore Past Positions + continue; + } + + const double dist_to_collision_point = + calc_distance_to_collisionPoint(traj_points, planner_data, collision_point.point); + if (!dist_to_collision_point) { + continue; + } + + const double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); + const double s_upper_bound = + current_s_obj + (v_obj * v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + + size_t object_time_segment_idx = 0; + for (size_t i = 0; i < time_vec.size() - 1; ++i) { + if (time_vec.at(i) < object_time && object_time < time_vec.at(i + 1)) { + object_time_segment_idx = i; + break; + } + } + + for (size_t i = 0; i <= object_time_segment_idx + 1; ++i) { + if (time_vec.at(i) < object_time && s_upper_bound < s_boundaries.at(i).max_s) { + s_boundaries.at(i).max_s = std::max(0.0, s_upper_bound); + s_boundaries.at(i).is_object = true; + } + } + } + + return s_boundaries; +} + +bool OptimizationBasedPlanner::checkOnTrajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const polygon_utils::PointWithStamp & point) +{ + // If the collision point is in the future, we return false + if ((rclcpp::Time(point.stamp) - clock_->now()).seconds() > 0.1) { + return false; + } + + const double lateral_offset = + std::fabs(autoware::motion_utils::calcLateralOffset(stop_traj_points, point.point)); + + if (lateral_offset < planner_data->vehicle_info_.max_lateral_offset_m + 0.1) { + return true; + } + + return false; +} + +std::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose, + const std::shared_ptr planner_data) +{ + const size_t ego_segment_idx = planner_data->find_segment_index(traj_points, ego_pose); + + const double traj_length = autoware::motion_utils::calcSignedArcLength( + traj_points, ego_pose.position, ego_segment_idx, traj_points.size() - 1); + + const auto dist_to_closest_stop_point = autoware::motion_utils::calcDistanceToForwardStopPoint( + traj_points, ego_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + if (dist_to_closest_stop_point) { + return std::min(traj_length, dist_to_closest_stop_point.value()); + } + + return traj_length; +} + +std::optional +OptimizationBasedPlanner::processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset) +{ + if ( + opt_result.t.empty() || opt_result.s.empty() || opt_result.v.empty() || opt_result.a.empty() || + opt_result.j.empty()) { + return std::nullopt; + } + + size_t break_id = opt_result.s.size(); + VelocityOptimizer::OptimizationResult processed_result; + processed_result.t.push_back(0.0); + processed_result.s.push_back(offset); + processed_result.v.push_back(v0); + processed_result.a.push_back(opt_result.a.front()); + processed_result.j.push_back(opt_result.j.front()); + + for (size_t i = 1; i < opt_result.s.size(); ++i) { + const double prev_s = processed_result.s.back(); + const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; + const double current_v = opt_result.v.at(i); + if (prev_s >= current_s) { + processed_result.v.back() = 0.0; + break_id = i; + break; + } else if (current_v < ZERO_VEL_THRESHOLD) { + break_id = i; + break; + } else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) { + processed_result.v.back() = current_v; + processed_result.s.back() = current_s; + continue; + } + + processed_result.t.push_back(opt_result.t.at(i)); + processed_result.s.push_back(current_s); + processed_result.v.push_back(current_v); + processed_result.a.push_back(opt_result.a.at(i)); + processed_result.j.push_back(opt_result.j.at(i)); + } + + // Padding Zero Velocity after break id + for (size_t i = break_id; i < opt_result.s.size(); ++i) { + const double prev_s = processed_result.s.back(); + const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; + if (prev_s >= current_s) { + processed_result.v.back() = 0.0; + continue; + } else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) { + processed_result.v.back() = 0.0; + processed_result.s.back() = current_s; + continue; + } + processed_result.t.push_back(opt_result.t.at(i)); + processed_result.s.push_back(current_s); + processed_result.v.push_back(0.0); + processed_result.a.push_back(0.0); + processed_result.j.push_back(0.0); + } + + return processed_result; +} + +void OptimizationBasedPlanner::publishDebugTrajectory( + [[maybe_unused]] const std::shared_ptr planner_data, const double offset, + const std::vector & time_vec, const SBoundaries & s_boundaries, + const VelocityOptimizer::OptimizationResult & opt_result) +{ + const auto & current_time = clock_->now(); + // Publish optimized result and boundary + Trajectory boundary_traj; + boundary_traj.header.stamp = current_time; + boundary_traj.points.resize(s_boundaries.size()); + double boundary_s_max = 0.0; + for (size_t i = 0; i < s_boundaries.size(); ++i) { + const double bound_s = s_boundaries.at(i).max_s; + const double bound_t = time_vec.at(i); + boundary_traj.points.at(i).pose.position.x = bound_s; + boundary_traj.points.at(i).pose.position.y = bound_t; + boundary_s_max = std::max(bound_s, boundary_s_max); + } + boundary_pub_->publish(boundary_traj); + + Trajectory optimized_sv_traj; + optimized_sv_traj.header.stamp = current_time; + optimized_sv_traj.points.resize(opt_result.s.size()); + for (size_t i = 0; i < opt_result.s.size(); ++i) { + const double s = opt_result.s.at(i); + const double v = opt_result.v.at(i); + optimized_sv_traj.points.at(i).pose.position.x = s + offset; + optimized_sv_traj.points.at(i).pose.position.y = v; + } + optimized_sv_pub_->publish(optimized_sv_traj); + + Trajectory optimized_st_graph; + optimized_st_graph.header.stamp = current_time; + optimized_st_graph.points.resize(opt_result.s.size()); + for (size_t i = 0; i < opt_result.s.size(); ++i) { + const double bound_s = opt_result.s.at(i); + const double bound_t = opt_result.t.at(i); + optimized_st_graph.points.at(i).pose.position.x = bound_s; + optimized_st_graph.points.at(i).pose.position.y = bound_t; + } + optimized_st_graph_pub_->publish(optimized_st_graph); +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.hpp new file mode 100644 index 0000000000000..efb924159d471 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.hpp @@ -0,0 +1,141 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +#define OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT + +#include "../cruise_planner_interface.hpp" +#include "../type_alias.hpp" +#include "../types.hpp" +#include "s_boundary.hpp" +#include "velocity_optimizer.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class OptimizationBasedPlanner : public CruisePlannerInterface +{ +public: + OptimizationBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param); + + std::vector plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) override; + + void update_parameters( + [[maybe_unused]] const std::vector & parameters) override + { + } + +private: + // Member Functions + std::vector createTimeVector(); + std::tuple calcInitialMotion( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & prev_traj_points); + + static bool checkHasReachedGoal( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points); + + std::optional getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, const std::vector & time_vec); + + std::optional getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, const CruiseObstacle & object, + const std::vector & time_vec, const double traj_length); + + std::optional getSBoundariesForOnTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) const; + + std::optional getSBoundariesForOffTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length); + + bool checkOnTrajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const polygon_utils::PointWithStamp & point); + + static std::optional calcTrajectoryLengthFromCurrentPose( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose, + const std::shared_ptr planner_data); + + geometry_msgs::msg::Pose transformBaseLink2Center( + const geometry_msgs::msg::Pose & pose_base_link); + + static std::optional processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset); + + void publishDebugTrajectory( + [[maybe_unused]] const std::shared_ptr planner_data, const double offset, + const std::vector & time_vec, const SBoundaries & s_boundaries, + const VelocityOptimizer::OptimizationResult & opt_result); + + std::vector prev_output_; + + // Velocity Optimizer + std::shared_ptr velocity_optimizer_ptr_; + + // Publisher + rclcpp::Publisher::SharedPtr boundary_pub_; + rclcpp::Publisher::SharedPtr optimized_sv_pub_; + rclcpp::Publisher::SharedPtr optimized_st_graph_pub_; + rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; + + // Subscriber + rclcpp::Subscription::SharedPtr smoothed_traj_sub_; + + Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr}; + + // Resampling Parameter + double dense_resampling_time_interval_; + double sparse_resampling_time_interval_; + double dense_time_horizon_; + double max_time_horizon_; + + double t_dangerous_; + double velocity_margin_; + + double replan_vel_deviation_; + double engage_velocity_; + double engage_acceleration_; + double engage_exit_ratio_; + double stop_dist_to_prohibit_engage_; +}; +} // namespace autoware::motion_velocity_planner +// clang-format off +#endif // OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +// clang-format on diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/s_boundary.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/s_boundary.hpp new file mode 100644 index 0000000000000..66d1ddfdc2b4e --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/s_boundary.hpp @@ -0,0 +1,36 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ +#define OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ + +#include +#include + +namespace autoware::motion_velocity_planner +{ +class SBoundary +{ +public: + SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {} + SBoundary() : max_s(std::numeric_limits::max()), min_s(0.0) {} + + double max_s = std::numeric_limits::max(); + double min_s = 0.0; + bool is_object = false; +}; + +using SBoundaries = std::vector; +} // namespace autoware::motion_velocity_planner + +#endif // OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp new file mode 100644 index 0000000000000..d8e8c1aff5474 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp @@ -0,0 +1,287 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "velocity_optimizer.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +VelocityOptimizer::VelocityOptimizer( + const double max_s_weight, const double max_v_weight, const double over_s_safety_weight, + const double over_s_ideal_weight, const double over_v_weight, const double over_a_weight, + const double over_j_weight) +: max_s_weight_(max_s_weight), + max_v_weight_(max_v_weight), + over_s_safety_weight_(over_s_safety_weight), + over_s_ideal_weight_(over_s_ideal_weight), + over_v_weight_(over_v_weight), + over_a_weight_(over_a_weight), + over_j_weight_(over_j_weight) +{ + qp_solver_.updateMaxIter(200000); + qp_solver_.updateRhoInterval(0); // 0 means automatic + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const OptimizationData & data) +{ + const std::vector time_vec = data.time_vec; + const size_t N = time_vec.size(); + const double s0 = data.s0; + const double v0 = data.v0; + const double a0 = data.a0; + const double v_max = std::max(data.v_max, 0.1); + const double a_max = data.a_max; + const double a_min = data.a_min; + const double limit_a_max = data.limit_a_max; + const double limit_a_min = data.limit_a_min; + const double limit_j_max = data.limit_j_max; + const double limit_j_min = data.limit_j_min; + const double j_max = data.j_max; + const double j_min = data.j_min; + const double a_range = std::max(a_max - a_min, 0.1); + const double j_range = std::max(j_max - j_min, 0.1); + const double t_dangerous = data.t_dangerous; + const double t_idling = data.idling_time; + const auto s_boundary = data.s_boundary; + + // Variables: s_i, v_i, a_i, j_i, over_s_safety_i, over_s_ideal_i, over_v_i, over_a_i, over_j_i + const int IDX_S0 = 0; + const int IDX_V0 = N; + const int IDX_A0 = 2 * N; + const int IDX_J0 = 3 * N; + const int IDX_OVER_S_SAFETY0 = 4 * N; + const int IDX_OVER_S_IDEAL0 = 5 * N; + const int IDX_OVER_V0 = 6 * N; + const int IDX_OVER_A0 = 7 * N; + const int IDX_OVER_J0 = 8 * N; + const int l_variables = 9 * N; + const int l_constraints = 7 * N + 3 * (N - 1) + 3; + + // the matrix size depends on constraint numbers. + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(l_constraints, l_variables); + std::vector lower_bound(l_constraints, 0.0); + std::vector upper_bound(l_constraints, 0.0); + + // Object Variables + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables); + std::vector q(l_variables, 0.0); + + // Object Function + // min w_s*(s_i - s_ideal_i)^2 + w_v * v0 * ((v_i - v_max)^2 / v_max^2) + // + over_s_safety^2 + over_s_ideal^2 + over_v_ideal^2 + over_a_ideal^2 + // + over_j_ideal^2 + constexpr double MINIMUM_MAX_S_BOUND = 0.01; + for (size_t i = 0; i < N; ++i) { + const double dt = + i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2); + const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND); + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + if (s_boundary.at(i).is_object) { + q.at(IDX_S0 + i) += -max_s_weight_ / max_s * dt; + q.at(IDX_V0 + i) += -max_s_weight_ / max_s * v_coeff * dt; + } else { + q.at(IDX_S0 + i) += -max_s_weight_ / max_s * dt; + } + + q.at(IDX_V0 + i) += -max_v_weight_ / v_max * dt; + } + + for (size_t i = 0; i < N; ++i) { + const double dt = + i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2); + const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND); + P(IDX_OVER_S_SAFETY0 + i, IDX_OVER_S_SAFETY0 + i) += + over_s_safety_weight_ / (max_s * max_s) * dt; + P(IDX_OVER_S_IDEAL0 + i, IDX_OVER_S_IDEAL0 + i) += over_s_ideal_weight_ / (max_s * max_s) * dt; + P(IDX_OVER_V0 + i, IDX_OVER_V0 + i) += over_v_weight_ / (v_max * v_max) * dt; + P(IDX_OVER_A0 + i, IDX_OVER_A0 + i) += over_a_weight_ / a_range * dt; + P(IDX_OVER_J0 + i, IDX_OVER_J0 + i) += over_j_weight_ / j_range * dt; + + if (s_boundary.at(i).is_object) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt; + P(IDX_V0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * v_coeff * dt; + P(IDX_S0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * dt; + P(IDX_V0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * dt; + } else { + P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt; + } + + P(IDX_V0 + i, IDX_V0 + i) += max_v_weight_ / (v_max * v_max) * dt; + } + + // Constraint + size_t constr_idx = 0; + + // Safety Position Constraint: s_boundary_min < s_i + v_i*t_dangerous + v0*v_i/(2*|a_min|) - + // over_s_safety_i < s_boundary_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_dangerous; + A(constr_idx, IDX_S0 + i) = 1.0; // s_i + if (s_boundary.at(i).is_object) { + A(constr_idx, IDX_V0 + i) = v_coeff; // v_i * (t_dangerous + v0/(2*|a_min|)) + } + A(constr_idx, IDX_OVER_S_SAFETY0 + i) = -1.0; // over_s_safety_i + upper_bound.at(constr_idx) = s_boundary.at(i).max_s; + lower_bound.at(constr_idx) = 0.0; + } + + // Ideal Position Constraint: s_boundary_min < s_i + v_i * t_idling + v0*v_i/(2*|a_min|) - + // over_s_ideal_i < s_boundary_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + A(constr_idx, IDX_S0 + i) = 1.0; // s_i + if (s_boundary.at(i).is_object) { + A(constr_idx, IDX_V0 + i) = v_coeff; // v_i * (t_idling + v0/(2*|a_min|)) + } + A(constr_idx, IDX_OVER_S_IDEAL0 + i) = -1.0; // over_s_ideal_i + upper_bound.at(constr_idx) = s_boundary.at(i).max_s; + lower_bound.at(constr_idx) = 0.0; + } + + // Soft Velocity Constraint: 0 < v_i - over_v_i < v_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_V0 + i) = 1.0; // v_i + A(constr_idx, IDX_OVER_V0 + i) = -1.0; // over_v_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : v_max; + lower_bound.at(constr_idx) = 0.0; + } + + // Soft Acceleration Constraint: a_min < a_i - over_a_i < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + A(constr_idx, IDX_OVER_A0 + i) = -1.0; // over_a_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_max; + lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_min; + } + + // Hard Acceleration Constraint: limit_a_min < a_i < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + upper_bound.at(constr_idx) = limit_a_max; + lower_bound.at(constr_idx) = limit_a_min; + } + + // Soft Jerk Constraint: j_min < j_i - over_j_i < j_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_J0 + i) = 1.0; // j_i + A(constr_idx, IDX_OVER_J0 + i) = -1.0; // over_j_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_max; + lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_min; + } + + // Hard Jerk Constraint: limit_j_min < j_i < limit_j_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_J0 + i) = 1.0; // j_i + upper_bound.at(constr_idx) = limit_j_max; + lower_bound.at(constr_idx) = limit_j_min; + } + + // Dynamic Constraint + // s_i+1 = s_i + v_i * dt + 0.5 * a_i * dt^2 + 1/6 * j_i * dt^3 + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_S0 + i + 1) = 1.0; // s_i+1 + A(constr_idx, IDX_S0 + i) = -1.0; // -s_i + A(constr_idx, IDX_V0 + i) = -dt; // -v_i*dt + A(constr_idx, IDX_A0 + i) = -0.5 * dt * dt; // -0.5 * a_i * dt^2 + A(constr_idx, IDX_J0 + i) = -1.0 / 6.0 * dt * dt * dt; // -1.0/6.0 * j_i * dt^3 + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // v_i+1 = v_i + a_i * dt + 0.5 * j_i * dt^2 + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_V0 + i + 1) = 1.0; // v_i+1 + A(constr_idx, IDX_V0 + i) = -1.0; // -v_i + A(constr_idx, IDX_A0 + i) = -dt; // -a_i * dt + A(constr_idx, IDX_J0 + i) = -0.5 * dt * dt; // -0.5 * j_i * dt^2 + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // a_i+1 = a_i + j_i * dt + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_A0 + i + 1) = 1.0; // a_i+1 + A(constr_idx, IDX_A0 + i) = -1.0; // -a_i + A(constr_idx, IDX_J0 + i) = -dt; // -j_i * dt + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // initial condition + { + A(constr_idx, IDX_S0) = 1.0; // s0 + upper_bound[constr_idx] = s0; + lower_bound[constr_idx] = s0; + ++constr_idx; + + A(constr_idx, IDX_V0) = 1.0; // v0 + upper_bound[constr_idx] = v0; + lower_bound[constr_idx] = v0; + ++constr_idx; + + A(constr_idx, IDX_A0) = 1.0; // a0 + upper_bound[constr_idx] = a0; + lower_bound[constr_idx] = a0; + } + + // execute optimization + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + const std::vector optval = std::get<0>(result); + + const int status_val = std::get<3>(result); + if (status_val != 1) + std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl; + + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) std::cerr << "optimization failed : result contains NaN values\n"; + + OptimizationResult optimized_result; + const auto is_optimization_failed = status_val != 1 || has_nan; + if (!is_optimization_failed) { + std::vector opt_time = time_vec; + std::vector opt_pos(N); + std::vector opt_vel(N); + std::vector opt_acc(N); + std::vector opt_jerk(N); + for (size_t i = 0; i < N; ++i) { + opt_pos.at(i) = optval.at(IDX_S0 + i); + opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0); + opt_acc.at(i) = optval.at(IDX_A0 + i); + opt_jerk.at(i) = optval.at(IDX_J0 + i); + } + opt_vel.back() = 0.0; + + optimized_result.t = opt_time; + optimized_result.s = opt_pos; + optimized_result.v = opt_vel; + optimized_result.a = opt_acc; + optimized_result.j = opt_jerk; + } + + return optimized_result; +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.hpp new file mode 100644 index 0000000000000..36f6d9f75fb99 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.hpp @@ -0,0 +1,77 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ +#define OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ + +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "s_boundary.hpp" + +#include + +namespace autoware::motion_velocity_planner +{ +class VelocityOptimizer +{ +public: + struct OptimizationData + { + std::vector time_vec; + double s0; + double v0; + double a0; + double v_max; + double a_max; + double a_min; + double limit_a_max; + double limit_a_min; + double limit_j_max; + double limit_j_min; + double j_max; + double j_min; + double t_dangerous; + double idling_time; + SBoundaries s_boundary; + }; + + struct OptimizationResult + { + std::vector t; + std::vector s; + std::vector v; + std::vector a; + std::vector j; + }; + + VelocityOptimizer( + const double max_s_weight, const double max_v_weight, const double over_s_safety_weight, + const double over_s_ideal_weight, const double over_v_weight, const double over_a_weight, + const double over_j_weight); + + OptimizationResult optimize(const OptimizationData & data); + +private: + // Parameter + double max_s_weight_; + double max_v_weight_; + double over_s_safety_weight_; + double over_s_ideal_weight_; + double over_v_weight_; + double over_a_weight_; + double over_j_weight_; + + // QPSolver + autoware::osqp_interface::OSQPInterface qp_solver_; +}; +} // namespace autoware::motion_velocity_planner +#endif // OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp new file mode 100644 index 0000000000000..af50fb0d4325a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp @@ -0,0 +1,159 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PARAMETERS_HPP_ +#define PARAMETERS_HPP_ + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +struct CommonParam +{ + double max_accel{}; + double min_accel{}; + double max_jerk{}; + double min_jerk{}; + double limit_max_accel{}; + double limit_min_accel{}; + double limit_max_jerk{}; + double limit_min_jerk{}; + + CommonParam() = default; + explicit CommonParam(rclcpp::Node & node) + { + max_accel = getOrDeclareParameter(node, "normal.max_acc"); + min_accel = getOrDeclareParameter(node, "normal.min_acc"); + max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); + min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); + limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); + limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); + limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + } +}; + +struct ObstacleFilteringParam +{ + std::vector inside_object_types{}; + std::vector outside_object_types{}; + // bool use_pointcloud{}; + + double max_lat_margin{}; + + double crossing_obstacle_velocity_threshold{}; + double crossing_obstacle_traj_angle_threshold{}; + + double outside_obstacle_velocity_threshold{}; + double ego_obstacle_overlap_time_threshold{}; + double max_prediction_time_for_collision_check{}; + double max_lateral_time_margin{}; + int num_of_predicted_paths_for_outside_cruise_obstacle{}; + + bool enable_yield{}; + double yield_lat_distance_threshold{}; + double max_lat_dist_between_obstacles{}; + double max_obstacles_collision_time{}; + double stopped_obstacle_velocity_threshold{}; + + double obstacle_velocity_threshold_from_cruise{}; + double obstacle_velocity_threshold_to_cruise{}; + + ObstacleFilteringParam() = default; + explicit ObstacleFilteringParam(rclcpp::Node & node) + { + inside_object_types = + utils::get_target_object_type(node, "obstacle_cruise.obstacle_filtering.object_type.inside."); + outside_object_types = utils::get_target_object_type( + node, "obstacle_cruise.obstacle_filtering.object_type.outside."); + // use_pointcloud = getOrDeclareParameter( + // node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud"); + + max_lat_margin = + getOrDeclareParameter(node, "obstacle_cruise.obstacle_filtering.max_lat_margin"); + + crossing_obstacle_velocity_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.crossing_obstacle.obstacle_velocity_threshold"); + crossing_obstacle_traj_angle_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold"); + + outside_obstacle_velocity_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.outside_obstacle.obstacle_velocity_threshold"); + ego_obstacle_overlap_time_threshold = getOrDeclareParameter( + node, + "obstacle_cruise.obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold"); + max_prediction_time_for_collision_check = getOrDeclareParameter( + node, + "obstacle_cruise.obstacle_filtering.outside_obstacle.max_prediction_time_for_collision_" + "check"); + max_lateral_time_margin = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.outside_obstacle.max_lateral_time_margin"); + num_of_predicted_paths_for_outside_cruise_obstacle = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.outside_obstacle.num_of_predicted_paths"); + enable_yield = + getOrDeclareParameter(node, "obstacle_cruise.obstacle_filtering.yield.enable_yield"); + yield_lat_distance_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.lat_distance_threshold"); + max_lat_dist_between_obstacles = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.max_lat_dist_between_obstacles"); + max_obstacles_collision_time = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.max_obstacles_collision_time"); + stopped_obstacle_velocity_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.stopped_obstacle_velocity_threshold"); + obstacle_velocity_threshold_from_cruise = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.obstacle_velocity_threshold_from_cruise"); + obstacle_velocity_threshold_to_cruise = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.obstacle_velocity_threshold_to_cruise"); + } +}; + +struct CruisePlanningParam +{ + double idling_time{}; + double min_ego_accel_for_rss{}; + double min_object_accel_for_rss{}; + double safe_distance_margin{}; + + CruisePlanningParam() = default; + explicit CruisePlanningParam(rclcpp::Node & node) + { + idling_time = + getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.idling_time"); + min_ego_accel_for_rss = + getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.min_ego_accel_for_rss"); + min_object_accel_for_rss = getOrDeclareParameter( + node, "obstacle_cruise.cruise_planning.min_object_accel_for_rss"); + safe_distance_margin = + getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.safe_distance_margin"); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // PARAMETERS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/cruise_planning_debug_info.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/cruise_planning_debug_info.hpp new file mode 100644 index 0000000000000..7d74b602d85dc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/cruise_planning_debug_info.hpp @@ -0,0 +1,100 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#define PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; + +class CruisePlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // cruise planning + CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, // index: 3 + CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, + CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + CRUISE_TARGET_OBSTACLE_DISTANCE, + CRUISE_ERROR_DISTANCE_RAW, + CRUISE_ERROR_DISTANCE_FILTERED, + + CRUISE_RELATIVE_EGO_VELOCITY, // index: 10 + CRUISE_TIME_TO_COLLISION, + + CRUISE_TARGET_VELOCITY, // index: 12 + CRUISE_TARGET_ACCELERATION, + CRUISE_TARGET_JERK_RATIO, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + static int getIndex(const TYPE type) { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convert_to_message(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp new file mode 100644 index 0000000000000..6bada506b977b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp @@ -0,0 +1,774 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pid_based_planner.hpp" + +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" + +#include "tier4_planning_msgs/msg/velocity_limit.hpp" + +#include +#include +#include +#include + +using autoware::signal_processing::LowpassFilter1d; +namespace autoware::motion_velocity_planner +{ + +namespace +{ +VelocityLimit create_velocity_limit_message( + const rclcpp::Time & current_time, const double vel, const double acc, const double max_jerk, + const double min_jerk) +{ + VelocityLimit msg; + msg.stamp = current_time; + msg.sender = "obstacle_cruise"; + msg.use_constraints = true; + + msg.max_velocity = vel; + if (acc < 0) { + msg.constraints.min_acceleration = acc; + } + msg.constraints.max_jerk = max_jerk; + msg.constraints.min_jerk = min_jerk; + + return msg; +} + +template +T getSign(const T val) +{ + if (0 < val) { + return static_cast(1); + } else if (val < 0) { + return static_cast(-1); + } + return static_cast(0); +} +} // namespace + +PIDBasedPlanner::PIDBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param) +: CruisePlannerInterface(node, common_param, cruise_planning_param) +{ + min_accel_during_cruise_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.min_accel_during_cruise"); + + use_velocity_limit_based_planner_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.use_velocity_limit_based_planner"); + + { // velocity limit based planner + const double kp = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kp"); + const double ki = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.ki"); + const double kd = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kd"); + velocity_limit_based_planner_param_.pid_vel_controller = + std::make_unique(kp, ki, kd); + + velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.output_ratio_" + "during_accel"); + velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.vel_to_acc_" + "weight"); + + velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.enable_" + "jerk_limit_to_output_acc"); + + velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.disable_" + "target_acceleration"); + } + + { // velocity insertion based planner + // pid controller for acc + const double kp_acc = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_acc"); + const double ki_acc = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_acc"); + const double kd_acc = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_acc"); + velocity_insertion_based_planner_param_.pid_acc_controller = + std::make_unique(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + const double kp_jerk = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_jerk"); + const double ki_jerk = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_jerk"); + const double kd_jerk = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_jerk"); + velocity_insertion_based_planner_param_.pid_jerk_controller = + std::make_unique(kp_jerk, ki_jerk, kd_jerk); + + velocity_insertion_based_planner_param_.output_acc_ratio_during_accel = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "acc_ratio_during_accel"); + velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "jerk_ratio_during_accel"); + + velocity_insertion_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.enable_" + "jerk_limit_to_output_acc"); + } + + min_cruise_target_vel_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.min_cruise_target_vel"); + time_to_evaluate_rss_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.time_to_evaluate_rss"); + const auto error_function_type = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.error_function_type"); + error_func_ = [&]() -> std::function { + if (error_function_type == "linear") { + return [](const double val) { return val; }; + } else if (error_function_type == "quadratic") { + return [](const double val) { return getSign(val) * std::pow(val, 2); }; + } + throw std::domain_error("error function type is not supported"); + }(); + + // low pass filter + const double lpf_normalized_error_cruise_dist_gain = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.lpf_normalized_error_cruise_dist_gain"); + lpf_normalized_error_cruise_dist_ptr_ = + std::make_shared(lpf_normalized_error_cruise_dist_gain); + lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); + lpf_obstacle_vel_ptr_ = std::make_shared(0.5); + lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_vel_ptr_ = std::make_shared(0.5); + lpf_output_acc_ptr_ = std::make_shared(0.5); + lpf_output_jerk_ptr_ = std::make_shared(0.5); +} + +std::vector PIDBasedPlanner::plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) +{ + cruise_planning_debug_info_.reset(); + + // calc obstacles to cruise + const auto cruise_obstacle_info = + calc_obstacle_to_cruise(stop_traj_points, planner_data, obstacles); + + // plan cruise + const auto cruise_traj_points = plan_cruise_trajectory( + planner_data, stop_traj_points, debug_data_ptr, planning_factor_interface, velocity_limit, + cruise_obstacle_info); + + prev_traj_points_ = cruise_traj_points; + return cruise_traj_points; +} + +std::optional PIDBasedPlanner::calc_obstacle_to_cruise( + const std::vector & stop_traj_points, + const std::shared_ptr planner_data, + const std::vector & obstacles) +{ + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, + planner_data->current_odometry.twist.twist.linear.x); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, + planner_data->current_acceleration.accel.accel.linear.x); + + auto modified_target_obstacles = obstacles; // TODO(murooka) what is this variable? + + // search highest probability obstacle for cruise + std::optional cruise_obstacle_info; + for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) { + const auto & obstacle = obstacles.at(o_idx); + + if (obstacle.collision_points.empty()) { + continue; + } + + // NOTE: from ego's front to obstacle's back + // TODO(murooka) this is not dist to obstacle + const double dist_to_obstacle = + calc_distance_to_collisionPoint( + stop_traj_points, planner_data, obstacle.collision_points.front().point) + + (obstacle.velocity - planner_data->current_odometry.twist.twist.linear.x) * + time_to_evaluate_rss_; + + // calculate distance between ego and obstacle based on RSS + const double target_dist_to_obstacle = calc_rss_distance( + planner_data->current_odometry.twist.twist.linear.x, obstacle.velocity, + cruise_planning_param_.safe_distance_margin); + + // calculate error distance and normalized one + const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle; + if (cruise_obstacle_info) { + if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { + continue; + } + } + cruise_obstacle_info = + CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle); + } + + if (!cruise_obstacle_info) { // if obstacle to cruise was not found + lpf_dist_to_obstacle_ptr_->reset(); + lpf_obstacle_vel_ptr_->reset(); + lpf_error_cruise_dist_ptr_->reset(); + return {}; + } + + // if obstacle to cruise was found + { // debug data + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY, + planner_data->current_odometry.twist.twist.linear.x - + cruise_obstacle_info->obstacle.velocity); + const double non_zero_relative_vel = [&]() { + const double relative_vel = planner_data->current_odometry.twist.twist.linear.x - + cruise_obstacle_info->obstacle.velocity; + constexpr double epsilon = 0.001; + if (epsilon < std::abs(relative_vel)) { + return relative_vel; + } + return getSign(relative_vel) * epsilon; + }(); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION, + cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel); + } + + { // dist to obstacle + const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const double filtered_dist_to_obstacle = + lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + filtered_dist_to_obstacle); + + cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle; + } + + { // obstacle velocity + const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity; + const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + filtered_obstacle_velocity); + + cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity; + } + + { // error distance for cruising + const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double filtered_error_cruise_dist = + lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist); + + cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist; + } + + { // target dist for cruising + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE, + cruise_obstacle_info->target_dist_to_obstacle); + } + + return cruise_obstacle_info; +} + +std::vector PIDBasedPlanner::plan_cruise_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit, + const std::optional & cruise_obstacle_info) +{ + // do cruise + if (cruise_obstacle_info) { + RCLCPP_DEBUG(logger_, "cruise planning"); + + { // update debug marker + // virtual wall marker for cruise + const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const size_t ego_idx = + planner_data->find_index(stop_traj_points, planner_data->current_odometry.pose.pose); + const double abs_ego_offset = + planner_data->is_driving_forward + ? std::abs(planner_data->vehicle_info_.max_longitudinal_offset_m) + : std::abs(planner_data->vehicle_info_.min_longitudinal_offset_m); + const double dist_to_rss_wall = + std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + const size_t wall_idx = + utils::get_index_with_longitudinal_offset(stop_traj_points, dist_to_rss_wall, ego_idx); + + const std::string wall_reason_string = cruise_obstacle_info->obstacle.is_yield_obstacle + ? "obstacle cruise (yield)" + : "obstacle cruise"; + auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + stop_traj_points.at(wall_idx).pose, wall_reason_string, clock_->now(), 0); + // NOTE: use a different color from slow down one to visualize cruise and slow down + // separately. + markers.markers.front().color = + autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr->cruise_wall_marker); + + // cruise obstacle + debug_data_ptr->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + + planning_factor_interface->add( + stop_traj_points, planner_data->current_odometry.pose.pose, + stop_traj_points.at(wall_idx).pose, PlanningFactor::NONE, SafetyFactorArray{}); + } + + // do cruise planning + if (!use_velocity_limit_based_planner_) { + const auto cruise_traj = + plan_cruise_with_trajectory(planner_data, stop_traj_points, *cruise_obstacle_info); + return cruise_traj; + } + + velocity_limit = plan_cruise_with_velocity_limit(planner_data, *cruise_obstacle_info); + return stop_traj_points; + } + + // reset previous cruise data if adaptive cruise is not enabled + prev_target_acc_ = {}; + lpf_normalized_error_cruise_dist_ptr_->reset(); + lpf_output_acc_ptr_->reset(); + lpf_output_vel_ptr_->reset(); + lpf_output_jerk_ptr_->reset(); + + // delete marker + const auto markers = + autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(clock_->now(), 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr->cruise_wall_marker); + + return stop_traj_points; +} + +VelocityLimit PIDBasedPlanner::plan_cruise_with_velocity_limit( + const std::shared_ptr planner_data, + const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_limit_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); + + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist); + const double additional_vel = [&]() { + if (modified_error_cruise_dist > 0) { + return pid_output_vel * p.output_ratio_during_accel; + } + return pid_output_vel; + }(); + + const double positive_target_vel = lpf_output_vel_ptr_->filter(std::max( + min_cruise_target_vel_, planner_data->current_odometry.twist.twist.linear.x + additional_vel)); + + // calculate target acceleration + const double target_acc = [&]() { + if (p.disable_target_acceleration) { + return min_accel_during_cruise_; + } + + double target_acc = p.vel_to_acc_weight * additional_vel; + + // apply acc limit + target_acc = + std::clamp(target_acc, min_accel_during_cruise_, common_param_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return lpf_output_acc_ptr_->filter(target_acc); + } + + if (p.enable_jerk_limit_to_output_acc) { // apply jerk limit + const double jerk = (target_acc - *prev_target_acc_) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = std::clamp(jerk, common_param_.min_jerk, common_param_.max_jerk); + target_acc = *prev_target_acc_ + limited_jerk * 0.1; + } + + return lpf_output_acc_ptr_->filter(target_acc); + }(); + prev_target_acc_ = target_acc; + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + + RCLCPP_DEBUG(logger_, "target_velocity %f", positive_target_vel); + + // set target longitudinal motion + const auto velocity_limit = create_velocity_limit_message( + clock_->now(), positive_target_vel, target_acc, common_param_.max_jerk, common_param_.min_jerk); + + return velocity_limit; +} + +std::vector PIDBasedPlanner::plan_cruise_with_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_insertion_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); + + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + // calculate target acceleration + const double target_acc = [&]() { + double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_acc *= p.output_acc_ratio_during_accel; + } + + target_acc = + std::clamp(target_acc, min_accel_during_cruise_, common_param_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return target_acc; + } + + if (p.enable_jerk_limit_to_output_acc) { + const double jerk = (target_acc - *prev_target_acc_) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = std::clamp(jerk, common_param_.min_jerk, common_param_.max_jerk); + target_acc = *prev_target_acc_ + limited_jerk * 0.1; + } + + return target_acc; + }(); + prev_target_acc_ = target_acc; + + const double target_jerk_ratio = [&]() { + double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_jerk_ratio *= p.output_jerk_ratio_during_accel; + } + + target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); + return target_jerk_ratio; + }(); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio); + + // set target longitudinal motion + const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { + // if (smoothed_trajectory_ptr_) { + // return autoware::motion_utils::calcInterpolatedPoint( + // *smoothed_trajectory_ptr_, planner_data->current_odometry.pose.pose, + // nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + // } + return autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(prev_traj_points_), + planner_data->current_odometry.pose.pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + }(); + const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; + const double a0 = prev_traj_closest_point.acceleration_mps2; + + auto cruise_traj_points = get_acceleration_limited_trajectory( + stop_traj_points, planner_data->current_odometry.pose.pose, v0, a0, target_acc, + target_jerk_ratio, planner_data); + + const auto zero_vel_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(cruise_traj_points); + if (!zero_vel_idx_opt) { + return cruise_traj_points; + } + + for (size_t i = zero_vel_idx_opt.value(); i < cruise_traj_points.size(); ++i) { + cruise_traj_points.at(i).longitudinal_velocity_mps = 0.0; + } + + return cruise_traj_points; +} + +std::vector PIDBasedPlanner::get_acceleration_limited_trajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & start_pose, + const double v0, const double a0, const double target_acc, const double target_jerk_ratio, + const std::shared_ptr planner_data) const +{ + // calculate vt function + const auto vt = [&]( + const double v0, const double a0, const double jerk, const double t, + const double target_acc) { + const double t1 = (target_acc - a0) / jerk; + + const double v = [&]() { + if (t < t1) { + return v0 + a0 * t + jerk * t * t / 2.0; + } + + const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; + return v1 + target_acc * (t - t1); + }(); + + constexpr double max_vel = 100.0; + return std::clamp(v, 0.0, max_vel); + }; + + // calculate sv graph + const double s_traj = autoware::motion_utils::calcArcLength(traj_points); + // const double t_max = 10.0; + const double s_max = 50.0; + const double max_sampling_num = 100.0; + + const double t_delta_min = 0.1; + const double s_delta_min = 0.1; + + // NOTE: v0 of motion_velocity_planner_common may be smaller than v0 of motion_velocity_smoother + // Therefore, we calculate v1 (velocity at next step) and use it for initial velocity. + const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0; + const double a1 = a0; // + jerk * 0.1; + const double jerk = target_acc > a1 ? common_param_.max_jerk * target_jerk_ratio + : common_param_.min_jerk * target_jerk_ratio; + + double t_current = 0.0; + std::vector s_vec{0.0}; + std::vector v_vec{v1}; + for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) { + if (v_vec.back() <= 0.0) { + s_vec.push_back(s_vec.back() + s_delta_min); + v_vec.push_back(0.0); + } else { + const double v_current = vt( + v1, a1, jerk, t_current + t_delta_min, + target_acc); // TODO(murooka) + t_delta_min is not required + + const double s_delta = std::max(s_delta_min, v_current * t_delta_min); + const double s_current = s_vec.back() + s_delta; + + s_vec.push_back(s_current); + v_vec.push_back(v_current); + + const double t_delta = [&]() { + if (v_current <= 0) { + return 0.0; + } + + constexpr double t_delta_max = 100.0; // NOTE: to avoid computation explosion + return std::min(t_delta_max, s_delta / v_current); + }(); + + t_current += t_delta; + } + + if (s_traj < s_vec.back() /*|| t_max < t_current*/ || s_max < s_vec.back()) { + break; + } + } + + std::vector unique_s_vec{s_vec.front()}; + std::vector unique_v_vec{v_vec.front()}; + for (size_t i = 0; i < s_vec.size(); ++i) { + if (s_vec.at(i) == unique_s_vec.back()) { + continue; + } + + unique_s_vec.push_back(s_vec.at(i)); + unique_v_vec.push_back(v_vec.at(i)); + } + + if (unique_s_vec.size() < 2) { + return traj_points; + } + + auto acc_limited_traj_points = traj_points; + const size_t ego_seg_idx = planner_data->find_index(acc_limited_traj_points, start_pose); + double sum_dist = 0.0; + for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { + if (i != ego_seg_idx) { + sum_dist += autoware::universe_utils::calcDistance2d( + acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); + } + + const double vel = [&]() { + if (unique_s_vec.back() < sum_dist) { + return unique_v_vec.back(); + } + return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + }(); + + acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp( + vel, 0.0, + static_cast( + acc_limited_traj_points.at(i) + .longitudinal_velocity_mps)); // TODO(murooka) this assumes forward driving + } + + return acc_limited_traj_points; +} + +void PIDBasedPlanner::update_parameters(const std::vector & parameters) +{ + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.min_accel_during_cruise", + min_accel_during_cruise_); + + { // velocity limit based planner + auto & p = velocity_limit_based_planner_param_; + + double kp = p.pid_vel_controller->getKp(); + double ki = p.pid_vel_controller->getKi(); + double kd = p.pid_vel_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kp", kp); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.ki", ki); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kd", kd); + p.pid_vel_controller->updateParam(kp, ki, kd); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.output_ratio_" + "during_accel", + p.output_ratio_during_accel); + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.vel_to_acc_weight", + p.vel_to_acc_weight); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.enable_jerk_" + "limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.disable_" + "target_acceleration", + p.disable_target_acceleration); + } + + { // velocity insertion based planner + auto & p = velocity_insertion_based_planner_param_; + + // pid controller for acc + double kp_acc = p.pid_acc_controller->getKp(); + double ki_acc = p.pid_acc_controller->getKi(); + double kd_acc = p.pid_acc_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_acc", + kp_acc); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_acc", + ki_acc); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_acc", + kd_acc); + p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + double kp_jerk = p.pid_jerk_controller->getKp(); + double ki_jerk = p.pid_jerk_controller->getKi(); + double kd_jerk = p.pid_jerk_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_jerk", + kp_jerk); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_jerk", + ki_jerk); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_jerk", + kd_jerk); + p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "acc_ratio_during_accel", + p.output_acc_ratio_during_accel); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "jerk_ratio_during_accel", + p.output_jerk_ratio_during_accel); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.enable_" + "jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } + + // min_cruise_target_vel + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.min_cruise_target_vel", + min_cruise_target_vel_); + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.time_to_evaluate_rss", + time_to_evaluate_rss_); +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.hpp new file mode 100644 index 0000000000000..54836bde8d96f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.hpp @@ -0,0 +1,158 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ +#define PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ + +#include "../cruise_planner_interface.hpp" +#include "../type_alias.hpp" +#include "../types.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" +#include "cruise_planning_debug_info.hpp" +#include "pid_controller.hpp" + +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include + +using autoware::signal_processing::LowpassFilter1d; + +namespace autoware::motion_velocity_planner +{ + +class PIDBasedPlanner : public CruisePlannerInterface +{ +public: + struct CruiseObstacleInfo + { + CruiseObstacleInfo( + const CruiseObstacle & obstacle_arg, const double error_cruise_dist_arg, + const double dist_to_obstacle_arg, const double target_dist_to_obstacle_arg) + : obstacle(obstacle_arg), + error_cruise_dist(error_cruise_dist_arg), + dist_to_obstacle(dist_to_obstacle_arg), + target_dist_to_obstacle(target_dist_to_obstacle_arg) + { + } + + CruiseObstacle obstacle; + double error_cruise_dist; + double dist_to_obstacle; + double target_dist_to_obstacle; + }; + + PIDBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param); + + std::vector plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) override; + + void update_parameters(const std::vector & parameters) override; + +private: + Float32MultiArrayStamped get_cruise_planning_debug_message( + const rclcpp::Time & current_time) const override + { + return cruise_planning_debug_info_.convert_to_message(current_time); + } + + std::optional calc_obstacle_to_cruise( + const std::vector & stop_traj_points, + const std::shared_ptr planner_data, + const std::vector & obstacles); + + std::vector plan_cruise_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit, + const std::optional & cruise_obstacle_info); + + // velocity limit based planner + VelocityLimit plan_cruise_with_velocity_limit( + const std::shared_ptr planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + + // velocity insertion based planner + std::vector plan_cruise_with_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const CruiseObstacleInfo & cruise_obstacle_info); + std::vector get_acceleration_limited_trajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & start_pose, + const double v0, const double a0, const double target_acc, const double target_jerk_ratio, + const std::shared_ptr planner_data) const; + + // ROS parameters + double min_accel_during_cruise_; + double min_cruise_target_vel_; + + CruisePlanningDebugInfo cruise_planning_debug_info_; + + struct VelocityLimitBasedPlannerParam + { + std::unique_ptr pid_vel_controller; + double output_ratio_during_accel; + double vel_to_acc_weight; + bool enable_jerk_limit_to_output_acc{false}; + bool disable_target_acceleration{false}; + }; + VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; + + struct VelocityInsertionBasedPlannerParam + { + std::unique_ptr pid_acc_controller; + std::unique_ptr pid_jerk_controller; + double output_acc_ratio_during_accel; + double output_jerk_ratio_during_accel; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityInsertionBasedPlannerParam velocity_insertion_based_planner_param_; + + std::optional prev_target_acc_; + + // lpf for nodes's input + std::shared_ptr lpf_dist_to_obstacle_ptr_; + std::shared_ptr lpf_error_cruise_dist_ptr_; + std::shared_ptr lpf_obstacle_vel_ptr_; + + // lpf for planner's input + std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; + + // lpf for output + std::shared_ptr lpf_output_vel_ptr_; + std::shared_ptr lpf_output_acc_ptr_; + std::shared_ptr lpf_output_jerk_ptr_; + + std::vector prev_traj_points_; + + bool use_velocity_limit_based_planner_{true}; + + double time_to_evaluate_rss_; + + std::function error_func_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_controller.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_controller.hpp new file mode 100644 index 0000000000000..adcae7784103d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_controller.hpp @@ -0,0 +1,65 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PID_BASED_PLANNER__PID_CONTROLLER_HPP_ +#define PID_BASED_PLANNER__PID_CONTROLLER_HPP_ + +#include + +namespace autoware::motion_velocity_planner +{ +class PIDController +{ +public: + PIDController(const double kp, const double ki, const double kd) + : kp_(kp), ki_(ki), kd_(kd), error_sum_(0.0) + { + } + + double calc(const double error) + { + error_sum_ += error; + + // TODO(murooka) use time for d gain calculation + const double output = + kp_ * error + ki_ * error_sum_ + (prev_error_ ? kd_ * (error - *prev_error_) : 0.0); + prev_error_ = error; + return output; + } + + double getKp() const { return kp_; } + double getKi() const { return ki_; } + double getKd() const { return kd_; } + + void updateParam(const double kp, const double ki, const double kd) + { + kp_ = kp; + ki_ = ki; + kd_ = kd; + + error_sum_ = 0.0; + prev_error_ = {}; + } + +private: + double kp_; + double ki_; + double kd_; + + double error_sum_; + std::optional prev_error_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PID_BASED_PLANNER__PID_CONTROLLER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp new file mode 100644 index 0000000000000..673574c24a914 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp @@ -0,0 +1,70 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; +using PointCloud = pcl::PointCloud; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; +} // namespace autoware::motion_velocity_planner + +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/types.hpp new file mode 100644 index 0000000000000..7f4fcfc7e3bb1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/types.hpp @@ -0,0 +1,67 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES_HPP_ +#define TYPES_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "type_alias.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct CruiseObstacle +{ + CruiseObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, + const double arg_lat_velocity, + const std::vector & arg_collision_points, + bool arg_is_yield_obstacle = false) + : uuid(arg_uuid), + stamp(arg_stamp), + pose(arg_pose), + velocity(arg_lon_velocity), + lat_velocity(arg_lat_velocity), + collision_points(arg_collision_points), + is_yield_obstacle(arg_is_yield_obstacle) + { + } + std::string uuid{}; + rclcpp::Time stamp{}; + geometry_msgs::msg::Pose pose{}; // interpolated with the current stamp + double velocity{}; // longitudinal velocity against ego's trajectory + double lat_velocity{}; // lateral velocity against ego's trajectory + + std::vector collision_points{}; // time-series collision points + bool is_yield_obstacle{}; +}; + +struct DebugData +{ + DebugData() = default; + std::vector> intentionally_ignored_obstacles{}; + std::vector obstacles_to_cruise{}; + std::vector decimated_traj_polys{}; + MarkerArray cruise_wall_marker{}; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/CMakeLists.txt new file mode 100644 index 0000000000000..d74b292785fa6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_obstacle_slow_down_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/README.md new file mode 100644 index 0000000000000..25c15b59075c1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/README.md @@ -0,0 +1,52 @@ +# Obstacle Slow Down + +## Role + +The `obstacle_slow_down` module does the slow down planning when there is a static/dynamic obstacle near the trajectory. + +## Activation + +This module is activated if the launch parameter `launch_obstacle_slow_down_module` is set to true. + +## Inner-workings / Algorithms + +### Obstacle Filtering + +Among obstacles which are not for cruising and stopping, the obstacles meeting the following condition are determined as obstacles for slowing down. + +- The object type is for slowing down according to `obstacle_filtering.object_type.*`. +- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`. + +### Slow Down Planning + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type, making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. Each obstacle type has a `static` and a `moving` parameter set, so it is possible to customize the slow down response of the ego vehicle according to the obstacle type and if it is moving or not. If an obstacle is determined to be moving, the corresponding `moving` set of parameters will be used to compute the vehicle slow down, otherwise, the `static` parameters will be used. The `static` and `moving` separation is useful for customizing the ego vehicle slow down behavior to, for example, slow down more significantly when passing stopped vehicles that might cause occlusion or that might suddenly open its doors. + +An obstacle is classified as `static` if its total speed is less than the `moving_object_speed_threshold` parameter. Furthermore, a hysteresis based approach is used to avoid chattering, it uses the `moving_object_hysteresis_range` parameter range and the obstacle's previous state (`moving` or `static`) to determine if the obstacle is moving or not. In other words, if an obstacle was previously classified as `static`, it will not change its classification to `moving` unless its total speed is greater than `moving_object_speed_threshold` + `moving_object_hysteresis_range`. Likewise, an obstacle previously classified as `moving`, will only change to `static` if its speed is lower than `moving_object_speed_threshold` - `moving_object_hysteresis_range`. + +The closest point on the obstacle to the ego's trajectory is calculated. +Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. + +![slow_down_velocity_calculation](./docs/slow_down_velocity_calculation.svg) + +| Variable | Description | +| ---------- | ----------------------------------- | +| `v_{out}` | calculated velocity for slow down | +| `v_{min}` | `min_lat_velocity` | +| `v_{max}` | `max_lat_velocity` | +| `l_{min}` | `min_lat_margin` | +| `l_{max}` | `max_lat_margin` | +| `l'_{max}` | `obstacle_filtering.max_lat_margin` | + +The calculated velocity is inserted in the trajectory where the obstacle is inside the area with `obstacle_filtering.max_lat_margin`. + +![slow_down_planning](./docs/slow_down_planning.drawio.svg) + +## Debugging + +### Obstacle for slow down + +Yellow sphere which is an obstacle for slow_down is visualized by `obstacles_to_slow_down` in the `~/debug/marker` topic. + +Yellow wall which means a safe distance to slow_down if the ego's front meets the wall is visualized in the `~/debug/slow_down/virtual_wall` topic. + +![slow_down_visualization](./docs/slow_down_visualization.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/config/obstacle_slow_down.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/config/obstacle_slow_down.param.yaml new file mode 100644 index 0000000000000..f826e4ccd6618 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/config/obstacle_slow_down.param.yaml @@ -0,0 +1,50 @@ +/**: + ros__parameters: + obstacle_slow_down: + slow_down_planning: + slow_down_min_acc: -1.0 # slow down min deceleration [m/ss] + slow_down_min_jerk: -1.0 # slow down min jerk [m/sss] + + lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity + lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path + lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + + time_margin_on_target_velocity: 1.5 # [s] + + # parameters to calculate slow down velocity by linear interpolation + object_type_specified_params: + types: + - "default" + default: + moving: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + static: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 4.0 + max_ego_velocity: 8.0 + + moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" + moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold + + obstacle_filtering: + object_type: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + pointcloud: false + + min_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width to avoid the conflict with the obstacle_stop + max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width + lat_hysteresis_margin: 0.2 + + successive_num_to_entry_slow_down_condition: 5 + successive_num_to_exit_slow_down_condition: 5 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_planning.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_planning.drawio.svg new file mode 100644 index 0000000000000..603e6b37a653f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_planning.drawio.svg @@ -0,0 +1,133 @@ + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + +
+
+
+ trajectory velocity +
+
+
+ + trajecto... + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ max_slow_down_lat_margin +
+
+
+ + max_slow_d... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_velocity_calculation.svg b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_velocity_calculation.svg new file mode 100644 index 0000000000000..ee53e6e60a0c0 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_velocity_calculation.svg @@ -0,0 +1,534 @@ + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + + + + + + + + + + +
+
+
+ slow down +
+ velocity +
+
+
+ + slow down... + + + + + + +
+
+
+ lateral distance +
+
+
+ + lateral distance + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{max}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{min}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{min}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{max}` + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ linear interpolation of slow down velocity +
+
+
+ + linear interpolation of slow down velocity + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{out}` + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{max}... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_visualization.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_visualization.png new file mode 100644 index 0000000000000..472b3bb6c982a Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_visualization.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml new file mode 100644 index 0000000000000..aa1ec002ce5cc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_obstacle_slow_down_module + 0.40.0 + obstacle slow down feature in motion_velocity_planner + + Takayuki Murooka + Yuki Takagi + Maxime Clement + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_motion_utils + autoware_motion_velocity_planner_common_universe + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_signal_processing + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + libboost-dev + pluginlib + rclcpp + tf2 + tier4_metric_msgs + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/plugins.xml new file mode 100644 index 0000000000000..0cfaadbfc9796 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/metrics_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/metrics_manager.hpp new file mode 100644 index 0000000000000..11554121a0882 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/metrics_manager.hpp @@ -0,0 +1,60 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef METRICS_MANAGER_HPP_ +#define METRICS_MANAGER_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MetricsManager +{ +public: + void init() { metrics_.clear(); } + + void calculate_metrics(const std::string & module_name, const std::string & reason) + { + // Create status + { + // Decision + Metric decision_metric; + decision_metric.name = module_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics_.push_back(decision_metric); + } + } + + MetricArray create_metric_array(const rclcpp::Time & current_time) + { + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), metrics_.begin(), metrics_.end()); + return metrics_msg; + } + +private: + std::vector metrics_; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // METRICS_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp new file mode 100644 index 0000000000000..286c10c0c20d1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp @@ -0,0 +1,907 @@ +// Copyright 2025 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_slow_down_module.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +namespace +{ +bool is_lower_considering_hysteresis( + const double current_val, const bool was_low, const double high_val, const double low_val) +{ + if (was_low) { + if (high_val < current_val) { + return false; + } + return true; + } + if (current_val < low_val) { + return true; + } + return false; +} + +geometry_msgs::msg::Point to_geom_point(const autoware::universe_utils::Point2d & point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x(); + geom_point.y = point.y(); + return geom_point; +} + +template +std::optional get_object_from_uuid( + const std::vector & objects, const std::string & target_uuid) +{ + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; +} + +// TODO(murooka) following two functions are copied from behavior_velocity_planner. +// These should be refactored. +double find_reach_time( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max) +{ + const double j = jerk; + const double a = accel; + const double v = velocity; + const double d = distance; + const double min = t_min; + const double max = t_max; + auto f = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { + throw std::logic_error( + "[motion_velocity_planner_common](find_reach_time): search range is invalid"); + } + const double eps = 1e-5; + const int warn_iter = 100; + double lower = min; + double upper = max; + double t; + int iter = 0; + for (int i = 0;; i++) { + t = 0.5 * (lower + upper); + const double fx = f(t, j, a, v, d); + // std::cout<<"fx: "< 0.0) { + upper = t; + } else { + lower = t; + } + iter++; + if (iter > warn_iter) + std::cerr << "[motion_velocity_planner_common](find_reach_time): current iter is over warning" + << std::endl; + } + return t; +} + +double calc_deceleration_velocity_from_distance_to_target( + const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel, + const double current_velocity, const double distance_to_target) +{ + if (max_slowdown_jerk > 0 || max_slowdown_accel > 0) { + throw std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); + } + // case0: distance to target is behind ego + if (distance_to_target <= 0) return current_velocity; + auto ft = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + auto vt = [](const double t, const double j, const double a, const double v) { + return j * t * t / 2.0 + a * t + v; + }; + const double j_max = max_slowdown_jerk; + const double a0 = current_accel; + const double a_max = max_slowdown_accel; + const double v0 = current_velocity; + const double l = distance_to_target; + const double t_const_jerk = (a_max - a0) / j_max; + const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0); + const double d_const_acc_stop = l - d_const_jerk_stop; + + if (d_const_acc_stop < 0) { + // case0: distance to target is within constant jerk deceleration + // use binary search instead of solving cubic equation + const double t_jerk = find_reach_time(j_max, a0, v0, l, 0, t_const_jerk); + const double velocity = vt(t_jerk, j_max, a0, v0); + return velocity; + } else { + const double v1 = vt(t_const_jerk, j_max, a0, v0); + const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; + // case3: distance to target is farther than distance to stop + if (discriminant_of_stop <= 0) { + return 0.0; + } + // case2: distance to target is within constant accel deceleration + // solve d = 0.5*a^2+v*t by t + const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; + return vt(t_acc, 0.0, a_max, v1); + } + return current_velocity; +} + +VelocityLimitClearCommand create_velocity_limit_clear_command( + const rclcpp::Time & current_time, [[maybe_unused]] const std::string & module_name) +{ + VelocityLimitClearCommand msg; + msg.stamp = current_time; + msg.sender = "obstacle_slow_down"; + msg.command = true; + return msg; +} + +Float64Stamped create_float64_stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} +} // namespace + +void ObstacleSlowDownModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + // ros parameters + common_param_ = CommonParam(node); + slow_down_planning_param_ = SlowDownPlanningParam(node); + obstacle_filtering_param_ = ObstacleFilteringParam(node); + + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "motion_velocity_planner_common"); + + // common publisher + processing_time_publisher_ = + node.create_publisher("~/debug/obstacle_slow_down/processing_time_ms", 1); + virtual_wall_publisher_ = + node.create_publisher("~/obstacle_slow_down/virtual_walls", 1); + debug_publisher_ = node.create_publisher("~/obstacle_slow_down/debug_markers", 1); + + // module publisher + metrics_pub_ = node.create_publisher("~/slow_down/metrics", 10); + debug_slow_down_planning_info_pub_ = + node.create_publisher("~/debug/slow_down_planning_info", 1); + processing_time_detail_pub_ = node.create_publisher( + "~/debug/processing_time_detail_ms/obstacle_slow_down", 1); + + // interface publisher + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "obstacle_slow_down"); + planning_factor_interface_ = + std::make_unique( + &node, "obstacle_slow_down"); + + // time keeper + time_keeper_ = std::make_shared(processing_time_detail_pub_); +} + +void ObstacleSlowDownModule::update_parameters( + [[maybe_unused]] const std::vector & parameters) +{ +} + +VelocityPlanningResult ObstacleSlowDownModule::plan( + const std::vector & raw_trajectory_points, + [[maybe_unused]] const std::vector & + smoothed_trajectory_points, + const std::shared_ptr planner_data) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + stop_watch_.tic(); + debug_data_ptr_ = std::make_shared(); + metrics_manager_.init(); + decimated_traj_polys_ = std::nullopt; + + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + raw_trajectory_points, planner_data->current_odometry.pose.pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold, + planner_data->trajectory_polygon_collision_check.decimate_trajectory_step_length, 0.0); + + const auto slow_down_obstacles = filter_slow_down_obstacle_for_predicted_object( + planner_data->current_odometry, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold, raw_trajectory_points, decimated_traj_points, + planner_data->objects, rclcpp::Time(planner_data->predicted_objects_header.stamp), + planner_data->vehicle_info_, planner_data->trajectory_polygon_collision_check); + + VelocityPlanningResult result; + result.slowdown_intervals = plan_slow_down( + planner_data, raw_trajectory_points, slow_down_obstacles, result.velocity_limit, + planner_data->vehicle_info_); + metrics_manager_.calculate_metrics("PlannerInterface", "cruise"); + + // clear velocity limit if necessary + if (result.velocity_limit) { + need_to_clear_velocity_limit_ = true; + } else { + if (need_to_clear_velocity_limit_) { + // clear velocity limit + result.velocity_limit_clear_command = + create_velocity_limit_clear_command(clock_->now(), module_name_); + need_to_clear_velocity_limit_ = false; + } + } + + publish_debug_info(); + + return result; +} + +std::string ObstacleSlowDownModule::get_module_name() const +{ + return module_name_; +} + +std::vector +ObstacleSlowDownModule::filter_slow_down_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & current_pose = odometry.pose.pose; + + // calculate collision points with trajectory with lateral stop margin + // NOTE: For additional margin, hysteresis is not divided by two. + const auto & p = obstacle_filtering_param_; + const auto & tp = trajectory_polygon_collision_check; + const auto decimated_traj_polys_with_lat_margin = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, odometry.pose.pose, + p.max_lat_margin + p.lat_hysteresis_margin, tp.enable_to_consider_current_pose, + tp.time_to_convergence, tp.decimate_trajectory_step_length); + debug_data_ptr_->decimated_traj_polys = decimated_traj_polys_with_lat_margin; + + // slow down + slow_down_condition_counter_.reset_current_uuids(); + std::vector slow_down_obstacles; + for (const auto & object : objects) { + // 1. rough filtering + // 1.1. Check if the obstacle is in front of the ego. + const double lon_dist_from_ego_to_obj = + object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + if (lon_dist_from_ego_to_obj < 0.0) { + continue; + } + + // 1.2. Check if the rough lateral distance is smaller than the threshold. + const double min_lat_dist_to_traj_poly = + utils::calc_possible_min_dist_from_obj_to_traj_poly(object, traj_points, vehicle_info); + if (obstacle_filtering_param_.max_lat_margin < min_lat_dist_to_traj_poly) { + continue; + } + + // 2. precise filtering + const auto & decimated_traj_polys = get_decimated_traj_polys( + traj_points, current_pose, vehicle_info, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold, trajectory_polygon_collision_check); + const double dist_from_obj_poly_to_traj_poly = + object->get_dist_to_traj_poly(decimated_traj_polys); + const auto slow_down_obstacle = create_slow_down_obstacle_for_predicted_object( + traj_points, decimated_traj_polys_with_lat_margin, object, predicted_objects_stamp, + dist_from_obj_poly_to_traj_poly); + if (slow_down_obstacle) { + slow_down_obstacles.push_back(*slow_down_obstacle); + continue; + } + } + slow_down_condition_counter_.remove_counter_unless_updated(); + prev_slow_down_object_obstacles_ = slow_down_obstacles; + + RCLCPP_DEBUG( + logger_, "The number of output obstacles of filter_slow_down_obstacles is %ld", + slow_down_obstacles.size()); + return slow_down_obstacles; +} + +std::optional +ObstacleSlowDownModule::create_slow_down_obstacle_for_predicted_object( + const std::vector & traj_points, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & p = obstacle_filtering_param_; + + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_label = object->predicted_object.classification.at(0).label; + slow_down_condition_counter_.add_current_uuid(obj_uuid_str); + + const bool is_prev_obstacle_slow_down = + utils::get_obstacle_from_uuid(prev_slow_down_object_obstacles_, obj_uuid_str).has_value(); + + if (!is_slow_down_obstacle(obj_label)) { + return std::nullopt; + } + + if (dist_from_obj_poly_to_traj_poly <= p.min_lat_margin) { + return std::nullopt; + } + + // check lateral distance considering hysteresis + const bool is_lat_dist_low = is_lower_considering_hysteresis( + dist_from_obj_poly_to_traj_poly, is_prev_obstacle_slow_down, + p.max_lat_margin + p.lat_hysteresis_margin / 2.0, + p.max_lat_margin - p.lat_hysteresis_margin / 2.0); + + const bool is_slow_down_required = [&]() { + if (is_prev_obstacle_slow_down) { + // check if exiting slow down + if (!is_lat_dist_low) { + const int count = slow_down_condition_counter_.decrease_counter(obj_uuid_str); + if (count <= -p.successive_num_to_exit_slow_down_condition) { + slow_down_condition_counter_.reset(obj_uuid_str); + return false; + } + } + return true; + } + // check if entering slow down + if (is_lat_dist_low) { + const int count = slow_down_condition_counter_.increase_counter(obj_uuid_str); + if (p.successive_num_to_entry_slow_down_condition <= count) { + slow_down_condition_counter_.reset(obj_uuid_str); + return true; + } + } + return false; + }(); + if (!is_slow_down_required) { + RCLCPP_DEBUG( + logger_, "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", + obj_uuid_str.substr(0, 4).c_str(), dist_from_obj_poly_to_traj_poly); + return std::nullopt; + } + + const auto obstacle_poly = autoware::universe_utils::toPolygon2d( + object->predicted_object.kinematics.initial_pose_with_covariance.pose, + object->predicted_object.shape); + + std::vector front_collision_polygons; + size_t front_seg_idx = 0; + std::vector back_collision_polygons; + size_t back_seg_idx = 0; + for (size_t i = 0; i < decimated_traj_polys_with_lat_margin.size(); ++i) { + std::vector collision_polygons; + bg::intersection(decimated_traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); + + if (!collision_polygons.empty()) { + if (front_collision_polygons.empty()) { + front_collision_polygons = collision_polygons; + front_seg_idx = i == 0 ? i : i - 1; + } + back_collision_polygons = collision_polygons; + back_seg_idx = i == 0 ? i : i - 1; + } else { + if (!back_collision_polygons.empty()) { + break; // for efficient calculation + } + } + } + + if (front_collision_polygons.empty() || back_collision_polygons.empty()) { + RCLCPP_DEBUG( + logger_, "[SlowDown] Ignore obstacle (%s) since there is no collision point", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + // calculate front collision point + double front_min_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point front_collision_point; + for (const auto & collision_poly : front_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = to_geom_point(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, front_seg_idx, collision_geom_point); + if (dist < front_min_dist) { + front_min_dist = dist; + front_collision_point = collision_geom_point; + } + } + } + + // calculate back collision point + double back_max_dist = -std::numeric_limits::max(); + geometry_msgs::msg::Point back_collision_point = front_collision_point; + for (const auto & collision_poly : back_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = to_geom_point(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, back_seg_idx, collision_geom_point); + if (back_max_dist < dist) { + back_max_dist = dist; + back_collision_point = collision_geom_point; + } + } + } + + return SlowDownObstacle{ + obj_uuid_str, + predicted_objects_stamp, + object->predicted_object.classification.at(0), + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->get_lon_vel_relative_to_traj(traj_points), + object->get_lat_vel_relative_to_traj(traj_points), + dist_from_obj_poly_to_traj_poly, + front_collision_point, + back_collision_point}; +} + +std::vector ObstacleSlowDownModule::plan_slow_down( + const std::shared_ptr planner_data, + const std::vector & traj_points, const std::vector & obstacles, + [[maybe_unused]] std::optional & velocity_limit, const VehicleInfo & vehicle_info) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + auto slow_down_traj_points = traj_points; + slow_down_debug_multi_array_ = Float32MultiArrayStamped(); + + const double dist_to_ego = [&]() { + const size_t ego_seg_idx = planner_data->find_segment_index( + slow_down_traj_points, planner_data->current_odometry.pose.pose); + return autoware::motion_utils::calcSignedArcLength( + slow_down_traj_points, 0, planner_data->current_odometry.pose.pose.position, ego_seg_idx); + }(); + const double abs_ego_offset = planner_data->is_driving_forward + ? std::abs(vehicle_info.max_longitudinal_offset_m) + : std::abs(vehicle_info.min_longitudinal_offset_m); + + // define function to insert slow down velocity to trajectory + const auto insert_point_in_trajectory = [&](const double lon_dist) -> std::optional { + const auto inserted_idx = + autoware::motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); + if (inserted_idx) { + if (inserted_idx.value() + 1 <= slow_down_traj_points.size() - 1) { + // zero-order hold for velocity interpolation + slow_down_traj_points.at(inserted_idx.value()).longitudinal_velocity_mps = + slow_down_traj_points.at(inserted_idx.value() + 1).longitudinal_velocity_mps; + } + return inserted_idx.value(); + } + return std::nullopt; + }; + + std::vector slowdown_intervals; + std::vector new_prev_slow_down_output; + for (size_t i = 0; i < obstacles.size(); ++i) { + const auto & obstacle = obstacles.at(i); + const auto prev_output = get_object_from_uuid(prev_slow_down_output_, obstacle.uuid); + + const bool is_obstacle_moving = [&]() -> bool { + const auto & p = slow_down_planning_param_; + const auto object_vel_norm = std::hypot(obstacle.velocity, obstacle.lat_velocity); + if (!prev_output) { + return object_vel_norm > p.moving_object_speed_threshold; + } + if (prev_output->is_obstacle_moving) { + return object_vel_norm > p.moving_object_speed_threshold - p.moving_object_hysteresis_range; + } + return object_vel_norm > p.moving_object_speed_threshold + p.moving_object_hysteresis_range; + }(); + + // calculate slow down start distance, and insert slow down velocity + const auto dist_vec_to_slow_down = calculate_distance_to_slow_down_with_constraints( + planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego, vehicle_info, + is_obstacle_moving); + if (!dist_vec_to_slow_down) { + RCLCPP_DEBUG( + logger_, "[SlowDown] Ignore obstacle (%s) since distance to slow down is not valid", + obstacle.uuid.c_str()); + continue; + } + const auto dist_to_slow_down_start = std::get<0>(*dist_vec_to_slow_down); + const auto dist_to_slow_down_end = std::get<1>(*dist_vec_to_slow_down); + const auto feasible_slow_down_vel = std::get<2>(*dist_vec_to_slow_down); + + // calculate slow down end distance, and insert slow down velocity + // NOTE: slow_down_start_idx will not be wrong since inserted back point is after inserted + // front point. + const auto slow_down_start_idx = insert_point_in_trajectory(dist_to_slow_down_start); + const auto slow_down_end_idx = dist_to_slow_down_start < dist_to_slow_down_end + ? insert_point_in_trajectory(dist_to_slow_down_end) + : std::nullopt; + if (!slow_down_end_idx) { + continue; + } + + // calculate slow down velocity + const double stable_slow_down_vel = [&]() { + if (prev_output) { + return autoware::signal_processing::lowpassFilter( + feasible_slow_down_vel, prev_output->target_vel, + slow_down_planning_param_.lpf_gain_slow_down_vel); + } + return feasible_slow_down_vel; + }(); + + // insert slow down velocity between slow start and end + slowdown_intervals.push_back(SlowdownInterval{ + slow_down_traj_points.at(slow_down_start_idx ? *slow_down_start_idx : 0).pose.position, + slow_down_traj_points.at(*slow_down_end_idx).pose.position, stable_slow_down_vel}); + + // add debug data + slow_down_debug_multi_array_.data.push_back(obstacle.dist_to_traj_poly); + slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start); + slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end); + slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel); + slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); + slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0); + slow_down_debug_multi_array_.data.push_back(*slow_down_end_idx); + + // add virtual wall + if (slow_down_start_idx && slow_down_end_idx) { + const size_t ego_idx = + planner_data->find_index(slow_down_traj_points, planner_data->current_odometry.pose.pose); + const size_t slow_down_wall_idx = [&]() { + if (ego_idx < *slow_down_start_idx) return *slow_down_start_idx; + if (ego_idx < *slow_down_end_idx) return ego_idx; + return *slow_down_end_idx; + }(); + + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", clock_->now(), i, + abs_ego_offset, "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + + // update planning factor + planning_factor_interface_->add( + slow_down_traj_points, planner_data->current_odometry.pose.pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}, planner_data->is_driving_forward, stable_slow_down_vel); + } + + // add debug virtual wall + if (slow_down_start_idx) { + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", + clock_->now(), i * 2, abs_ego_offset, "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); + } + if (slow_down_end_idx) { + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", clock_->now(), + i * 2 + 1, abs_ego_offset, "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); + } + + // Add debug data + debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); + + // update prev_slow_down_output_ + new_prev_slow_down_output.push_back(SlowDownOutput{ + obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx, + stable_slow_down_vel, feasible_slow_down_vel, obstacle.dist_to_traj_poly, + is_obstacle_moving}); + } + + // update prev_slow_down_output_ + prev_slow_down_output_ = new_prev_slow_down_output; + + return slowdown_intervals; +} + +Float32MultiArrayStamped ObstacleSlowDownModule::get_slow_down_planning_debug_message( + const rclcpp::Time & current_time) +{ + slow_down_debug_multi_array_.stamp = current_time; + return slow_down_debug_multi_array_; +} + +void ObstacleSlowDownModule::publish_debug_info() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. debug marker + MarkerArray debug_marker; + + // 1.1. obstacles + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_slow_down.size(); ++i) { + // obstacle + const auto obstacle_marker = utils::get_object_marker( + debug_data_ptr_->obstacles_to_slow_down.at(i).pose, i, "obstacles", 0.7, 0.7, 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", i * 2 + 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + front_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; + auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", i * 2 + 1, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + back_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; + + debug_marker.markers.push_back(front_collision_point_marker); + debug_marker.markers.push_back(back_collision_point_marker); + } + + // 1.2. slow down debug wall marker + autoware::universe_utils::appendMarkerArray( + debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + + // 1.3. detection area + auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { + for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { + const auto & current_point = decimated_traj_poly.outer().at(dp_idx); + const auto & next_point = + decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); + + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(decimated_traj_polys_marker); + + debug_publisher_->publish(debug_marker); + + // 2. virtual wall + virtual_wall_publisher_->publish(debug_data_ptr_->slow_down_wall_marker); + + // 3. slow down planning info + const auto slow_down_debug_msg = get_slow_down_planning_debug_message(clock_->now()); + debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); + + // 4. objects of interest + objects_of_interest_marker_interface_->publishMarkerArray(); + + // 5. metrics + const auto metrics_msg = metrics_manager_.create_metric_array(clock_->now()); + metrics_pub_->publish(metrics_msg); + + // 6. processing time + processing_time_publisher_->publish(create_float64_stamped(clock_->now(), stop_watch_.toc())); + + // 7. planning factor + planning_factor_interface_->publish(); +} + +bool ObstacleSlowDownModule::is_slow_down_obstacle(const uint8_t label) const +{ + const auto & types = obstacle_filtering_param_.object_types; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +std::optional> +ObstacleSlowDownModule::calculate_distance_to_slow_down_with_constraints( + const std::shared_ptr planner_data, + const std::vector & traj_points, const SlowDownObstacle & obstacle, + const std::optional & prev_output, const double dist_to_ego, + const VehicleInfo & vehicle_info, const bool is_obstacle_moving) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const double abs_ego_offset = planner_data->is_driving_forward + ? std::abs(vehicle_info.max_longitudinal_offset_m) + : std::abs(vehicle_info.min_longitudinal_offset_m); + const double obstacle_vel = obstacle.velocity; + // calculate slow down velocity + const double slow_down_vel = + calculate_slow_down_velocity(obstacle, prev_output, is_obstacle_moving); + + // calculate distance to collision points + const double dist_to_front_collision = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); + const double dist_to_back_collision = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); + + // calculate offset distance to first collision considering relative velocity + const double relative_vel = + planner_data->current_odometry.twist.twist.linear.x - obstacle.velocity; + const double offset_dist_to_collision = [&]() { + if (dist_to_front_collision < dist_to_ego + abs_ego_offset) { + return 0.0; + } + + // NOTE: This min_relative_vel forces the relative velocity positive if the ego velocity is + // lower than the obstacle velocity. Without this, the slow down feature will flicker where + // the ego velocity is very close to the obstacle velocity. + constexpr double min_relative_vel = 1.0; + const double time_to_collision = (dist_to_front_collision - dist_to_ego - abs_ego_offset) / + std::max(min_relative_vel, relative_vel); + + constexpr double time_to_collision_margin = 1.0; + const double cropped_time_to_collision = + std::max(0.0, time_to_collision - time_to_collision_margin); + return obstacle_vel * cropped_time_to_collision; + }(); + + // calculate distance during deceleration, slow down preparation, and slow down + const double min_slow_down_prepare_dist = 3.0; + const double slow_down_prepare_dist = std::max( + min_slow_down_prepare_dist, + slow_down_vel * slow_down_planning_param_.time_margin_on_target_velocity); + const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision - + abs_ego_offset - dist_to_ego - slow_down_prepare_dist; + const double slow_down_dist = + dist_to_back_collision - dist_to_front_collision + slow_down_prepare_dist; + + // calculate distance to start/end slow down + const double dist_to_slow_down_start = dist_to_ego + deceleration_dist; + const double dist_to_slow_down_end = dist_to_ego + deceleration_dist + slow_down_dist; + if (100.0 < dist_to_slow_down_start) { + // NOTE: distance to slow down is too far. + return std::nullopt; + } + + // apply low-pass filter to distance to start/end slow down + const auto apply_lowpass_filter = [&](const double dist_to_slow_down, const auto prev_point) { + if (prev_output && prev_point) { + const size_t seg_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); + const double prev_dist_to_slow_down = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); + return autoware::signal_processing::lowpassFilter( + dist_to_slow_down, prev_dist_to_slow_down, + slow_down_planning_param_.lpf_gain_dist_to_slow_down); + } + return dist_to_slow_down; + }; + const double filtered_dist_to_slow_down_start = + apply_lowpass_filter(dist_to_slow_down_start, prev_output->start_point); + const double filtered_dist_to_slow_down_end = + apply_lowpass_filter(dist_to_slow_down_end, prev_output->end_point); + + // calculate velocity considering constraints + const double feasible_slow_down_vel = [&]() { + if (deceleration_dist < 0) { + if (prev_output) { + return prev_output->target_vel; + } + return std::max(planner_data->current_odometry.twist.twist.linear.x, slow_down_vel); + } + if (planner_data->current_odometry.twist.twist.linear.x < slow_down_vel) { + return slow_down_vel; + } + + const double one_shot_feasible_slow_down_vel = [&]() { + if (planner_data->current_acceleration.accel.accel.linear.x < common_param_.min_accel) { + const double squared_vel = + std::pow(planner_data->current_odometry.twist.twist.linear.x, 2) + + 2 * deceleration_dist * common_param_.min_accel; + if (squared_vel < 0) { + return slow_down_vel; + } + return std::max(std::sqrt(squared_vel), slow_down_vel); + } + // TODO(murooka) Calculate more precisely. Final acceleration should be zero. + const double min_feasible_slow_down_vel = calc_deceleration_velocity_from_distance_to_target( + slow_down_planning_param_.slow_down_min_jerk, slow_down_planning_param_.slow_down_min_acc, + planner_data->current_acceleration.accel.accel.linear.x, + planner_data->current_odometry.twist.twist.linear.x, deceleration_dist); + return min_feasible_slow_down_vel; + }(); + if (prev_output) { + // NOTE: If longitudinal controllability is not good, one_shot_slow_down_vel may be getting + // larger since we use actual ego's velocity and acceleration for its calculation. + // Suppress one_shot_slow_down_vel getting larger here. + const double feasible_slow_down_vel = + std::min(one_shot_feasible_slow_down_vel, prev_output->feasible_target_vel); + return std::max(slow_down_vel, feasible_slow_down_vel); + } + return std::max(slow_down_vel, one_shot_feasible_slow_down_vel); + }(); + + return std::make_tuple( + filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); +} + +double ObstacleSlowDownModule::calculate_slow_down_velocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const +{ + const auto & p = slow_down_planning_param_.get_object_param_by_label( + obstacle.classification, is_obstacle_moving); + const double stable_dist_from_obj_poly_to_traj_poly = [&]() { + if (prev_output) { + return autoware::signal_processing::lowpassFilter( + obstacle.dist_to_traj_poly, prev_output->dist_from_obj_poly_to_traj_poly, + slow_down_planning_param_.lpf_gain_lat_dist); + } + return obstacle.dist_to_traj_poly; + }(); + + const double ratio = std::clamp( + (std::abs(stable_dist_from_obj_poly_to_traj_poly) - p.min_lat_margin) / + (p.max_lat_margin - p.min_lat_margin), + 0.0, 1.0); + const double slow_down_vel = + p.min_ego_velocity + ratio * (p.max_ego_velocity - p.min_ego_velocity); + + return slow_down_vel; +} + +std::vector ObstacleSlowDownModule::get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + if (!decimated_traj_polys_) { + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold, + p.decimate_trajectory_step_length, p.goal_extended_trajectory_length); + decimated_traj_polys_ = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + } + return *decimated_traj_polys_; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::ObstacleSlowDownModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp new file mode 100644 index 0000000000000..1d367adfb6f4e --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp @@ -0,0 +1,123 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_SLOW_DOWN_MODULE_HPP_ +#define OBSTACLE_SLOW_DOWN_MODULE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "metrics_manager.hpp" +#include "parameters.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class ObstacleSlowDownModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + VelocityPlanningResult plan( + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override; + +private: + std::string module_name_; + rclcpp::Clock::SharedPtr clock_{}; + + // ros parameters + CommonParam common_param_; + SlowDownPlanningParam slow_down_planning_param_; + ObstacleFilteringParam obstacle_filtering_param_; + + // module publisher + rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr debug_slow_down_planning_info_pub_; + rclcpp::Publisher::SharedPtr + processing_time_detail_pub_; + + // interface publisher + std::unique_ptr + objects_of_interest_marker_interface_; + + // internal variables + std::vector prev_slow_down_object_obstacles_; + std::vector prev_slow_down_output_; + SlowDownConditionCounter slow_down_condition_counter_; + Float32MultiArrayStamped slow_down_debug_multi_array_; + autoware::universe_utils::StopWatch stop_watch_; + mutable std::shared_ptr debug_data_ptr_; + MetricsManager metrics_manager_; + bool need_to_clear_velocity_limit_{false}; + mutable std::shared_ptr time_keeper_; + mutable std::optional> decimated_traj_polys_{std::nullopt}; + + std::vector filter_slow_down_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check); + std::optional create_slow_down_obstacle_for_predicted_object( + const std::vector & traj_points, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly); + std::vector plan_slow_down( + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::vector & obstacles, + [[maybe_unused]] std::optional & velocity_limit, + const VehicleInfo & vehicle_info); + Float32MultiArrayStamped get_slow_down_planning_debug_message(const rclcpp::Time & current_time); + void publish_debug_info(); + bool is_slow_down_obstacle(const uint8_t label) const; + std::optional> + calculate_distance_to_slow_down_with_constraints( + const std::shared_ptr planner_data, + const std::vector & traj_points, const SlowDownObstacle & obstacle, + const std::optional & prev_output, const double dist_to_ego, + const VehicleInfo & vehicle_info, const bool is_obstacle_moving) const; + double calculate_slow_down_velocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const; + std::vector get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OBSTACLE_SLOW_DOWN_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp new file mode 100644 index 0000000000000..b2159def25224 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp @@ -0,0 +1,176 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PARAMETERS_HPP_ +#define PARAMETERS_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +struct CommonParam +{ + double max_accel{}; + double min_accel{}; + double max_jerk{}; + double min_jerk{}; + double limit_max_accel{}; + double limit_min_accel{}; + double limit_max_jerk{}; + double limit_min_jerk{}; + + CommonParam() = default; + explicit CommonParam(rclcpp::Node & node) + { + max_accel = getOrDeclareParameter(node, "normal.max_acc"); + min_accel = getOrDeclareParameter(node, "normal.min_acc"); + max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); + min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); + limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); + limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); + limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + } +}; + +struct ObstacleFilteringParam +{ + std::vector object_types{}; + + double min_lat_margin{}; + double max_lat_margin{}; + double lat_hysteresis_margin{}; + + int successive_num_to_entry_slow_down_condition{}; + int successive_num_to_exit_slow_down_condition{}; + + ObstacleFilteringParam() = default; + explicit ObstacleFilteringParam(rclcpp::Node & node) + { + object_types = + utils::get_target_object_type(node, "obstacle_slow_down.obstacle_filtering.object_type."); + min_lat_margin = + getOrDeclareParameter(node, "obstacle_slow_down.obstacle_filtering.min_lat_margin"); + max_lat_margin = + getOrDeclareParameter(node, "obstacle_slow_down.obstacle_filtering.max_lat_margin"); + lat_hysteresis_margin = getOrDeclareParameter( + node, "obstacle_slow_down.obstacle_filtering.lat_hysteresis_margin"); + successive_num_to_entry_slow_down_condition = getOrDeclareParameter( + node, "obstacle_slow_down.obstacle_filtering.successive_num_to_entry_slow_down_condition"); + successive_num_to_exit_slow_down_condition = getOrDeclareParameter( + node, "obstacle_slow_down.obstacle_filtering.successive_num_to_exit_slow_down_condition"); + } +}; + +struct SlowDownPlanningParam +{ + double slow_down_min_acc{}; + double slow_down_min_jerk{}; + + double lpf_gain_slow_down_vel{}; + double lpf_gain_lat_dist{}; + double lpf_gain_dist_to_slow_down{}; + + double time_margin_on_target_velocity{}; + + double moving_object_speed_threshold{}; + double moving_object_hysteresis_range{}; + + std::vector obstacle_labels{"default"}; + std::vector obstacle_moving_classification{"static", "moving"}; + struct ObjectTypeSpecificParams + { + double min_lat_margin; + double max_lat_margin; + double min_ego_velocity; + double max_ego_velocity; + }; + std::unordered_map object_types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map object_type_specific_param_map; + + SlowDownPlanningParam() = default; + explicit SlowDownPlanningParam(rclcpp::Node & node) + { + slow_down_min_acc = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.slow_down_min_acc"); + slow_down_min_jerk = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.slow_down_min_jerk"); + + lpf_gain_slow_down_vel = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.lpf_gain_slow_down_vel"); + lpf_gain_lat_dist = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.lpf_gain_lat_dist"); + lpf_gain_dist_to_slow_down = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.lpf_gain_dist_to_slow_down"); + time_margin_on_target_velocity = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.time_margin_on_target_velocity"); + + moving_object_speed_threshold = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.moving_object_speed_threshold"); + moving_object_hysteresis_range = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.moving_object_hysteresis_range"); + + const std::string param_prefix = + "obstacle_slow_down.slow_down_planning.object_type_specified_params."; + const auto object_types = + getOrDeclareParameter>(node, param_prefix + "types"); + + for (const auto & type_str : object_types) { + for (const auto & movement_type : std::vector{"moving", "static"}) { + ObjectTypeSpecificParams param{ + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".min_lat_margin"), + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".max_lat_margin"), + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".min_ego_velocity"), + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".max_ego_velocity")}; + + object_type_specific_param_map.emplace(type_str + "." + movement_type, param); + } + } + } + + std::string get_param_type(const ObjectClassification label) const + { + const auto type_str = object_types_maps.at(label.label); + if (object_type_specific_param_map.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObjectTypeSpecificParams get_object_param_by_label( + const ObjectClassification label, const bool is_obstacle_moving) const + { + const std::string movement_type = is_obstacle_moving ? "moving" : "static"; + return object_type_specific_param_map.at(get_param_type(label) + "." + movement_type); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // PARAMETERS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp new file mode 100644 index 0000000000000..673574c24a914 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp @@ -0,0 +1,70 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; +using PointCloud = pcl::PointCloud; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; +} // namespace autoware::motion_velocity_planner + +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/types.hpp new file mode 100644 index 0000000000000..c21c6fd9103a2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/types.hpp @@ -0,0 +1,144 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES_HPP_ +#define TYPES_HPP_ + +#include "type_alias.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct SlowDownObstacle +{ + SlowDownObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const double arg_lon_velocity, const double arg_lat_velocity, + const double arg_dist_to_traj_poly, const geometry_msgs::msg::Point & arg_front_collision_point, + const geometry_msgs::msg::Point & arg_back_collision_point) + : uuid(arg_uuid), + stamp(arg_stamp), + pose(arg_pose), + velocity(arg_lon_velocity), + lat_velocity(arg_lat_velocity), + dist_to_traj_poly(arg_dist_to_traj_poly), + front_collision_point(arg_front_collision_point), + back_collision_point(arg_back_collision_point), + classification(object_classification) + { + } + std::string uuid{}; + rclcpp::Time stamp{}; + geometry_msgs::msg::Pose pose{}; // interpolated with the current stamp + double velocity{}; // longitudinal velocity against ego's trajectory + double lat_velocity{}; // lateral velocity against ego's trajectory + + double dist_to_traj_poly{}; // for efficient calculation + geometry_msgs::msg::Point front_collision_point{}; + geometry_msgs::msg::Point back_collision_point{}; + ObjectClassification classification{}; +}; + +struct SlowDownOutput +{ + SlowDownOutput() = default; + SlowDownOutput( + const std::string & arg_uuid, const std::vector & traj_points, + const std::optional & start_idx, const std::optional & end_idx, + const double arg_target_vel, const double arg_feasible_target_vel, + const double arg_dist_from_obj_poly_to_traj_poly, const bool is_obstacle_moving) + : uuid(arg_uuid), + target_vel(arg_target_vel), + feasible_target_vel(arg_feasible_target_vel), + dist_from_obj_poly_to_traj_poly(arg_dist_from_obj_poly_to_traj_poly), + is_obstacle_moving(is_obstacle_moving) + { + if (start_idx) { + start_point = traj_points.at(*start_idx).pose; + } + if (end_idx) { + end_point = traj_points.at(*end_idx).pose; + } + } + + std::string uuid{}; + double target_vel{}; + double feasible_target_vel{}; + double dist_from_obj_poly_to_traj_poly{}; + std::optional start_point{std::nullopt}; + std::optional end_point{std::nullopt}; + bool is_obstacle_moving{}; +}; + +struct SlowDownConditionCounter +{ + void reset_current_uuids() { current_uuids_.clear(); } + void add_current_uuid(const std::string & uuid) { current_uuids_.push_back(uuid); } + void remove_counter_unless_updated() + { + std::vector obsolete_uuids; + for (const auto & key_and_value : counter_) { + if ( + std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) == + current_uuids_.end()) { + obsolete_uuids.push_back(key_and_value.first); + } + } + + for (const auto & obsolete_uuid : obsolete_uuids) { + counter_.erase(obsolete_uuid); + } + } + + int increase_counter(const std::string & uuid) + { + if (counter_.count(uuid) != 0) { + counter_.at(uuid) = std::max(1, counter_.at(uuid) + 1); + } else { + counter_.emplace(uuid, 1); + } + return counter_.at(uuid); + } + int decrease_counter(const std::string & uuid) + { + if (counter_.count(uuid) != 0) { + counter_.at(uuid) = std::min(-1, counter_.at(uuid) - 1); + } else { + counter_.emplace(uuid, -1); + } + return counter_.at(uuid); + } + void reset(const std::string & uuid) { counter_.emplace(uuid, 0); } + + // NOTE: positive is for meeting entering condition, and negative is for exiting. + std::unordered_map counter_{}; + std::vector current_uuids_{}; +}; + +struct DebugData +{ + DebugData() = default; + std::vector obstacles_to_slow_down{}; + std::vector decimated_traj_polys{}; + MarkerArray slow_down_debug_wall_marker{}; + MarkerArray slow_down_wall_marker{}; +}; +} // namespace autoware::motion_velocity_planner + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/CMakeLists.txt new file mode 100644 index 0000000000000..4bcd50db64cb8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_obstacle_stop_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/README.md new file mode 100644 index 0000000000000..412a4e28f0580 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/README.md @@ -0,0 +1,82 @@ +# Obstacle Stop + +## Role + +The `obstacle_stop` module does the stop planning when there is a static obstacle near the trajectory. + +## Activation + +This module is activated if the launch parameter `launch_obstacle_stop_module` is set to true. + +## Inner-workings / Algorithms + +### Obstacle Filtering + +The obstacles meeting the following condition are determined as obstacles for stopping. + +- The object type is for stopping according to `obstacle_filtering.object_type.*`. +- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`. +- The object velocity along the ego's trajectory is smaller than `obstacle_filtering.obstacle_velocity_threshold_from_stop`. +- The object + - does not cross the ego's trajectory (\*1) + - and its collision time margin is large enough (\*2). + +#### NOTE + +##### \*1: Crossing obstacles + +Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold`. + +##### \*2: Enough collision time margin + +We predict the collision area and its time by the ego with a constant velocity motion and the obstacle with its predicted path. +Then, we calculate a collision time margin which is the difference of the time when the ego will be inside the collision area and the obstacle will be inside the collision area. +When this time margin is smaller than `obstacle_filtering.crossing_obstacle.collision_time_margin`, the margin is not enough. + +### Stop Planning + +The role of the stop planning is keeping a safe distance with static vehicle objects or dynamic/static non vehicle objects. + +The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles. +The safe distance is parameterized as `stop_planning.stop_margin`. +When it stops at the end of the trajectory, and obstacle is on the same point, the safe distance becomes `stop_planning.terminal_stop_margin`. + +When inserting the stop point, the required acceleration for the ego to stop in front of the stop point is calculated. +If the acceleration is less than `common.min_strong_accel`, the stop planning will be cancelled since this package does not assume a strong sudden brake for emergency. + +### Minor functions + +#### Prioritization of behavior module's stop point + +When stopping for a pedestrian walking on the crosswalk, the behavior module inserts the zero velocity in the trajectory in front of the crosswalk. +Also `autoware_obstacle_cruise_planner`'s stop planning also works, and the ego may not reach the behavior module's stop point since the safe distance defined in `autoware_obstacle_cruise_planner` may be longer than the behavior module's safe distance. +To resolve this non-alignment of the stop point between the behavior module and this module, `stop_planning.min_behavior_stop_margin` is defined. +In the case of the crosswalk described above, this module inserts the stop point with a distance `stop_planning.min_behavior_stop_margin` at minimum between the ego and obstacle. + +#### A function to keep the closest stop obstacle in target obstacles + +In order to keep the closest stop obstacle in the target obstacles, we check whether it is disappeared or not from the target obstacles in the `check_consistency` function. +If the previous closest stop obstacle is remove from the lists, we keep it in the lists for `obstacle_filtering.stop_obstacle_hold_time_threshold` seconds. +Note that if a new stop obstacle appears and the previous closest obstacle removes from the lists, we do not add it to the target obstacles again. + +## Debugging + +### Detection area + +Green polygons which is a detection area is visualized by `detection_polygons` in the `~/debug/marker` topic. + +![detection_area](./docs/detection_area.png) + +### Collision points + +Red points which are collision points with obstacle are visualized by `*_collision_points` for each behavior in the `~/debug/marker` topic. + +![collision_point](./docs/collision_point.png) + +### Obstacle for stop + +Red sphere which is an obstacle for stop is visualized by `obstacles_to_stop` in the `~/debug/marker` topic. + +Red wall which means a safe distance to stop if the ego's front meets the wall is visualized in the `~/virtual_wall` topic. + +![stop_visualization](./docs/stop_visualization.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml new file mode 100644 index 0000000000000..21dbf5f5bc611 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml @@ -0,0 +1,74 @@ +/**: + ros__parameters: + obstacle_stop: + option: + ignore_crossing_obstacle: true + suppress_sudden_stop: true + + stop_planning: + stop_margin : 5.0 # longitudinal margin to obstacle [m] + terminal_stop_margin : 3.0 # Stop margin at the goal. This value cannot exceed stop margin. [m] + min_behavior_stop_margin: 3.0 # [m] + + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] + + stop_on_curve: + enable_approaching: false + additional_stop_margin: 3.0 # [m] + min_stop_margin: 3.0 # [m] + + object_type_specified_params: + types: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as "sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as "sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. + + obstacle_filtering: + object_type: + pointcloud: false + + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + outside: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: false + pedestrian: false + + # hysteresis for velocity + obstacle_velocity_threshold_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + + max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint + max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint + + min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s] + stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle + + outside_obstacle: + max_lateral_time_margin: 4.5 # time threshold of lateral margin between the obstacles and ego's footprint [s] + num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego + pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss] + bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss] + + crossing_obstacle: + collision_time_margin : 1.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/collision_point.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/collision_point.png new file mode 100644 index 0000000000000..29a4f913891b3 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/collision_point.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/detection_area.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/detection_area.png new file mode 100644 index 0000000000000..6e1a831979fc0 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/detection_area.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/stop_visualization.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/stop_visualization.png new file mode 100644 index 0000000000000..f3cec33fd754b Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/stop_visualization.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml new file mode 100644 index 0000000000000..1425e72cdd886 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_obstacle_stop_module + 0.40.0 + obstacle stop feature in motion_velocity_planner + + Takayuki Murooka + Yuki Takagi + Maxime Clement + Berkay Karaman + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_motion_utils + autoware_motion_velocity_planner_common_universe + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + libboost-dev + pluginlib + rclcpp + tf2 + tier4_metric_msgs + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/plugins.xml new file mode 100644 index 0000000000000..c51cab7b7c2c8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/metrics_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/metrics_manager.hpp new file mode 100644 index 0000000000000..444799aeef615 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/metrics_manager.hpp @@ -0,0 +1,102 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef METRICS_MANAGER_HPP_ +#define METRICS_MANAGER_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MetricsManager +{ +public: + void init() { metrics_.clear(); } + + void calculate_metrics( + const std::string & module_name, const std::string & reason, + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::optional & stop_pose, + const std::optional & stop_obstacle) + { + // Create status + { + // Decision + Metric decision_metric; + decision_metric.name = module_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics_.push_back(decision_metric); + } + + if (stop_pose.has_value() && planner_data) { // Stop info + Metric stop_position_metric; + stop_position_metric.name = module_name + "/stop_position"; + stop_position_metric.unit = "string"; + const auto & p = stop_pose.value().position; + stop_position_metric.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + metrics_.push_back(stop_position_metric); + + Metric stop_orientation_metric; + stop_orientation_metric.name = module_name + "/stop_orientation"; + stop_orientation_metric.unit = "string"; + const auto & o = stop_pose.value().orientation; + stop_orientation_metric.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + + ", " + std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; + metrics_.push_back(stop_orientation_metric); + + const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( + traj_points, planner_data->current_odometry.pose.pose.position, stop_pose.value().position); + + Metric dist_to_stop_pose_metric; + dist_to_stop_pose_metric.name = module_name + "/distance_to_stop_pose"; + dist_to_stop_pose_metric.unit = "double"; + dist_to_stop_pose_metric.value = std::to_string(dist_to_stop_pose); + metrics_.push_back(dist_to_stop_pose_metric); + } + + if (stop_obstacle.has_value()) { + // Obstacle info + Metric collision_point_metric; + const auto & p = stop_obstacle.value().collision_point; + collision_point_metric.name = module_name + "/collision_point"; + collision_point_metric.unit = "string"; + collision_point_metric.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + metrics_.push_back(collision_point_metric); + } + } + + MetricArray create_metric_array(const rclcpp::Time & current_time) + { + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), metrics_.begin(), metrics_.end()); + return metrics_msg; + } + +private: + std::vector metrics_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // METRICS_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp new file mode 100644 index 0000000000000..d45ca3894587d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -0,0 +1,1180 @@ +// Copyright 2025 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_stop_module.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +namespace +{ +template +bool is_in_vector(const T variable, const std::vector & vec) +{ + return std::find(vec.begin(), vec.end(), variable) != vec.end(); +} + +double calc_minimum_distance_to_stop( + const double initial_vel, const double max_acc, const double min_acc) +{ + if (initial_vel < 0.0) { + return -std::pow(initial_vel, 2) / 2.0 / max_acc; + } + + return -std::pow(initial_vel, 2) / 2.0 / min_acc; +} + +autoware::universe_utils::Point2d convert_point(const geometry_msgs::msg::Point & p) +{ + return autoware::universe_utils::Point2d{p.x, p.y}; +} + +std::vector resample_trajectory_points( + const std::vector & traj_points, const double interval) +{ + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +std::vector resample_highest_confidence_predicted_paths( + const std::vector & predicted_paths, const double time_interval, + const double time_horizon, const size_t num_paths) +{ + std::vector sorted_paths = predicted_paths; + + // Sort paths by descending confidence + std::sort( + sorted_paths.begin(), sorted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; }); + + std::vector selected_paths; + size_t path_count = 0; + + // Select paths that meet the confidence thresholds + for (const auto & path : sorted_paths) { + if (path_count < num_paths) { + selected_paths.push_back(path); + ++path_count; + } + } + + // Resample each selected path + std::vector resampled_paths; + for (const auto & path : selected_paths) { + if (path.path.size() < 2) { + continue; + } + resampled_paths.push_back( + autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + } + + return resampled_paths; +} + +template +std::vector concat_vectors( + const std::vector & first_vector, const std::vector & second_vector) +{ + std::vector concatenated_vector; + concatenated_vector.insert(concatenated_vector.end(), first_vector.begin(), first_vector.end()); + concatenated_vector.insert(concatenated_vector.end(), second_vector.begin(), second_vector.end()); + return concatenated_vector; +} + +double calc_dist_to_bumper(const bool is_driving_forward, const VehicleInfo & vehicle_info) +{ + if (is_driving_forward) { + return std::abs(vehicle_info.max_longitudinal_offset_m); + } + return std::abs(vehicle_info.min_longitudinal_offset_m); +} + +Float64Stamped create_float64_stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} + +double calc_time_to_reach_collision_point( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double dist_to_bumper, + const double min_velocity_to_reach_collision_point) +{ + const double dist_from_ego_to_obstacle = + std::abs(autoware::motion_utils::calcSignedArcLength( + traj_points, odometry.pose.pose.position, collision_point)) - + dist_to_bumper; + return dist_from_ego_to_obstacle / + std::max(min_velocity_to_reach_collision_point, std::abs(odometry.twist.twist.linear.x)); +} +} // namespace + +void ObstacleStopModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + // ros parameters + ignore_crossing_obstacle_ = + getOrDeclareParameter(node, "obstacle_stop.option.ignore_crossing_obstacle"); + suppress_sudden_stop_ = + getOrDeclareParameter(node, "obstacle_stop.option.suppress_sudden_stop"); + + common_param_ = CommonParam(node); + stop_planning_param_ = StopPlanningParam(node, common_param_); + obstacle_filtering_param_ = ObstacleFilteringParam(node); + + // common publisher + processing_time_publisher_ = + node.create_publisher("~/debug/obstacle_stop/processing_time_ms", 1); + virtual_wall_publisher_ = + node.create_publisher("~/obstacle_stop/virtual_walls", 1); + debug_publisher_ = + node.create_publisher("~/obstacle_stop/debug_markers", 1); + + // module publisher + debug_stop_planning_info_pub_ = + node.create_publisher("~/debug/stop_planning_info", 1); + metrics_pub_ = node.create_publisher("~/stop/metrics", 10); + processing_time_detail_pub_ = node.create_publisher( + "~/debug/processing_time_detail_ms/obstacle_stop", 1); + // interface publisher + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "obstacle_stop"); + planning_factor_interface_ = + std::make_unique( + &node, "obstacle_stop"); + + // time keeper + time_keeper_ = std::make_shared(processing_time_detail_pub_); +} + +void ObstacleStopModule::update_parameters(const std::vector & parameters) +{ + using universe_utils::updateParam; + + updateParam( + parameters, "obstacle_stop.option.ignore_crossing_obstacle", ignore_crossing_obstacle_); + updateParam(parameters, "obstacle_stop.option.suppress_sudden_stop", suppress_sudden_stop_); +} + +VelocityPlanningResult ObstacleStopModule::plan( + const std::vector & raw_trajectory_points, + [[maybe_unused]] const std::vector & + smoothed_trajectory_points, + const std::shared_ptr planner_data) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. init variables + stop_watch_.tic(); + debug_data_ptr_ = std::make_shared(); + metrics_manager_.init(); + const double dist_to_bumper = + calc_dist_to_bumper(planner_data->is_driving_forward, planner_data->vehicle_info_); + stop_planning_debug_info_.reset(); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data->current_odometry.twist.twist.linear.x); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_ACCELERATION, + planner_data->current_acceleration.accel.accel.linear.x); + trajectory_polygon_for_inside_map_.clear(); + trajectory_polygon_for_outside_ = std::nullopt; + decimated_traj_polys_ = std::nullopt; + + // 2. pre-process + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + raw_trajectory_points, planner_data->current_odometry.pose.pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold, + planner_data->trajectory_polygon_collision_check.decimate_trajectory_step_length, + stop_planning_param_.stop_margin); + + // 3. filter obstacles of predicted objects + const auto stop_obstacles_for_predicted_object = filter_stop_obstacle_for_predicted_object( + planner_data->current_odometry, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold, + rclcpp::Time(planner_data->predicted_objects_header.stamp), raw_trajectory_points, + decimated_traj_points, planner_data->objects, planner_data->is_driving_forward, + planner_data->vehicle_info_, dist_to_bumper, planner_data->trajectory_polygon_collision_check); + + // 4. filter obstacles of point cloud + const auto stop_obstacles_for_point_cloud = + std::vector{}; // filter_stop_obstacle_for_point_cloud(); + + // 5. concat stop obstacles by predicted objects and point cloud + const auto stop_obstacles = stop_obstacles_for_predicted_object; + concat_vectors(stop_obstacles_for_predicted_object, stop_obstacles_for_point_cloud); + + // 6. plan stop + const auto stop_point = + plan_stop(planner_data, raw_trajectory_points, stop_obstacles, dist_to_bumper); + + // 7. publish messages for debugging + publish_debug_info(); + + // 8. generate VelocityPlanningResult + VelocityPlanningResult result; + if (stop_point) { + result.stop_points.push_back(*stop_point); + } + + return result; +} + +std::vector ObstacleStopModule::filter_stop_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, const bool is_driving_forward, + const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & current_pose = odometry.pose.pose; + + std::vector stop_obstacles; + for (const auto & object : objects) { + autoware::universe_utils::ScopedTimeTrack st_for_each_object("for_each_object", *time_keeper_); + + // 1. rough filtering + // 1.1. Check if the obstacle is in front of the ego. + const double lon_dist_from_ego_to_obj = + object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + if (lon_dist_from_ego_to_obj < 0.0) { + continue; + } + + // 1.2. Check if the rough lateral distance is smaller than the threshold. + // TODO(murooka) outside obstacle stop was removed. + const double min_lat_dist_to_traj_poly = + utils::calc_possible_min_dist_from_obj_to_traj_poly(object, traj_points, vehicle_info); + const uint8_t obj_label = object->predicted_object.classification.at(0).label; + if (get_max_lat_margin(obj_label) < min_lat_dist_to_traj_poly) { + continue; + } + + // 2. precise filtering + const auto & decimated_traj_polys = [&]() { + autoware::universe_utils::ScopedTimeTrack st_get_decimated_traj_polys( + "get_decimated_traj_polys", *time_keeper_); + return get_decimated_traj_polys( + traj_points, current_pose, vehicle_info, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold, trajectory_polygon_collision_check); + }(); + const double dist_from_obj_to_traj_poly = [&]() { + autoware::universe_utils::ScopedTimeTrack st_get_dist_to_traj_poly( + "get_dist_to_traj_poly", *time_keeper_); + return object->get_dist_to_traj_poly(decimated_traj_polys); + }(); + + // 2.1. filter target object inside trajectory + const auto inside_stop_obstacle = filter_inside_stop_obstacle_for_predicted_object( + odometry, traj_points, decimated_traj_points, object, predicted_objects_stamp, + dist_from_obj_to_traj_poly, vehicle_info, dist_to_bumper, trajectory_polygon_collision_check); + if (inside_stop_obstacle) { + stop_obstacles.push_back(*inside_stop_obstacle); + continue; + } + + // 2.2 filter target object outside trajectory + const auto outside_stop_obstacle = filter_outside_stop_obstacle_for_predicted_object( + odometry, traj_points, decimated_traj_points, predicted_objects_stamp, object, + dist_from_obj_to_traj_poly, is_driving_forward, vehicle_info, dist_to_bumper, + trajectory_polygon_collision_check); + if (outside_stop_obstacle) { + stop_obstacles.push_back(*outside_stop_obstacle); + } + } + + // Check target obstacles' consistency + check_consistency(predicted_objects_stamp, objects, stop_obstacles); + + prev_stop_obstacles_ = stop_obstacles; + + return stop_obstacles; +} + +std::optional ObstacleStopModule::filter_inside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const VehicleInfo & vehicle_info, + const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & predicted_object = object->predicted_object; + const auto & obj_pose = object->get_predicted_pose(clock_->now(), predicted_objects_stamp); + + // 1. filter by label + const uint8_t obj_label = predicted_object.classification.at(0).label; + if (!is_in_vector(obj_label, obstacle_filtering_param_.inside_stop_object_types)) { + return std::nullopt; + } + + // 2. filter by lateral distance + const double max_lat_margin = get_max_lat_margin(obj_label); + // NOTE: max_lat_margin can be negative, so apply std::max with 1e-3. + if (std::max(max_lat_margin, 1e-3) <= dist_from_obj_poly_to_traj_poly) { + return std::nullopt; + } + + // 3. filter by velocity + if (!is_inside_stop_obstacle_velocity(object, traj_points)) { + return std::nullopt; + } + + // calculate collision points with trajectory with lateral stop margin + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_polys_with_lat_margin = get_trajectory_polygon_for_inside( + decimated_traj_points, vehicle_info, odometry.pose.pose, max_lat_margin, + p.enable_to_consider_current_pose, p.time_to_convergence, p.decimate_trajectory_step_length); + debug_data_ptr_->decimated_traj_polys = decimated_traj_polys_with_lat_margin; + + // 4. check if the obstacle really collides with the trajectory + const auto collision_point = polygon_utils::get_collision_point( + decimated_traj_points, decimated_traj_polys_with_lat_margin, obj_pose, clock_->now(), + predicted_object.shape, dist_to_bumper); + if (!collision_point) { + return std::nullopt; + } + + // 5. filter if the obstacle will cross and go out of trajectory soon + if ( + ignore_crossing_obstacle_ && + is_crossing_transient_obstacle( + odometry, traj_points, decimated_traj_points, object, dist_to_bumper, + decimated_traj_polys_with_lat_margin, collision_point)) { + return std::nullopt; + } + + return StopObstacle{ + autoware::universe_utils::toHexString(predicted_object.object_id), + predicted_objects_stamp, + predicted_object.classification.at(0), + obj_pose, + predicted_object.shape, + object->get_lon_vel_relative_to_traj(traj_points), + collision_point->first, + collision_point->second}; +} + +bool ObstacleStopModule::is_inside_stop_obstacle_velocity( + const std::shared_ptr object, + const std::vector & traj_points) const +{ + const bool is_prev_object_stop = + utils::get_obstacle_from_uuid( + prev_stop_obstacles_, + autoware::universe_utils::toHexString(object->predicted_object.object_id)) + .has_value(); + + if (is_prev_object_stop) { + if ( + obstacle_filtering_param_.obstacle_velocity_threshold_from_stop < + object->get_lon_vel_relative_to_traj(traj_points)) { + return false; + } + return true; + } + if ( + object->get_lon_vel_relative_to_traj(traj_points) < + obstacle_filtering_param_.obstacle_velocity_threshold_to_stop) { + return true; + } + return false; +} + +bool ObstacleStopModule::is_crossing_transient_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const double dist_to_bumper, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::optional> & collision_point) const +{ + // calculate the time to reach the collision point + const double time_to_reach_stop_point = calc_time_to_reach_collision_point( + odometry, collision_point->first, traj_points, + stop_planning_param_.min_behavior_stop_margin + dist_to_bumper, + obstacle_filtering_param_.min_velocity_to_reach_collision_point); + if ( + time_to_reach_stop_point <= obstacle_filtering_param_.crossing_obstacle_collision_time_margin) { + return false; + } + + // get the highest confident predicted paths + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + constexpr double prediction_resampling_time_interval = 0.1; + constexpr double prediction_resampling_time_horizon = 10.0; + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, prediction_resampling_time_horizon, 1); + if (resampled_predicted_paths.empty() || resampled_predicted_paths.front().path.empty()) { + return false; + } + + // predict object pose when the ego reaches the collision point + const auto future_obj_pose = [&]() { + const auto opt_future_obj_pose = autoware::object_recognition_utils::calcInterpolatedPose( + resampled_predicted_paths.front(), + time_to_reach_stop_point - obstacle_filtering_param_.crossing_obstacle_collision_time_margin); + if (opt_future_obj_pose) { + return *opt_future_obj_pose; + } + return resampled_predicted_paths.front().path.back(); + }(); + + // check if the ego will collide with the obstacle + auto future_predicted_object = object->predicted_object; + future_predicted_object.kinematics.initial_pose_with_covariance.pose = future_obj_pose; + const auto future_collision_point = polygon_utils::get_collision_point( + decimated_traj_points, decimated_traj_polys_with_lat_margin, + future_predicted_object.kinematics.initial_pose_with_covariance.pose, clock_->now(), + future_predicted_object.shape, dist_to_bumper); + const bool no_collision = !future_collision_point; + + return no_collision; +} + +std::optional ObstacleStopModule::filter_outside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const rclcpp::Time & predicted_objects_stamp, const std::shared_ptr object, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & object_id = + autoware::universe_utils::toHexString(object->predicted_object.object_id); + + // 1. filter by label + const uint8_t obj_label = object->predicted_object.classification.at(0).label; + if (!is_in_vector(obj_label, obstacle_filtering_param_.outside_stop_object_types)) { + return std::nullopt; + } + + // 2. filter by lateral distance + const double max_lat_margin = obj_label == ObjectClassification::UNKNOWN + ? obstacle_filtering_param_.max_lat_margin_against_unknown + : obstacle_filtering_param_.max_lat_margin; + if (dist_from_obj_poly_to_traj_poly < std::max(max_lat_margin, 1e-3)) { + // Obstacle that is not inside of trajectory + return std::nullopt; + } + + const auto time_to_traj = dist_from_obj_poly_to_traj_poly / + std::max(1e-6, object->get_lat_vel_relative_to_traj(traj_points)); + if (time_to_traj > obstacle_filtering_param_.outside_max_lat_time_margin) { + RCLCPP_DEBUG( + logger_, "[Stop] Ignore outside obstacle (%s) since it's far from trajectory.", + object_id.substr(0, 4).c_str()); + return std::nullopt; + } + + // brkay54: For the pedestrians and bicycles, we need to check the collision point by thinking + // they will stop with a predefined deceleration rate to avoid unnecessary stops. + double resample_time_horizon = 10.0; + if (obj_label == ObjectClassification::PEDESTRIAN) { + resample_time_horizon = + std::sqrt( + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, 2) + + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y, 2)) / + (2.0 * obstacle_filtering_param_.outside_pedestrian_deceleration_rate); + } else if (obj_label == ObjectClassification::BICYCLE) { + resample_time_horizon = + std::sqrt( + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, 2) + + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y, 2)) / + (2.0 * obstacle_filtering_param_.outside_bicycle_deceleration_rate); + } + + // Get the highest confidence predicted path + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + constexpr double prediction_resampling_time_interval = 0.1; + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, resample_time_horizon, + obstacle_filtering_param_.outside_num_of_predicted_paths); + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } + + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_polys_with_lat_margin = get_trajectory_polygon_for_outside( + decimated_traj_points, vehicle_info, odometry.pose.pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + + const auto get_collision_point = + [&]() -> std::optional> { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_point = create_collision_point_for_outside_stop_obstacle( + odometry, traj_points, decimated_traj_points, decimated_traj_polys_with_lat_margin, object, + predicted_objects_stamp, predicted_path, max_lat_margin, is_driving_forward, vehicle_info, + dist_to_bumper, trajectory_polygon_collision_check.decimate_trajectory_step_length); + if (collision_point) { + return collision_point; + } + } + return std::nullopt; + }; + + const auto collision_point = get_collision_point(); + + if (collision_point) { + return StopObstacle{ + object_id, + predicted_objects_stamp, + object->predicted_object.classification.at(0), + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->predicted_object.shape, + object->get_lon_vel_relative_to_traj(traj_points), + collision_point->first, + collision_point->second}; + } + return std::nullopt; +} + +std::optional> +ObstacleStopModule::create_collision_point_for_outside_stop_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const PredictedPath & resampled_predicted_path, double max_lat_margin, + const bool is_driving_forward, const VehicleInfo & vehicle_info, const double dist_to_bumper, + const double decimate_trajectory_step_length) const +{ + const auto & object_id = + autoware::universe_utils::toHexString(object->predicted_object.object_id); + + std::vector collision_index; + const auto collision_points = polygon_utils::get_collision_points( + decimated_traj_points, decimated_traj_polys, predicted_objects_stamp, resampled_predicted_path, + object->predicted_object.shape, clock_->now(), is_driving_forward, collision_index, + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape) + + decimate_trajectory_step_length + + std::hypot( + vehicle_info.vehicle_length_m, vehicle_info.vehicle_width_m * 0.5 + max_lat_margin)); + if (collision_points.empty()) { + RCLCPP_DEBUG( + logger_, + "[Stop] Ignore outside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + const double collision_time_margin = + calc_collision_time_margin(odometry, collision_points, traj_points, dist_to_bumper); + if (obstacle_filtering_param_.crossing_obstacle_collision_time_margin < collision_time_margin) { + RCLCPP_DEBUG( + logger_, "[Stop] Ignore outside obstacle (%s) since it will not collide with the ego.", + object_id.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + return polygon_utils::get_collision_point( + decimated_traj_points, collision_index.front(), collision_points, dist_to_bumper); +} + +std::optional ObstacleStopModule::plan_stop( + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::vector & stop_obstacles, const double dist_to_bumper) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (stop_obstacles.empty()) { + const auto markers = + autoware::motion_utils::createDeletedStopVirtualWallMarker(clock_->now(), 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return std::nullopt; + } + + std::optional determined_stop_obstacle{}; + std::optional determined_zero_vel_dist{}; + std::optional determined_desired_stop_margin{}; + + const auto closest_stop_obstacles = get_closest_stop_obstacles(stop_obstacles); + for (const auto & stop_obstacle : closest_stop_obstacles) { + const auto ego_segment_idx = + planner_data->find_segment_index(traj_points, planner_data->current_odometry.pose.pose); + + // calculate dist to collide + const double dist_to_collide_on_ref_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, ego_segment_idx) + + stop_obstacle.dist_to_collide_on_decimated_traj; + + // calculate desired stop margin + const double desired_stop_margin = calc_desired_stop_margin( + planner_data, traj_points, stop_obstacle, dist_to_bumper, ego_segment_idx, + dist_to_collide_on_ref_traj); + + // calculate stop point against the obstacle + const auto candidate_zero_vel_dist = calc_candidate_zero_vel_dist( + planner_data, traj_points, stop_obstacle, dist_to_collide_on_ref_traj, desired_stop_margin); + if (!candidate_zero_vel_dist) { + continue; + } + + if (determined_stop_obstacle) { + const bool is_same_param_types = + (stop_obstacle.classification.label == determined_stop_obstacle->classification.label); + if ( + (is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj > + determined_stop_obstacle->dist_to_collide_on_decimated_traj) || + (!is_same_param_types && *candidate_zero_vel_dist > determined_zero_vel_dist)) { + continue; + } + } + determined_zero_vel_dist = *candidate_zero_vel_dist; + determined_stop_obstacle = stop_obstacle; + determined_desired_stop_margin = desired_stop_margin; + } + + if (!determined_zero_vel_dist) { + // delete marker + const auto markers = + autoware::motion_utils::createDeletedStopVirtualWallMarker(clock_->now(), 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return std::nullopt; + } + + // Hold previous stop distance if necessary + hold_previous_stop_if_necessary(planner_data, traj_points, determined_zero_vel_dist); + + // Insert stop point + const auto stop_point = calc_stop_point( + planner_data, traj_points, dist_to_bumper, determined_stop_obstacle, determined_zero_vel_dist); + + // set stop_planning_debug_info + set_stop_planning_debug_info(determined_stop_obstacle, determined_desired_stop_margin); + + return stop_point; +} + +double ObstacleStopModule::calc_desired_stop_margin( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const size_t ego_segment_idx, + const double dist_to_collide_on_ref_traj) +{ + // calculate default stop margin + const double default_stop_margin = [&]() { + const auto ref_traj_length = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, traj_points.size() - 1); + if (dist_to_collide_on_ref_traj > ref_traj_length) { + // Use terminal margin (terminal_stop_margin) for obstacle stop + return stop_planning_param_.terminal_stop_margin; + } + return stop_planning_param_.stop_margin; + }(); + + // calculate stop margin on curve + const double stop_margin_on_curve = calc_margin_from_obstacle_on_curve( + planner_data, traj_points, stop_obstacle, dist_to_bumper, default_stop_margin); + + // calculate stop margin considering behavior's stop point + // NOTE: If behavior stop point is ahead of the closest_obstacle_stop point within a certain + // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance + const auto closest_behavior_stop_idx = + autoware::motion_utils::searchZeroVelocityIndex(traj_points, ego_segment_idx + 1); + if (closest_behavior_stop_idx) { + const double closest_behavior_stop_dist_on_ref_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, *closest_behavior_stop_idx); + const double stop_dist_diff = + closest_behavior_stop_dist_on_ref_traj - (dist_to_collide_on_ref_traj - stop_margin_on_curve); + if (0.0 < stop_dist_diff && stop_dist_diff < stop_margin_on_curve) { + return stop_planning_param_.min_behavior_stop_margin; + } + } + return stop_margin_on_curve; +} + +std::optional ObstacleStopModule::calc_candidate_zero_vel_dist( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_collide_on_ref_traj, const double desired_stop_margin) +{ + double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_stop_margin); + if (suppress_sudden_stop_) { + const auto acceptable_stop_acc = [&]() -> std::optional { + if (stop_planning_param_.get_param_type(stop_obstacle.classification) == "default") { + return common_param_.limit_min_accel; + } + const double distance_to_judge_suddenness = std::min( + calc_minimum_distance_to_stop( + planner_data->current_odometry.twist.twist.linear.x, common_param_.limit_max_accel, + stop_planning_param_.get_param(stop_obstacle.classification).sudden_object_acc_threshold), + stop_planning_param_.get_param(stop_obstacle.classification).sudden_object_dist_threshold); + if (candidate_zero_vel_dist > distance_to_judge_suddenness) { + return common_param_.limit_min_accel; + } + if (stop_planning_param_.get_param(stop_obstacle.classification).abandon_to_stop) { + RCLCPP_WARN( + rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"), + "[Cruise] abandon to stop against %s object", + stop_planning_param_.object_types_maps.at(stop_obstacle.classification.label).c_str()); + return std::nullopt; + } else { + return stop_planning_param_.get_param(stop_obstacle.classification).limit_min_acc; + } + }(); + if (!acceptable_stop_acc) { + return std::nullopt; + } + + const double acceptable_stop_pos = + autoware::motion_utils::calcSignedArcLength( + traj_points, 0, planner_data->current_odometry.pose.pose.position) + + calc_minimum_distance_to_stop( + planner_data->current_odometry.twist.twist.linear.x, common_param_.limit_max_accel, + acceptable_stop_acc.value()); + if (acceptable_stop_pos > candidate_zero_vel_dist) { + candidate_zero_vel_dist = acceptable_stop_pos; + } + } + return candidate_zero_vel_dist; +} + +void ObstacleStopModule::hold_previous_stop_if_necessary( + const std::shared_ptr planner_data, + const std::vector & traj_points, + std::optional & determined_zero_vel_dist) +{ + if ( + std::abs(planner_data->current_odometry.twist.twist.linear.x) < + stop_planning_param_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, traj_points.front().pose); + const double diff_dist_front_points = autoware::motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) < + stop_planning_param_.hold_stop_distance_threshold) { + determined_zero_vel_dist.value() = prev_zero_vel_dist; + } + } +} + +std::optional ObstacleStopModule::calc_stop_point( + const std::shared_ptr planner_data, + const std::vector & traj_points, const double dist_to_bumper, + const std::optional & determined_stop_obstacle, + const std::optional & determined_zero_vel_dist) +{ + auto output_traj_points = traj_points; + + // insert stop point + const auto zero_vel_idx = + autoware::motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); + if (!zero_vel_idx) { + return std::nullopt; + } + + // virtual wall marker for stop obstacle + const auto markers = autoware::motion_utils::createStopVirtualWallMarker( + output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", clock_->now(), 0, dist_to_bumper, + "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); + + // update planning factor + const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; + planning_factor_interface_->add( + output_traj_points, planner_data->current_odometry.pose.pose, stop_pose, PlanningFactor::STOP, + SafetyFactorArray{}); + + // Store stop reason debug data + metrics_manager_.calculate_metrics( + "PlannerInterface", "stop", planner_data, traj_points, stop_pose, *determined_stop_obstacle); + + prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value()); + + return stop_pose.position; +} + +void ObstacleStopModule::set_stop_planning_debug_info( + const std::optional & determined_stop_obstacle, + const std::optional & determined_desired_stop_margin) const +{ + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, + determined_stop_obstacle->dist_to_collide_on_decimated_traj); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, + determined_stop_obstacle->velocity); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, + determined_desired_stop_margin.value()); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); +} + +void ObstacleStopModule::publish_debug_info() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. debug marker + MarkerArray debug_marker; + + // 1.1. obstacles + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_stop.size(); ++i) { + // obstacle + const auto obstacle_marker = utils::get_object_marker( + debug_data_ptr_->obstacles_to_stop.at(i).pose, i, "obstacles", 1.0, 0.0, 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision point + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; + debug_marker.markers.push_back(collision_point_marker); + } + + // 1.2. intentionally ignored obstacles + for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) { + const auto marker = utils::get_object_marker( + debug_data_ptr_->intentionally_ignored_obstacles.at(i) + ->predicted_object.kinematics.initial_pose_with_covariance.pose, + i, "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); + debug_marker.markers.push_back(marker); + } + + // 1.3. detection area + auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { + for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { + const auto & current_point = decimated_traj_poly.outer().at(dp_idx); + const auto & next_point = + decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); + + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(decimated_traj_polys_marker); + + debug_publisher_->publish(debug_marker); + + // 2. virtual wall + virtual_wall_publisher_->publish(debug_data_ptr_->stop_wall_marker); + + // 3. stop planning info + const auto stop_debug_msg = stop_planning_debug_info_.convert_to_message(clock_->now()); + debug_stop_planning_info_pub_->publish(stop_debug_msg); + + // 4. objects of interest + objects_of_interest_marker_interface_->publishMarkerArray(); + + // 5. metrics + const auto metrics_msg = metrics_manager_.create_metric_array(clock_->now()); + metrics_pub_->publish(metrics_msg); + + // 6. processing time + processing_time_publisher_->publish(create_float64_stamped(clock_->now(), stop_watch_.toc())); + + // 7. planning factor + planning_factor_interface_->publish(); +} + +double ObstacleStopModule::calc_collision_time_margin( + const Odometry & odometry, const std::vector & collision_points, + const std::vector & traj_points, const double dist_to_bumper) const +{ + const double time_to_reach_stop_point = calc_time_to_reach_collision_point( + odometry, collision_points.front().point, traj_points, + stop_planning_param_.min_behavior_stop_margin + dist_to_bumper, + obstacle_filtering_param_.min_velocity_to_reach_collision_point); + + const double time_to_leave_collision_point = + time_to_reach_stop_point + + dist_to_bumper / std::max( + obstacle_filtering_param_.min_velocity_to_reach_collision_point, + odometry.twist.twist.linear.x); + + const double time_to_start_cross = + (rclcpp::Time(collision_points.front().stamp) - clock_->now()).seconds(); + const double time_to_end_cross = + (rclcpp::Time(collision_points.back().stamp) - clock_->now()).seconds(); + + if (time_to_leave_collision_point < time_to_start_cross) { // Ego goes first. + return time_to_start_cross - time_to_reach_stop_point; + } + if (time_to_end_cross < time_to_reach_stop_point) { // Obstacle goes first. + return time_to_reach_stop_point - time_to_end_cross; + } + return 0.0; // Ego and obstacle will collide. +} + +std::vector ObstacleStopModule::get_trajectory_polygon_for_inside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const +{ + if (trajectory_polygon_for_inside_map_.count(lat_margin) == 0) { + const auto traj_polys = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_ego_pose, lat_margin, + enable_to_consider_current_pose, time_to_convergence, decimate_trajectory_step_length); + trajectory_polygon_for_inside_map_.emplace(lat_margin, traj_polys); + } + return trajectory_polygon_for_inside_map_.at(lat_margin); +} + +std::vector ObstacleStopModule::get_trajectory_polygon_for_outside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const +{ + if (!trajectory_polygon_for_outside_) { + trajectory_polygon_for_outside_ = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_ego_pose, lat_margin, + enable_to_consider_current_pose, time_to_convergence, decimate_trajectory_step_length); + } + return *trajectory_polygon_for_outside_; +} + +void ObstacleStopModule::check_consistency( + const rclcpp::Time & current_time, + const std::vector> & objects, + std::vector & stop_obstacles) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + const auto object_itr = std::find_if( + objects.begin(), objects.end(), + [&prev_closest_stop_obstacle](const std::shared_ptr & o) { + return autoware::universe_utils::toHexString(o->predicted_object.object_id) == + prev_closest_stop_obstacle.uuid; + }); + // If previous closest obstacle disappear from the perception result, do nothing anymore. + if (object_itr == objects.end()) { + continue; + } + + const auto is_disappeared_from_stop_obstacle = std::none_of( + stop_obstacles.begin(), stop_obstacles.end(), + [&prev_closest_stop_obstacle](const StopObstacle & so) { + return so.uuid == prev_closest_stop_obstacle.uuid; + }); + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied + const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); + if ( + (*object_itr)->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x < + obstacle_filtering_param_.obstacle_velocity_threshold_from_stop && + elapsed_time < obstacle_filtering_param_.stop_obstacle_hold_time_threshold) { + stop_obstacles.push_back(prev_closest_stop_obstacle); + } + } + } + + prev_closest_stop_obstacles_ = get_closest_stop_obstacles(stop_obstacles); +} + +double ObstacleStopModule::calc_margin_from_obstacle_on_curve( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const double default_stop_margin) const +{ + if ( + !stop_planning_param_.enable_approaching_on_curve || obstacle_filtering_param_.use_pointcloud) { + return default_stop_margin; + } + + // calculate short trajectory points towards obstacle + const size_t obj_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, stop_obstacle.collision_point); + std::vector short_traj_points{traj_points.at(obj_segment_idx + 1)}; + double sum_short_traj_length{0.0}; + for (int i = obj_segment_idx; 0 <= i; --i) { + short_traj_points.push_back(traj_points.at(i)); + + if ( + 1 < short_traj_points.size() && + stop_planning_param_.stop_margin + dist_to_bumper < sum_short_traj_length) { + break; + } + sum_short_traj_length += + autoware::universe_utils::calcDistance2d(traj_points.at(i), traj_points.at(i + 1)); + } + std::reverse(short_traj_points.begin(), short_traj_points.end()); + if (short_traj_points.size() < 2) { + return default_stop_margin; + } + + // calculate collision index between straight line from ego pose and object + const auto calculate_distance_from_straight_ego_path = + [&](const auto & ego_pose, const auto & object_polygon) { + const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose( + ego_pose, stop_planning_param_.stop_margin + 3.0, 0.0, 0.0); + const auto ego_straight_segment = autoware::universe_utils::Segment2d{ + convert_point(ego_pose.position), convert_point(forward_ego_pose.position)}; + return boost::geometry::distance(ego_straight_segment, object_polygon); + }; + const auto resampled_short_traj_points = resample_trajectory_points(short_traj_points, 0.5); + const auto object_polygon = + autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto collision_idx = [&]() -> std::optional { + for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { + const double dist_to_obj = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(i).pose, object_polygon); + if (dist_to_obj < planner_data->vehicle_info_.vehicle_width_m / 2.0) { + return i; + } + } + return std::nullopt; + }(); + if (!collision_idx) { + return stop_planning_param_.min_stop_margin_on_curve; + } + if (*collision_idx == 0) { + return default_stop_margin; + } + + // calculate margin from obstacle + const double partial_segment_length = [&]() { + const double collision_segment_length = autoware::universe_utils::calcDistance2d( + resampled_short_traj_points.at(*collision_idx - 1), + resampled_short_traj_points.at(*collision_idx)); + const double prev_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); + const double next_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx).pose, object_polygon); + return (next_dist - planner_data->vehicle_info_.vehicle_width_m / 2.0) / + (next_dist - prev_dist) * collision_segment_length; + }(); + + const double short_margin_from_obstacle = + partial_segment_length + + autoware::motion_utils::calcSignedArcLength( + resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - + dist_to_bumper + stop_planning_param_.additional_stop_margin_on_curve; + + return std::min( + default_stop_margin, + std::max(stop_planning_param_.min_stop_margin_on_curve, short_margin_from_obstacle)); +} + +std::vector ObstacleStopModule::get_closest_stop_obstacles( + const std::vector & stop_obstacles) +{ + std::vector candidates{}; + for (const auto & stop_obstacle : stop_obstacles) { + const auto itr = + std::find_if(candidates.begin(), candidates.end(), [&stop_obstacle](const StopObstacle & co) { + return co.classification.label == stop_obstacle.classification.label; + }); + if (itr == candidates.end()) { + candidates.emplace_back(stop_obstacle); + } else if ( + stop_obstacle.dist_to_collide_on_decimated_traj < itr->dist_to_collide_on_decimated_traj) { + *itr = stop_obstacle; + } + } + return candidates; +} + +double ObstacleStopModule::get_max_lat_margin(const uint8_t obj_label) const +{ + if (obj_label == ObjectClassification::UNKNOWN) { + return obstacle_filtering_param_.max_lat_margin_against_unknown; + } + return obstacle_filtering_param_.max_lat_margin; +} + +std::vector ObstacleStopModule::get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + if (!decimated_traj_polys_) { + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold, + p.decimate_trajectory_step_length, p.goal_extended_trajectory_length); + decimated_traj_polys_ = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + } + return *decimated_traj_polys_; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::ObstacleStopModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp new file mode 100644 index 0000000000000..e1f94eb243814 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp @@ -0,0 +1,196 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_STOP_MODULE_HPP_ +#define OBSTACLE_STOP_MODULE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "metrics_manager.hpp" +#include "parameters.hpp" +#include "stop_planning_debug_info.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class ObstacleStopModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + std::string get_module_name() const override { return module_name_; } + + VelocityPlanningResult plan( + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, + const std::shared_ptr planner_data) override; + +private: + std::string module_name_{}; + rclcpp::Clock::SharedPtr clock_{}; + + // ros parameters + bool ignore_crossing_obstacle_{}; + bool suppress_sudden_stop_{}; + CommonParam common_param_{}; + StopPlanningParam stop_planning_param_{}; + ObstacleFilteringParam obstacle_filtering_param_{}; + + // module publisher + rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_{}; + rclcpp::Publisher::SharedPtr metrics_pub_{}; + rclcpp::Publisher::SharedPtr + processing_time_detail_pub_{}; + + // interface publisher + std::unique_ptr + objects_of_interest_marker_interface_{}; + + // internal variables + mutable StopPlanningDebugInfo stop_planning_debug_info_{}; + mutable std::shared_ptr debug_data_ptr_{}; + std::vector prev_closest_stop_obstacles_{}; + std::vector prev_stop_obstacles_{}; + MetricsManager metrics_manager_{}; + // previous trajectory and distance to stop + // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or + // crossing lanes. + std::optional, double>> prev_stop_distance_info_{ + std::nullopt}; + autoware::universe_utils::StopWatch stop_watch_{}; + mutable std::unordered_map> trajectory_polygon_for_inside_map_{}; + mutable std::optional> trajectory_polygon_for_outside_{std::nullopt}; + mutable std::optional> decimated_traj_polys_{std::nullopt}; + mutable std::shared_ptr time_keeper_{}; + + std::vector get_trajectory_polygon_for_inside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const; + + std::vector get_trajectory_polygon_for_outside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const; + + std::vector filter_stop_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, + const bool is_driving_forward, const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check); + + std::optional plan_stop( + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::vector & stop_obstacles, const double dist_to_bumper); + double calc_desired_stop_margin( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const size_t ego_segment_idx, + const double dist_to_collide_on_ref_traj); + std::optional calc_candidate_zero_vel_dist( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_collide_on_ref_traj, const double desired_stop_margin); + void hold_previous_stop_if_necessary( + const std::shared_ptr planner_data, + const std::vector & traj_points, + std::optional & determined_zero_vel_dist); + std::optional calc_stop_point( + const std::shared_ptr planner_data, + const std::vector & traj_points, const double dist_to_bumper, + const std::optional & determined_stop_obstacle, + const std::optional & determined_zero_vel_dist); + void set_stop_planning_debug_info( + const std::optional & determined_stop_obstacle, + const std::optional & determined_desired_stop_margin) const; + void publish_debug_info(); + + std::optional filter_inside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const VehicleInfo & vehicle_info, + const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + bool is_inside_stop_obstacle_velocity( + const std::shared_ptr object, + const std::vector & traj_points) const; + bool is_crossing_transient_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const double dist_to_bumper, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::optional> & collision_point) const; + + std::optional filter_outside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const rclcpp::Time & predicted_objects_stamp, const std::shared_ptr object, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional> + create_collision_point_for_outside_stop_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const PredictedPath & resampled_predicted_path, double max_lat_margin, + const bool is_driving_forward, const VehicleInfo & vehicle_info, const double dist_to_bumper, + const double decimate_trajectory_step_length) const; + double calc_collision_time_margin( + const Odometry & odometry, const std::vector & collision_points, + const std::vector & traj_points, const double dist_to_bumper) const; + void check_consistency( + const rclcpp::Time & current_time, + const std::vector> & objects, + std::vector & stop_obstacles); + double calc_margin_from_obstacle_on_curve( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const double default_stop_margin) const; + std::vector get_closest_stop_obstacles( + const std::vector & stop_obstacles); + double get_max_lat_margin(const uint8_t obj_label) const; + std::vector get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OBSTACLE_STOP_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp new file mode 100644 index 0000000000000..ed17f60b6f417 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp @@ -0,0 +1,210 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PARAMETERS_HPP_ +#define PARAMETERS_HPP_ + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +struct CommonParam +{ + double max_accel{}; + double min_accel{}; + double max_jerk{}; + double min_jerk{}; + double limit_max_accel{}; + double limit_min_accel{}; + double limit_max_jerk{}; + double limit_min_jerk{}; + + CommonParam() = default; + explicit CommonParam(rclcpp::Node & node) + { + max_accel = getOrDeclareParameter(node, "normal.max_acc"); + min_accel = getOrDeclareParameter(node, "normal.min_acc"); + max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); + min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); + limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); + limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); + limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + } +}; + +struct ObstacleFilteringParam +{ + bool use_pointcloud{}; + std::vector inside_stop_object_types{}; + std::vector outside_stop_object_types{}; + + double obstacle_velocity_threshold_to_stop{}; + double obstacle_velocity_threshold_from_stop{}; + + double max_lat_margin{}; + double max_lat_margin_against_unknown{}; + + double min_velocity_to_reach_collision_point{}; + double stop_obstacle_hold_time_threshold{}; + + double outside_max_lat_time_margin{}; + int outside_num_of_predicted_paths{}; + double outside_pedestrian_deceleration_rate{}; + double outside_bicycle_deceleration_rate{}; + + double crossing_obstacle_collision_time_margin{}; + + ObstacleFilteringParam() = default; + explicit ObstacleFilteringParam(rclcpp::Node & node) + { + inside_stop_object_types = + utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.inside."); + outside_stop_object_types = + utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.outside."); + use_pointcloud = + getOrDeclareParameter(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); + + obstacle_velocity_threshold_to_stop = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_to_stop"); + obstacle_velocity_threshold_from_stop = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_from_stop"); + + max_lat_margin = + getOrDeclareParameter(node, "obstacle_stop.obstacle_filtering.max_lat_margin"); + max_lat_margin_against_unknown = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.max_lat_margin_against_unknown"); + + min_velocity_to_reach_collision_point = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.min_velocity_to_reach_collision_point"); + stop_obstacle_hold_time_threshold = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.stop_obstacle_hold_time_threshold"); + + outside_max_lat_time_margin = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.max_lateral_time_margin"); + outside_num_of_predicted_paths = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.num_of_predicted_paths"); + outside_pedestrian_deceleration_rate = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.pedestrian_deceleration_rate"); + outside_bicycle_deceleration_rate = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.bicycle_deceleration_rate"); + + crossing_obstacle_collision_time_margin = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.crossing_obstacle.collision_time_margin"); + } +}; + +struct StopPlanningParam +{ + double stop_margin{}; + double terminal_stop_margin{}; + double min_behavior_stop_margin{}; + double hold_stop_velocity_threshold{}; + double hold_stop_distance_threshold{}; + bool enable_approaching_on_curve{}; + double additional_stop_margin_on_curve{}; + double min_stop_margin_on_curve{}; + + struct ObjectTypeSpecificParams + { + double limit_min_acc{}; + double sudden_object_acc_threshold{}; + double sudden_object_dist_threshold{}; + bool abandon_to_stop{}; + }; + std::unordered_map object_types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map object_type_specific_param_map; + + StopPlanningParam() = default; + StopPlanningParam(rclcpp::Node & node, const CommonParam & common_param) + { + stop_margin = getOrDeclareParameter(node, "obstacle_stop.stop_planning.stop_margin"); + terminal_stop_margin = + getOrDeclareParameter(node, "obstacle_stop.stop_planning.terminal_stop_margin"); + min_behavior_stop_margin = + getOrDeclareParameter(node, "obstacle_stop.stop_planning.min_behavior_stop_margin"); + hold_stop_velocity_threshold = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.hold_stop_velocity_threshold"); + hold_stop_distance_threshold = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.hold_stop_distance_threshold"); + enable_approaching_on_curve = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.stop_on_curve.enable_approaching"); + additional_stop_margin_on_curve = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.stop_on_curve.additional_stop_margin"); + min_stop_margin_on_curve = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.stop_on_curve.min_stop_margin"); + + const std::string param_prefix = "obstacle_stop.stop_planning.object_type_specified_params."; + const auto object_types = + getOrDeclareParameter>(node, param_prefix + "types"); + + for (const auto & type_str : object_types) { + if (type_str != "default") { + ObjectTypeSpecificParams param{ + getOrDeclareParameter(node, param_prefix + type_str + ".limit_min_acc"), + getOrDeclareParameter( + node, param_prefix + type_str + ".sudden_object_acc_threshold"), + getOrDeclareParameter( + node, param_prefix + type_str + ".sudden_object_dist_threshold"), + getOrDeclareParameter(node, param_prefix + type_str + ".abandon_to_stop")}; + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, common_param.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + + object_type_specific_param_map.emplace(type_str, param); + } + } + } + + std::string get_param_type(const ObjectClassification label) + { + const auto type_str = object_types_maps.at(label.label); + if (object_type_specific_param_map.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObjectTypeSpecificParams get_param(const ObjectClassification label) + { + return object_type_specific_param_map.at(get_param_type(label)); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // PARAMETERS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/stop_planning_debug_info.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/stop_planning_debug_info.hpp new file mode 100644 index 0000000000000..2028a1123e956 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/stop_planning_debug_info.hpp @@ -0,0 +1,90 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STOP_PLANNING_DEBUG_INFO_HPP_ +#define STOP_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; + +class StopPlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // stop planning + STOP_CURRENT_OBSTACLE_DISTANCE, // index: 3 + STOP_CURRENT_OBSTACLE_VELOCITY, + STOP_TARGET_OBSTACLE_DISTANCE, + STOP_TARGET_VELOCITY, + STOP_TARGET_ACCELERATION, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + static int getIndex(const TYPE type) { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convert_to_message(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; +} // namespace autoware::motion_velocity_planner +#endif // STOP_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp new file mode 100644 index 0000000000000..b3c278fcafa50 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp @@ -0,0 +1,63 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; +using PointCloud = pcl::PointCloud; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; +} // namespace autoware::motion_velocity_planner +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/types.hpp new file mode 100644 index 0000000000000..23b0cb701afab --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/types.hpp @@ -0,0 +1,73 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES_HPP_ +#define TYPES_HPP_ + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "type_alias.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct StopObstacle +{ + StopObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const Shape & arg_shape, const double arg_lon_velocity, + const geometry_msgs::msg::Point arg_collision_point, + const double arg_dist_to_collide_on_decimated_traj) + : uuid(arg_uuid), + stamp(arg_stamp), + pose(arg_pose), + velocity(arg_lon_velocity), + shape(arg_shape), + collision_point(arg_collision_point), + dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj), + classification(object_classification) + { + } + std::string uuid; + rclcpp::Time stamp; + geometry_msgs::msg::Pose pose; // interpolated with the current stamp + double velocity; // longitudinal velocity against ego's trajectory + + Shape shape; + geometry_msgs::msg::Point + collision_point; // TODO(yuki_takagi): this member variable still used in + // calculateMarginFromObstacleOnCurve() and should be removed as it can be + // replaced by ”dist_to_collide_on_decimated_traj” + double dist_to_collide_on_decimated_traj; + ObjectClassification classification; +}; + +struct DebugData +{ + DebugData() = default; + std::vector> intentionally_ignored_obstacles; + std::vector obstacles_to_stop; + std::vector decimated_traj_polys; + MarkerArray stop_wall_marker; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index 9be7f52bca99a..1e63713a61a4d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { multi_polygon_t createPolygonMasks( - const std::vector & dynamic_obstacles, const double buffer, + const std::vector> & dynamic_obstacles, const double buffer, const double min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index ad746fb2a7b7e..d7a944514cd26 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const std::vector & dynamic_obstacles, const double buffer, + const std::vector> & dynamic_obstacles, const double buffer, const double min_vel); /// @brief create footprint polygons from projection lines diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index eb89c027a3b48..baa98ea1f79b8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -135,7 +135,9 @@ void ObstacleVelocityLimiterModule::update_parameters( } VelocityPlanningResult ObstacleVelocityLimiterModule::plan( - const std::vector & ego_trajectory_points, + [[maybe_unused]] const std::vector & + raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { autoware::universe_utils::StopWatch stopwatch; @@ -143,14 +145,14 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( VelocityPlanningResult result; stopwatch.tic("preprocessing"); const auto ego_idx = autoware::motion_utils::findNearestIndex( - ego_trajectory_points, planner_data->current_odometry.pose.pose); + smoothed_trajectory_points, planner_data->current_odometry.pose.pose); if (!ego_idx) { RCLCPP_WARN_THROTTLE( logger_, *clock_, rcutils_duration_value_t(1000), "Cannot calculate ego index on the trajectory"); return result; } - auto original_traj_points = ego_trajectory_points; + auto original_traj_points = smoothed_trajectory_points; if (preprocessing_params_.calculate_steering_angles) obstacle_velocity_limiter::calculateSteeringAngles( original_traj_points, projection_params_.wheel_base); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp index 54f9c5c12e59e..f89a8d43e9e93 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp @@ -49,7 +49,8 @@ class ObstacleVelocityLimiterModule : public PluginModuleInterface void init(rclcpp::Node & node, const std::string & module_name) override; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp index 187f028dc5969..be38ac92f92ec 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp @@ -59,11 +59,12 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const std::vector & objects, const double buffer, const double min_velocity) + const std::vector> & objects, const double buffer, + const double min_velocity) { multi_polygon_t polygons; for (const auto & object : objects) { - const auto & predicted_object = object.predicted_object; + const auto & predicted_object = object->predicted_object; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index aff99140744e4..ddcb1161b6f59 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -164,7 +164,8 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const std::vector & objects, const double buffer, const double min_velocity); + const std::vector> & objects, const double buffer, + const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object /// @param[out] obstacles Obstacles object in which to add the sensor obstacles diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index 926b730dcc3dd..a538c7f6610a0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -394,7 +394,7 @@ TEST(TestCollisionDistance, createObjPolygons) using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; - std::vector objects; + std::vector> objects; auto polygons = createObjectPolygons(objects, 0.0, 0.0); EXPECT_TRUE(polygons.empty()); @@ -407,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; - objects.push_back(PlannerData::Object(object1)); + objects.push_back(std::make_shared(object1)); polygons = createObjectPolygons(objects, 0.0, 1.0); EXPECT_TRUE(polygons.empty()); @@ -434,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; - objects.push_back(PlannerData::Object(object2)); + objects.push_back(std::make_shared(object2)); polygons = createObjectPolygons(objects, 0.0, 2.0); ASSERT_EQ(polygons.size(), 1ul); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index a6c8368c20571..baf481a56bbff 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -108,7 +108,7 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = planner_data.predicted_objects_header; for (const auto & object : planner_data.objects) { - const auto & predicted_object = object.predicted_object; + const auto & predicted_object = object->predicted_object; const auto is_pedestrian = std::find_if( predicted_object.classification.begin(), predicted_object.classification.end(), diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 814af37aaa800..c215eacfa5c7e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -148,22 +148,23 @@ void OutOfLaneModule::update_parameters(const std::vector & p void OutOfLaneModule::limit_trajectory_size( out_of_lane::EgoData & ego_data, - const std::vector & ego_trajectory_points, + const std::vector & smoothed_trajectory_points, const double max_arc_length) { ego_data.first_trajectory_idx = - motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(smoothed_trajectory_points, ego_data.pose.position); ego_data.longitudinal_offset_to_first_trajectory_index = motion_utils::calcLongitudinalOffsetToSegment( - ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); + smoothed_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); auto l = -ego_data.longitudinal_offset_to_first_trajectory_index; - ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]); - for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) { - l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]); + ego_data.trajectory_points.push_back(smoothed_trajectory_points[ego_data.first_trajectory_idx]); + for (auto i = ego_data.first_trajectory_idx + 1; i < smoothed_trajectory_points.size(); ++i) { + l += universe_utils::calcDistance2d( + smoothed_trajectory_points[i - 1], smoothed_trajectory_points[i]); if (l >= max_arc_length) { break; } - ego_data.trajectory_points.push_back(ego_trajectory_points[i]); + ego_data.trajectory_points.push_back(smoothed_trajectory_points[i]); } } @@ -213,7 +214,9 @@ out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData & } VelocityPlanningResult OutOfLaneModule::plan( - const std::vector & ego_trajectory_points, + [[maybe_unused]] const std::vector & + raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { VelocityPlanningResult result; @@ -223,7 +226,7 @@ VelocityPlanningResult OutOfLaneModule::plan( stopwatch.tic("preprocessing"); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length); + limit_trajectory_size(ego_data, smoothed_trajectory_points, params_.max_arc_length); out_of_lane::calculate_min_stop_and_slowdown_distances( ego_data, *planner_data, previous_slowdown_pose_); prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length); @@ -314,7 +317,7 @@ VelocityPlanningResult OutOfLaneModule::plan( } planning_factor_interface_->add( - ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, + smoothed_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, SafetyFactorArray{}); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index b6eb03f5f0469..1e011022ff99e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -41,7 +41,8 @@ class OutOfLaneModule : public PluginModuleInterface void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } @@ -51,7 +52,7 @@ class OutOfLaneModule : public PluginModuleInterface /// given length static void limit_trajectory_size( out_of_lane::EgoData & ego_data, - const std::vector & ego_trajectory_points, + const std::vector & smoothed_trajectory_points, const double max_arc_length); out_of_lane::PlannerParam params_{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp index 4d4917f23496c..54af835171d14 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLANNER_DATA_HPP_ #include +#include #include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -43,11 +45,22 @@ namespace autoware::motion_velocity_planner { +using autoware_planning_msgs::msg::TrajectoryPoint; + struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; autoware_perception_msgs::msg::TrafficLightGroup signal; }; + +struct TrajectoryPolygonCollisionCheck +{ + double decimate_trajectory_step_length; + double goal_extended_trajectory_length; + bool enable_to_consider_current_pose; + double time_to_convergence; +}; + struct PlannerData { explicit PlannerData(rclcpp::Node & node) @@ -65,28 +78,27 @@ struct PlannerData } autoware_perception_msgs::msg::PredictedObject predicted_object; - // double get_lon_vel_relative_to_traj() - // { - // if (!lon_vel_relative_to_traj) { - // lon_vel_relative_to_traj = 0.0; - // } - // return *lon_vel_relative_to_traj; - // } - // double get_lat_vel_relative_to_traj() - // { - // if (!lat_vel_relative_to_traj) { - // lat_vel_relative_to_traj = 0.0; - // } - // return *lat_vel_relative_to_traj; - // } + + double get_dist_to_traj_poly( + const std::vector & decimated_traj_polys) const; + double get_dist_to_traj_lateral(const std::vector & traj_points) const; + double get_dist_from_ego_longitudinal( + const std::vector & traj_points, + const geometry_msgs::msg::Point & ego_pos) const; + double get_lon_vel_relative_to_traj(const std::vector & traj_points) const; + double get_lat_vel_relative_to_traj(const std::vector & traj_points) const; + geometry_msgs::msg::Pose get_predicted_pose( + const rclcpp::Time & current_stamp, const rclcpp::Time & predicted_object_stamp) const; private: - // TODO(murooka) implement the following variables and their functions. - // std::optional dist_to_traj_poly{std::nullopt}; - // std::optional dist_to_traj_lateral{std::nullopt}; - // std::optional dist_from_ego_longitudinal{std::nullopt}; - // std::optional lon_vel_relative_to_traj{std::nullopt}; - // std::optional lat_vel_relative_to_traj{std::nullopt}; + void calc_vel_relative_to_traj(const std::vector & traj_points) const; + + mutable std::optional dist_to_traj_poly{std::nullopt}; + mutable std::optional dist_to_traj_lateral{std::nullopt}; + mutable std::optional dist_from_ego_longitudinal{std::nullopt}; + mutable std::optional lon_vel_relative_to_traj{std::nullopt}; + mutable std::optional lat_vel_relative_to_traj{std::nullopt}; + mutable std::optional predicted_pose; }; struct Pointcloud @@ -105,21 +117,13 @@ struct PlannerData }; void process_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) - { - predicted_objects_header = predicted_objects.header; - - objects.clear(); - for (const auto & predicted_object : predicted_objects.objects) { - objects.push_back(Object(predicted_object)); - } - } + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects); // msgs from callbacks that are used for data-ready nav_msgs::msg::Odometry current_odometry; geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; std_msgs::msg::Header predicted_objects_header; - std::vector objects; + std::vector> objects; Pointcloud no_ground_pointcloud; nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; @@ -128,6 +132,8 @@ struct PlannerData double ego_nearest_dist_threshold{}; double ego_nearest_yaw_threshold{}; + TrajectoryPolygonCollisionCheck trajectory_polygon_collision_check{}; + // other internal data // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the // last observed infomation for UNKNOWN @@ -139,6 +145,8 @@ struct PlannerData // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + bool is_driving_forward{true}; + /** *@fn *@brief queries the traffic signal information of given Id. if keep_last_observation = true, @@ -163,6 +171,20 @@ struct PlannerData current_acceleration.accel.accel.linear.x, velocity_smoother_->getMinDecel(), std::abs(velocity_smoother_->getMinJerk()), velocity_smoother_->getMinJerk()); } + + size_t find_index( + const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const + { + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + } + + size_t find_segment_index( + const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const + { + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + } }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp index a01b39646e6e2..d8c32f93d7c15 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp @@ -42,7 +42,8 @@ class PluginModuleInterface virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; virtual void update_parameters(const std::vector & parameters) = 0; virtual VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; virtual void publish_planning_factor() {} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp new file mode 100644 index 0000000000000..8f7b0920a4372 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp @@ -0,0 +1,82 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ + +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include + +#include "autoware_perception_msgs/msg/predicted_path.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" + +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace polygon_utils +{ +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +namespace bg = boost::geometry; +using autoware::vehicle_info_utils::VehicleInfo; + +struct PointWithStamp +{ + rclcpp::Time stamp; + geometry_msgs::msg::Point point; +}; + +std::optional> get_collision_point( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose obj_pose, const rclcpp::Time obj_stamp, const Shape & obj_shape, + const double dist_to_bumper); + +std::optional> get_collision_point( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const double dist_to_bumper); + +std::vector get_collision_points( + const std::vector & traj_points, const std::vector & traj_polygons, + const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, + const rclcpp::Time & current_time, const bool is_driving_forward, + std::vector & collision_index, const double max_dist = std::numeric_limits::max(), + const double max_prediction_time_for_collision_check = std::numeric_limits::max()); + +std::vector create_one_step_polygons( + const std::vector & traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length); +} // namespace polygon_utils +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp new file mode 100644 index 0000000000000..9f3baf6d0710a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp @@ -0,0 +1,134 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "planner_data.hpp" + +#include + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::utils +{ +using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using nav_msgs::msg::Odometry; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +std::vector decimate_trajectory_points_from_ego( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const double decimate_trajectory_step_length, const double goal_extended_trajectory_length); + +template +std::optional get_obstacle_from_uuid( + const std::vector & obstacles, const std::string & target_uuid) +{ + const auto itr = std::find_if(obstacles.begin(), obstacles.end(), [&](const auto & obstacle) { + return obstacle.uuid == target_uuid; + }); + + if (itr == obstacles.end()) { + return std::nullopt; + } + return *itr; +} + +std::vector get_target_object_type(rclcpp::Node & node, const std::string & param_prefix); + +double calc_object_possible_max_dist_from_center(const Shape & shape); + +Marker get_object_marker( + const geometry_msgs::msg::Pose & obj_pose, size_t idx, const std::string & ns, const double r, + const double g, const double b); + +template +size_t get_index_with_longitudinal_offset( + const T & points, const double longitudinal_offset, std::optional start_idx) +{ + if (points.empty()) { + throw std::logic_error("points is empty."); + } + + if (start_idx) { + if (/*start_idx.get() < 0 || */ points.size() <= *start_idx) { + throw std::out_of_range("start_idx is out of range."); + } + } else { + if (longitudinal_offset > 0) { + start_idx = 0; + } else { + start_idx = points.size() - 1; + } + } + + double sum_length = 0.0; + if (longitudinal_offset > 0) { + for (size_t i = *start_idx; i < points.size() - 1; ++i) { + const double segment_length = + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + sum_length += segment_length; + if (sum_length >= longitudinal_offset) { + const double back_length = sum_length - longitudinal_offset; + const double front_length = segment_length - back_length; + if (front_length < back_length) { + return i; + } else { + return i + 1; + } + } + } + return points.size() - 1; + } + + for (size_t i = *start_idx; 0 < i; --i) { + const double segment_length = + autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); + sum_length += segment_length; + if (sum_length >= -longitudinal_offset) { + const double back_length = sum_length + longitudinal_offset; + const double front_length = segment_length - back_length; + if (front_length < back_length) { + return i; + } else { + return i - 1; + } + } + } + return 0; +} + +double calc_possible_min_dist_from_obj_to_traj_poly( + const std::shared_ptr object, + const std::vector & traj_points, const VehicleInfo & vehicle_info); +} // namespace autoware::motion_velocity_planner::utils + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp index d3dfa7a270a16..4faeb698e2bcd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include @@ -42,6 +44,9 @@ struct VelocityPlanningResult { std::vector stop_points{}; std::vector slowdown_intervals{}; + std::optional velocity_limit{std::nullopt}; + std::optional velocity_limit_clear_command{ + std::nullopt}; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml index 91d569dc3f8cc..8c33cfc862271 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml @@ -19,6 +19,7 @@ autoware_behavior_velocity_planner_common autoware_internal_debug_msgs autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp new file mode 100644 index 0000000000000..a83c9d51aab96 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp @@ -0,0 +1,189 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" + +#include "autoware_perception_msgs/msg/predicted_path.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_perception_msgs::msg::PredictedPath; +namespace bg = boost::geometry; + +namespace +{ +std::optional get_predicted_object_pose_from_predicted_path( + const PredictedPath & predicted_path, const rclcpp::Time & obj_stamp, + const rclcpp::Time & current_stamp) +{ + const double rel_time = (current_stamp - obj_stamp).seconds(); + if (rel_time < 0.0) { + return std::nullopt; + } + + const auto pose = + autoware::object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time); + if (!pose) { + return std::nullopt; + } + return pose.get(); +} + +std::optional get_predicted_object_pose_from_predicted_paths( + const std::vector & predicted_paths, const rclcpp::Time & obj_stamp, + const rclcpp::Time & current_stamp) +{ + if (predicted_paths.empty()) { + return std::nullopt; + } + + // Get the most reliable path + const auto predicted_path = std::max_element( + predicted_paths.begin(), predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + return get_predicted_object_pose_from_predicted_path(*predicted_path, obj_stamp, current_stamp); +} +} // namespace + +double PlannerData::Object::get_dist_to_traj_poly( + const std::vector & decimated_traj_polys) const +{ + if (!dist_to_traj_poly) { + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, predicted_object.shape); + dist_to_traj_poly = std::numeric_limits::max(); + for (const auto & traj_poly : decimated_traj_polys) { + const double current_dist_to_traj_poly = bg::distance(traj_poly, obj_poly); + dist_to_traj_poly = std::min(*dist_to_traj_poly, current_dist_to_traj_poly); + } + } + return *dist_to_traj_poly; +} + +double PlannerData::Object::get_dist_to_traj_lateral( + const std::vector & traj_points) const +{ + if (!dist_to_traj_lateral) { + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + dist_to_traj_lateral = autoware::motion_utils::calcLateralOffset(traj_points, obj_pos); + } + return *dist_to_traj_lateral; +} + +double PlannerData::Object::get_dist_from_ego_longitudinal( + const std::vector & traj_points, const geometry_msgs::msg::Point & ego_pos) const +{ + if (!dist_from_ego_longitudinal) { + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + dist_from_ego_longitudinal = + autoware::motion_utils::calcSignedArcLength(traj_points, ego_pos, obj_pos); + } + return *dist_from_ego_longitudinal; +} + +double PlannerData::Object::get_lon_vel_relative_to_traj( + const std::vector & traj_points) const +{ + if (!lon_vel_relative_to_traj) { + calc_vel_relative_to_traj(traj_points); + } + return *lon_vel_relative_to_traj; +} + +double PlannerData::Object::get_lat_vel_relative_to_traj( + const std::vector & traj_points) const +{ + if (!lat_vel_relative_to_traj) { + calc_vel_relative_to_traj(traj_points); + } + return *lat_vel_relative_to_traj; +} + +void PlannerData::Object::calc_vel_relative_to_traj( + const std::vector & traj_points) const +{ + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto & obj_twist = predicted_object.kinematics.initial_twist_with_covariance.twist; + + const size_t object_idx = + autoware::motion_utils::findNearestIndex(traj_points, obj_pose.position); + const auto & nearest_traj_point = traj_points.at(object_idx); + + const double traj_yaw = tf2::getYaw(nearest_traj_point.pose.orientation); + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const Eigen::Rotation2Dd R_ego_to_obstacle( + autoware::universe_utils::normalizeRadian(obj_yaw - traj_yaw)); + + // Calculate the trajectory direction and the vector from the trajectory to the obstacle + const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); + const Eigen::Vector2d traj_to_obstacle( + obj_pose.position.x - nearest_traj_point.pose.position.x, + obj_pose.position.y - nearest_traj_point.pose.position.y); + + // Determine if the obstacle is to the left or right of the trajectory using the cross product + const double cross_product = + traj_direction.x() * traj_to_obstacle.y() - traj_direction.y() * traj_to_obstacle.x(); + const int sign = (cross_product > 0) ? -1 : 1; + + const Eigen::Vector2d obstacle_velocity(obj_twist.linear.x, obj_twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + lon_vel_relative_to_traj = projected_velocity[0]; + lat_vel_relative_to_traj = sign * projected_velocity[1]; +} + +geometry_msgs::msg::Pose PlannerData::Object::get_predicted_pose( + const rclcpp::Time & current_stamp, const rclcpp::Time & predicted_objects_stamp) const +{ + if (!predicted_pose) { + const auto obj_stamp = predicted_objects_stamp; + const auto predicted_pose_opt = get_predicted_object_pose_from_predicted_paths( + predicted_object.kinematics.predicted_paths, obj_stamp, current_stamp); + + if (predicted_pose_opt) { + predicted_pose = *predicted_pose_opt; + } else { + predicted_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_planner_common"), + "Failed to calculate the predicted object pose."); + } + } + + return *predicted_pose; +} + +void PlannerData::process_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) +{ + predicted_objects_header = predicted_objects.header; + + objects.clear(); + for (const auto & predicted_object : predicted_objects.objects) { + objects.push_back(std::make_shared(predicted_object)); + } +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp new file mode 100644 index 0000000000000..21deaec2c0c02 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp @@ -0,0 +1,276 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::polygon_utils +{ +namespace +{ +PointWithStamp calc_nearest_collision_point( + const size_t first_within_idx, const std::vector & collision_points, + const std::vector & decimated_traj_points, const bool is_driving_forward) +{ + const size_t prev_idx = first_within_idx == 0 ? first_within_idx : first_within_idx - 1; + const size_t next_idx = first_within_idx == 0 ? first_within_idx + 1 : first_within_idx; + + std::vector segment_points{ + decimated_traj_points.at(prev_idx).pose, decimated_traj_points.at(next_idx).pose}; + if (!is_driving_forward) { + std::reverse(segment_points.begin(), segment_points.end()); + } + + std::vector dist_vec; + for (const auto & collision_point : collision_points) { + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + segment_points, 0, collision_point.point); + dist_vec.push_back(dist); + } + + const size_t min_idx = std::min_element(dist_vec.begin(), dist_vec.end()) - dist_vec.begin(); + return collision_points.at(min_idx); +} + +// NOTE: max_dist is used for efficient calculation to suppress boost::geometry's polygon +// calculation. +std::optional>> get_collision_index( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, + const Shape & object_shape, const double max_dist = std::numeric_limits::max()) +{ + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape); + for (size_t i = 0; i < traj_polygons.size(); ++i) { + const double approximated_dist = + autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + if (approximated_dist > max_dist) { + continue; + } + + std::vector collision_polygons; + boost::geometry::intersection(traj_polygons.at(i), obj_polygon, collision_polygons); + + std::vector collision_geom_points; + bool has_collision = false; + for (const auto & collision_polygon : collision_polygons) { + if (boost::geometry::area(collision_polygon) > 0.0) { + has_collision = true; + + for (const auto & collision_point : collision_polygon.outer()) { + PointWithStamp collision_geom_point; + collision_geom_point.stamp = object_time; + collision_geom_point.point.x = collision_point.x(); + collision_geom_point.point.y = collision_point.y(); + collision_geom_point.point.z = traj_points.at(i).pose.position.z; + collision_geom_points.push_back(collision_geom_point); + } + } + } + + if (has_collision) { + const auto collision_info = + std::pair>{i, collision_geom_points}; + return collision_info; + } + } + + return std::nullopt; +} +} // namespace + +std::optional> get_collision_point( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose obj_pose, const rclcpp::Time obj_stamp, const Shape & obj_shape, + const double dist_to_bumper) +{ + const auto collision_info = + get_collision_index(traj_points, traj_polygons, obj_pose, obj_stamp, obj_shape); + if (!collision_info) { + return std::nullopt; + } + + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info->first).pose, dist_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info->second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - + *max_collision_length); +} + +std::optional> get_collision_point( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const double dist_to_bumper) +{ + std::pair> collision_info; + collision_info.first = collision_idx; + collision_info.second = collision_points; + + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info.first).pose, dist_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info.second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info.first) - + *max_collision_length); +} + +// NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon +// calculation. +std::vector get_collision_points( + const std::vector & traj_points, const std::vector & traj_polygons, + const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, + const rclcpp::Time & current_time, const bool is_driving_forward, + std::vector & collision_index, const double max_lat_dist, + const double max_prediction_time_for_collision_check) +{ + std::vector collision_points; + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + if ( + max_prediction_time_for_collision_check < + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) { + break; + } + + const auto object_time = + rclcpp::Time(obstacle_stamp) + rclcpp::Duration(predicted_path.time_step) * i; + // Ignore past position + if ((object_time - current_time).seconds() < 0.0) { + continue; + } + + const auto collision_info = get_collision_index( + traj_points, traj_polygons, predicted_path.path.at(i), object_time, shape, max_lat_dist); + if (collision_info) { + const auto nearest_collision_point = calc_nearest_collision_point( + collision_info->first, collision_info->second, traj_points, is_driving_forward); + collision_points.push_back(nearest_collision_point); + collision_index.push_back(collision_info->first); + } + } + + return collision_points; +} + +std::vector create_one_step_polygons( + const std::vector & traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) +{ + const double front_length = vehicle_info.max_longitudinal_offset_m; + const double rear_length = vehicle_info.rear_overhang_m; + const double vehicle_width = vehicle_info.vehicle_width_m; + + const size_t nearest_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); + const auto nearest_pose = traj_points.at(nearest_idx).pose; + const auto current_ego_pose_error = + autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); + const double current_ego_lat_error = current_ego_pose_error.position.y; + const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); + double time_elapsed{0.0}; + + std::vector output_polygons; + Polygon2d tmp_polys{}; + for (size_t i = 0; i < traj_points.size(); ++i) { + std::vector current_poses = {traj_points.at(i).pose}; + + // estimate the future ego pose with assuming that the pose error against the reference path + // will decrease to zero by the time_to_convergence + if (enable_to_consider_current_pose && time_elapsed < time_to_convergence) { + const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence; + geometry_msgs::msg::Pose indexed_pose_err; + indexed_pose_err.set__orientation( + autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + indexed_pose_err.set__position( + autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + current_poses.push_back( + autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { + time_elapsed += + decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); + } else { + time_elapsed = std::numeric_limits::max(); + } + } + + Polygon2d idx_poly{}; + for (const auto & pose : current_poses) { + if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { + boost::geometry::append( + idx_poly, + autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) + .outer()); + boost::geometry::append( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); + boost::geometry::append( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( + pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) + .position) + .to_2d()); + } else { + boost::geometry::append( + idx_poly, autoware::universe_utils::toFootprint( + pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) + .outer()); + } + } + + boost::geometry::append(tmp_polys, idx_poly.outer()); + Polygon2d hull_polygon; + boost::geometry::convex_hull(tmp_polys, hull_polygon); + boost::geometry::correct(hull_polygon); + + output_polygons.push_back(hull_polygon); + tmp_polys = std::move(idx_poly); + } + return output_polygons; +} +} // namespace autoware::motion_velocity_planner::polygon_utils diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp new file mode 100644 index 0000000000000..de86f64fa7965 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp @@ -0,0 +1,176 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::utils +{ +namespace +{ +TrajectoryPoint extend_trajectory_point( + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) +{ + TrajectoryPoint extended_trajectory_point; + extended_trajectory_point.pose = autoware::universe_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + extended_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extended_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extended_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extended_trajectory_point; +} + +std::vector get_extended_trajectory_points( + const std::vector & input_points, const double extend_distance, + const double step_length) +{ + auto output_points = input_points; + const auto is_driving_forward_opt = + autoware::motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; + + if (extend_distance < std::numeric_limits::epsilon()) { + return output_points; + } + + const auto goal_point = input_points.back(); + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - step_length)) { + const auto extended_trajectory_point = + extend_trajectory_point(extend_sum, goal_point, is_driving_forward); + output_points.push_back(extended_trajectory_point); + extend_sum += step_length; + } + const auto extended_trajectory_point = + extend_trajectory_point(extend_distance, goal_point, is_driving_forward); + output_points.push_back(extended_trajectory_point); + + return output_points; +} + +std::vector resample_trajectory_points( + const std::vector & traj_points, const double interval) +{ + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +} // namespace + +std::vector decimate_trajectory_points_from_ego( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const double decimate_trajectory_step_length, const double goal_extended_trajectory_length) +{ + // trim trajectory points from ego pose + const size_t traj_ego_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + const auto traj_points_from_ego = + std::vector(traj_points.begin() + traj_ego_seg_idx, traj_points.end()); + + // decimate trajectory + const auto decimated_traj_points_from_ego = + resample_trajectory_points(traj_points_from_ego, decimate_trajectory_step_length); + + // extend trajectory + const auto extended_traj_points_from_ego = get_extended_trajectory_points( + decimated_traj_points_from_ego, goal_extended_trajectory_length, + decimate_trajectory_step_length); + if (extended_traj_points_from_ego.size() < 2) { + return traj_points; + } + return extended_traj_points_from_ego; +} + +std::vector get_target_object_type(rclcpp::Node & node, const std::string & param_prefix) +{ + std::unordered_map types_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + + std::vector types; + for (const auto & type : types_map) { + if (node.declare_parameter(param_prefix + type.first)) { + types.push_back(type.second); + } + } + return types; +} + +double calc_object_possible_max_dist_from_center(const Shape & shape) +{ + if (shape.type == Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in motion_velocity_planner_common."); +} +visualization_msgs::msg::Marker get_object_marker( + const geometry_msgs::msg::Pose & obj_pose, size_t idx, const std::string & ns, const double r, + const double g, const double b) +{ + const auto current_time = rclcpp::Clock().now(); + + auto marker = autoware::universe_utils::createDefaultMarker( + "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, + autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0), + autoware::universe_utils::createMarkerColor(r, g, b, 0.8)); + + marker.pose = obj_pose; + + return marker; +} + +double calc_possible_min_dist_from_obj_to_traj_poly( + const std::shared_ptr object, + const std::vector & traj_points, const VehicleInfo & vehicle_info) +{ + const double object_possible_max_dist = + calc_object_possible_max_dist_from_center(object->predicted_object.shape); + const double possible_min_dist_to_traj_poly = + std::abs(object->get_dist_to_traj_lateral(traj_points)) - vehicle_info.vehicle_width_m - + object_possible_max_dist; + return possible_min_dist_to_traj_poly; +} +} // namespace autoware::motion_velocity_planner::utils diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml index 5b2fea537d4f7..4d5409cef23c2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml @@ -1,3 +1,14 @@ /**: ros__parameters: smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning + + trajectory_polygon_collision_check: + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking + goal_extended_trajectory_length: 6.0 + + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: true + time_to_convergence: 1.5 #[s] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json index 7db22e5e39d17..9b5fcf0c91eb1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json @@ -10,6 +10,38 @@ "type": "boolean", "default": true, "description": "if true, smooth the velocity profile of the input trajectory before planning" + }, + "trajectory_polygon_collision_check": { + "type": "object", + "properties": { + "decimate_trajectory_step_length": { + "type": "number", + "default": 2.0, + "description": "trajectory's step length to decimate for the polygon-based collision check" + }, + "goal_extended_trajectory_length": { + "type": "number", + "default": 6.0, + "description": "trajectory's extended length on the goal for the polygon-based collision check" + }, + "consider_current_pose": { + "type": "object", + "properties": { + "enable_to_consider_current_pose": { + "type": "boolean", + "default": true, + "description": "whether to consider the current pose to create the trajectory polygon for the polygon-based collision check" + }, + "time_to_convergence": { + "type": "number", + "default": 1.5, + "description": "time for the ego to converge to the trajectory for the polygon-based collision check" + } + }, + "required": ["enable_to_consider_current_pose", "time_to_convergence"] + } + }, + "required": ["decimate_trajectory_step_length", "goal_extended_trajectory_length"] } }, "required": ["smooth_velocity_before_planning"], diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp index 8f5aa761573b7..32c7877b1dccd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp @@ -82,6 +82,10 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = this->create_publisher("~/output/trajectory", 1); + velocity_limit_pub_ = this->create_publisher( + "~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); + clear_velocity_limit_pub_ = this->create_publisher( + "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); processing_time_publisher_ = this->create_publisher( "~/debug/processing_time_ms", 1); @@ -95,6 +99,17 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & planner_data_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + + planner_data_.trajectory_polygon_collision_check.decimate_trajectory_step_length = + declare_parameter("trajectory_polygon_collision_check.decimate_trajectory_step_length"); + planner_data_.trajectory_polygon_collision_check.goal_extended_trajectory_length = + declare_parameter("trajectory_polygon_collision_check.goal_extended_trajectory_length"); + planner_data_.trajectory_polygon_collision_check.enable_to_consider_current_pose = + declare_parameter( + "trajectory_polygon_collision_check.consider_current_pose.enable_to_consider_current_pose"); + planner_data_.trajectory_polygon_collision_check.time_to_convergence = declare_parameter( + "trajectory_polygon_collision_check.consider_current_pose.time_to_convergence"); + // set velocity smoother param set_velocity_smoother_params(); @@ -131,7 +146,8 @@ void MotionVelocityPlannerNode::on_unload_plugin( // NOTE: argument planner_data must not be referenced for multithreading bool MotionVelocityPlannerNode::update_planner_data( - std::map & processing_times) + std::map & processing_times, + const std::vector & input_traj_points) { auto clock = *get_clock(); auto is_ready = true; @@ -181,6 +197,14 @@ bool MotionVelocityPlannerNode::update_planner_data( planner_data_.velocity_smoother_, "Waiting for the initialization of the velocity smoother"); processing_times["update_planner_data.smoother"] = sw.toc(true); + // is driving forward + const auto is_driving_forward = + autoware::motion_utils::isDrivingForwardWithTwist(input_traj_points); + if (is_driving_forward) { + planner_data_.is_driving_forward = is_driving_forward.value(); + } + processing_times["update_planner_data.is_driving_forward"] = sw.toc(true); + // optional data const auto traffic_signals_ptr = sub_traffic_signals_.takeData(); if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr); @@ -269,7 +293,7 @@ void MotionVelocityPlannerNode::on_trajectory( std::map processing_times; stop_watch.tic("Total"); - if (!update_planner_data(processing_times)) { + if (!update_planner_data(processing_times, input_trajectory_msg->points)) { return; } processing_times["update_planner_data"] = stop_watch.toc(true); @@ -383,40 +407,46 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s } autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( - autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points, + const autoware::motion_velocity_planner::TrajectoryPoints & input_trajectory_points, std::map & processing_times) { universe_utils::StopWatch stop_watch; autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; - if (smooth_velocity_before_planning_) { - stop_watch.tic("smooth"); - input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); - processing_times["velocity_smoothing"] = stop_watch.toc("smooth"); - } + + stop_watch.tic("smooth"); + const auto smoothed_trajectory_points = [&]() { + if (smooth_velocity_before_planning_) { + return smooth_trajectory(input_trajectory_points, planner_data_); + } + return input_trajectory_points; + }(); + processing_times["velocity_smoothing"] = stop_watch.toc("smooth"); + stop_watch.tic("resample"); - TrajectoryPoints resampled_trajectory; + TrajectoryPoints resampled_smoothed_trajectory_points; // skip points that are too close together to make computation easier - if (!input_trajectory_points.empty()) { - resampled_trajectory.push_back(input_trajectory_points.front()); + if (!smoothed_trajectory_points.empty()) { + resampled_smoothed_trajectory_points.push_back(smoothed_trajectory_points.front()); constexpr auto min_interval_squared = 0.5 * 0.5; // TODO(Maxime): change to a parameter - for (auto i = 1UL; i < input_trajectory_points.size(); ++i) { - const auto & p = input_trajectory_points[i]; + for (auto i = 1UL; i < smoothed_trajectory_points.size(); ++i) { + const auto & p = smoothed_trajectory_points[i]; const auto dist_to_prev_point = - universe_utils::calcSquaredDistance2d(resampled_trajectory.back(), p); + universe_utils::calcSquaredDistance2d(resampled_smoothed_trajectory_points.back(), p); if (dist_to_prev_point > min_interval_squared) { - resampled_trajectory.push_back(p); + resampled_smoothed_trajectory_points.push_back(p); } } } processing_times["resample"] = stop_watch.toc("resample"); stop_watch.tic("calculate_time_from_start"); motion_utils::calculate_time_from_start( - resampled_trajectory, planner_data_.current_odometry.pose.pose.position); + resampled_smoothed_trajectory_points, planner_data_.current_odometry.pose.pose.position); processing_times["calculate_time_from_start"] = stop_watch.toc("calculate_time_from_start"); stop_watch.tic("plan_velocities"); const auto planning_results = planner_manager_.plan_velocities( - resampled_trajectory, std::make_shared(planner_data_)); + input_trajectory_points, resampled_smoothed_trajectory_points, + std::make_shared(planner_data_)); processing_times["plan_velocities"] = stop_watch.toc("plan_velocities"); for (const auto & planning_result : planning_results) { @@ -424,6 +454,12 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj insert_stop(output_trajectory_msg, stop_point); for (const auto & slowdown_interval : planning_result.slowdown_intervals) insert_slowdown(output_trajectory_msg, slowdown_interval); + if (planning_result.velocity_limit) { + velocity_limit_pub_->publish(*planning_result.velocity_limit); + } + if (planning_result.velocity_limit_clear_command) { + clear_velocity_limit_pub_->publish(*planning_result.velocity_limit_clear_command); + } } return output_trajectory_msg; @@ -442,6 +478,19 @@ rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param updateParam(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); + updateParam( + parameters, "trajectory_polygon_collision_check.decimate_trajectory_step_length", + planner_data_.trajectory_polygon_collision_check.decimate_trajectory_step_length); + updateParam( + parameters, "trajectory_polygon_collision_check.goal_extended_trajectory_length", + planner_data_.trajectory_polygon_collision_check.goal_extended_trajectory_length); + updateParam( + parameters, + "trajectory_polygon_collision_check.consider_current_pose.enable_to_consider_current_pose", + planner_data_.trajectory_polygon_collision_check.enable_to_consider_current_pose); + updateParam( + parameters, "trajectory_polygon_collision_check.consider_current_pose.time_to_convergence", + planner_data_.trajectory_polygon_collision_check.time_to_convergence); // set_velocity_smoother_params(); TODO(Maxime): fix update parameters of the velocity smoother diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp index 826c740d85b76..dee10e94ee64f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp @@ -33,6 +33,8 @@ #include #include #include +#include +#include #include #include @@ -50,6 +52,8 @@ using autoware_map_msgs::msg::LaneletMapBin; using autoware_motion_velocity_planner_node_universe::srv::LoadPlugin; using autoware_motion_velocity_planner_node_universe::srv::UnloadPlugin; using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; using TrajectoryPoints = std::vector; class MotionVelocityPlannerNode : public rclcpp::Node @@ -92,6 +96,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node // publishers rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr velocity_limit_pub_; + rclcpp::Publisher::SharedPtr clear_velocity_limit_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; @@ -128,7 +134,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node // function /// @brief update the PlannerData instance with the latest messages received /// @return false if some data is not available - bool update_planner_data(std::map & processing_times); + bool update_planner_data( + std::map & processing_times, + const std::vector & input_traj_points); void insert_stop( autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const; @@ -139,7 +147,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, const autoware::motion_velocity_planner::PlannerData & planner_data) const; autoware_planning_msgs::msg::Trajectory generate_trajectory( - autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points, + const autoware::motion_velocity_planner::TrajectoryPoints & input_trajectory_points, std::map & processing_times); std::unique_ptr logger_configure_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp index 6d889343641a9..d6b7e5760dccb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp @@ -94,12 +94,14 @@ std::shared_ptr MotionVelocityPlannerManager::get_metrics( } std::vector MotionVelocityPlannerManager::plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { std::vector results; for (auto & plugin : loaded_plugins_) { - VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); + VelocityPlanningResult res = + plugin->plan(raw_trajectory_points, smoothed_trajectory_points, planner_data); results.push_back(res); plugin->publish_planning_factor(); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp index 48f6a246c6456..3e1cb429992c6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp @@ -49,7 +49,8 @@ class MotionVelocityPlannerManager void unload_module_plugin(rclcpp::Node & node, const std::string & name); void update_module_parameters(const std::vector & parameters); std::vector plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data); // Metrics diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp index b4a95dc020ffd..6ce5e3e506262 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp @@ -32,13 +32,8 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: motion_velocity_planner → test_node_ - test_manager->setTrajectorySubscriber("motion_velocity_planner_node/output/trajectory"); - - // set motion_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("motion_velocity_planner_node/input/trajectory"); - - test_manager->setInitialPoseTopicName("motion_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->subscribeOutput( + "motion_velocity_planner_node/output/trajectory"); return test_manager; } @@ -89,20 +84,30 @@ void publishMandatoryTopics( std::shared_ptr test_target_node) { // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "motion_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "motion_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "motion_velocity_planner_node/input/traffic_signals"); - test_manager->publishOccupancyGrid( - test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); + test_manager->publishInput( + test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map")); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/dynamic_objects", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud", + sensor_msgs::msg::PointCloud2{}.set__header( + std_msgs::msg::Header{}.set__frame_id("base_link"))); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/vehicle_odometry", + autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/vector_map", + autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/traffic_signals", + autoware_perception_msgs::msg::TrafficLightGroupArray{}); + test_manager->publishInput( + test_target_node, "motion_velocity_planner_node/input/occupancy_grid", + autoware::test_utils::makeCostMapMsg()); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -113,12 +118,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "motion_velocity_planner_node/input/trajectory"; + // test with nominal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalTrajectory(test_target_node, input_trajectory_topic)); rclcpp::shutdown(); } @@ -130,13 +139,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + const std::string input_trajectory_topic = "motion_velocity_planner_node/input/trajectory"; + const std::string input_odometry_topic = "motion_velocity_planner_node/input/vehicle_odometry"; + // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalTrajectory(test_target_node, input_trajectory_topic)); // make sure motion_velocity_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); rclcpp::shutdown(); } diff --git a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index dc0cb0eb5188a..92127a2affcf1 100644 --- a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -17,9 +17,9 @@ #include "tier4_planning_rviz_plugin/path/display_base.hpp" +#include #include #include -#include #include #include @@ -188,7 +188,7 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay }; class AutowarePathWithLaneIdDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT public: @@ -198,9 +198,9 @@ class AutowarePathWithLaneIdDisplay private: void preProcessMessageDetail() override; void preVisualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; void visualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) override; rviz_common::properties::BoolProperty property_lane_id_view_; diff --git a/visualization/tier4_planning_rviz_plugin/src/path/display.cpp b/visualization/tier4_planning_rviz_plugin/src/path/display.cpp index 869f0c4d91793..79c6e745b20db 100644 --- a/visualization/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/visualization/tier4_planning_rviz_plugin/src/path/display.cpp @@ -55,7 +55,7 @@ AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() } void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) { const size_t size = msg_ptr->points.size(); // clear previous text @@ -77,7 +77,8 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( } void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) { const auto & point = msg_ptr->points.at(p_idx);