diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp b/common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp index 247844123881b..365ed0adff19b 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp +++ b/common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp @@ -19,36 +19,36 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace autoware::component_interface_specs::planning { struct SetLaneletRoute { - using Service = tier4_planning_msgs::srv::SetLaneletRoute; + using Service = autoware_planning_msgs::srv::SetLaneletRoute; static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_lanelet_route"; }; struct SetWaypointRoute { - using Service = tier4_planning_msgs::srv::SetWaypointRoute; + using Service = autoware_planning_msgs::srv::SetWaypointRoute; static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_waypoint_route"; }; struct ClearRoute { - using Service = tier4_planning_msgs::srv::ClearRoute; + using Service = autoware_planning_msgs::srv::ClearRoute; static constexpr char name[] = "/planning/mission_planning/route_selector/main/clear_route"; }; struct RouteState { - using Message = tier4_planning_msgs::msg::RouteState; + using Message = autoware_planning_msgs::msg::RouteState; static constexpr char name[] = "/planning/mission_planning/route_selector/main/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/autoware_component_interface_specs/package.xml b/common/autoware_component_interface_specs/package.xml index 4bbe62e7e9701..a4341b3b4ba35 100644 --- a/common/autoware_component_interface_specs/package.xml +++ b/common/autoware_component_interface_specs/package.xml @@ -22,7 +22,6 @@ rosidl_runtime_cpp tier4_control_msgs tier4_localization_msgs - tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs diff --git a/common/autoware_motion_utils/README.md b/common/autoware_motion_utils/README.md index 993ec7a3ea7c3..ca6359126766d 100644 --- a/common/autoware_motion_utils/README.md +++ b/common/autoware_motion_utils/README.md @@ -47,7 +47,7 @@ The second function finds the nearest index in the lane whose id is `lane_id`. ```cpp size_t findNearestIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); ``` diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp index 7b720c69c6701..3370cbeb44d16 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp @@ -21,11 +21,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -34,10 +34,10 @@ namespace autoware::motion_utils { using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::ControlPoint; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::PlanningFactorArray; -using tier4_planning_msgs::msg::SafetyFactorArray; +using autoware_planning_msgs::msg::ControlPoint; +using autoware_planning_msgs::msg::PlanningFactor; +using autoware_planning_msgs::msg::PlanningFactorArray; +using autoware_planning_msgs::msg::SafetyFactorArray; class PlanningFactorInterface { @@ -124,13 +124,13 @@ class PlanningFactorInterface const SafetyFactorArray & safety_factors, const bool is_driving_forward = true, const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") { - const auto control_point = tier4_planning_msgs::build() + const auto control_point = autoware_planning_msgs::build() .pose(control_point_pose) .velocity(velocity) .shift_length(shift_length) .distance(distance); - const auto factor = tier4_planning_msgs::build() + const auto factor = autoware_planning_msgs::build() .module(name_) .is_driving_forward(is_driving_forward) .control_points({control_point}) @@ -161,19 +161,19 @@ class PlanningFactorInterface const bool is_driving_forward = true, const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") { - const auto control_start_point = tier4_planning_msgs::build() + const auto control_start_point = autoware_planning_msgs::build() .pose(start_pose) .velocity(velocity) .shift_length(shift_length) .distance(start_distance); - const auto control_end_point = tier4_planning_msgs::build() + const auto control_end_point = autoware_planning_msgs::build() .pose(end_pose) .velocity(velocity) .shift_length(shift_length) .distance(end_distance); - const auto factor = tier4_planning_msgs::build() + const auto factor = autoware_planning_msgs::build() .module(name_) .is_driving_forward(is_driving_forward) .control_points({control_start_point, control_end_point}) @@ -209,8 +209,8 @@ class PlanningFactorInterface std::vector factors_; }; -extern template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +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( @@ -222,8 +222,8 @@ extern template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +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( diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp index 19328179932c4..7d656073c569a 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 @@ -17,7 +17,7 @@ #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -113,8 +113,8 @@ std::vector resamplePoseVector( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, +autoware_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -136,8 +136,8 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, +autoware_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_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); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp index d4f88c17c4d70..fe1d6dd57101b 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 @@ -19,7 +19,7 @@ #include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" -#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include @@ -58,7 +58,7 @@ autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input template <> inline autoware_planning_msgs::msg::Path convertToPath( - const tier4_planning_msgs::msg::PathWithLaneId & input) + const autoware_planning_msgs::msg::PathWithLaneId & input) { autoware_planning_msgs::msg::Path output{}; output.header = input.header; @@ -80,7 +80,7 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) template <> inline TrajectoryPoints convertToTrajectoryPoints( - const tier4_planning_msgs::msg::PathWithLaneId & input) + const autoware_planning_msgs::msg::PathWithLaneId & input) { TrajectoryPoints output{}; for (const auto & p : input.points) { @@ -95,19 +95,19 @@ inline TrajectoryPoints convertToTrajectoryPoints( } template -tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input) +autoware_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); } template <> -inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( +inline autoware_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( const TrajectoryPoints & input) { - tier4_planning_msgs::msg::PathWithLaneId output{}; + autoware_planning_msgs::msg::PathWithLaneId output{}; for (const auto & p : input) { - tier4_planning_msgs::msg::PathPointWithLaneId pp; + autoware_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose = p.pose; pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; output.points.emplace_back(pp); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp index 5272478cccd78..c13a4fb443042 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 @@ -18,7 +18,7 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -51,8 +51,8 @@ autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( * twist information * @return resampled path(poses) */ -tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const tier4_planning_msgs::msg::PathWithLaneId & path, +autoware_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp index 0d875e636bad5..0c181dec46acc 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -23,14 +23,14 @@ namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + const autoware_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); size_t findNearestSegmentIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_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 @@ -38,8 +38,8 @@ size_t findNearestSegmentIndexFromLaneId( // @param [in] path with position to be followed by the center of the vehicle // @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle // center follow the input path NOTE: rear_to_cog is supposed to be positive -tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +autoware_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); } // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 0ce42fbe1080f..77d4e234bf9da 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 @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -58,8 +58,8 @@ void validateNonEmpty(const T & points) extern template void validateNonEmpty>( const std::vector &); -extern template void validateNonEmpty>( - const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); extern template void validateNonEmpty>( const std::vector &); @@ -116,8 +116,8 @@ extern template std::optional isDrivingForward>( const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional isDrivingForward>( const std::vector &); @@ -151,8 +151,8 @@ extern template std::optional isDrivingForwardWithTwist>( const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -196,9 +196,9 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) extern template std::vector removeOverlapPoints>( const std::vector & points, const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); extern template std::vector removeOverlapPoints>( @@ -311,8 +311,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>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t findNearestIndex>( const std::vector & points, @@ -379,8 +379,8 @@ findNearestIndex>( const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -462,8 +462,8 @@ calcLongitudinalOffsetToSegment & 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, +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>( @@ -503,8 +503,8 @@ extern template size_t findNearestSegmentIndex & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t findNearestSegmentIndex>( @@ -555,8 +555,8 @@ findNearestSegmentIndex>( const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -629,8 +629,8 @@ extern template double calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); extern template double calcLateralOffset>( @@ -691,8 +691,8 @@ extern template double calcLateralOffset & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double calcLateralOffset>( const std::vector & points, @@ -733,8 +733,8 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -783,8 +783,8 @@ calcSignedArcLengthPartialSum & 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, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template std::vector calcSignedArcLengthPartialSum>( @@ -825,8 +825,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -861,8 +861,8 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double calcSignedArcLength>( @@ -908,8 +908,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double calcSignedArcLength>( @@ -936,8 +936,8 @@ double calcArcLength(const T & points) extern template double calcArcLength>( const std::vector & points); -extern template double calcArcLength>( - const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); extern template double calcArcLength>( const std::vector & points); @@ -974,8 +974,8 @@ extern template std::vector calcCurvature>( const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector calcCurvature>( const std::vector & points); @@ -1032,8 +1032,8 @@ extern template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); extern template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); +calcCurvatureAndSegmentLength>( + const std::vector & points); extern template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); @@ -1146,8 +1146,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional calcLongitudinalOffsetPoint>( @@ -1191,8 +1191,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional calcLongitudinalOffsetPoint>( @@ -1292,8 +1292,8 @@ calcLongitudinalOffsetPose>( const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional @@ -1339,8 +1339,8 @@ calcLongitudinalOffsetPose>( const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional @@ -1441,9 +1441,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1496,9 +1496,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1569,9 +1569,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1623,9 +1623,9 @@ insertTargetPoint>( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional @@ -1674,9 +1674,9 @@ insertStopPoint>( std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional insertStopPoint>( @@ -1723,9 +1723,9 @@ insertStopPoint>( std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional insertStopPoint>( @@ -1779,9 +1779,9 @@ insertStopPoint>( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional @@ -1906,8 +1906,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>( - std::vector & points, +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); extern template void insertOrientation>( std::vector & points, @@ -1974,8 +1974,8 @@ extern template double calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double @@ -2013,8 +2013,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -2050,8 +2050,8 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double calcSignedArcLength>( @@ -2153,8 +2153,8 @@ findFirstNearestIndexWithSoftConstraints::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2209,8 +2209,8 @@ extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2273,8 +2273,8 @@ calcDistanceToForwardStopPoint::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -2312,9 +2312,9 @@ cropForwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); extern template std::vector @@ -2352,9 +2352,9 @@ cropBackwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); extern template std::vector @@ -2394,9 +2394,9 @@ cropPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); extern template std::vector @@ -2460,8 +2460,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>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); extern template double calcYawDeviation>( const std::vector & points, @@ -2495,8 +2495,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>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); extern template bool isTargetPointFront>( diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 029985a53f020..902518abe59c3 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -32,7 +32,6 @@ rclcpp tf2 tf2_geometry_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp index d09355b4003ae..0dc2d29ddf37a 100644 --- a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp @@ -19,8 +19,8 @@ namespace autoware::motion_utils { -template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +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( @@ -32,8 +32,8 @@ template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +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( diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp index cfa3c91eac43e..361b3103a26b3 100644 --- a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -48,8 +48,8 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, const VelocityFactorStatus, const std::string &); template void VelocityFactorInterface::set( const std::vector &, const Pose &, const Pose &, diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 9e736444495fa..52b80a3695295 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -186,8 +186,8 @@ std::vector resamplePoseVector( return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, +autoware_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -340,7 +340,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( return input_path; } - tier4_planning_msgs::msg::PathWithLaneId resampled_path; + autoware_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; @@ -359,8 +359,8 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( return resampled_path; } -tier4_planning_msgs::msg::PathWithLaneId resamplePath( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, +autoware_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_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) { diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index 0ae9d44683f62..9a46f2ae47b2f 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -19,8 +19,8 @@ using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; namespace autoware::motion_utils { diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index 55c7360a25c41..18ad117ef050b 100644 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -25,7 +25,7 @@ namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) + const autoware_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -56,7 +56,7 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id) { const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); @@ -66,7 +66,7 @@ size_t findNearestIndexFromLaneId( validateNonEmpty(path.points); - const auto sub_points = std::vector{ + const auto sub_points = std::vector{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; validateNonEmpty(sub_points); @@ -77,7 +77,7 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id) { const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); @@ -99,8 +99,8 @@ size_t findNearestSegmentIndexFromLaneId( } // NOTE: rear_to_cog is supposed to be positive -tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +autoware_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation) { auto cog_path = path; diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 35e39fda75a69..4d36874d60018 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -24,8 +24,8 @@ namespace autoware::motion_utils // template void validateNonEmpty>( const std::vector &); -template void validateNonEmpty>( - const std::vector &); +template void validateNonEmpty>( + const std::vector &); template void validateNonEmpty>( const std::vector &); @@ -33,8 +33,8 @@ template void validateNonEmpty isDrivingForward>( const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); template std::optional isDrivingForward>( const std::vector &); @@ -44,8 +44,8 @@ template std::optional isDrivingForwardWithTwist>( const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -54,9 +54,9 @@ isDrivingForwardWithTwist removeOverlapPoints>( const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); template std::vector removeOverlapPoints>( @@ -83,8 +83,8 @@ searchZeroVelocityIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); template size_t findNearestIndex>( const std::vector & points, @@ -96,8 +96,8 @@ findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional findNearestIndex>( @@ -110,8 +110,8 @@ calcLongitudinalOffsetToSegment & 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, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double calcLongitudinalOffsetToSegment>( @@ -122,8 +122,8 @@ calcLongitudinalOffsetToSegment>( const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); template size_t findNearestSegmentIndex>( const std::vector & points, @@ -135,8 +135,8 @@ findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional findNearestSegmentIndex>( @@ -147,8 +147,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>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); template double calcLateralOffset>( const std::vector & points, @@ -158,8 +158,8 @@ template double calcLateralOffset>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double calcLateralOffset>( const std::vector & points, @@ -169,8 +169,8 @@ 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, +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, @@ -182,8 +182,8 @@ calcSignedArcLengthPartialSum & points, const size_t src_idx, const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); template std::vector calcSignedArcLengthPartialSum>( @@ -194,8 +194,8 @@ calcSignedArcLengthPartialSum>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); template double calcSignedArcLength>( const std::vector &, @@ -205,8 +205,8 @@ 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, +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, @@ -216,8 +216,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>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); template double calcSignedArcLength>( const std::vector & points, @@ -226,8 +226,8 @@ template double calcSignedArcLength>( const std::vector & points); -template double calcArcLength>( - const std::vector & points); +template double calcArcLength>( + const std::vector & points); template double calcArcLength>( const std::vector & points); @@ -235,8 +235,8 @@ template double calcArcLength calcCurvature>( const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); template std::vector calcCurvature>( const std::vector & points); @@ -246,8 +246,8 @@ template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); +calcCurvatureAndSegmentLength>( + const std::vector & points); template std::vector>> calcCurvatureAndSegmentLength>( const std::vector & points); @@ -264,8 +264,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional calcLongitudinalOffsetPoint>( @@ -278,8 +278,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional calcLongitudinalOffsetPoint>( @@ -293,8 +293,8 @@ calcLongitudinalOffsetPose>( const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional @@ -310,8 +310,8 @@ calcLongitudinalOffsetPose>( const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional @@ -326,9 +326,9 @@ insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -342,9 +342,9 @@ insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -358,9 +358,9 @@ insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -375,9 +375,9 @@ insertTargetPoint>( std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -391,9 +391,9 @@ template std::optional insertStopPoint & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional insertStopPoint>( @@ -407,9 +407,9 @@ template std::optional insertStopPoint & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional insertStopPoint>( @@ -423,9 +423,9 @@ template std::optional insertStopPoint & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional insertStopPoint>( @@ -450,8 +450,8 @@ insertDecelPoint>( // template void insertOrientation>( std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); template void insertOrientation>( std::vector & points, @@ -462,8 +462,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>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); template double calcSignedArcLength>( @@ -475,8 +475,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>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); template double calcSignedArcLength>( const std::vector & points, @@ -486,8 +486,8 @@ 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, +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, @@ -499,8 +499,8 @@ findFirstNearestIndexWithSoftConstraints & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints>( @@ -513,8 +513,8 @@ template size_t findFirstNearestSegmentIndexWithSoftConstraints< const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< std::vector>( @@ -533,9 +533,9 @@ cropForwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); template std::vector @@ -550,9 +550,9 @@ cropBackwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); template std::vector @@ -567,9 +567,9 @@ cropPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); template std::vector @@ -582,8 +582,8 @@ cropPoints>( template double calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); template double calcYawDeviation>( const std::vector & points, @@ -594,8 +594,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>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); template bool isTargetPointFront>( diff --git a/common/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp index ef97dfa410d2d..6354395384f32 100644 --- a/common/autoware_motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -35,8 +35,8 @@ using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -316,7 +316,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Output key is not same as input { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -422,7 +422,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Duplicated points in the original path { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(11); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -456,7 +456,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) { // Input path size is not enough for interpolation { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -489,7 +489,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -523,7 +523,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength is longer than input path { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -562,7 +562,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) using autoware::motion_utils::resamplePath; { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -670,7 +670,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // change initial orientation { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -794,7 +794,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp x, y { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -871,7 +871,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Slerp z { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -950,7 +950,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp v { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( diff --git a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp index b4b60ff403048..c443415a69fa1 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp @@ -32,8 +32,8 @@ using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::transformPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -351,7 +351,7 @@ TEST(Interpolation, interpolate_path_for_path) using autoware::motion_utils::calcInterpolatedPoint; { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -515,7 +515,7 @@ TEST(Interpolation, interpolate_path_for_path) // Duplicated Points { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 62e4ac74cb639..9e0620289682e 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -23,8 +23,8 @@ namespace { using autoware::universe_utils::createPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -55,7 +55,7 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { using autoware::motion_utils::getPathIndexRangeWithLaneId; - using tier4_planning_msgs::msg::PathWithLaneId; + using autoware_planning_msgs::msg::PathWithLaneId; // Usual cases { diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index b98af2133f175..7063f78ee9d12 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 @@ -30,9 +30,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include @@ -54,8 +54,8 @@ using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -63,7 +63,7 @@ using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using tf2_msgs::msg::TFMessage; -using tier4_planning_msgs::msg::Scenario; +using autoware_planning_msgs::msg::Scenario; /** * @brief Creates a Pose message with the specified position and orientation. 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 5e07237e2c495..1944b896e6040 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 @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include @@ -71,8 +71,8 @@ using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistWithCovariance; using nav_msgs::msg::Odometry; using std_msgs::msg::Header; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; /** diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index c328f37ba357d..3867f63520f58 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -33,7 +33,6 @@ tf2_msgs tf2_ros tier4_api_msgs - tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor diff --git a/common/autoware_test_utils/test/test_mock_data_parser.cpp b/common/autoware_test_utils/test/test_mock_data_parser.cpp index 036f16c827713..dc29dd70ade16 100644 --- a/common/autoware_test_utils/test/test_mock_data_parser.cpp +++ b/common/autoware_test_utils/test/test_mock_data_parser.cpp @@ -18,13 +18,13 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "autoware_test_utils/mock_data_parser.hpp" -#include +#include #include namespace autoware::test_utils { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; // Example YAML structure as a string for testing const char g_complete_yaml[] = R"( diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp index 3346d4ce8104f..8cff37c6c0d91 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include @@ -38,7 +38,7 @@ geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p); geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p); geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p); geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p); -geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p); +geometry_msgs::msg::Point to_point(const autoware_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/include/autoware/trajectory/path_point_with_lane_id.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp index 0ed92a10a7ea4..dc98ee54b593c 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp @@ -18,7 +18,7 @@ #include "autoware/trajectory/path_point.hpp" #include "autoware/trajectory/point.hpp" -#include +#include #include #include @@ -27,11 +27,11 @@ namespace autoware::trajectory { template <> -class Trajectory +class Trajectory : public Trajectory { using BaseClass = Trajectory; - using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; + using PointType = autoware_planning_msgs::msg::PathPointWithLaneId; using LaneIdType = std::vector; public: diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml index eaa5fca80ff46..2d035393c0316 100644 --- a/common/autoware_trajectory/package.xml +++ b/common/autoware_trajectory/package.xml @@ -19,7 +19,6 @@ lanelet2_core rclcpp tf2 - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/common/autoware_trajectory/src/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp index 4c9649ef1f3ab..fe0c2d013dff5 100644 --- a/common/autoware_trajectory/src/detail/util.cpp +++ b/common/autoware_trajectory/src/detail/util.cpp @@ -46,7 +46,7 @@ geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint return point; } -geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPointWithLaneId & p) { geometry_msgs::msg::Point point; point.x = p.point.pose.position.x; diff --git a/common/autoware_trajectory/src/path_point_with_lane_id.cpp b/common/autoware_trajectory/src/path_point_with_lane_id.cpp index c3d7441fef405..ea222e1a927ef 100644 --- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp +++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp @@ -21,7 +21,7 @@ namespace autoware::trajectory { -using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; +using PointType = autoware_planning_msgs::msg::PathPointWithLaneId; bool Trajectory::build(const std::vector & points) { diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp index 6f6d52107b348..a75760a1a6702 100644 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -18,12 +18,12 @@ #include #include -using Trajectory = autoware::trajectory::Trajectory; +using Trajectory = autoware::trajectory::Trajectory; -tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( +autoware_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( double x, double y, uint8_t lane_id) { - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_planning_msgs::msg::PathPointWithLaneId point; point.point.pose.position.x = x; point.point.pose.position.y = y; point.lane_ids.emplace_back(lane_id); @@ -33,13 +33,13 @@ tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( TEST(TrajectoryCreatorTest, create) { { - std::vector points{ + std::vector points{ path_point_with_lane_id(0.00, 0.00, 0)}; auto trajectory = Trajectory::Builder{}.build(points); ASSERT_TRUE(!trajectory); } { - std::vector points{ + std::vector points{ path_point_with_lane_id(0.00, 0.00, 0), path_point_with_lane_id(0.81, 1.68, 0), path_point_with_lane_id(1.65, 2.98, 0), path_point_with_lane_id(3.30, 4.01, 1)}; auto trajectory = Trajectory::Builder{}.build(points); @@ -54,7 +54,7 @@ class TrajectoryTest : public ::testing::Test void SetUp() override { - std::vector points{ + std::vector points{ path_point_with_lane_id(0.00, 0.00, 0), path_point_with_lane_id(0.81, 1.68, 0), path_point_with_lane_id(1.65, 2.98, 0), path_point_with_lane_id(3.30, 4.01, 1), path_point_with_lane_id(4.70, 4.52, 1), path_point_with_lane_id(6.49, 5.20, 1), diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp index e6d0363846b20..07d2384addbd0 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 @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include @@ -135,7 +135,7 @@ inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::Pat } template <> -inline geometry_msgs::msg::Point getPoint(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose.position; } @@ -172,7 +172,7 @@ inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::PathP } template <> -inline geometry_msgs::msg::Pose getPose(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose; } @@ -197,7 +197,7 @@ inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::PathPoi } template <> -inline double getLongitudinalVelocity(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } @@ -236,7 +236,7 @@ inline void setPose( template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, tier4_planning_msgs::msg::PathPointWithLaneId & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::PathPointWithLaneId & p) { p.point.pose = pose; } @@ -279,7 +279,7 @@ inline void setLongitudinalVelocity( template <> inline void setLongitudinalVelocity( - const float velocity, tier4_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, autoware_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } diff --git a/common/autoware_universe_utils/package.xml b/common/autoware_universe_utils/package.xml index d04e1a57e78ab..0a3421b73ed3e 100644 --- a/common/autoware_universe_utils/package.xml +++ b/common/autoware_universe_utils/package.xml @@ -27,7 +27,6 @@ tf2_eigen tf2_geometry_msgs tier4_debug_msgs - tier4_planning_msgs unique_identifier_msgs visualization_msgs diff --git a/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index 4ed1c5497c6ae..f08c8618bafbf 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -24,7 +24,7 @@ TEST(geometry, getPoint_PathWithLaneId) const double y_ans = 2.0; const double z_ans = 3.0; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -46,7 +46,7 @@ TEST(geometry, getPose_PathWithLaneId) const double q_z_ans = 0.3; const double q_w_ans = 0.4; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_planning_msgs::msg::PathPointWithLaneId p; p.point.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -96,7 +96,7 @@ TEST(geometry, setPose_PathWithLaneId) p.orientation.z = q_z_ans; p.orientation.w = q_w_ans; - tier4_planning_msgs::msg::PathPointWithLaneId p_out{}; + autoware_planning_msgs::msg::PathPointWithLaneId p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans); @@ -113,7 +113,7 @@ TEST(geometry, setLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - tier4_planning_msgs::msg::PathPointWithLaneId p{}; + autoware_planning_msgs::msg::PathPointWithLaneId p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.point.longitudinal_velocity_mps, velocity); } diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 1ac984394a25e..68339482dc985 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 @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include @@ -54,7 +54,7 @@ using autoware::universe_utils::Segment2d; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp index bde404603749b..eafeaa2acb4b2 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 @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include @@ -37,7 +37,7 @@ using autoware::universe_utils::MultiPoint2d; using autoware::universe_utils::PoseDeviation; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; /** diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp index e17e2bc697272..ab921fb0a943c 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 @@ -24,8 +24,8 @@ using autoware::universe_utils::LinearRing2d; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::PoseWithCovariance; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; namespace diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index e2f9d17769e1a..5b16f522066d7 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -34,7 +34,6 @@ tf2_ros tier4_control_msgs tier4_external_api_msgs - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_costmap_generator/README.md b/planning/autoware_costmap_generator/README.md index 78ce05cc46dc9..67b8c575deca1 100644 --- a/planning/autoware_costmap_generator/README.md +++ b/planning/autoware_costmap_generator/README.md @@ -11,7 +11,7 @@ This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `Occupan | `~input/objects` | autoware_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | | `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | | `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map, for drivable areas | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| `~input/scenario` | autoware_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index 4fed5bcb6cff3..ebf07fa55501d 100644 --- a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -62,7 +62,7 @@ #include #include #include -#include +#include #include #include @@ -106,7 +106,7 @@ class CostmapGenerator : public rclcpp::Node sub_points_{this, "~/input/points_no_ground", autoware::universe_utils::SingleDepthSensorQoS()}; autoware::universe_utils::InterProcessPollingSubscriber sub_objects_{ this, "~/input/objects"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_scenario_{this, "~/input/scenario"}; rclcpp::TimerBase::SharedPtr timer_; @@ -119,7 +119,7 @@ class CostmapGenerator : public rclcpp::Node PointsToCostmap points2costmap_{}; ObjectsToCostmap objects2costmap_; - tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; + autoware_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; struct LayerName { diff --git a/planning/autoware_costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml index 9097109de0a0b..ba0eb42da453c 100644 --- a/planning/autoware_costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -32,7 +32,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 081a6ead91cda..02c27f252d7c2 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -334,7 +334,7 @@ bool CostmapGenerator::isActive() if (!scenario_) return false; const auto & s = scenario_->activating_scenarios; return std::any_of(s.begin(), s.end(), [](const auto scenario) { - return scenario == tier4_planning_msgs::msg::Scenario::PARKING; + return scenario == autoware_planning_msgs::msg::Scenario::PARKING; }); } diff --git a/planning/autoware_costmap_generator/test/test_costmap_generator.cpp b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp index f0db05b6fdd6f..8a36a3d307b18 100644 --- a/planning/autoware_costmap_generator/test/test_costmap_generator.cpp +++ b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp @@ -25,7 +25,7 @@ #include using autoware::costmap_generator::CostmapGenerator; -using tier4_planning_msgs::msg::Scenario; +using autoware_planning_msgs::msg::Scenario; class TestCostmapGenerator : public ::testing::Test { diff --git a/planning/autoware_external_velocity_limit_selector/README.md b/planning/autoware_external_velocity_limit_selector/README.md index 92579bfd0abce..3da285dd7f03c 100644 --- a/planning/autoware_external_velocity_limit_selector/README.md +++ b/planning/autoware_external_velocity_limit_selector/README.md @@ -39,15 +39,15 @@ Example: | Name | Type | Description | | --------------------------------------------------- | ---------------------------------------------- | --------------------------------------------- | -| `~input/velocity_limit_from_api` | tier4_planning_msgs::VelocityLimit | velocity limit from api | -| `~input/velocity_limit_from_internal` | tier4_planning_msgs::VelocityLimit | velocity limit from autoware internal modules | -| `~input/velocity_limit_clear_command_from_internal` | tier4_planning_msgs::VelocityLimitClearCommand | velocity limit clear command | +| `~input/velocity_limit_from_api` | autoware_planning_msgs::VelocityLimit | velocity limit from api | +| `~input/velocity_limit_from_internal` | autoware_planning_msgs::VelocityLimit | velocity limit from autoware internal modules | +| `~input/velocity_limit_clear_command_from_internal` | autoware_planning_msgs::VelocityLimitClearCommand | velocity limit clear command | ## Outputs | Name | Type | Description | | ---------------------- | ---------------------------------- | ------------------------------------------------- | -| `~output/max_velocity` | tier4_planning_msgs::VelocityLimit | current information of the hardest velocity limit | +| `~output/max_velocity` | autoware_planning_msgs::VelocityLimit | current information of the hardest velocity limit | ## Parameters diff --git a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 6a8fae3bf394c..2a6d6effe1f3d 100644 --- a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -30,9 +30,9 @@ namespace autoware::external_velocity_limit_selector { using tier4_debug_msgs::msg::StringStamped; -using tier4_planning_msgs::msg::VelocityLimit; -using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using tier4_planning_msgs::msg::VelocityLimitConstraints; +using autoware_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::VelocityLimitClearCommand; +using autoware_planning_msgs::msg::VelocityLimitConstraints; using VelocityLimitTable = std::unordered_map; diff --git a/planning/autoware_external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml index b75a4fab72d7c..73d5ebdaaa3ee 100644 --- a/planning/autoware_external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -21,7 +21,7 @@ rclcpp rclcpp_components tier4_debug_msgs - tier4_planning_msgs + autoware_planning_msgs ros2cli topic_tools diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index 56e096944e739..ecd19ca32f29c 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -18,7 +18,7 @@ In other words, the output trajectory doesn't include both forward and backward | `~input/route` | autoware_planning_msgs::Route | route and goal pose | | `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | | `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| `~input/scenario` | autoware_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 1f5a52ca040a0..d5b48d7e6ff68 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -84,7 +84,7 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::Scenario; +using autoware_planning_msgs::msg::Scenario; struct NodeParam { diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp index 1f2347958cfd2..17a11fe1286da 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include @@ -41,7 +41,7 @@ using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::Scenario; +using autoware_planning_msgs::msg::Scenario; PoseArray trajectory_to_pose_array(const Trajectory & trajectory); diff --git a/planning/autoware_freespace_planner/package.xml b/planning/autoware_freespace_planner/package.xml index fd7e39d191647..a82def729c211 100644 --- a/planning/autoware_freespace_planner/package.xml +++ b/planning/autoware_freespace_planner/package.xml @@ -29,7 +29,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index b5993d0106add..33a7a846ba84f 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner/README.md @@ -36,15 +36,15 @@ It distributes route requests and planning results according to current MRM oper | Name | Type | Description | | ------------------------------------------------------------------- | ---------------------------------------- | ------------------------------------------ | -| `/planning/mission_planning/mission_planner/clear_route` | tier4_planning_msgs/srv/ClearRoute | route clear request | -| `/planning/mission_planning/mission_planner/set_waypoint_route` | tier4_planning_msgs/srv/SetWaypointRoute | route request with lanelet waypoints. | -| `/planning/mission_planning/mission_planner/set_lanelet_route` | tier4_planning_msgs/srv/SetLaneletRoute | route request with pose waypoints. | -| `/planning/mission_planning/route_selector/main/clear_route` | tier4_planning_msgs/srv/ClearRoute | main route clear request | -| `/planning/mission_planning/route_selector/main/set_waypoint_route` | tier4_planning_msgs/srv/SetWaypointRoute | main route request with lanelet waypoints. | -| `/planning/mission_planning/route_selector/main/set_lanelet_route` | tier4_planning_msgs/srv/SetLaneletRoute | main route request with pose waypoints. | -| `/planning/mission_planning/route_selector/mrm/clear_route` | tier4_planning_msgs/srv/ClearRoute | mrm route clear request | -| `/planning/mission_planning/route_selector/mrm/set_waypoint_route` | tier4_planning_msgs/srv/SetWaypointRoute | mrm route request with lanelet waypoints. | -| `/planning/mission_planning/route_selector/mrm/set_lanelet_route` | tier4_planning_msgs/srv/SetLaneletRoute | mrm route request with pose waypoints. | +| `/planning/mission_planning/mission_planner/clear_route` | autoware_planning_msgs/srv/ClearRoute | route clear request | +| `/planning/mission_planning/mission_planner/set_waypoint_route` | autoware_planning_msgs/srv/SetWaypointRoute | route request with lanelet waypoints. | +| `/planning/mission_planning/mission_planner/set_lanelet_route` | autoware_planning_msgs/srv/SetLaneletRoute | route request with pose waypoints. | +| `/planning/mission_planning/route_selector/main/clear_route` | autoware_planning_msgs/srv/ClearRoute | main route clear request | +| `/planning/mission_planning/route_selector/main/set_waypoint_route` | autoware_planning_msgs/srv/SetWaypointRoute | main route request with lanelet waypoints. | +| `/planning/mission_planning/route_selector/main/set_lanelet_route` | autoware_planning_msgs/srv/SetLaneletRoute | main route request with pose waypoints. | +| `/planning/mission_planning/route_selector/mrm/clear_route` | autoware_planning_msgs/srv/ClearRoute | mrm route clear request | +| `/planning/mission_planning/route_selector/mrm/set_waypoint_route` | autoware_planning_msgs/srv/SetWaypointRoute | mrm route request with lanelet waypoints. | +| `/planning/mission_planning/route_selector/mrm/set_lanelet_route` | autoware_planning_msgs/srv/SetLaneletRoute | mrm route request with pose waypoints. | ### Subscriptions @@ -58,11 +58,11 @@ It distributes route requests and planning results according to current MRM oper | Name | Type | Description | | ------------------------------------------------------ | ----------------------------------- | ------------------------ | -| `/planning/mission_planning/state` | tier4_planning_msgs/msg/RouteState | route state | +| `/planning/mission_planning/state` | autoware_planning_msgs/msg/RouteState | route state | | `/planning/mission_planning/route` | autoware_planning_msgs/LaneletRoute | route | -| `/planning/mission_planning/route_selector/main/state` | tier4_planning_msgs/msg/RouteState | main route state | +| `/planning/mission_planning/route_selector/main/state` | autoware_planning_msgs/msg/RouteState | main route state | | `/planning/mission_planning/route_selector/main/route` | autoware_planning_msgs/LaneletRoute | main route | -| `/planning/mission_planning/route_selector/mrm/state` | tier4_planning_msgs/msg/RouteState | mrm route state | +| `/planning/mission_planning/route_selector/mrm/state` | autoware_planning_msgs/msg/RouteState | mrm route state | | `/planning/mission_planning/route_selector/mrm/route` | autoware_planning_msgs/LaneletRoute | mrm route | | `~/debug/route_marker` | visualization_msgs/msg/MarkerArray | route marker for debug | | `~/debug/goal_footprint` | visualization_msgs/msg/MarkerArray | goal footprint for debug | diff --git a/planning/autoware_mission_planner/package.xml b/planning/autoware_mission_planner/package.xml index e7cf974b2ba25..d660387d4bb4f 100644 --- a/planning/autoware_mission_planner/package.xml +++ b/planning/autoware_mission_planner/package.xml @@ -31,7 +31,6 @@ rclcpp_components tf2_geometry_msgs tf2_ros - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index 987ca6482ec11..2373cddaeaed8 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -31,11 +31,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -58,11 +58,11 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; using std_msgs::msg::Header; -using tier4_planning_msgs::msg::RerouteAvailability; -using tier4_planning_msgs::msg::RouteState; -using tier4_planning_msgs::srv::ClearRoute; -using tier4_planning_msgs::srv::SetLaneletRoute; -using tier4_planning_msgs::srv::SetWaypointRoute; +using autoware_planning_msgs::msg::RerouteAvailability; +using autoware_planning_msgs::msg::RouteState; +using autoware_planning_msgs::srv::ClearRoute; +using autoware_planning_msgs::srv::SetLaneletRoute; +using autoware_planning_msgs::srv::SetWaypointRoute; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; diff --git a/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp b/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp index 5c8f81aaf216e..bc0493b9b125a 100644 --- a/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp @@ -20,10 +20,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -33,10 +33,10 @@ namespace autoware::mission_planner using autoware_common_msgs::msg::ResponseStatus; using autoware_planning_msgs::msg::LaneletRoute; -using tier4_planning_msgs::msg::RouteState; -using tier4_planning_msgs::srv::ClearRoute; -using tier4_planning_msgs::srv::SetLaneletRoute; -using tier4_planning_msgs::srv::SetWaypointRoute; +using autoware_planning_msgs::msg::RouteState; +using autoware_planning_msgs::srv::ClearRoute; +using autoware_planning_msgs::srv::SetLaneletRoute; +using autoware_planning_msgs::srv::SetWaypointRoute; using unique_identifier_msgs::msg::UUID; class RouteInterface diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 273363489d56f..b06b40ce83520 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -26,8 +26,8 @@ The `autoware_obstacle_cruise_planner` package has following modules. | Name | Type | Description | | ------------------------------- | ---------------------------------------------- | -------------------------------- | | `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory | -| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | -| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | +| `~/output/velocity_limit` | autoware_planning_msgs::VelocityLimit | velocity limit for cruising | +| `~/output/clear_velocity_limit` | autoware_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | ## Design diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 04badd2a956ef..70bdd1c1ea302 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -30,9 +30,9 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_debug_msgs/msg/float64_stamped.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" -#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "autoware_planning_msgs/msg/stop_speed_exceeded.hpp" +#include "autoware_planning_msgs/msg/velocity_limit.hpp" +#include "autoware_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include @@ -56,9 +56,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32Stamped; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_planning_msgs::msg::StopSpeedExceeded; -using tier4_planning_msgs::msg::VelocityLimit; -using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using autoware_planning_msgs::msg::StopSpeedExceeded; +using autoware_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::VelocityLimitClearCommand; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 8583d0639e5ce..805d6665dd5ed 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -40,7 +40,6 @@ tf2_ros tier4_debug_msgs tier4_metric_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 544597f05ff75..2c46f6544edb4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -21,7 +21,7 @@ #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "autoware_planning_msgs/msg/velocity_limit.hpp" #include #include diff --git a/planning/autoware_obstacle_stop_planner/README.md b/planning/autoware_obstacle_stop_planner/README.md index 700cb657786ff..fdbc9c2386f97 100644 --- a/planning/autoware_obstacle_stop_planner/README.md +++ b/planning/autoware_obstacle_stop_planner/README.md @@ -20,7 +20,7 @@ | `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | | `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | | `~/input/dynamic_objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | +| `~/input/expand_stop_range` | autoware_planning_msgs::msg::ExpandStopRange | expand stop range | ### Output topics diff --git a/planning/autoware_obstacle_stop_planner/package.xml b/planning/autoware_obstacle_stop_planner/package.xml index c9c78b3e87c3b..96ffc810a669e 100644 --- a/planning/autoware_obstacle_stop_planner/package.xml +++ b/planning/autoware_obstacle_stop_planner/package.xml @@ -41,7 +41,6 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/autoware_obstacle_stop_planner/src/node.hpp b/planning/autoware_obstacle_stop_planner/src/node.hpp index fba24061896b6..bc4980ed3354d 100644 --- a/planning/autoware_obstacle_stop_planner/src/node.hpp +++ b/planning/autoware_obstacle_stop_planner/src/node.hpp @@ -40,9 +40,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -88,9 +88,9 @@ using tier4_debug_msgs::msg::BoolStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_planning_msgs::msg::ExpandStopRange; -using tier4_planning_msgs::msg::VelocityLimit; -using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using autoware_planning_msgs::msg::ExpandStopRange; +using autoware_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::VelocityLimitClearCommand; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 21ce487af6f4c..a1a4a784f8117 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -29,7 +29,6 @@ std_msgs tf2_ros tier4_debug_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros 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 2c9a935c2077b..aa1e6d3c55efb 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 @@ -54,11 +54,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -89,11 +89,11 @@ using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_api_msgs::msg::IntersectionStatus; -using tier4_planning_msgs::msg::ExpandStopRange; -using tier4_planning_msgs::msg::LateralOffset; -using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::Scenario; -using tier4_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::ExpandStopRange; +using autoware_planning_msgs::msg::LateralOffset; +using autoware_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Scenario; +using autoware_planning_msgs::msg::VelocityLimit; using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; enum class ModuleName { diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index 80801a8102453..50c29bd256f6b 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -32,7 +32,6 @@ tf2_msgs tf2_ros tier4_api_msgs - tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor 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 62f89cab44e7b..3cd8d01ed8a26 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 @@ -307,7 +307,7 @@ 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_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 924e6d62d6aba..1d6428d85431f 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -23,7 +23,6 @@ rclcpp rclcpp_components std_msgs - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 383e85731604e..8a27be48034a4 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -54,7 +54,7 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( sub_route_ = create_subscription( "~/input/route", qos_transient_local, std::bind(&RemainingDistanceTimeCalculatorNode::on_route, this, _1)); - sub_planning_velocity_ = create_subscription( + sub_planning_velocity_ = create_subscription( "/planning/scenario_planning/current_max_velocity", qos_transient_local, std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1)); diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index d6c900af4dfed..a0137c5951a0d 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -52,7 +52,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Odometry = nav_msgs::msg::Odometry; - using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit; + using VelocityLimit = autoware_planning_msgs::msg::VelocityLimit; using MissionRemainingDistanceTime = autoware_internal_msgs::msg::MissionRemainingDistanceTime; rclcpp::Subscription::SharedPtr sub_route_; diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 6202be28e7e87..9e16495616002 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 @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include @@ -43,7 +43,7 @@ using autoware_planning_msgs::msg::LaneletSegment; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using std_msgs::msg::Header; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; using RouteSections = std::vector; diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 89930fd4a23a9..d26d9ccb9af1e 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -56,8 +56,8 @@ using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; using lanelet::utils::to2D; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; bool exists(const std::vector & primitives, const int64_t & id) { diff --git a/planning/autoware_scenario_selector/README.md b/planning/autoware_scenario_selector/README.md index a0b78a572c734..bfdd1ca3f14d0 100644 --- a/planning/autoware_scenario_selector/README.md +++ b/planning/autoware_scenario_selector/README.md @@ -19,7 +19,7 @@ | Name | Type | Description | | -------------------- | ---------------------------------- | ---------------------------------------------- | -| `~output/scenario` | tier4_planning_msgs::Scenario | current scenario and scenarios to be activated | +| `~output/scenario` | autoware_planning_msgs::Scenario | current scenario and scenarios to be activated | | `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | ### Output TFs diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 244876ee2cb22..08cb65d015ac8 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include @@ -79,12 +79,12 @@ class ScenarioSelectorNode : public rclcpp::Node inline bool isCurrentLaneDriving() const { - return current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return current_scenario_ == autoware_planning_msgs::msg::Scenario::LANEDRIVING; } inline bool isCurrentParking() const { - return current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING; + return current_scenario_ == autoware_planning_msgs::msg::Scenario::PARKING; } rclcpp::TimerBase::SharedPtr timer_; @@ -96,7 +96,7 @@ class ScenarioSelectorNode : public rclcpp::Node sub_lane_driving_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Publisher::SharedPtr pub_trajectory_; - rclcpp::Publisher::SharedPtr pub_scenario_; + rclcpp::Publisher::SharedPtr pub_scenario_; rclcpp::Publisher::SharedPtr pub_processing_time_; // polling subscribers diff --git a/planning/autoware_scenario_selector/package.xml b/planning/autoware_scenario_selector/package.xml index e726e40acecce..4afd298cfccbd 100644 --- a/planning/autoware_scenario_selector/package.xml +++ b/planning/autoware_scenario_selector/package.xml @@ -28,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_planning_msgs ros2cli topic_tools diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 4bb64e27368d8..4da47e06bad45 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -132,10 +132,10 @@ bool isStopped( autoware_planning_msgs::msg::Trajectory::ConstSharedPtr ScenarioSelectorNode::getScenarioTrajectory( const std::string & scenario) { - if (scenario == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { + if (scenario == autoware_planning_msgs::msg::Scenario::LANEDRIVING) { return lane_driving_trajectory_; } - if (scenario == tier4_planning_msgs::msg::Scenario::PARKING) { + if (scenario == autoware_planning_msgs::msg::Scenario::PARKING) { return parking_trajectory_; } RCLCPP_ERROR_STREAM(this->get_logger(), "invalid scenario argument: " << scenario); @@ -151,26 +151,26 @@ std::string ScenarioSelectorNode::selectScenarioByPosition() const auto is_in_parking_lot = isInParkingLot(route_handler_->getLaneletMapPtr(), current_pose_->pose.pose); - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::EMPTY) { + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::EMPTY) { if (is_in_lane && is_goal_in_lane) { - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return autoware_planning_msgs::msg::Scenario::LANEDRIVING; } else if (is_in_parking_lot) { - return tier4_planning_msgs::msg::Scenario::PARKING; + return autoware_planning_msgs::msg::Scenario::PARKING; } else { - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return autoware_planning_msgs::msg::Scenario::LANEDRIVING; } } - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::LANEDRIVING) { if (is_in_parking_lot && !is_goal_in_lane) { - return tier4_planning_msgs::msg::Scenario::PARKING; + return autoware_planning_msgs::msg::Scenario::PARKING; } } - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING) { + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::PARKING) { if (is_parking_completed_ && is_in_lane) { is_parking_completed_ = false; - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return autoware_planning_msgs::msg::Scenario::LANEDRIVING; } } @@ -194,10 +194,10 @@ void ScenarioSelectorNode::updateCurrentScenario() if (enable_mode_switching_) { if (isCurrentLaneDriving()) { current_scenario_ = isSwitchToParking(is_stopped) - ? tier4_planning_msgs::msg::Scenario::PARKING + ? autoware_planning_msgs::msg::Scenario::PARKING : current_scenario_; } else if (isCurrentParking()) { - current_scenario_ = isSwitchToLaneDriving() ? tier4_planning_msgs::msg::Scenario::LANEDRIVING + current_scenario_ = isSwitchToLaneDriving() ? autoware_planning_msgs::msg::Scenario::LANEDRIVING : current_scenario_; } } @@ -275,7 +275,7 @@ void ScenarioSelectorNode::onRoute( // When the route id is the same (e.g. rerouting with modified goal) keep the current scenario. // Otherwise, reset the scenario. if (!route_handler_ || route_handler_->getRouteUuid() != msg->uuid) { - current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; + current_scenario_ = autoware_planning_msgs::msg::Scenario::EMPTY; } route_ = msg; @@ -376,15 +376,15 @@ void ScenarioSelectorNode::onTimer() } // Initialize Scenario - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::EMPTY) { + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::EMPTY) { current_scenario_ = selectScenarioByPosition(); } updateCurrentScenario(); - tier4_planning_msgs::msg::Scenario scenario; + autoware_planning_msgs::msg::Scenario scenario; scenario.current_scenario = current_scenario_; - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING) { + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::PARKING) { scenario.activating_scenarios.push_back(current_scenario_); } @@ -402,7 +402,7 @@ void ScenarioSelectorNode::onLaneDrivingTrajectory( { lane_driving_trajectory_ = msg; - if (current_scenario_ != tier4_planning_msgs::msg::Scenario::LANEDRIVING) { + if (current_scenario_ != autoware_planning_msgs::msg::Scenario::LANEDRIVING) { return; } @@ -414,7 +414,7 @@ void ScenarioSelectorNode::onParkingTrajectory( { parking_trajectory_ = msg; - if (current_scenario_ != tier4_planning_msgs::msg::Scenario::PARKING) { + if (current_scenario_ != autoware_planning_msgs::msg::Scenario::PARKING) { return; } @@ -439,7 +439,7 @@ void ScenarioSelectorNode::publishTrajectory( ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_options) : Node("scenario_selector", node_options), - current_scenario_(tier4_planning_msgs::msg::Scenario::EMPTY), + current_scenario_(autoware_planning_msgs::msg::Scenario::EMPTY), update_rate_(this->declare_parameter("update_rate")), th_max_message_delay_sec_(this->declare_parameter("th_max_message_delay_sec")), th_arrived_distance_m_(this->declare_parameter("th_arrived_distance_m")), @@ -480,7 +480,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti // Output pub_scenario_ = - this->create_publisher("output/scenario", rclcpp::QoS{1}); + this->create_publisher("output/scenario", rclcpp::QoS{1}); pub_trajectory_ = this->create_publisher( "output/trajectory", rclcpp::QoS{1}); diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index 2b7b99bfe81be..cdf6e66e53a0c 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -39,7 +39,7 @@ using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_surround_obstacle_checker/README.md b/planning/autoware_surround_obstacle_checker/README.md index fbe749f2eec08..1c4fa3cf8fa3f 100644 --- a/planning/autoware_surround_obstacle_checker/README.md +++ b/planning/autoware_surround_obstacle_checker/README.md @@ -88,8 +88,8 @@ As mentioned in stop condition section, it prevents chattering by changing thres | Name | Type | Description | | --------------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------- | -| `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | -| `~/output/max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | +| `~/output/velocity_limit_clear_command` | `autoware_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | +| `~/output/max_velocity` | `autoware_planning_msgs::msg::VelocityLimit` | Velocity limit command | | `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus` | No start reason | | `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | | `~/debug/footprint` | `geometry_msgs::msg::PolygonStamped` | Ego vehicle base footprint for visualization | diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 4a6f946b8eb90..6ad9df7542fe1 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -34,7 +34,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index f84ad3a8f5987..0c0bfbffd9c24 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -30,8 +30,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -52,8 +52,8 @@ using autoware::motion_utils::VehicleStopChecker; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; -using tier4_planning_msgs::msg::VelocityLimit; -using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using autoware_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::VelocityLimitClearCommand; using Obstacle = std::pair; diff --git a/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md index b19f556e59830..1f55972808cb5 100644 --- a/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -88,8 +88,8 @@ Stop condition の項で述べたように、状態によって障害物判定 | Name | Type | Description | | --------------------------------------- | ----------------------------------------------------- | ---------------------------- | -| `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | -| `~/output/max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | +| `~/output/velocity_limit_clear_command` | `autoware_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | +| `~/output/max_velocity` | `autoware_planning_msgs::msg::VelocityLimit` | Velocity limit command | | `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus` | No start reason | | `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index fc0066b1a84f3..8b7ed526c6e52 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -45,8 +45,8 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary -#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "autoware_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary +#include "autoware_planning_msgs/msg/velocity_limit.hpp" // temporary #include "visualization_msgs/msg/marker_array.hpp" #include @@ -68,8 +68,8 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary -using tier4_planning_msgs::msg::VelocityLimit; // temporary +using autoware_planning_msgs::msg::StopSpeedExceeded; // temporary +using autoware_planning_msgs::msg::VelocityLimit; // temporary using visualization_msgs::msg::MarkerArray; struct Motion diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 3d1252ff0b7a6..4488136b92720 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -35,7 +35,6 @@ rclcpp tf2 tf2_ros - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index 83c361d064fb0..66ba795cf7751 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -30,7 +30,7 @@ autoware_universe_utils pluginlib rclcpp - tier4_planning_msgs + autoware_planning_msgs visualization_msgs ament_cmake_ros 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 44b56f4ea0990..844d335271c16 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 @@ -23,9 +23,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -59,7 +59,7 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedPath; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; struct MinMaxValue { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index e3aefb01c797d..918db64454011 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -34,7 +34,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index af46393d9b9ae..be406c59a300a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -28,7 +28,7 @@ autoware_universe_utils pluginlib rclcpp - tier4_planning_msgs + autoware_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index 56e207c160873..1dba3ca92b29a 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 @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include @@ -67,7 +67,7 @@ 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 tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; std::vector g_colors = { "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 102e5ef53b164..6cb878ab2901d 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 @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include @@ -66,7 +66,7 @@ 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 tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; std::vector g_colors = { "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 3b57ab67b265d..95386ea544f98 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index f670e3b05fa77..3bb39429c60ec 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 @@ -18,13 +18,13 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include -#include +#include #include using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 3a1773324fe24..b3ba0f1c0fe0e 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 @@ -26,7 +26,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp index e1e91a1bd8af6..2fb1f4f8f90ed 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index e32488965f69a..567a410d85fd2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 89181b258fbea..92786589ee894 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 5b336a7de6acc..f55046be6a695 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 @@ -19,7 +19,7 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include -#include +#include #include #include @@ -27,7 +27,7 @@ using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 08e0b8508c991..673b888ed21ed 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index f00d9d41622e9..21da456b2c57b 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 @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -40,7 +40,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Shape = autoware_perception_msgs::msg::Shape; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index 232c548b28da3..8eca144461047 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -29,7 +29,7 @@ autoware_universe_utils pluginlib rclcpp - tier4_planning_msgs + autoware_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 160bb33dc07de..de9be9f2e9af1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -24,7 +24,7 @@ namespace autoware::behavior_path_planner { using Point2d = autoware::universe_utils::Point2d; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { @@ -91,7 +91,7 @@ PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the // refine_goal_search_radius_range is recommended bool is_valid_path{false}; - tier4_planning_msgs::msg::PathWithLaneId refined_path; + autoware_planning_msgs::msg::PathWithLaneId refined_path; while (goal_search_radius >= 0 && !is_valid_path) { refined_path = utils::refinePathForGoal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index ce2552a41d380..6624342c9f887 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 @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include @@ -44,7 +44,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using lane_change::PathSafetyStatus; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_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 9d6fbd65acfd8..97ee6dc9498ac 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 @@ -28,7 +28,7 @@ #include #include -#include +#include #include @@ -42,7 +42,7 @@ using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_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 6367caaea6829..907cc9faf91ca 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 @@ -35,7 +35,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using lane_change::LanesPolygon; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using utils::path_safety_checker::ExtendedPredictedObjects; using utils::path_safety_checker::RSSparams; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 0fa0a9d977a26..02f8a20830b17 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -19,14 +19,14 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include namespace autoware::behavior_path_planner::lane_change { using autoware::behavior_path_planner::TurnSignalInfo; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; struct Path { PathWithLaneId path; 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 69362994dbbac..a27c74b27d3ad 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 @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include @@ -60,7 +60,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; rclcpp::Logger get_logger(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index dd4dbe63e41d4..22d9a8f2404be 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -29,7 +29,7 @@ pluginlib range-v3 rclcpp - tier4_planning_msgs + autoware_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 3277b8f031486..d9fabf6372c75 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,7 +21,7 @@ #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 147ef3856ac4e..d447f2fe05cfb 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 @@ -68,9 +68,9 @@ using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 114a38a77876c..776fdf5cc2718 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 @@ -46,7 +46,7 @@ using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_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 ab8d02dc83f92..6bef97bca3db1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md @@ -98,8 +98,8 @@ The Planner Manager's responsibilities include: | ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | | ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | | ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | -| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | +| ~/input/scenario | ○ | `autoware_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | +| ~/input/lateral_offset | △ | `autoware_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | | ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | - ○ Mandatory: Planning Module would not work if anyone of this is not present. @@ -109,18 +109,18 @@ The Planner Manager's responsibilities include: | Name | Type | Description | QoS Durability | | :---------------------------- | :-------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | -| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | +| ~/output/path | `autoware_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` | +| ~/output/reroute_availability | `autoware_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | ### Debug | Name | Type | Description | QoS Durability | | :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------- | -------------- | -| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | -| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | +| ~/debug/avoidance_debug_message_array | `autoware_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | +| ~/debug/lane_change_debug_message_array | `autoware_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | | ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | | ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | | ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md index 1e8532eba2f3a..addf32a1a407b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -139,9 +139,9 @@ Scene modules receives necessary data and RTC command, and outputs candidate pat | :-- | :-------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | | IN | `behavior_path_planner::BehaviorModuleOutput` | previous module output. contains data necessary for path planning. | | IN | `behavior_path_planner::PlannerData` | contains data necessary for path planning. | -| IN | `tier4_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_rtc_interface/README.md)) | +| IN | `autoware_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_rtc_interface/README.md)) | | OUT | `behavior_path_planner::BehaviorModuleOutput` | contains modified path, turn signal information, etc... | -| OUT | `tier4_planning_msgs::msg::CooperateStatus` | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_rtc_interface/README.md)) | +| OUT | `autoware_planning_msgs::msg::CooperateStatus` | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_rtc_interface/README.md)) | | OUT | `autoware_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | | OUT | `autoware_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | | OUT | `visualization_msgs::msg::MarkerArray` | virtual wall, debug info, etc... | 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 ebe7c47e6fab2..b681b91cf4966 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 @@ -34,13 +34,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -65,11 +64,11 @@ using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; -using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; -using tier4_planning_msgs::msg::LateralOffset; -using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::RerouteAvailability; -using tier4_planning_msgs::msg::Scenario; +using autoware_planning_msgs::msg::AvoidanceDebugMsgArray; +using autoware_planning_msgs::msg::LateralOffset; +using autoware_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::RerouteAvailability; +using autoware_planning_msgs::msg::Scenario; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -111,7 +110,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber operation_mode_subscriber_{ this, "/system/operation_mode/state", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber external_limit_max_velocity_subscriber_{this, "/planning/scenario_planning/max_velocity"}; // publisher @@ -157,7 +156,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onOperationMode(const OperationModeState::ConstSharedPtr msg); void onLateralOffset(const LateralOffset::ConstSharedPtr msg); void on_external_velocity_limiter( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); SetParametersResult onSetParam(const std::vector & parameters); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 3bed1e6a88bd8..aec60c7773d19 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include @@ -40,7 +40,7 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::StopWatch; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = autoware::universe_utils::DebugPublisher; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 6141480d1597a..548239df184b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -66,7 +66,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 7758ab530e88c..0efcb4c0d5a41 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 @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -29,7 +29,7 @@ namespace autoware::behavior_path_planner { using autoware::vehicle_info_utils::VehicleInfoUtils; -using tier4_planning_msgs::msg::PathChangeModuleId; +using autoware_planning_msgs::msg::PathChangeModuleId; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) : Node("behavior_path_planner", node_options) @@ -694,7 +694,7 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = autoware::motion_utils::convertToPath( + 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(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp index f1617cdd9ead0..1277194e727c0 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 @@ -20,8 +20,8 @@ #include "autoware_planning_msgs/msg/path_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "tier4_planning_msgs/msg/path_point_with_lane_id.hpp" -#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -30,8 +30,8 @@ namespace autoware::behavior_path_planner using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 749d732aecd00..0a70a88d7dd40 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 @@ -37,9 +37,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -62,11 +62,11 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::LateralOffset; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::LateralOffset; +using autoware_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; using lanelet::TrafficLight; -using tier4_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::VelocityLimit; using unique_identifier_msgs::msg::UUID; struct TrafficSignalStamped 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 391ac92411220..bd5ac81164465 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 @@ -37,8 +37,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -64,8 +64,8 @@ using autoware::universe_utils::generateUUID; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_adapi_v1_msgs::msg::VelocityFactor; -using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::AvoidanceDebugMsgArray; +using autoware_planning_msgs::msg::PathWithLaneId; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp index e2d5d276070ee..9b1dd40d0118b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ -#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include "autoware_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" #include namespace autoware::behavior_path_planner @@ -29,8 +29,8 @@ class StartPlannerModule; class GoalPlannerModule; class SideShiftModule; -using tier4_planning_msgs::msg::AvoidanceDebugMsg; -using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using autoware_planning_msgs::msg::AvoidanceDebugMsg; +using autoware_planning_msgs::msg::AvoidanceDebugMsgArray; class SceneModuleVisitor { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp index 85c2563c5d98c..63aabb2816fc8 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 @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include @@ -49,7 +49,7 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index 90cf533a8589d..fb14803d76821 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 @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include @@ -48,7 +48,7 @@ using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; const std::map g_signal_map = { {"left", TurnIndicatorsCommand::ENABLE_LEFT}, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index afe88ac04f302..43c87ed51c291 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 @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include @@ -33,8 +33,8 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using autoware::universe_utils::LineString2d; using autoware::universe_utils::MultiLineString2d; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 434eb79189eaf..b6d6788c96f58 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 @@ -19,7 +19,7 @@ #include #include -#include +#include #include @@ -121,7 +121,7 @@ class OccupancyGridBasedCollisionDetector * @return true if a collision is detected, false if no collision is detected. */ [[nodiscard]] bool hasObstacleOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; + const autoware_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; /** * @brief Detects if a collision occurs at the specified base index in the occupancy grid map. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 9cc9bec8e963a..db7f646e532b8 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 @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include @@ -35,8 +35,8 @@ namespace autoware::behavior_path_planner { using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; struct ParallelParkingParameters { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 2ed9fdd9197df..468af42723e08 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 @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -32,7 +32,7 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { using autoware_perception_msgs::msg::PredictedObject; -using tier4_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters object based on velocity. @@ -88,7 +88,7 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using tier4_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters objects based on object centroid position. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 7c16d7184c42e..5f64eadea5731 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 @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -32,8 +32,8 @@ namespace autoware::behavior_path_planner using autoware::universe_utils::generateUUID; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; struct ShiftLine diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp index 3e4f180035b20..9f8bf00f35e70 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 @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include @@ -39,7 +39,7 @@ using autoware_planning_msgs::msg::Path; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 7162f49b450e8..3d77163e1a7d2 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 @@ -29,8 +29,8 @@ #include #include #include -#include -#include +#include +#include #include @@ -52,8 +52,8 @@ using autoware::universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; static constexpr double eps = 0.01; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index f23a9980f1237..0e1e2b5a52947 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -62,7 +62,6 @@ magic_enum rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 33f46420ad537..36038c41cf0da 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,7 @@ bool OccupancyGridBasedCollisionDetector::detectCollision( } bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const + const autoware_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const { for (const auto & p : path.points) { const auto pose_local = global2local(costmap_, p.point.pose); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 789741153bf4a..877beec3e324a 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 @@ -42,7 +42,7 @@ using autoware::universe_utils::transformPose; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 9b99a5ef7b31d..2eb0c8c6ca3f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -41,7 +41,7 @@ namespace { double calcInterpolatedZ( - const tier4_planning_msgs::msg::PathWithLaneId & input, + const autoware_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( @@ -60,7 +60,7 @@ double calcInterpolatedZ( } double calcInterpolatedVelocity( - const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) + const autoware_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { const double seg_dist = autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); @@ -204,7 +204,7 @@ double calcLongitudinalDistanceFromEgoToObjects( } std::optional findIndexOutOfGoalSearchRange( - const std::vector & points, const Pose & goal, + const std::vector & points, const Pose & goal, const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) { if (points.empty()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp index 8c732a972a212..14f3b1c650ffb 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 @@ -38,7 +38,7 @@ using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; using TwistWithCovariance = geometry_msgs::msg::TwistWithCovariance; using autoware_perception_msgs::msg::PredictedPath; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp index b212cce551252..bd369278e0c95 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp @@ -125,7 +125,7 @@ TEST_F(OccupancyGridBasedCollisionDetectorTest, detectCollision) TEST_F(OccupancyGridBasedCollisionDetectorTest, hasObstacleOnPath) { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; detector_.setMap(costmap_); // Condition: empty path @@ -135,7 +135,7 @@ TEST_F(OccupancyGridBasedCollisionDetectorTest, hasObstacleOnPath) size_t path_length = 10; path.points.reserve(path_length); for (size_t i = 0; i < path_length; i++) { - tier4_planning_msgs::msg::PathPointWithLaneId path_point; + autoware_planning_msgs::msg::PathPointWithLaneId path_point; path_point.point.pose = createPose(static_cast(i), 0.0, 0.0, 0.0, 0.0, 0.0); path.points.push_back(path_point); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp index cefa9151dd604..1c31ef7bde87c 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 @@ -32,8 +32,8 @@ constexpr double epsilon = 1e-6; using autoware::behavior_path_planner::PlannerData; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using autoware::test_utils::generateTrajectory; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp index 562e8abfd432a..b8d0af66c0170 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp @@ -17,14 +17,14 @@ #include -#include +#include #include #include #include -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; @@ -67,7 +67,7 @@ TEST(BehaviorPathPlanningPathUtilTest, getIdxByArclength) { using autoware::behavior_path_planner::utils::getIdxByArclength; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; // Condition: empty points EXPECT_ANY_THROW(getIdxByArclength(path, 5, 1.0)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index c5694db10f65e..4e05e06aa5a61 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 @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include @@ -49,7 +49,7 @@ using autoware::test_utils::generateTrajectory; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; std::vector create_test_path() { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp index ab6076b328d14..d64f30964e461 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 @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -128,7 +128,7 @@ TEST(StaticDrivableArea, getOverlappedLaneletId) TEST(StaticDrivableArea, cutOverlappedLanes) { using autoware::behavior_path_planner::utils::cutOverlappedLanes; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; std::vector lanes; { // empty inputs const auto result = cutOverlappedLanes(path, lanes); @@ -258,8 +258,8 @@ TEST(StaticDrivableArea, generateDrivableLanes) TEST(StaticDrivableArea, generateDrivableArea_subfunction) { using autoware::behavior_path_planner::utils::generateDrivableArea; - tier4_planning_msgs::msg::PathWithLaneId path; - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathPointWithLaneId p; generateDrivableArea(path, 0.0, 0.0, true); EXPECT_TRUE(path.left_bound.empty()); EXPECT_TRUE(path.right_bound.empty()); @@ -450,7 +450,7 @@ TEST(DISABLED_StaticDrivableArea, generateDrivableArea) { using autoware::behavior_path_planner::PlannerData; using autoware::behavior_path_planner::utils::generateDrivableArea; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; std::vector lanes; bool enable_expanding_hatched_road_markings = true; bool enable_expanding_intersection_areas = true; @@ -474,7 +474,7 @@ TEST(DISABLED_StaticDrivableArea, generateDrivableArea) lanes = autoware::behavior_path_planner::utils::generateDrivableLanes({ll, shoulder_ll}); lanelet::BasicLineString2d path_ls; for (const auto & p : ll.centerline().basicLineString()) { - tier4_planning_msgs::msg::PathPointWithLaneId pp; + autoware_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose.position.x = p.x(); pp.point.pose.position.y = p.y(); pp.point.pose.position.z = p.z(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp index a5d77b96292f9..49b5aada7bdd3 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 @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -127,7 +127,7 @@ TEST_F(TrafficLightTest, calcDistanceToRedTrafficLight) using autoware::behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; { - const tier4_planning_msgs::msg::PathWithLaneId path; + const autoware_planning_msgs::msg::PathWithLaneId path; const lanelet::ConstLanelets empty_lanelets; EXPECT_FALSE(calcDistanceToRedTrafficLight(empty_lanelets, path, planner_data_).has_value()); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index b6149e3b5f99c..50dca6fd1be37 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 @@ -33,7 +33,7 @@ using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; constexpr double nearest_dist_threshold = 5.0; constexpr double nearest_yaw_threshold = M_PI / 3.0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp index 13fec41092cc8..47d34ee9dd84e 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 @@ -30,8 +30,8 @@ using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_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 c9b75f7fa96de..ca5f1177b37f3 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 "tier4_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/lateral_offset.hpp" +#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp index 4408645b26095..0cd545d2d6a57 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -35,7 +35,7 @@ namespace autoware::behavior_path_planner { using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; struct SoftConstraintsInputs diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index a5d78061448ed..ac1fec3bb7af7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -37,7 +37,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/data_structs.hpp index 92565197abf70..9af65a02db5ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/data_structs.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include @@ -25,7 +25,7 @@ namespace autoware::behavior_path_planner { -using tier4_planning_msgs::msg::LateralOffset; +using autoware_planning_msgs::msg::LateralOffset; enum class SideShiftStatus { STOP = 0, BEFORE_SHIFT, SHIFTING, AFTER_SHIFT }; 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 15d81ff45abcf..7a17b97a7f4a0 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 #include @@ -34,8 +34,8 @@ namespace autoware::behavior_path_planner { using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; -using tier4_planning_msgs::msg::LateralOffset; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::LateralOffset; +using autoware_planning_msgs::msg::PathWithLaneId; class SideShiftModule : public SceneModuleInterface { diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp index 5a3e51c353c39..2111cf27a6fcc 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 @@ -20,14 +20,14 @@ #include #include #include -#include +#include namespace autoware::behavior_path_planner { using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; /** * @brief Sets the orientation (yaw angle) for all points in the path. diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml index 9bc2b608381fa..3cd0a51883d1c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml @@ -26,7 +26,6 @@ geometry_msgs pluginlib rclcpp - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index e358e35ed7794..9d1c0f6ab6f12 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 18d1f3c3b9b81..6378f98e2cf14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -22,7 +22,7 @@ #include -#include +#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp index 764499ea6dbcf..e54416214f218 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp @@ -17,14 +17,14 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include namespace autoware::behavior_path_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index f606679e7f2da..dfc70aee63b1b 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 @@ -23,7 +23,7 @@ #include "autoware/universe_utils/system/time_keeper.hpp" #include -#include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class PullOutPlannerBase { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index d1b4c9dabdd7f..99a2fa88d9be4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -21,7 +21,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 47eecb54e2b2a..f83a4ff507b25 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -31,7 +31,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp index 60c5ccedd93f5..3cd415e7f76d9 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 @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include @@ -42,7 +42,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index c7cabb403f164..c86da8b6121ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -27,7 +27,7 @@ autoware_universe_utils pluginlib rclcpp - tier4_planning_msgs + autoware_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 5848cc75f148e..87994abe6ccf3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -37,7 +37,7 @@ namespace autoware::behavior_path_planner { using helper::static_obstacle_avoidance::AvoidanceHelper; -using tier4_planning_msgs::msg::AvoidanceDebugMsg; +using autoware_planning_msgs::msg::AvoidanceDebugMsg; class StaticObstacleAvoidanceModule : public SceneModuleInterface { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 293b2d9bc6f28..c6eb2028f4b32 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 @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -38,7 +38,7 @@ using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; // ROS 2 general msgs using geometry_msgs::msg::Point; @@ -50,9 +50,9 @@ using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -// tier4 msgs -using tier4_planning_msgs::msg::AvoidanceDebugMsg; -using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +// autoware msgs +using autoware_planning_msgs::msg::AvoidanceDebugMsg; +using autoware_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_rtc_msgs::msg::State; // tier4 utils functions diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 5446e33073b13..e7e41beb6f035 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -43,7 +43,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs + autoware_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index f296ad03cbac6..55e1a9f7e9ee4 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 @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -40,10 +40,10 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC private: BlindSpotModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_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 1bfa41d86ffbe..f376557df642a 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 @@ -115,15 +115,15 @@ class BlindSpotModule : public SceneModuleInterface BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path); // setSafe(), setDistance() void setRTCStatus( - const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + const BlindSpotDecision & decision, const autoware_planning_msgs::msg::PathWithLaneId & path); template void setRTCStatusByDecision( - const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + const Decision & decision, const autoware_planning_msgs::msg::PathWithLaneId & path); // stop/GO void reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path); template void reactRTCApprovalByDecision( - const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path); + const Decision & decision, autoware_planning_msgs::msg::PathWithLaneId * path); /** * @brief Generate a stop line and insert it into the path. @@ -136,10 +136,10 @@ class BlindSpotModule : public SceneModuleInterface */ std::optional> generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * path) const; + autoware_planning_msgs::msg::PathWithLaneId * path) const; std::optional isOverPassJudge( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const geometry_msgs::msg::Pose & stop_point_pose) const; double computeTimeToPassStopLine( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index 9d908414b6d95..18ce4bbbdd578 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 @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -37,7 +37,7 @@ enum class TurnDirection { LEFT, RIGHT }; struct InterpolatedPathInfo { /** the interpolated path */ - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ @@ -47,7 +47,7 @@ struct InterpolatedPathInfo }; std::optional generateInterpolatedPathInfo( - const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const lanelet::Id lane_id, const autoware_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger); std::optional getSiblingStraightLanelet( @@ -73,7 +73,7 @@ lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, + const autoware_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, const double adjacent_extend_width, const double opposite_adjacent_extend_width); /** @@ -84,7 +84,7 @@ lanelet::ConstLanelets generateBlindSpotLanelets( * @return Blind spot polygons */ std::optional generateBlindSpotPolygons( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const double backward_detection_length); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index a2ea4a82a884d..e51dd12e7f8f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -28,7 +28,6 @@ pluginlib rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 64e1435167a89..0a139952ad281 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -24,7 +24,7 @@ namespace autoware::behavior_velocity_planner */ template void BlindSpotModule::setRTCStatusByDecision( - const T &, [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + const T &, [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path) { static_assert("Unsupported type passed to setRTCStatus"); return; @@ -33,7 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision( template void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const T & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) + [[maybe_unused]] autoware_planning_msgs::msg::PathWithLaneId * path) { static_assert("Unsupported type passed to reactRTCApprovalByDecision"); } @@ -44,7 +44,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -52,7 +52,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) + [[maybe_unused]] autoware_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -63,7 +63,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -71,7 +71,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path) + [[maybe_unused]] autoware_planning_msgs::msg::PathWithLaneId * path) { return; } @@ -81,7 +81,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Unsafe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) + const Unsafe & decision, const autoware_planning_msgs::msg::PathWithLaneId & path) { setSafe(false); const auto & current_pose = planner_data_->current_odometry->pose; @@ -92,7 +92,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) + const Unsafe & decision, autoware_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; @@ -112,7 +112,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Safe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) + const Safe & decision, const autoware_planning_msgs::msg::PathWithLaneId & path) { setSafe(true); const auto & current_pose = planner_data_->current_odometry->pose; @@ -123,7 +123,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path) + const Safe & decision, autoware_planning_msgs::msg::PathWithLaneId * path) { if (!isActivated()) { constexpr double stop_vel = 0.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index cc63b42df68e4..b518a7e8932dd 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 @@ -53,7 +53,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); } -void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void BlindSpotModuleManager::launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -85,7 +85,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7697786adb19d..85a385cd0d1c7 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 @@ -151,7 +151,7 @@ template VisitorSwitch(Ts...) -> VisitorSwitch; void BlindSpotModule::setRTCStatus( - const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) + const BlindSpotDecision & decision, const autoware_planning_msgs::msg::PathWithLaneId & path) { std::visit( VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, @@ -201,7 +201,7 @@ static std::optional getFirstPointIntersectsLineByFootprint( } static std::optional getDuplicatedPointIdx( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) + const autoware_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; @@ -216,7 +216,7 @@ static std::optional getDuplicatedPointIdx( } static std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, autoware_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); @@ -229,7 +229,7 @@ static std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) size_t insert_idx = closest_idx; - tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); + autoware_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 { @@ -248,7 +248,7 @@ static std::optional insertPointIndex( std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * path) const + autoware_planning_msgs::msg::PathWithLaneId * path) const { // NOTE: this is optionally int for later subtraction const int margin_idx_dist = @@ -335,7 +335,7 @@ autoware_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWi } std::optional BlindSpotModule::isOverPassJudge( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const geometry_msgs::msg::Pose & stop_point_pose) const { const auto & current_pose = planner_data_->current_odometry->pose; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 5c5aa0a26b3b4..e2510d974263f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -34,7 +34,7 @@ namespace autoware::behavior_velocity_planner namespace { static bool hasLaneIds( - const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) + const autoware_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) { for (const auto & pid : p.lane_ids) { if (pid == id) { @@ -45,7 +45,7 @@ static bool hasLaneIds( } static std::optional> findLaneIdInterval( - const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) + const autoware_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) { bool found = false; size_t start = 0; @@ -73,7 +73,7 @@ static std::optional> findLaneIdInterval( } // namespace std::optional generateInterpolatedPathInfo( - const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const lanelet::Id lane_id, const autoware_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger) { constexpr double ds = 0.2; @@ -203,7 +203,7 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, + const autoware_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, const double adjacent_extend_width, const double opposite_adjacent_extend_width) { const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); @@ -305,7 +305,7 @@ lanelet::ConstLanelets generateBlindSpotLanelets( } std::optional generateBlindSpotPolygons( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const double backward_detection_length) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_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 64c98017d3007..bc53181601329 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp @@ -34,15 +34,15 @@ #include #include -#include -#include +#include +#include namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_planning_msgs::msg::StopFactor; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::StopFactor; enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; @@ -90,12 +90,12 @@ struct DebugData std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); std::optional> diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp index d6f6ddfb7b6ad..126a30ca4538b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 9b771c0d503a4..dead1c9c16f97 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 @@ -57,7 +57,7 @@ using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; namespace { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index 50de1b1a8e1ff..56fbb7c29a088 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,7 @@ using autoware::universe_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::vector> crosswalks; @@ -93,7 +93,7 @@ std::vector> getCrosswalksOnPath( std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::set crosswalk_id_set; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 3f39696ff6bcc..fa1e1d1b56886 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -53,7 +53,7 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) } void DetectionAreaModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { for (const auto & detection_area_with_lane_id : planning_utils::getRegElemMapOnPath( @@ -76,7 +76,7 @@ void DetectionAreaModuleManager::launchNewModules( std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 7aa60cbbaa18b..e9ef6e2566d03 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: DetectionAreaModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..40ea01a4fe0c7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -36,7 +36,7 @@ namespace autoware::behavior_velocity_planner using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class DetectionAreaModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index b056f71614da3..6f4b9fee725ef 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -38,7 +38,6 @@ pluginlib rclcpp tf2_geometry_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp index d8bada002e37b..2a87850b55050 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -15,7 +15,7 @@ #ifndef INTERPOLATED_PATH_INFO_HPP_ #define INTERPOLATED_PATH_INFO_HPP_ -#include +#include #include @@ -32,7 +32,7 @@ namespace autoware::behavior_velocity_planner struct InterpolatedPathInfo { /** the interpolated path */ - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 39ed8e80412a6..8ee48f059bdc2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -302,7 +302,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) } void IntersectionModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -355,7 +355,7 @@ void IntersectionModuleManager::launchNewModules( std::function &)> IntersectionModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -446,7 +446,7 @@ void IntersectionModuleManager::setActivation() } void IntersectionModuleManager::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -483,7 +483,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node } void MergeFromPrivateModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -550,7 +550,7 @@ void MergeFromPrivateModuleManager::launchNewModules( std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index 80c87e55c6696..31459baf422d4 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,7 +24,7 @@ #include #include -#include +#include #include #include @@ -45,10 +45,10 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -56,7 +56,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void sendRTC(const Time & stamp) override; void setActivation() override; /* called from SceneModuleInterface::updateSceneModuleInstances */ - void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr @@ -73,10 +73,10 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 6f7841ebb0bbb..3731d1d1a924d 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 @@ -492,7 +492,7 @@ VisitorSwitch(Ts...) -> VisitorSwitch; template void prepareRTCByDecisionResult( [[maybe_unused]] const T & result, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -503,7 +503,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const InternalError & result, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -513,7 +513,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const OverPassJudge & result, - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -522,7 +522,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const StuckStop & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -543,7 +543,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const YieldStuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const YieldStuckStop & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -559,7 +559,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const NonOccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const NonOccludedCollisionStop & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -578,7 +578,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const FirstWaitBeforeOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const FirstWaitBeforeOcclusion & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -597,7 +597,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const PeekingTowardOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const PeekingTowardOcclusion & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -616,7 +616,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const OccludedAbsenceTrafficLight & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const OccludedAbsenceTrafficLight & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -633,7 +633,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const OccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const OccludedCollisionStop & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -652,7 +652,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const Safe & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); @@ -670,7 +670,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const FullyPrioritized & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const FullyPrioritized & result, const autoware_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -687,7 +687,7 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const DecisionResult & decision_result, const tier4_planning_msgs::msg::PathWithLaneId & path) + const DecisionResult & decision_result, const autoware_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -710,7 +710,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_occlusion_approved, [[maybe_unused]] const T & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -725,7 +725,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -739,7 +739,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { @@ -751,7 +751,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -790,7 +790,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -818,7 +818,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -855,7 +855,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -900,7 +900,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -950,7 +950,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -991,7 +991,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -1037,7 +1037,7 @@ template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -1073,7 +1073,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, autoware_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -1106,7 +1106,7 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path) + const DecisionResult & decision_result, autoware_planning_msgs::msg::PathWithLaneId * path) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1227,7 +1227,7 @@ void IntersectionModule::updateTrafficSignalObservation() } IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const autoware_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b718dd84d33af..9f303bc7f6ea9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include @@ -510,13 +510,13 @@ class IntersectionModule : public SceneModuleInterface * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); + const DecisionResult &, const autoware_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( - const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path); + const DecisionResult & decision_result, autoware_planning_msgs::msg::PathWithLaneId * path); /** @}*/ private: @@ -563,7 +563,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * original_path) const; + autoware_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets @@ -634,7 +634,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ std::optional isStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( @@ -663,7 +663,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ std::optional isYieldStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const InterpolatedPathInfo & interpolated_path_info, const IntersectionStopLines & intersection_stoplines) const; @@ -719,7 +719,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_stopline */ PassJudgeStatus isOverPassJudgeLinesStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const autoware_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const IntersectionStopLines & intersection_stoplines); /** @} */ @@ -781,7 +781,7 @@ class IntersectionModule : public SceneModuleInterface * situation */ std::string generateEgoRiskEvasiveDiagnosis( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, const std::vector>> & too_late_detect_objects, @@ -807,7 +807,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const autoware_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; /** @} */ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 5a66bec15bab1..bd4943235ebd8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -603,7 +603,7 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( } std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, const std::vector< std::pair>> & @@ -813,7 +813,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const autoware_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 70e1d219c0d31..85d3800b62c36 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -72,7 +72,7 @@ lanelet::ConstLanelets getPrevLanelets( // end inclusive lanelet::ConstLanelet generatePathLanelet( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, const double interval) { lanelet::Points3d lefts; @@ -103,7 +103,7 @@ lanelet::ConstLanelet generatePathLanelet( } std::optional> getFirstPointInsidePolygons( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const std::vector & polygons, const bool search_forward = true) { @@ -355,7 +355,7 @@ std::optional IntersectionModule::generateIntersectionSto const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const InterpolatedPathInfo & interpolated_path_info, - tier4_planning_msgs::msg::PathWithLaneId * original_path) const + autoware_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; const double stopline_margin = planner_param_.common.default_stopline_margin; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 35f4e3b34dbee..10a46c3d4bae9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -123,7 +123,7 @@ namespace autoware::behavior_velocity_planner namespace bg = boost::geometry; std::optional IntersectionModule::isStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const { const auto closest_idx = intersection_stoplines.closest_idx; @@ -308,7 +308,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & pa } std::optional IntersectionModule::isYieldStuckStatus( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const InterpolatedPathInfo & interpolated_path_info, const IntersectionStopLines & intersection_stoplines) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index dc8bf1a96a7a2..e1f7d5ab0489e 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 @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index 5c9b74a9ad3bd..37651cd2ebb9d 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,7 @@ namespace autoware::behavior_velocity_planner::util namespace bg = boost::geometry; static std::optional getDuplicatedPointIdx( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) + const autoware_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 +62,7 @@ static std::optional getDuplicatedPointIdx( } std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, autoware_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 +75,7 @@ std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) int insert_idx = closest_idx; - tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); + autoware_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 +93,7 @@ std::optional insertPointIndex( } bool hasLaneIds( - const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) + const autoware_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) { for (const auto & pid : p.lane_ids) { if (ids.find(pid) != ids.end()) { @@ -104,7 +104,7 @@ bool hasLaneIds( } std::optional> findLaneIdsInterval( - const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) + const autoware_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) { bool found = false; size_t start = 0; @@ -180,7 +180,7 @@ getFirstPointInsidePolygonsByFootprint( } std::optional getFirstPointInsidePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward) { @@ -328,7 +328,7 @@ mergeLaneletsByTopologicalSort( } bool isOverTargetIndex( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -357,7 +357,7 @@ std::optional getIntersectionArea( std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { InterpolatedPathInfo interpolated_path_info; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp index 0fac44f6cdf97..41af5484433f1 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,14 @@ namespace autoware::behavior_velocity_planner::util * @return if insertion was successful return the inserted point index */ std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, autoware_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( - const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); + const autoware_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 +53,7 @@ bool hasLaneIds( * found, returns the pair (start-1, end) */ std::optional> findLaneIdsInterval( - const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); + const autoware_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** * @brief return the index of the first point which is inside the given polygon @@ -61,7 +61,7 @@ std::optional> findLaneIdsInterval( * @param[in] search_forward flag for search direction */ std::optional getFirstPointInsidePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); @@ -73,7 +73,7 @@ std::optional getFirstPointInsidePolygon( * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); std::optional getIntersectionArea( @@ -84,7 +84,7 @@ std::optional getIntersectionArea( */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 9cb1153a8edd2..5a53e181f086c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -33,7 +33,7 @@ NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) } void NoDrivableLaneModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -57,7 +57,7 @@ void NoDrivableLaneModuleManager::launchNewModules( std::function &)> NoDrivableLaneModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp index 545af1576c6a8..7e0d25066f287 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoDrivableLaneModuleManager : public SceneModuleManagerInterface private: NoDrivableLaneModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; }; class NoDrivableLaneModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d8c5e3e0425d1..df57299fc4444 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class NoDrivableLaneModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp index 289b04a2ce96d..f7f0f6e68da26 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -15,7 +15,7 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include +#include #include #include @@ -33,8 +33,8 @@ namespace autoware::behavior_velocity_planner namespace bg = boost::geometry; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and no drivable lane polygon struct PathWithNoDrivableLanePolygonIntersection diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 88fafeb5b90dc..a407bf5e12317 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -29,7 +29,6 @@ libboost-dev pluginlib rclcpp - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 9d66aa6672d36..1766c8f196344 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -47,7 +47,7 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) } void NoStoppingAreaModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -71,7 +71,7 @@ void NoStoppingAreaModuleManager::launchNewModules( std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 7009e94612580..6d7099c433f47 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: NoStoppingAreaModule::PlannerParam planner_param_{}; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_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 51f3a0d261ebd..6afb44ab6e120 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 @@ -25,7 +25,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp index 5ce56d756e919..842ea24717672 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp @@ -48,13 +48,13 @@ bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & obje } void insert_stop_point( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) + autoware_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) { auto insert_idx = stop_point.first + 1UL; const auto & stop_pose = stop_point.second; // To PathPointWithLaneId - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + autoware_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(insert_idx); stop_point_with_lane_id.point.pose = stop_pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; @@ -64,7 +64,7 @@ void insert_stop_point( } std::optional generate_stop_line( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, const double stop_line_margin) { @@ -127,7 +127,7 @@ bool is_stoppable( } Polygon2d generate_ego_no_stopping_area_lane_polygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, rclcpp::Clock & clock) @@ -136,7 +136,7 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( double dist_from_start_sum = 0.0; constexpr double interpolation_interval = 0.5; bool is_in_area = false; - tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + autoware_planning_msgs::msg::PathWithLaneId interpolated_path; if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger)) { return ego_area; } @@ -204,7 +204,7 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( } bool check_stop_lines_in_no_stopping_area( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + const autoware_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, no_stopping_area::DebugData & debug_data) { const double stop_vel = std::numeric_limits::min(); @@ -245,7 +245,7 @@ bool check_stop_lines_in_no_stopping_area( } std::optional get_stop_line_geometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp index 73b835b4701dc..7bc90a08f8604 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 @@ -22,7 +22,7 @@ #include #include -#include +#include #include @@ -85,7 +85,7 @@ bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & obje * @param stop_point stop line point on the lane */ void insert_stop_point( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + autoware_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); /** * @brief generate stop line from no stopping area polygons @@ -99,7 +99,7 @@ void insert_stop_point( * @param stop_line_margin [m] margin to keep between the stop line and the no stopping areas **/ std::optional generate_stop_line( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, const double stop_line_margin); @@ -128,7 +128,7 @@ bool is_stoppable( * @return generated polygon */ Polygon2d generate_ego_no_stopping_area_lane_polygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, rclcpp::Clock & clock); @@ -141,7 +141,7 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( * @return true if exists */ bool check_stop_lines_in_no_stopping_area( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + const autoware_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, DebugData & debug_data); /** @@ -156,7 +156,7 @@ bool check_stop_lines_in_no_stopping_area( * @return generated stop line */ std::optional get_stop_line_geometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp index cc5bbd2903a52..4bc54b61cae6a 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 @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -44,12 +44,12 @@ bool point_in_polygon(const Point & p, const Polygon & poly) }) != poly.outer().end(); } -tier4_planning_msgs::msg::PathWithLaneId generate_straight_path( +autoware_planning_msgs::msg::PathWithLaneId generate_straight_path( const size_t nb_points, const float velocity = 0.0, const double resolution = 1.0) { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; for (auto x = 0UL; x < nb_points; ++x) { - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = resolution * static_cast(x); p.point.pose.position.y = 0.0; p.point.longitudinal_velocity_mps = velocity; @@ -82,7 +82,7 @@ TEST(NoStoppingAreaTest, insertStopPoint) using autoware::behavior_velocity_planner::no_stopping_area::insert_stop_point; constexpr auto nb_points = 10; constexpr auto default_velocity = 5.0; - const tier4_planning_msgs::msg::PathWithLaneId base_path = + const autoware_planning_msgs::msg::PathWithLaneId base_path = generate_straight_path(nb_points, default_velocity); autoware::behavior_velocity_planner::no_stopping_area::PathIndexWithPose stop_point; // stop exactly at a point, expect no new points but a 0 velocity at the stop point and after @@ -123,7 +123,7 @@ TEST(NoStoppingAreaTest, generateStopLine) { using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; constexpr auto nb_points = 10; - const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(nb_points); + const autoware_planning_msgs::msg::PathWithLaneId path = generate_straight_path(nb_points); lanelet::ConstPolygons3d no_stopping_areas; double ego_width = 0.0; double stop_line_margin = 0.0; @@ -228,7 +228,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) geometry_msgs::msg::Pose ego_pose; // ego at (0,0) ego_pose.position.x = 0.0; ego_pose.position.y = 0.0; - const tier4_planning_msgs::msg::PathWithLaneId path = + const autoware_planning_msgs::msg::PathWithLaneId path = generate_straight_path(8); // ego path at x = 0, 1,..., 7 and y=0 double margin = 1.0; // desired margin added to the polygon after the end of the area double max_polygon_length = 10.0; // maximum length of the generated polygon @@ -307,7 +307,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) // cases where the polygon returned is empty // path is empty { - tier4_planning_msgs::msg::PathWithLaneId empty_path; + autoware_planning_msgs::msg::PathWithLaneId empty_path; const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( empty_path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, logger, clock); @@ -356,7 +356,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) { using autoware::behavior_velocity_planner::no_stopping_area::check_stop_lines_in_no_stopping_area; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; autoware::universe_utils::Polygon2d poly; autoware::behavior_velocity_planner::no_stopping_area::DebugData debug_data; @@ -365,7 +365,7 @@ TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) constexpr auto nb_points = 10; constexpr auto non_stopped_velocity = 5.0; - const tier4_planning_msgs::msg::PathWithLaneId non_stopping_path = + const autoware_planning_msgs::msg::PathWithLaneId non_stopping_path = generate_straight_path(nb_points, non_stopped_velocity); path = non_stopping_path; // set x=4 and x=5 to be stopping points @@ -396,7 +396,7 @@ TEST(NoStoppingAreaTest, getStopLineGeometry2d) { using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; using autoware::behavior_velocity_planner::no_stopping_area::get_stop_line_geometry2d; - const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(10); + const autoware_planning_msgs::msg::PathWithLaneId path = generate_straight_path(10); lanelet::Polygon3d no_stopping_area; no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index c4dace9d49244..ab85da72ab522 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -117,7 +117,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } void OcclusionSpotModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -130,7 +130,7 @@ void OcclusionSpotModuleManager::launchNewModules( std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 32b1818214952..8507e9be34332 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 @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include #include @@ -53,10 +53,10 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; }; class OcclusionSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index baf15cbebce81..5143de5fe8fcf 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,7 +95,7 @@ bool buildDetectionAreaPolygon( } void calcSlowDownPointsForPossibleCollision( - const int closest_idx, const tier4_planning_msgs::msg::PathWithLaneId & path, const double offset, + const int closest_idx, const autoware_planning_msgs::msg::PathWithLaneId & path, const double offset, std::vector & possible_collisions) { if (possible_collisions.empty()) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 7bff65d59157d..9683931126022 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 @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include #include @@ -59,8 +59,8 @@ using lanelet::BasicLineString2d; using lanelet::BasicPoint2d; using lanelet::BasicPolygon2d; using lanelet::LaneletMapPtr; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 9def3b0938998..1cde92fba7397 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -17,7 +17,7 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 35c409a9c459b..45a3606e2c818 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 @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 8fee8ee783fa2..5153465f2dece 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 @@ -31,7 +31,7 @@ using DynamicObjects = autoware_perception_msgs::msg::PredictedObjects; using DynamicObject = autoware_perception_msgs::msg::PredictedObject; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { @@ -45,7 +45,7 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) std::vector possible_collisions; size_t num = 2000; // make a path with 2000 points from x=0 to x=4 - tier4_planning_msgs::msg::PathWithLaneId path = test::generatePath(0.0, 3.0, 4.0, 3.0, num); + autoware_planning_msgs::msg::PathWithLaneId path = test::generatePath(0.0, 3.0, 4.0, 3.0, num); // make 2000 possible collision from x=0 to x=10 test::generatePossibleCollisions(possible_collisions, 0.0, 3.0, 4.0, 3.0, num); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp index 39a887ab16476..b618af1a300bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -19,22 +19,22 @@ #include -#include -#include +#include +#include #include namespace test { -inline tier4_planning_msgs::msg::PathWithLaneId generatePath( +inline autoware_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - tier4_planning_msgs::msg::PathWithLaneId path{}; + autoware_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point{}; + autoware_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md index 041e0b86e0d06..f44d63b0ac0c1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md @@ -30,7 +30,7 @@ So for example, in order to stop at a stop line with the vehicles' front on the | Name | Type | Description | | ----------------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | tier4_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/path_with_lane_id` | autoware_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 | @@ -43,7 +43,7 @@ So for example, in order to stop at a stop line with the vehicles' front on the | Name | Type | Description | | ---------------------- | ----------------------------------------- | -------------------------------------- | | `~output/path` | autoware_planning_msgs::msg::Path | path to be followed | -| `~output/stop_reasons` | tier4_planning_msgs::msg::StopReasonArray | reasons that cause the vehicle to stop | +| `~output/stop_reasons` | autoware_planning_msgs::msg::StopReasonArray | reasons that cause the vehicle to stop | ## Node parameters diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index e051374ed3dda..662b02c0088d1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -56,7 +56,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs tier4_v2x_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index d78bc883e6b35..52e304a3d96f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -46,7 +46,7 @@ namespace { autoware_planning_msgs::msg::Path to_path( - const tier4_planning_msgs::msg::PathWithLaneId & path_with_id) + const autoware_planning_msgs::msg::PathWithLaneId & path_with_id) { autoware_planning_msgs::msg::Path path; for (const auto & path_point : path_with_id.points) { @@ -67,7 +67,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Trigger Subscriber trigger_sub_path_with_lane_id_ = - this->create_subscription( + this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); srv_load_plugin_ = create_service( @@ -300,7 +300,7 @@ bool BehaviorVelocityPlannerNode::isDataReady(rclcpp::Clock clock) } void BehaviorVelocityPlannerNode::onTrigger( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) + const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { std::unique_lock lk(mutex_); @@ -334,7 +334,7 @@ void BehaviorVelocityPlannerNode::onTrigger( } autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data) { autoware_planning_msgs::msg::Path output_path_msg; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp index 4efb58e38b74f..ed3eb1b12df31 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp @@ -31,8 +31,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -48,7 +48,7 @@ namespace autoware::behavior_velocity_planner using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; -using tier4_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node { @@ -61,7 +61,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // subscriber - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr trigger_sub_path_with_lane_id_; // polling subscribers @@ -99,7 +99,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node sub_external_velocity_limit_{ this, "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local()}; - void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); + void onTrigger(const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); void onParam(); @@ -139,7 +139,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function bool isDataReady(rclcpp::Clock clock); autoware_planning_msgs::msg::Path generatePath( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); std::unique_ptr logger_configure_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 4820c340058ff..e7eb4b7d536c2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -73,11 +73,11 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( } } -tier4_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( +autoware_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg) + const autoware_planning_msgs::msg::PathWithLaneId & input_path_msg) { - tier4_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; + autoware_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp index fddd658cef06e..2d520cffd02a9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include @@ -47,9 +47,9 @@ class BehaviorVelocityPlannerManager void removeScenePlugin(rclcpp::Node & node, const std::string & name); // cppcheck-suppress functionConst - tier4_planning_msgs::msg::PathWithLaneId planPathVelocity( + autoware_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg); + const autoware_planning_msgs::msg::PathWithLaneId & input_path_msg); private: pluginlib::ClassLoader plugin_loader_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 983aaec2acf4f..658fe70ce070a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -63,7 +63,7 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; - std::optional external_velocity_limit; + std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; bool is_simulation = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp index 4867b14cbb806..9a695b03cedb1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -30,10 +30,10 @@ class PluginInterface public: virtual ~PluginInterface() = default; virtual void init(rclcpp::Node & node) = 0; - virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void plan(autoware_planning_msgs::msg::PathWithLaneId * path) = 0; virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + const autoware_planning_msgs::msg::PathWithLaneId & path) = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp index 44a00072a0be4..ae3ee3780644e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -28,13 +28,13 @@ class PluginWrapper : public PluginInterface { public: void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } - void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override + void plan(autoware_planning_msgs::msg::PathWithLaneId * path) override { scene_manager_->plan(path); }; void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) override + const autoware_planning_msgs::msg::PathWithLaneId & path) override { scene_manager_->updateSceneModuleInstances(planner_data, path); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index fadda66f12562..646f491641b5c 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 @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include #include @@ -59,7 +59,7 @@ using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; using builtin_interfaces::msg::Time; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; @@ -157,7 +157,7 @@ class SceneModuleInterface } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; }; class SceneModuleManagerInterface @@ -171,19 +171,19 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_planning_msgs::msg::PathWithLaneId & path); - virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } + virtual void plan(autoware_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(autoware_planning_msgs::msg::PathWithLaneId * path); - virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + getModuleExpiredFunction(const autoware_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path); + virtual void deleteExpiredModules(const autoware_planning_msgs::msg::PathWithLaneId & path); bool isModuleRegistered(const int64_t module_id) { @@ -193,7 +193,7 @@ class SceneModuleManagerInterface void registerModule(const std::shared_ptr & scene_module); size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; std::set> scene_modules_; std::set registered_module_id_set_; @@ -208,7 +208,7 @@ 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_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; rclcpp::Publisher::SharedPtr @@ -227,7 +227,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void plan(autoware_planning_msgs::msg::PathWithLaneId * path) override; protected: RTCInterface rtc_interface_; @@ -262,7 +262,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); - void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const autoware_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_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index fae13db2822e7..4b7fa64f90717 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -156,7 +156,7 @@ std::optional findOffsetSegment( } std::optional findOffsetSegment( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset); + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset); template geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) @@ -191,7 +191,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse } std::optional createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const autoware_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const double margin, const double vehicle_offset); } // namespace arc_lane_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 696d4a41b7673..4b66d1d376faf 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 @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include @@ -40,7 +40,7 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - tier4_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, point.pose.position.x, + autoware_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, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp index 5ac91cdaf1369..2ff21ee6e4dbe 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 @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include @@ -33,7 +33,7 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPathMarkerArray( - const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const autoware_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createObjectsMarkerArray( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp index 2d4dab018fb18..e1f07f36be4e5 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 @@ -18,13 +18,13 @@ #include #include -#include +#include namespace autoware::behavior_velocity_planner { bool splineInterpolate( - const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger); + const autoware_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger); autoware_planning_msgs::msg::Path interpolatePath( const autoware_planning_msgs::msg::Path & path, const double length, const double interval); autoware_planning_msgs::msg::Path filterLitterPathPoint( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 4dc355f0e6b1d..6e55f3da9e97e 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 @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index fc4f852ef82f7..073d6b731806e 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 @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include @@ -68,13 +68,13 @@ using Polygon2d = autoware::universe_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; using autoware_perception_msgs::msg::PredictedObjects; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; namespace planning_utils { size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx); bool createDetectionAreaPolygons( @@ -105,7 +105,7 @@ inline int64_t bitShift(int64_t original_id) bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const autoware_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); @@ -218,7 +218,7 @@ std::set getLaneIdSetOnPath( const geometry_msgs::msg::Pose & current_pose); bool isOverLine( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const autoware_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( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 6d94e20cfdb1b..2cb6a167db4a0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -44,7 +44,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs tier4_v2x_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index cdfde1ce51205..2557b7cf8a8bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -43,7 +43,7 @@ SceneModuleInterface::SceneModuleInterface( } size_t SceneModuleInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -62,7 +62,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); } if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( + pub_debug_path_ = node.create_publisher( std::string("~/debug/path_with_lane_id/") + module_name, 1); } pub_virtual_wall_ = node.create_publisher( @@ -82,7 +82,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( } size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -91,7 +91,7 @@ size_t SceneModuleManagerInterface::findEgoSegmentIndex( void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { planner_data_ = planner_data; @@ -100,7 +100,7 @@ void SceneModuleManagerInterface::updateSceneModuleInstances( } void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path) + autoware_planning_msgs::msg::PathWithLaneId * path) { universe_utils::ScopedTimeTrack st( "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); @@ -140,7 +140,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { - tier4_planning_msgs::msg::PathWithLaneId debug_path; + autoware_planning_msgs::msg::PathWithLaneId debug_path; debug_path.header = path->header; debug_path.points = path->points; pub_debug_path_->publish(debug_path); @@ -151,7 +151,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( } void SceneModuleManagerInterface::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -183,7 +183,7 @@ SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( { } -void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) +void SceneModuleManagerInterfaceWithRTC::plan(autoware_planning_msgs::msg::PathWithLaneId * path) { setActivation(); modifyPathVelocity(path); @@ -248,7 +248,7 @@ void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() } void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 9b02b374b0a6c..db9fcc49f15d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -89,7 +89,7 @@ std::optional checkCollision( } std::optional findOffsetSegment( - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset) + const autoware_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset) { if (offset >= 0) { return findForwardOffsetSegment(path, index, offset); @@ -99,7 +99,7 @@ std::optional findOffsetSegment( } std::optional createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const autoware_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const double margin, const double vehicle_offset) { // Find collision segment diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index 2ddf62bb584ff..5e3a69f77dbe9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -54,7 +54,7 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( } visualization_msgs::msg::MarkerArray createPathMarkerArray( - const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const autoware_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index b8e6e28bae7c2..3753edd3b9d45 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -23,8 +23,8 @@ namespace autoware::behavior_velocity_planner { bool splineInterpolate( - const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger) + const autoware_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger) { if (input.points.size() < 2) { RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index cb9b58ec48924..a8cb9253c1fc1 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 @@ -21,7 +21,7 @@ #include #include -#include +#include #include @@ -41,8 +41,8 @@ namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; @@ -59,7 +59,7 @@ bool smoothPath( const auto & smoother = planner_data->velocity_smoother_; auto trajectory = - autoware::motion_utils::convertToTrajectoryPoints( + autoware::motion_utils::convertToTrajectoryPoints( in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index a8f909201ac7f..74c4ddbbb2b61 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -42,7 +42,7 @@ namespace { size_t calcPointIndexFromSegmentIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) { const size_t prev_point_idx = seg_idx; @@ -102,7 +102,7 @@ using autoware::universe_utils::calcOffsetPose; using autoware_planning_msgs::msg::PathPoint; size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) { if (idx == 0) { @@ -300,7 +300,7 @@ bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return geometry_msgs::msg::Pose{}; @@ -593,7 +593,7 @@ std::vector getSubsequentLaneIdsSetOnPath( // TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const autoware_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( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp index 5ed490ab755a9..5c25996f5af5e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp @@ -28,8 +28,8 @@ TEST(smoothPath, nominal) { using autoware::behavior_velocity_planner::smoothPath; - using tier4_planning_msgs::msg::PathPointWithLaneId; - using tier4_planning_msgs::msg::PathWithLaneId; + using autoware_planning_msgs::msg::PathPointWithLaneId; + using autoware_planning_msgs::msg::PathWithLaneId; rclcpp::init(0, nullptr); rclcpp::NodeOptions options; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp index 8c8ef575b5a9a..4ee07dbc747eb 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 @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include @@ -63,7 +63,7 @@ TEST(PlanningUtilsTest, createDetectionAreaPolygons) // Input parameters Polygons2d da_polys; - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; geometry_msgs::msg::Pose target_pose; size_t target_seg_idx = 0; autoware::behavior_velocity_planner::DetectionRange da_range; @@ -83,7 +83,7 @@ TEST(PlanningUtilsTest, createDetectionAreaPolygons) // Path with some points for (double i = 0.0; i < 3.0; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_planning_msgs::msg::PathPointWithLaneId point; point.point.pose.position.x = i * 5.0; point.point.pose.position.y = 0.0; point.point.longitudinal_velocity_mps = 1.0; @@ -175,7 +175,7 @@ TEST(PlanningUtilsTest, insertDecelPoint) TEST(PlanningUtilsTest, insertVelocity) { auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); - tier4_planning_msgs::msg::PathPointWithLaneId path_point; + autoware_planning_msgs::msg::PathPointWithLaneId path_point; path_point.point.pose.position.x = 5.0; path_point.point.pose.position.y = 0.0; path_point.point.longitudinal_velocity_mps = 10.0; @@ -215,10 +215,10 @@ TEST(PlanningUtilsTest, insertStopPoint) // Test for getAheadPose TEST(PlanningUtilsTest, getAheadPose) { - tier4_planning_msgs::msg::PathWithLaneId path; - tier4_planning_msgs::msg::PathPointWithLaneId point1; - tier4_planning_msgs::msg::PathPointWithLaneId point2; - tier4_planning_msgs::msg::PathPointWithLaneId point3; + autoware_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathPointWithLaneId point1; + autoware_planning_msgs::msg::PathPointWithLaneId point2; + autoware_planning_msgs::msg::PathPointWithLaneId point3; point1.point.pose.position.x = 0.0; point2.point.pose.position.x = 5.0; point3.point.pose.position.x = 10.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp index 4dbf4c73fcd82..87b350986dd65 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 @@ -16,22 +16,22 @@ #define UTILS_HPP_ #include -#include -#include +#include +#include #include namespace test { -inline tier4_planning_msgs::msg::PathWithLaneId generatePath( +inline autoware_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - tier4_planning_msgs::msg::PathWithLaneId path{}; + autoware_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point{}; + autoware_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 6b4050fe9bfbb..e3c59d759fec3 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 @@ -53,9 +53,9 @@ using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; using run_out_utils::PlannerParam; using run_out_utils::PredictedPath; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; -using PathPointsWithLaneId = std::vector; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; /** * @brief base class for creating dynamic obstacles from multiple types of input diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4a16f263d0a54..9e9643475e20a 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,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) setDynamicObstacleCreator(node, debug_ptr_); } -void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void RunOutModuleManager::launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return; @@ -162,7 +162,7 @@ void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathW std::function &)> RunOutModuleManager::getModuleExpiredFunction( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path) { return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp index 131ba32f32100..c96e012fa2f3b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -37,10 +37,10 @@ class RunOutModuleManager : public SceneModuleManagerInterface std::shared_ptr debug_ptr_; std::unique_ptr dynamic_obstacle_creator_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp index e4e08d69a79f7..3e8372013c414 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp @@ -24,7 +24,7 @@ namespace autoware::behavior_velocity_planner::run_out_utils * This function returns the point with the smallest (signed) longitudinal distance */ geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.hpp index 62591af27af11..96ab0f1ae6198 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 @@ -16,7 +16,7 @@ #define PATH_UTILS_HPP_ #include -#include +#include #include #include @@ -31,7 +31,7 @@ namespace run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index efc2d458ae2a7..3b1ffd491de91 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -744,7 +744,7 @@ std::optional RunOutModule::calcStopPoint( bool RunOutModule::insertStopPoint( const std::optional stop_point, - tier4_planning_msgs::msg::PathWithLaneId & path) + autoware_planning_msgs::msg::PathWithLaneId & path) { // no stop point if (!stop_point) { @@ -765,7 +765,7 @@ bool RunOutModule::insertStopPoint( } // to PathPointWithLaneId - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + autoware_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(nearest_seg_idx); stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); @@ -889,7 +889,7 @@ void RunOutModule::insertApproachingVelocity( // to PathPointWithLaneId // use lane id of point behind inserted point - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + autoware_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); stop_point_with_lane_id.point.pose = *stop_point; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 3673a215bb749..d60dfebc9f23e 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 @@ -36,8 +36,8 @@ using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; using tier4_debug_msgs::msg::Float32Stamped; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; class RunOutModule : public SceneModuleInterface @@ -124,7 +124,7 @@ class RunOutModule : public SceneModuleInterface bool insertStopPoint( const std::optional stop_point, - tier4_planning_msgs::msg::PathWithLaneId & path); + autoware_planning_msgs::msg::PathWithLaneId & path); void insertVelocityForState( const std::optional & dynamic_obstacle, const PlannerData planner_data, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 1f948cc7faaa1..19fcc07bea06b 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 @@ -44,8 +44,8 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; using tier4_debug_msgs::msg::Float32Stamped; -using tier4_planning_msgs::msg::PathWithLaneId; -using PathPointsWithLaneId = std::vector; +using autoware_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; struct CommonParam { double normal_min_jerk; // min jerk limit for mild stop [m/sss] diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp index e6a838c77fb37..e68e871cdb938 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp @@ -30,8 +30,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -51,9 +51,9 @@ using autoware_perception_msgs::msg::ObjectClassification; using geometry_msgs::msg::Point; using Polygons2d = std::vector; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; -using PathPointsWithLaneId = std::vector; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; using autoware::behavior_velocity_planner::applyVoxelGridFilter; using autoware::behavior_velocity_planner::createPredictedPath; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp index eb8502f043b22..8ebb59c671175 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include @@ -32,8 +32,8 @@ using autoware::behavior_velocity_planner::run_out_utils::findLongitudinalNearestPoint; using geometry_msgs::msg::Point; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class TestPathUtils : public ::testing::Test { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp index d39436baa7dea..87f3173a14c22 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include @@ -38,8 +38,8 @@ using autoware::behavior_velocity_planner::run_out_utils::StateMachine; using autoware::behavior_velocity_planner::run_out_utils::StateParam; using geometry_msgs::msg::Point; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class TestStateMachine : public ::testing::Test { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp index c523dd0a53a71..c54a8e1ced405 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include @@ -57,8 +57,8 @@ using autoware::behavior_velocity_planner::run_out_utils::trimPathFromSelfPose; using autoware_perception_msgs::msg::ObjectClassification; using geometry_msgs::msg::Point; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class TestRunOutUtils : public ::testing::Test { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index 4677d49b78f4b..dae3daa1abbfd 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,7 @@ SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) static_cast(getOrDeclareParameter(node, ns + ".max_speed")); } -void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void SpeedBumpModuleManager::launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) { for (const auto & speed_bump_with_lane_id : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -70,7 +70,7 @@ void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa std::function &)> SpeedBumpModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto speed_bump_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp index 950bb8471cc22..59904cd42748f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface private: SpeedBumpModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; }; class SpeedBumpModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 176a01d5112c5..814fad2b84165 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class SpeedBumpModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp index 72bad3db86f73..7619d71a71f54 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 @@ -31,7 +31,7 @@ #include #include -#include +#include #include @@ -41,8 +41,8 @@ namespace autoware::behavior_velocity_planner namespace bg = boost::geometry; using geometry_msgs::msg::Point32; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and speed bump struct PathPolygonIntersectionStatus diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index b305a1d2aa404..9bb542d85cbd6 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,7 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::vector stop_lines_with_lane_id; @@ -62,7 +62,7 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } std::set StopLineModuleManager::getStopLineIdSetOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::set stop_line_id_set; @@ -73,7 +73,7 @@ std::set StopLineModuleManager::getStopLineIdSetOnPath( return stop_line_id_set; } -void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +void StopLineModuleManager::launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) { for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { @@ -88,7 +88,7 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat std::function &)> StopLineModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index bef8a5eef4ac0..5b63610157100 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -21,7 +21,7 @@ #include -#include +#include #include #include @@ -44,17 +44,17 @@ class StopLineModuleManager : public SceneModuleManagerInterface StopLineModule::PlannerParam planner_param_; std::vector getStopLinesWithLaneIdOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); std::set getStopLineIdSetOnPath( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; }; class StopLineModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 2a9c5dab1ebd9..f24ded1cab091 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 @@ -18,7 +18,7 @@ #include -#include +#include #include #include @@ -41,7 +41,7 @@ StopLineModule::StopLineModule( bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) { auto trajectory = - trajectory::Trajectory::Builder{}.build( + trajectory::Trajectory::Builder{}.build( path->points); if (!trajectory) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 9ac1463da8618..b87efcdbd76dc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -40,7 +40,7 @@ class StopLineModule : public SceneModuleInterface public: using StopLineWithLaneId = std::pair; using Trajectory = - autoware::trajectory::Trajectory; + autoware::trajectory::Trajectory; enum class State { APPROACH, STOPPED, START }; struct DebugData diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 1eafb71eb403c..a77df6d8392b3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -28,9 +28,9 @@ using autoware::behavior_velocity_planner::StopLineModule; -tier4_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) +autoware_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) { - tier4_planning_msgs::msg::PathPointWithLaneId p; + autoware_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x; p.point.pose.position.y = y; return p; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md index 0507d69af1b4e..6c7d91e1c6ff2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md @@ -50,13 +50,13 @@ The managing of your modules is defined in manager.hpp and manager.cpp. The mana #### `launchNewModules()` Method -- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `autoware_planning_msgs::msg::PathWithLaneId`. - It is responsible for launching new modules based on the provided path information (PathWithLaneId). The implementation of this method involves initializing and configuring modules specific to your behavior velocity planner by using the `TemplateModule` class. - In the provided source code, it initializes a `module_id` to 0 and checks if a module with the same ID is already registered. If not, it registers a new `TemplateModule` with the module ID. Note that each module managed by the `TemplateModuleManager` should have a unique ID. The template code registers a single module, so the `module_id` is set as 0 for simplicity. #### `getModuleExpiredFunction()` Method -- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `autoware_planning_msgs::msg::PathWithLaneId`. - It returns a `std::function&)>`. This function is used by the behavior velocity planner to determine whether a particular module has expired or not based on the given path. - The implementation of this method is expected to return a function that can be used to check the expiration status of modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 4325cc1d5aaf9..f0035b0e17673 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -38,7 +38,7 @@ TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) } void TemplateModuleManager::launchNewModules( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path) { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { @@ -49,7 +49,7 @@ void TemplateModuleManager::launchNewModules( std::function &)> TemplateModuleManager::getModuleExpiredFunction( - [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const autoware_planning_msgs::msg::PathWithLaneId & path) { return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp index a7a0d83be6368..2264c66197908 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -64,7 +64,7 @@ class TemplateModuleManager * * @param path The path with lane ID information to determine module launch. */ - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; /** * @brief Get a function to check module expiration. @@ -76,7 +76,7 @@ class TemplateModuleManager * @return A function for checking module expiration. */ std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; }; /** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 70cd5cb1235e7..aae6d3742400f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -23,7 +23,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class TemplateModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 082973d9431e5..77d711e79bafd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -34,7 +34,6 @@ tf2 tf2_eigen tf2_geometry_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros 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 b6747724ba6f7..b70aa09d060c3 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,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) "~/output/traffic_signal", 1); } -void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) +void TrafficLightModuleManager::modifyPathVelocity(autoware_planning_msgs::msg::PathWithLaneId * path) { visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; @@ -95,7 +95,7 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat } void TrafficLightModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { for (const auto & traffic_light_reg_elem : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -125,7 +125,7 @@ void TrafficLightModuleManager::launchNewModules( std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp index eb8ef53ddb604..9f1836752bc88 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 @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -40,12 +40,12 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC private: TrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; - void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void modifyPathVelocity(autoware_planning_msgs::msg::PathWithLaneId * path) override; bool isModuleRegisteredFromRegElement(const lanelet::Id & id, const size_t module_id) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 5eb0d5aa5f267..1288938b96437 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 @@ -272,11 +272,11 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const return false; } -tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( - const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, +autoware_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( + const autoware_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point) { - tier4_planning_msgs::msg::PathWithLaneId modified_path; + autoware_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; // Create stop pose diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 8221bb3740273..192924b4604dc 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 @@ -88,8 +88,8 @@ class TrafficLightModule : public SceneModuleInterface private: bool isStopSignal(); - tier4_planning_msgs::msg::PathWithLaneId insertStopPose( - const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, + autoware_planning_msgs::msg::PathWithLaneId insertStopPose( + const autoware_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point); bool isPassthrough(const double & signed_arc_length) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp index 9b13123380ce1..bb7db1f87b97b 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,7 +59,7 @@ auto findNearestCollisionPoint( } auto createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const autoware_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, const double offset) -> std::optional> { if (input.points.size() < 2) { @@ -124,7 +124,7 @@ auto createTargetPoint( } auto calcStopPointAndInsertIndex( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, const double & stop_line_extend_length) -> std::optional> { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp index ad4ed84cea116..6f6067ea03570 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,7 +60,7 @@ auto findNearestCollisionPoint( * point, return std::nullopt. */ auto createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const autoware_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, const double offset) -> std::optional>; /** @@ -73,7 +73,7 @@ auto createTargetPoint( * point, return std::nullopt. */ auto calcStopPointAndInsertIndex( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const autoware_planning_msgs::msg::PathWithLaneId & input_path, const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, const double & stop_line_extend_length) -> std::optional>; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp index a5d09acc103e9..b58a1b69bc412 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp @@ -20,8 +20,8 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternion; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 6d3bc68242d7c..b4828587ae4d5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -29,7 +29,6 @@ nlohmann-json-dev pluginlib rclcpp - tier4_planning_msgs tier4_v2x_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index a51cf9211c21f..d6324d6e67b25 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -56,7 +56,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node } void VirtualTrafficLightModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { autoware::universe_utils::LineString2d ego_path_linestring; for (const auto & path_point : path.points) { @@ -91,7 +91,7 @@ void VirtualTrafficLightModuleManager::launchNewModules( std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index ba5a4b23a6fe0..8490883177b48 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -38,10 +38,10 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface private: VirtualTrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const autoware_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + const autoware_planning_msgs::msg::PathWithLaneId & path) override; }; class VirtualTrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 206fb14b6d41c..9fc958763fc02 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -366,7 +366,7 @@ bool VirtualTrafficLightModule::hasRightOfWay( } void VirtualTrafficLightModule::insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) + autoware_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, *map_data_.stop_line, end_line_idx); @@ -427,7 +427,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } void VirtualTrafficLightModule::insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) + autoware_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx) { const auto collision = findLastCollisionBeforeEndLine(path->points, map_data_.end_lines, end_line_idx); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index b031ba5b2bb2b..2a20a96a91b08 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -57,7 +57,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface struct ModuleData { geometry_msgs::msg::Pose head_pose{}; - tier4_planning_msgs::msg::PathWithLaneId path{}; + autoware_planning_msgs::msg::PathWithLaneId path{}; std::optional stop_head_pose_at_stop_line; std::optional stop_head_pose_at_end_line; }; @@ -117,10 +117,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool hasRightOfWay(const tier4_v2x_msgs::msg::VirtualTrafficLightState & state); void insertStopVelocityAtStopLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); + autoware_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); void insertStopVelocityAtEndLine( - tier4_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); + autoware_planning_msgs::msg::PathWithLaneId * path, const size_t end_line_idx); }; } // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp index a1950bf768f4c..27be56019f253 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp @@ -86,7 +86,7 @@ geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Poi return geom_p; } -void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path) +void insertStopVelocityFromStart(autoware_planning_msgs::msg::PathWithLaneId * path) { for (auto & p : path->points) { p.point.longitudinal_velocity_mps = 0.0; @@ -95,7 +95,7 @@ void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, - tier4_planning_msgs::msg::PathWithLaneId * path) + autoware_planning_msgs::msg::PathWithLaneId * path) { const auto collision_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( path->points, collision.index, collision.point); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp index ab11ca5c81391..9bab2c6a9e9c7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -58,11 +58,11 @@ geometry_msgs::msg::Pose calcHeadPose( geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point3d & p); -void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path); +void insertStopVelocityFromStart(autoware_planning_msgs::msg::PathWithLaneId * path); std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, - tier4_planning_msgs::msg::PathWithLaneId * path); + autoware_planning_msgs::msg::PathWithLaneId * path); template std::optional findLastCollisionBeforeEndLine( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp index e6bfa0234951c..f0ba8642dd8f9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp @@ -33,11 +33,11 @@ using autoware::behavior_velocity_planner::virtual_traffic_light::insertStopVelo using autoware::behavior_velocity_planner::virtual_traffic_light::SegmentIndexWithPoint; using autoware::behavior_velocity_planner::virtual_traffic_light::toAutowarePoints; -tier4_planning_msgs::msg::PathWithLaneId generateStraightPath() +autoware_planning_msgs::msg::PathWithLaneId generateStraightPath() { - tier4_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; for (size_t i = 0; i < 10; ++i) { - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_planning_msgs::msg::PathPointWithLaneId point; point.point.pose.position.x = static_cast(i); point.point.pose.position.y = 0; point.point.pose.position.z = 0; @@ -168,8 +168,8 @@ TEST(VirtualTrafficLightTest, ConvertToGeomPoint) TEST(VirtualTrafficLightTest, InsertStopVelocityFromStart) { - tier4_planning_msgs::msg::PathWithLaneId path; - tier4_planning_msgs::msg::PathPointWithLaneId point; + autoware_planning_msgs::msg::PathWithLaneId path; + autoware_planning_msgs::msg::PathPointWithLaneId point; point.point.longitudinal_velocity_mps = 10.0; path.points.push_back(point); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp index 85d4495914823..5aa409e6775bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface { diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index a4119b03a2dd7..865f62a36f2dc 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -29,7 +29,6 @@ std_msgs tf2_ros tier4_debug_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/system/autoware_default_adapi/src/utils/route_conversion.hpp b/system/autoware_default_adapi/src/utils/route_conversion.hpp index 053dfd697c967..d6cad0326db37 100644 --- a/system/autoware_default_adapi/src/utils/route_conversion.hpp +++ b/system/autoware_default_adapi/src/utils/route_conversion.hpp @@ -23,10 +23,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace autoware::default_adapi::conversion { @@ -37,19 +37,19 @@ ExternalRoute create_empty_route(const rclcpp::Time & stamp); ExternalRoute convert_route(const InternalRoute & internal); using ExternalState = autoware_adapi_v1_msgs::msg::RouteState; -using InternalState = tier4_planning_msgs::msg::RouteState; +using InternalState = autoware_planning_msgs::msg::RouteState; ExternalState convert_state(const InternalState & internal); using ExternalClearRequest = autoware_adapi_v1_msgs::srv::ClearRoute::Request::SharedPtr; -using InternalClearRequest = tier4_planning_msgs::srv::ClearRoute::Request::SharedPtr; +using InternalClearRequest = autoware_planning_msgs::srv::ClearRoute::Request::SharedPtr; InternalClearRequest convert_request(const ExternalClearRequest & external); using ExternalLaneletRequest = autoware_adapi_v1_msgs::srv::SetRoute::Request::SharedPtr; -using InternalLaneletRequest = tier4_planning_msgs::srv::SetLaneletRoute::Request::SharedPtr; +using InternalLaneletRequest = autoware_planning_msgs::srv::SetLaneletRoute::Request::SharedPtr; InternalLaneletRequest convert_request(const ExternalLaneletRequest & external); using ExternalWaypointRequest = autoware_adapi_v1_msgs::srv::SetRoutePoints::Request::SharedPtr; -using InternalWaypointRequest = tier4_planning_msgs::srv::SetWaypointRoute::Request::SharedPtr; +using InternalWaypointRequest = autoware_planning_msgs::srv::SetWaypointRoute::Request::SharedPtr; InternalWaypointRequest convert_request(const ExternalWaypointRequest & external); using ExternalResponse = autoware_adapi_v1_msgs::msg::ResponseStatus; diff --git a/system/mrm_comfortable_stop_operator/README.md b/system/mrm_comfortable_stop_operator/README.md index dffa28f1dfa49..3026b68a3d853 100644 --- a/system/mrm_comfortable_stop_operator/README.md +++ b/system/mrm_comfortable_stop_operator/README.md @@ -19,8 +19,8 @@ MRM comfortable stop operator is a node that generates comfortable stop commands | Name | Type | Description | | -------------------------------------- | ----------------------------------------------------- | ---------------------------- | | `~/output/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | -| `~/output/velocity_limit` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | -| `~/output/velocity_limit/clear` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | +| `~/output/velocity_limit` | `autoware_planning_msgs::msg::VelocityLimit` | Velocity limit command | +| `~/output/velocity_limit/clear` | `autoware_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | ## Parameters diff --git a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp index eb84191f481f9..c42d65d507b27 100644 --- a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp +++ b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp @@ -20,9 +20,9 @@ #include // Autoware -#include -#include -#include +#include +#include +#include #include #include @@ -61,8 +61,8 @@ class MrmComfortableStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_clear_command_; void publishStatus() const; diff --git a/system/mrm_comfortable_stop_operator/package.xml b/system/mrm_comfortable_stop_operator/package.xml index 58ff309d825f1..87c3275b38a41 100644 --- a/system/mrm_comfortable_stop_operator/package.xml +++ b/system/mrm_comfortable_stop_operator/package.xml @@ -17,7 +17,7 @@ rclcpp_components std_msgs std_srvs - tier4_planning_msgs + autoware_planning_msgs tier4_system_msgs ament_lint_auto diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp index fc703ab16e259..e3d11baebafa4 100644 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +++ b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -39,10 +39,10 @@ MrmComfortableStopOperator::MrmComfortableStopOperator(const rclcpp::NodeOptions // Publisher pub_status_ = create_publisher( "~/output/mrm/comfortable_stop/status", 1); - pub_velocity_limit_ = create_publisher( + pub_velocity_limit_ = create_publisher( "~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_clear_command_ = - create_publisher( + create_publisher( "~/output/velocity_limit/clear", rclcpp::QoS{1}.transient_local()); // Timer @@ -96,7 +96,7 @@ void MrmComfortableStopOperator::publishStatus() const void MrmComfortableStopOperator::publishVelocityLimit() const { - auto velocity_limit = tier4_planning_msgs::msg::VelocityLimit(); + auto velocity_limit = autoware_planning_msgs::msg::VelocityLimit(); velocity_limit.stamp = this->now(); velocity_limit.max_velocity = 0; velocity_limit.use_constraints = true; @@ -110,7 +110,7 @@ void MrmComfortableStopOperator::publishVelocityLimit() const void MrmComfortableStopOperator::publishVelocityLimitClearCommand() const { - auto velocity_limit_clear_command = tier4_planning_msgs::msg::VelocityLimitClearCommand(); + auto velocity_limit_clear_command = autoware_planning_msgs::msg::VelocityLimitClearCommand(); velocity_limit_clear_command.stamp = this->now(); velocity_limit_clear_command.command = true; velocity_limit_clear_command.sender = "mrm_comfortable_stop_operator"; diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt index 95224b9967d4b..40394fdc0bd13 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -69,7 +69,7 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common rviz_rendering autoware_vehicle_msgs - tier4_planning_msgs + autoware_planning_msgs autoware_perception_msgs ) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md index 943d566ad109c..b3420eb65fd12 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md @@ -20,7 +20,7 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | `/vehicle/status/hazard_status` | `autoware_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | | `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | | `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic is status of gear | -| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | +| `/planning/scenario_planning/current_max_velocity` | `autoware_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | | `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The topic is status of traffic light | ## Parameter diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index f8d822473c415..c82462054f0b1 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -110,7 +110,7 @@ private Q_SLOTS: rclcpp::Subscription::SharedPtr hazard_lights_sub_; rclcpp::Subscription::SharedPtr traffic_sub_; - rclcpp::Subscription::SharedPtr speed_limit_sub_; + rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; @@ -121,7 +121,7 @@ private Q_SLOTS: const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); - void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateSpeedLimitData(const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateTrafficLightData( const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg); void drawWidget(QImage & hud); diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp index c88984a4c339f..cb2fadd6397c8 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -24,7 +24,7 @@ #include #include "autoware_vehicle_msgs/msg/velocity_report.hpp" -#include +#include #include #include @@ -41,7 +41,7 @@ class SpeedLimitDisplay QPainter & painter, const QRectF & backgroundRect, const QColor & color, const QColor & light_color, const QColor & dark_color, const QColor & bg_color, const float bg_alpha); - void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateSpeedLimitData(const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index 86bab15660d4d..5f47cb1951959 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -18,7 +18,7 @@ rviz_common rviz_ogre_vendor rviz_rendering - tier4_planning_msgs + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index 270d1837f2dba..d8985c60327e3 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -121,7 +121,7 @@ void SignalDisplay::onInitialize() speed_limit_topic_property_ = std::make_unique( "Speed Limit Topic", "/planning/scenario_planning/current_max_velocity", - "tier4_planning_msgs/msg/VelocityLimit", "Topic for Speed Limit Data", this, + "autoware_planning_msgs/msg/VelocityLimit", "Topic for Speed Limit Data", this, SLOT(topic_updated_speed_limit())); speed_limit_topic_property_->initialize(rviz_ros_node); @@ -217,7 +217,7 @@ void SignalDisplay::updateTrafficLightData( } void SignalDisplay::updateSpeedLimitData( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(property_mutex_); @@ -438,10 +438,10 @@ void SignalDisplay::topic_updated_speed_limit() speed_limit_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); speed_limit_sub_ = - rviz_ros_node->get_raw_node()->create_subscription( + rviz_ros_node->get_raw_node()->create_subscription( speed_limit_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + [this](const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { updateSpeedLimitData(msg); }); } diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp index 36b5212c08f75..fe03b499ef7c3 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -52,7 +52,7 @@ SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0), current_speed_(0.0) } void SpeedLimitDisplay::updateSpeedLimitData( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { current_limit = msg->max_velocity; } diff --git a/visualization/tier4_adapi_rviz_plugin/package.xml b/visualization/tier4_adapi_rviz_plugin/package.xml index da1af7b150c70..aa859fdc2542d 100644 --- a/visualization/tier4_adapi_rviz_plugin/package.xml +++ b/visualization/tier4_adapi_rviz_plugin/package.xml @@ -23,7 +23,7 @@ rviz_common rviz_default_plugins tier4_external_api_msgs - tier4_planning_msgs + autowaare_planning_msgs ament_lint_auto autoware_lint_common diff --git a/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp b/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp index 0525f5fdc1a09..16c78dc95ee9a 100644 --- a/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp +++ b/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp @@ -274,7 +274,7 @@ void StatePanel::onInitialize() client_emergency_stop_ = raw_node_->create_client( "/api/autoware/set/emergency"); - pub_velocity_limit_ = raw_node_->create_publisher( + pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); } @@ -559,7 +559,7 @@ void StatePanel::onEmergencyStatus( void StatePanel::onClickVelocityLimit() { - auto velocity_limit = std::make_shared(); + auto velocity_limit = std::make_shared(); velocity_limit->stamp = raw_node_->now(); velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; pub_velocity_limit_->publish(*velocity_limit); diff --git a/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp b/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp index b30494772061e..cb7440460995d 100644 --- a/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp +++ b/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include @@ -93,7 +93,7 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Subscription::SharedPtr sub_gear_; rclcpp::Client::SharedPtr client_emergency_stop_; rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; // Operation Mode QLabel * operation_mode_label_ptr_{nullptr}; diff --git a/visualization/tier4_planning_rviz_plugin/README.md b/visualization/tier4_planning_rviz_plugin/README.md index 5b6286139af0d..97f731cd6bd01 100644 --- a/visualization/tier4_planning_rviz_plugin/README.md +++ b/visualization/tier4_planning_rviz_plugin/README.md @@ -15,7 +15,7 @@ This plugin displays the path, trajectory, and maximum speed. | -------------------------------------------------- | ----------------------------------------- | ------------------------------------------ | | `/input/path` | `autoware_planning_msgs::msg::Path` | The topic on which to subscribe path | | `/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | -| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | +| `/planning/scenario_planning/current_max_velocity` | `autoware_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | ### Output diff --git a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index dc0cb0eb5188a..9d3d0731ccf30 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 @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -188,7 +188,7 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay }; class AutowarePathWithLaneIdDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT public: @@ -198,9 +198,9 @@ class AutowarePathWithLaneIdDisplay private: void preProcessMessageDetail() override; void preVisualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; void visualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) override; rviz_common::properties::BoolProperty property_lane_id_view_; diff --git a/visualization/tier4_planning_rviz_plugin/package.xml b/visualization/tier4_planning_rviz_plugin/package.xml index d615878f91354..9d8b27368742c 100644 --- a/visualization/tier4_planning_rviz_plugin/package.xml +++ b/visualization/tier4_planning_rviz_plugin/package.xml @@ -25,7 +25,6 @@ rviz_rendering tf2_geometry_msgs tf2_ros - tier4_planning_msgs ament_lint_auto autoware_lint_common diff --git a/visualization/tier4_planning_rviz_plugin/src/path/display.cpp b/visualization/tier4_planning_rviz_plugin/src/path/display.cpp index 869f0c4d91793..136be9d316dc5 100644 --- a/visualization/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/visualization/tier4_planning_rviz_plugin/src/path/display.cpp @@ -55,7 +55,7 @@ AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() } void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) + const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) { const size_t size = msg_ptr->points.size(); // clear previous text @@ -77,7 +77,7 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( } void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( - const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) + const autoware_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) { const auto & point = msg_ptr->points.at(p_idx); diff --git a/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp b/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp index 098be3a93ddd0..b61d61ed16bc4 100644 --- a/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp +++ b/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp @@ -116,7 +116,7 @@ void MaxVelocityDisplay::subscribe() std::string topic_name = property_topic_name_->getStdString(); if (topic_name.length() > 0 && topic_name != "/") { rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - max_vel_sub_ = raw_node->create_subscription( + max_vel_sub_ = raw_node->create_subscription( topic_name, rclcpp::QoS{1}.transient_local(), std::bind(&MaxVelocityDisplay::processMessage, this, std::placeholders::_1)); } @@ -128,7 +128,7 @@ void MaxVelocityDisplay::unsubscribe() } void MaxVelocityDisplay::processMessage( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) { if (!overlay_->isVisible()) { return; diff --git a/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.hpp b/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.hpp index 9098d37e4f7a0..e379d1fd1b754 100644 --- a/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.hpp +++ b/visualization/tier4_planning_rviz_plugin/src/tools/max_velocity.hpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include @@ -63,7 +63,7 @@ private Q_SLOTS: void updateVisualization(); protected: - void processMessage(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); + void processMessage(const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -73,8 +73,8 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_value_scale_; private: - rclcpp::Subscription::SharedPtr max_vel_sub_; - tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr last_msg_ptr_; + rclcpp::Subscription::SharedPtr max_vel_sub_; + autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/visualization/tier4_state_rviz_plugin/README.md b/visualization/tier4_state_rviz_plugin/README.md index e45be3bea13dc..83225c87710be 100644 --- a/visualization/tier4_state_rviz_plugin/README.md +++ b/visualization/tier4_state_rviz_plugin/README.md @@ -31,7 +31,7 @@ This plugin also can engage from the panel. | `/api/routing/clear_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | The service to clear route state | | `/api/motion/accept_start` | `autoware_adapi_v1_msgs::srv::AcceptStart` | The service to accept the vehicle to start | | `/api/autoware/set/emergency` | `tier4_external_api_msgs::srv::SetEmergency` | The service to set external emergency | -| `/planning/scenario_planning/max_velocity_default` | `tier4_planning_msgs::msg::VelocityLimit` | The topic to set maximum speed of the vehicle | +| `/planning/scenario_planning/max_velocity_default` | `autoware_planning_msgs::msg::VelocityLimit` | The topic to set maximum speed of the vehicle | ## HowToUse diff --git a/visualization/tier4_state_rviz_plugin/package.xml b/visualization/tier4_state_rviz_plugin/package.xml index f5f5fa8b9c0c1..3af6597e2ffc4 100644 --- a/visualization/tier4_state_rviz_plugin/package.xml +++ b/visualization/tier4_state_rviz_plugin/package.xml @@ -24,7 +24,7 @@ rviz_common tier4_control_msgs tier4_external_api_msgs - tier4_planning_msgs + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/visualization/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/visualization/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index ffcf801e758e0..3f6396ffbd1fe 100644 --- a/visualization/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/visualization/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -149,7 +149,7 @@ void AutowareStatePanel::onInitialize() client_emergency_stop_ = raw_node_->create_client( "/api/autoware/set/emergency"); - pub_velocity_limit_ = raw_node_->create_publisher( + pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); QObject::connect(segmented_button, &CustomSegmentedButton::buttonClicked, this, [this](int id) { @@ -804,7 +804,7 @@ void AutowareStatePanel::onSwitchStateChanged(int state) void AutowareStatePanel::onClickVelocityLimit() { - auto velocity_limit = std::make_shared(); + auto velocity_limit = std::make_shared(); velocity_limit->stamp = raw_node_->now(); velocity_limit->max_velocity = velocity_limit_value_label_->text().toDouble() / 3.6; pub_velocity_limit_->publish(*velocity_limit); diff --git a/visualization/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp b/visualization/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp index 3c106467b9beb..1e78cd8707ec7 100644 --- a/visualization/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp +++ b/visualization/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include @@ -119,7 +119,7 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Client::SharedPtr client_emergency_stop_; rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; QLabel * velocity_limit_value_label_{nullptr}; bool sliderIsDragging = false;