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 d6a848a4b50d1..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 "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -137,9 +137,10 @@ autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( * @return resampled path */ 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); + 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 31c812f9cbfb0..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 "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include @@ -95,7 +95,8 @@ inline TrajectoryPoints convertToTrajectoryPoints( } template -autoware_internal_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."); 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 1ae5ae87525d6..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_planning_msgs/msg/trajectory.hpp" #include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include 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 8b25f00fb74a7..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 @@ -26,12 +26,12 @@ std::optional> getPathIndexRangeWithLaneId( const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( - const autoware_internal_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 autoware_internal_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 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 9c1e8b729e9f3..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,7 +58,8 @@ void validateNonEmpty(const T & points) extern template void validateNonEmpty>( const std::vector &); -extern template void validateNonEmpty>( +extern template void +validateNonEmpty>( const std::vector &); extern template void validateNonEmpty>( const std::vector &); @@ -311,7 +312,8 @@ 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>( +extern template size_t +findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t findNearestIndex>( @@ -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, @@ -734,8 +737,8 @@ extern template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); + 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, @@ -862,8 +865,8 @@ extern template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); + 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, @@ -936,7 +939,8 @@ double calcArcLength(const T & points) extern template double calcArcLength>( const std::vector & points); -extern template double calcArcLength>( +extern template double +calcArcLength>( const std::vector & points); extern template double calcArcLength>( const std::vector & points); @@ -1032,7 +1036,8 @@ extern template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); extern template std::vector>> -calcCurvatureAndSegmentLength>( +calcCurvatureAndSegmentLength< + std::vector>( const std::vector & points); extern template std::vector>> calcCurvatureAndSegmentLength>( @@ -1147,8 +1152,8 @@ calcLongitudinalOffsetPoint> 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); + 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, @@ -1293,9 +1298,9 @@ calcLongitudinalOffsetPose>( 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); + 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, @@ -1906,7 +1911,8 @@ 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>( +extern template void +insertOrientation>( std::vector & points, const bool is_driving_forward); extern template void insertOrientation>( @@ -2051,8 +2057,8 @@ 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); + 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, @@ -2272,8 +2278,8 @@ 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>( +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()); @@ -2460,7 +2466,8 @@ 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>( +extern template double +calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); extern template double calcYawDeviation>( @@ -2495,7 +2502,8 @@ 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>( +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); diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 3995356d4e473..284bfd9749193 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -22,8 +22,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_interpolation autoware_internal_planning_msgs + autoware_interpolation autoware_planning_msgs autoware_universe_utils autoware_vehicle_msgs diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 404d548bddd20..333f5bd11a421 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -360,8 +360,8 @@ autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( } 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 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 0d5237275df97..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_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; 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; 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 577e03c3146f9..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 @@ -56,8 +56,8 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const autoware_internal_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) { @@ -77,8 +77,8 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const autoware_internal_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); diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index b4a049b1a9f01..61014d95c7588 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -24,7 +24,8 @@ namespace autoware::motion_utils // template void validateNonEmpty>( const std::vector &); -template void validateNonEmpty>( +template void +validateNonEmpty>( const std::vector &); template void validateNonEmpty>( const std::vector &); @@ -83,7 +84,8 @@ searchZeroVelocityIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( +template size_t +findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); template size_t 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,7 +124,8 @@ calcLongitudinalOffsetToSegment>( const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestSegmentIndex>( +template size_t +findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); template size_t findNearestSegmentIndex>( @@ -147,7 +150,8 @@ findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( +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>( @@ -158,7 +162,8 @@ template double calcLateralOffset>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( +template double +calcLateralOffset>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double calcLateralOffset>( @@ -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,7 +200,8 @@ calcSignedArcLengthPartialSum>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( +template double +calcSignedArcLength>( const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); template double calcSignedArcLength>( @@ -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,7 +224,8 @@ template double calcSignedArcLength>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( +template double +calcSignedArcLength>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); template double calcSignedArcLength>( @@ -226,7 +235,8 @@ template double calcSignedArcLength>( const std::vector & points); -template double calcArcLength>( +template double +calcArcLength>( const std::vector & points); template double calcArcLength>( const std::vector & points); @@ -245,8 +255,8 @@ calcCurvature>( template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector>> -calcCurvatureAndSegmentLength>( +template std::vector>> calcCurvatureAndSegmentLength< + std::vector>( const std::vector & points); template std::vector>> calcCurvatureAndSegmentLength>( @@ -265,8 +275,8 @@ calcLongitudinalOffsetPoint> 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); + 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, @@ -294,8 +304,8 @@ calcLongitudinalOffsetPose>( 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, + 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>( @@ -377,8 +387,8 @@ insertTargetPoint>( template std::optional 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, @@ -450,7 +460,8 @@ insertDecelPoint>( // template void insertOrientation>( std::vector & points, const bool is_driving_forward); -template void insertOrientation>( +template void +insertOrientation>( std::vector & points, const bool is_driving_forward); template void insertOrientation>( @@ -462,7 +473,8 @@ 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>( +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); @@ -475,7 +487,8 @@ 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>( +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>( @@ -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); @@ -582,7 +596,8 @@ cropPoints>( template double calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( +template double +calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); template double calcYawDeviation>( @@ -594,7 +609,8 @@ template bool isTargetPointFront & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool isTargetPointFront>( +template bool +isTargetPointFront>( const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); 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 8d132138ac3a7..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; 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 d9a0795c6a856..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_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; 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; constexpr double epsilon = 1e-6; 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 a5a150ae81def..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_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 daebd89736b11..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; /** diff --git a/common/autoware_test_utils/src/topic_snapshot_saver.cpp b/common/autoware_test_utils/src/topic_snapshot_saver.cpp index cd4986c665b8a..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 - autoware_internal_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_trajectory/include/autoware/trajectory/detail/utils.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp index 1bab6b8a83898..49bc5fdbb3a0c 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp @@ -19,10 +19,10 @@ #include +#include #include #include #include -#include #include #include @@ -38,7 +38,8 @@ 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 autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); +geometry_msgs::msg::Point to_point( + const autoware_internal_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); diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml index 19bc97b84d2a1..6e291576f08a2 100644 --- a/common/autoware_trajectory/package.xml +++ b/common/autoware_trajectory/package.xml @@ -14,8 +14,8 @@ ament_cmake_auto autoware_cmake - autoware_planning_msgs autoware_internal_planning_msgs + autoware_planning_msgs geometry_msgs lanelet2_core rclcpp diff --git a/common/autoware_trajectory/src/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp index cd655f42589b0..c82ac1087e9c7 100644 --- a/common/autoware_trajectory/src/detail/util.cpp +++ b/common/autoware_trajectory/src/detail/util.cpp @@ -46,7 +46,8 @@ geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint return point; } -geometry_msgs::msg::Point to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) +geometry_msgs::msg::Point to_point( + const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { geometry_msgs::msg::Point point; point.x = p.point.pose.position.x; diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp index 74c4d27f869d0..4492ff277c07a 100644 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -18,7 +18,8 @@ #include #include -using Trajectory = autoware::trajectory::Trajectory; +using Trajectory = + autoware::trajectory::Trajectory; autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( double x, double y, uint8_t lane_id) 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 25fd63441010f..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 autoware_internal_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 autoware_internal_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 autoware_internal_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, autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) + const geometry_msgs::msg::Pose & pose, + autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { p.point.pose = pose; } 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 380570ba80c16..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 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 d6fbabcab354c..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 autoware_internal_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 6af1fbe464b92..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_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::PoseWithCovariance; 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 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 dac3e7e8748c8..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 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 a5c1c60fbff5b..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 69e839ed7c324..de9a8b760198c 100644 --- a/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp +++ b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp @@ -19,10 +19,11 @@ namespace autoware::planning_factor_interface { -template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, - const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, - const std::string &); +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); template void PlanningFactorInterface::add( const std::vector &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, @@ -32,10 +33,11 @@ template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, - const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, - const double, const std::string &); +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, + const double, const double, const std::string &); template void PlanningFactorInterface::add( const std::vector &, const Pose &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index af4631d837ead..6afe346748d7f 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -52,7 +53,6 @@ #include #include #include -#include #include #include @@ -67,6 +67,7 @@ namespace autoware::planning_test_manager { 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::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightGroupArray; @@ -82,7 +83,6 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using autoware_internal_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; 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 252ca3e8f0fda..bb1a804888dec 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 @@ -285,7 +285,9 @@ void PlanningInterfaceTestManager::publishNominalPath( const auto path = autoware::test_utils::loadPathWithLaneIdInYaml(); autoware::test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - autoware::motion_utils::convertToPath(path), 5); + 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 617095ea4658c..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 autoware_internal_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 c62e557416b48..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_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 6efc8e7b6b8a1..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_perception_msgs::msg::PredictedPath; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedPath; struct MinMaxValue { 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 d9abe60b1a862..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_planning_msgs::msg::LaneletRoute; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::LaneletRoute; 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 3424b183d972c..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_planning_msgs::msg::LaneletRoute; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::LaneletRoute; 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/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 ca4a63715f444..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 geometry_msgs::msg::Pose; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; 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 aac06d8286a96..50f8adc8da1f0 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 @@ -23,8 +23,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/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 6137d346d790a..4f75c46369094 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 geometry_msgs::msg::Pose; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; 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/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index f3d164667ec29..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 autoware_internal_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_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 8313df88729b3..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 autoware_internal_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 1b651cd377c90..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 autoware_internal_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 632e307d1c017..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 autoware_internal_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/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 eb2c5a53f757c..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 autoware_internal_planning_msgs::msg::PathWithLaneId; rclcpp::Logger get_logger(); 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 1f7233e4aedf8..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 269bb62473d1f..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,11 +73,11 @@ 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 autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; 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 37e3c1737038f..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 autoware_internal_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 6dc3b8a04499e..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 | `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` | +| 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 34c304a2e5859..fa3224db78339 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 @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -37,7 +38,6 @@ #include #include #include -#include #include #include #include @@ -53,6 +53,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; @@ -67,7 +68,6 @@ using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; -using autoware_internal_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/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 9745e46d70926..51d9da9953fc7 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 @@ -681,8 +681,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/test/input.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp index 69ff5ce9dbbc1..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 "autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp" -#include "autoware_internal_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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_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 990ef48358f0e..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 autoware_internal_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 3b8cd80fad138..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 tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; 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 a3669c71a104b..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 autoware_internal_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 0d5cde1da7091..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 autoware_internal_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 04cbc91a556ca..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_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 c14ecbfb02ab5..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 autoware_internal_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 4acdaae496840..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 geometry_msgs::msg::Point; -using geometry_msgs::msg::Pose; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; 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 c71b6cc1791f5..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_perception_msgs::msg::PredictedObject; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; /** * @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 autoware_internal_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 d0e19bc1d63aa..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 geometry_msgs::msg::Point; -using geometry_msgs::msg::Pose; 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 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 f4ada8a8e9dd0..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 autoware_internal_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 def4b2c68321a..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_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 2185734581208..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 autoware_internal_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 cae26bf39f697..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 autoware_internal_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 2dec7b481d401..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 @@ -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 0d725c149dbd0..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 autoware_internal_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_parking_departure_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp index 2c793e673d9b8..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_planning_msgs::msg::Trajectory; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Trajectory; using autoware::test_utils::generateTrajectory; 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 7d2f0f7b0221d..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 autoware_internal_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 763021dbe2faa..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 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 946ac43e596ba..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 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 01b34d6b8f8bf..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 autoware_internal_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 81702cff68486..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_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 be3c2528088bb..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 "tier4_planning_msgs/msg/lateral_offset.hpp" #include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" +#include "tier4_planning_msgs/msg/lateral_offset.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 50dccf6aa8c7e..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 @@ -34,8 +34,8 @@ #include namespace autoware::behavior_path_planner { -using geometry_msgs::msg::Pose; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; 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 f54c4c3becf89..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 autoware_internal_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 04ccc4bdf3562..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 autoware_internal_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/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 b7c398ab04f9e..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 geometry_msgs::msg::Pose; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; class PullOutPlannerBase { 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 55f487656359d..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 autoware_internal_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_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 f27f4251a5d7b..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 autoware_internal_planning_msgs::msg::PathWithLaneId; // ROS 2 general msgs using geometry_msgs::msg::Point; 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 bb4268bcc111f..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 @@ -44,7 +44,8 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const autoware_internal_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 44614ad849de5..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,7 +105,8 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path); // setSafe(), setDistance() void setRTCStatus( - const BlindSpotDecision & decision, const autoware_internal_planning_msgs::msg::PathWithLaneId & path); + const BlindSpotDecision & decision, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); template void setRTCStatusByDecision( const Decision & decision, const autoware_internal_planning_msgs::msg::PathWithLaneId & path); 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 ebedebc0483d3..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 @@ -49,8 +49,8 @@ struct InterpolatedPathInfo }; std::optional generateInterpolatedPathInfo( - const lanelet::Id lane_id, const autoware_internal_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, 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 2034ad3b3444d..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 autoware_internal_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(), 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 1bcb328c5f2a9..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 autoware_internal_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 autoware_internal_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, autoware_internal_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; - autoware_internal_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 { 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 258cce4302d98..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 @@ -73,8 +73,8 @@ static std::optional> findLaneIdInterval( } // namespace std::optional generateInterpolatedPathInfo( - const lanelet::Id lane_id, const autoware_internal_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; 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 2a51e08319aa6..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 @@ -91,12 +91,14 @@ struct DebugData std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_internal_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 autoware_internal_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/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 4833ff905c28f..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 autoware_internal_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 b37d80f7605d4..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 autoware_internal_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 autoware_internal_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_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index c0b32a0300151..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 @@ -49,7 +49,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const autoware_internal_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 autoware_internal_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 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 96b88fbf31c2f..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 @@ -552,9 +552,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const NonOccludedCollisionStop & result, const autoware_internal_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 autoware_internal_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 autoware_internal_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 autoware_internal_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 autoware_internal_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 autoware_internal_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 autoware_internal_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 autoware_internal_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(); @@ -1163,7 +1165,8 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const DecisionResult & decision_result, autoware_internal_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( 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 d16a313e97f9e..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 @@ -520,7 +520,8 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * @brief act based on current RTC approval */ void reactRTCApproval( - const DecisionResult & decision_result, autoware_internal_planning_msgs::msg::PathWithLaneId * path); + const DecisionResult & decision_result, + autoware_internal_planning_msgs::msg::PathWithLaneId * path); /** @}*/ private: @@ -723,8 +724,8 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC * intersection_stoplines.occlusion_stopline */ PassJudgeStatus isOverPassJudgeLinesStatus( - const autoware_internal_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: 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 9f3cba5c9010a..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 c15739835eccd..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 autoware_internal_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, autoware_internal_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; - autoware_internal_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 autoware_internal_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()) { 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 74cb01a4d9139..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, autoware_internal_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 autoware_internal_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 autoware_internal_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 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 221821a6715a5..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 @@ -42,7 +42,8 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const autoware_internal_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 2356728666700..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 db4f0cff3fae7..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 @@ -127,7 +127,8 @@ bool is_stoppable( } Polygon2d generate_ego_no_stopping_area_lane_polygon( - const autoware_internal_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) 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 9123e52bb4733..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( - autoware_internal_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 @@ -128,7 +129,8 @@ bool is_stoppable( * @return generated polygon */ Polygon2d generate_ego_no_stopping_area_lane_polygon( - const autoware_internal_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); 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 b48348d37b886..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 @@ -123,7 +123,8 @@ TEST(NoStoppingAreaTest, generateStopLine) { using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; constexpr auto nb_points = 10; - const autoware_internal_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; 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 29cbbcd0d771b..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 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 5bc2fcce246cb..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 autoware_internal_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 fde0e19dd0282..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_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/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 5695bc4cd9da1..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 15f2a9de8ad2a..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 autoware_internal_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 - autoware_internal_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_planner/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md index e127ba74d849d..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` | autoware_internal_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 63475a45dd1e8..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 @@ -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 autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); + void onTrigger( + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); void onParam(); 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 5ff800f4c2666..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 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 9b109652972d6..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,7 +73,8 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( } } -autoware_internal_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( +autoware_internal_planning_msgs::msg::PathWithLaneId +BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path_msg) { 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 41da013063754..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 builtin_interfaces::msg::Time; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using builtin_interfaces::msg::Time; using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest @@ -161,7 +161,10 @@ class SceneModuleManagerInterface deleteExpiredModules(path); } - virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } + virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) + { + modifyPathVelocity(path); + } protected: virtual void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) @@ -198,12 +201,14 @@ class SceneModuleManagerInterface std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); } - virtual void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual void launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> getModuleExpiredFunction( const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) + virtual void deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -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_; 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 272e1c6e819dc..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 @@ -156,7 +156,8 @@ std::optional findOffsetSegment( } std::optional findOffsetSegment( - const autoware_internal_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) 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 53350e0ea9dd9..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( - autoware_internal_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 0818ba8166802..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 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 5c4081b4cd10f..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,8 +17,8 @@ #include -#include #include +#include namespace autoware::behavior_velocity_planner { 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 21f39ad601454..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 autoware_internal_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 3632d10aa2647..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,9 +67,9 @@ using LineString2d = autoware::universe_utils::LineString2d; using Polygon2d = autoware::universe_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; namespace planning_utils { @@ -218,8 +218,9 @@ std::set getLaneIdSetOnPath( const geometry_msgs::msg::Pose & current_pose); bool isOverLine( - 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); + 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/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 7917175a58f63..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 @@ -89,7 +89,8 @@ std::optional checkCollision( } std::optional findOffsetSegment( - const autoware_internal_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); 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 e2b36dc73f370..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_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; 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 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 036ca2a2853d2..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 @@ -593,8 +593,9 @@ std::vector getSubsequentLaneIdsSetOnPath( // TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( - 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) + 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_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp index ea32e9c15172f..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 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 6c72307d9ccc9..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,9 +15,9 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include #include #include +#include #include 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 d5e5084c8e597..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 builtin_interfaces::msg::Time; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using builtin_interfaces::msg::Time; using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; @@ -123,7 +123,8 @@ class SceneModuleManagerInterfaceWithRTC void publishObjectsOfInterestMarker(); - void deleteExpiredModules(const autoware_internal_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) { 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 74d5dcbcf17b5..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(autoware_internal_planning_msgs::msg::PathWithLaneId * path) +void SceneModuleManagerInterfaceWithRTC::plan( + autoware_internal_planning_msgs::msg::PathWithLaneId * path) { setActivation(); modifyPathVelocity(path); 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 bd17d7d2c5fbd..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,8 +55,6 @@ using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; using run_out_utils::PlannerParam; using run_out_utils::PredictedPath; -using autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; /** 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 62c9dd73d0387..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 autoware_internal_planning_msgs::msg::PathWithLaneId & path) +void RunOutModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return; 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 35ffb685acbca..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 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 38b97610dfe94..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 autoware_internal_planning_msgs::msg::PathPointWithLaneId; -using autoware_internal_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; class RunOutModule : public SceneModuleInterface 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 ef4bf52424587..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,11 +40,11 @@ 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 autoware_internal_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; struct CommonParam { 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 a5f5aa951256f..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 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 bf7be54d0bd41..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 @@ -31,9 +31,9 @@ #include using autoware::behavior_velocity_planner::run_out_utils::findLongitudinalNearestPoint; -using geometry_msgs::msg::Point; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Point; 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 489d3ea99368b..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 @@ -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 geometry_msgs::msg::Point; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Point; 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 223b448798f4f..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_perception_msgs::msg::ObjectClassification; -using geometry_msgs::msg::Point; 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; 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 609689b4d5c61..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 autoware_internal_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(), 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 edc0f0d5b8c70..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 geometry_msgs::msg::Point32; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Point32; // 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 fd3a9f51409ac..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 autoware_internal_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 autoware_internal_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 autoware_internal_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())) { 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 d56dceaba9e0e..637c383caba2c 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 @@ -44,8 +44,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; 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 8cecaad11e62c..5edad3f12b089 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 @@ -46,7 +46,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) "~/output/traffic_signal", 1); } -void TrafficLightModuleManager::modifyPathVelocity(autoware_internal_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; 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 61b6e503a89be..e3884821101f6 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 @@ -43,7 +43,8 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> - getModuleExpiredFunction(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; + getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override; 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 f32bf2a41311d..fe603b46285bf 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 @@ -275,8 +275,8 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const } 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) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, + const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point) { autoware_internal_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; 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 1f34a6e6a7857..bb4849b9470c4 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 @@ -92,8 +92,8 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC bool isStopSignal(); 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); + 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 edb2245f49b05..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 autoware_internal_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; 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 7bdf3b19d2576..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 autoware_internal_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. 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 ea498e0062768..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 diff --git a/visualization/tier4_planning_rviz_plugin/src/path/display.cpp b/visualization/tier4_planning_rviz_plugin/src/path/display.cpp index b0f4af2220a03..79c6e745b20db 100644 --- a/visualization/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/visualization/tier4_planning_rviz_plugin/src/path/display.cpp @@ -77,7 +77,8 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( } void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( - const autoware_internal_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);