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/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_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/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 1d5dbd607cfb5..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 @@ -16,9 +16,9 @@ #include +#include #include #include -#include #include #include @@ -26,10 +26,10 @@ 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; -using tier4_planning_msgs::msg::PathWithLaneId; PlanningInterfaceTestManager::PlanningInterfaceTestManager() { @@ -95,8 +95,7 @@ void PlanningInterfaceTestManager::testWithNormalPath( try { const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); publishInput( - target_node, topic_name, - autoware::motion_utils::convertToPath(path), 5); + target_node, topic_name, autoware::motion_utils::convertToPath(path), 5); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } 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/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_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_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 27273fa37a3e8..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 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/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_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_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 d3bc4a25e97ca..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 @@ -31,7 +31,7 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->subscribeOutput( + test_manager->subscribeOutput( "behavior_path_planner/output/path"); return test_manager; 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_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_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_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_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_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_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_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_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_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_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_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/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 25f34b7e809d2..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 @@ -63,7 +63,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) } } -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; @@ -102,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(), @@ -136,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); 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 e2ecedfdf31de..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 @@ -23,8 +23,8 @@ #include #include +#include #include -#include #include #include @@ -43,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; 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 78eb824603340..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 @@ -294,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 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 c1577a95f6536..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 @@ -106,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; 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_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_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/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);