fix(autoware_motion_utils): adaption to autoware_utils

Signed-off-by: NorahXiong <[email protected]>

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 autoware_planning_msgs::msg::PathWithLaneId & path,
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & path,
   const geometry_msgs::msg::Point & pos, const int64_t lane_id);
 ```
 

common/autoware_motion_utils/docs/vehicle/vehicle.md

@@ -36,7 +36,7 @@ bool isVehicleStopped(const double stop_duration)
 Necessary includes:
 
 ```c++
-#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
+#include <autoware/motion_utils/vehicle/vehicle_state_checker.hpp>
 ```
 
 1.Create a checker instance.
@@ -116,7 +116,7 @@ bool isVehicleStoppedAtStopPoint(const double stop_duration)
 Necessary includes:
 
 ```c++
-#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
+#include <autoware/motion_utils/vehicle/vehicle_state_checker.hpp>
 ```
 
 1.Create a checker instance.

common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp

@@ -18,12 +18,12 @@
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_planning_msgs/msg/control_point.hpp>
+#include <autoware_internal_planning_msgs/msg/control_point.hpp>
 #include <autoware_planning_msgs/msg/path_point.hpp>
-#include <autoware_planning_msgs/msg/path_point_with_lane_id.hpp>
-#include <autoware_planning_msgs/msg/planning_factor.hpp>
-#include <autoware_planning_msgs/msg/planning_factor_array.hpp>
-#include <autoware_planning_msgs/msg/safety_factor_array.hpp>
+#include <autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor_array.hpp>
+#include <autoware_internal_planning_msgs/msg/safety_factor_array.hpp>
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
@@ -33,10 +33,10 @@
 namespace autoware::motion_utils
 {
 
-using autoware_planning_msgs::msg::ControlPoint;
-using autoware_planning_msgs::msg::PlanningFactor;
-using autoware_planning_msgs::msg::PlanningFactorArray;
-using autoware_planning_msgs::msg::SafetyFactorArray;
+using autoware_internal_planning_msgs::msg::ControlPoint;
+using autoware_internal_planning_msgs::msg::PlanningFactor;
+using autoware_internal_planning_msgs::msg::PlanningFactorArray;
+using autoware_internal_planning_msgs::msg::SafetyFactorArray;
 using geometry_msgs::msg::Pose;
 
 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 = autoware_planning_msgs::build<ControlPoint>()
+    const auto control_point = autoware_internal_planning_msgs::build<ControlPoint>()
                                  .pose(control_point_pose)
                                  .velocity(velocity)
                                  .shift_length(shift_length)
                                  .distance(distance);
 
-    const auto factor = autoware_planning_msgs::build<PlanningFactor>()
+    const auto factor = autoware_internal_planning_msgs::build<PlanningFactor>()
                           .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 = autoware_planning_msgs::build<ControlPoint>()
+    const auto control_start_point = autoware_internal_planning_msgs::build<ControlPoint>()
                                        .pose(start_pose)
                                        .velocity(velocity)
                                        .shift_length(shift_length)
                                        .distance(start_distance);
 
-    const auto control_end_point = autoware_planning_msgs::build<ControlPoint>()
+    const auto control_end_point = autoware_internal_planning_msgs::build<ControlPoint>()
                                      .pose(end_pose)
                                      .velocity(velocity)
                                      .shift_length(shift_length)
                                      .distance(end_distance);
 
-    const auto factor = autoware_planning_msgs::build<PlanningFactor>()
+    const auto factor = autoware_internal_planning_msgs::build<PlanningFactor>()
                           .module(name_)
                           .is_driving_forward(is_driving_forward)
                           .control_points({control_start_point, control_end_point})
@@ -209,8 +209,8 @@ class PlanningFactorInterface
   std::vector<PlanningFactor> factors_;
 };
 
-extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::PathPointWithLaneId>(
-  const std::vector<autoware_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
+extern template void PlanningFactorInterface::add<autoware_internal_planning_msgs::msg::PathPointWithLaneId>(
+  const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
   const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
   const std::string &);
 extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::PathPoint>(
@@ -222,8 +222,8 @@ extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::T
   const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double,
   const std::string &);
 
-extern template void PlanningFactorInterface::add<autoware_planning_msgs::msg::PathPointWithLaneId>(
-  const std::vector<autoware_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
+extern template void PlanningFactorInterface::add<autoware_internal_planning_msgs::msg::PathPointWithLaneId>(
+  const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> &, 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<autoware_planning_msgs::msg::PathPoint>(

common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp

@@ -16,7 +16,7 @@
 #define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
 
 #include "autoware_planning_msgs/msg/path.hpp"
-#include "autoware_planning_msgs/msg/path_with_lane_id.hpp"
+#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 
 #include <vector>
@@ -113,8 +113,8 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
  *        longitudinal and lateral velocity. Otherwise, it uses linear interpolation
  * @return resampled path
  */
-autoware_planning_msgs::msg::PathWithLaneId resamplePath(
-  const autoware_planning_msgs::msg::PathWithLaneId & input_path,
+autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path,
   const std::vector<double> & 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 @@ autoware_planning_msgs::msg::PathWithLaneId resamplePath(
  * @param resample_input_path_stop_point If true, resample closest stop point in input path
  * @return resampled path
  */
-autoware_planning_msgs::msg::PathWithLaneId resamplePath(
-  const autoware_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval,
+autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval,
   const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true,
   const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true);
 

common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp

@@ -15,11 +15,11 @@
 #ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
 #define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
 
-#include "autoware/universe_utils/system/backtrace.hpp"
+#include "autoware_utils/system/backtrace.hpp"
 
 #include <autoware/motion_utils/constants.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
-#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware_utils/geometry/geometry.hpp>
 
 #include <vector>
 
@@ -50,9 +50,9 @@ template <class T>
 bool validate_points_duplication(const T & points)
 {
   for (size_t i = 0; i < points.size() - 1; ++i) {
-    const auto & curr_pt = autoware::universe_utils::getPoint(points.at(i));
-    const auto & next_pt = autoware::universe_utils::getPoint(points.at(i + 1));
-    const double ds = autoware::universe_utils::calcDistance2d(curr_pt, next_pt);
+    const auto & curr_pt = autoware_utils::get_point(points.at(i));
+    const auto & next_pt = autoware_utils::get_point(points.at(i + 1));
+    const double ds = autoware_utils::calc_distance2d(curr_pt, next_pt);
     if (ds < close_s_threshold) {
       return false;
     }
@@ -67,27 +67,27 @@ bool validate_arguments(const T & input_points, const std::vector<double> & resa
   // Check size of the arguments
   if (!validate_size(input_points)) {
     RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
-    autoware::universe_utils::print_backtrace();
+    autoware_utils::print_backtrace();
     return false;
   }
   if (!validate_size(resampling_intervals)) {
     RCLCPP_DEBUG(
       get_logger(), "invalid argument: The number of resampling intervals is less than 2");
-    autoware::universe_utils::print_backtrace();
+    autoware_utils::print_backtrace();
     return false;
   }
 
   // Check resampling range
   if (!validate_resampling_range(input_points, resampling_intervals)) {
     RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points");
-    autoware::universe_utils::print_backtrace();
+    autoware_utils::print_backtrace();
     return false;
   }
 
   // Check duplication
   if (!validate_points_duplication(input_points)) {
     RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
-    autoware::universe_utils::print_backtrace();
+    autoware_utils::print_backtrace();
     return false;
   }
 
@@ -100,7 +100,7 @@ bool validate_arguments(const T & input_points, const double resampling_interval
   // Check size of the arguments
   if (!validate_size(input_points)) {
     RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
-    autoware::universe_utils::print_backtrace();
+    autoware_utils::print_backtrace();
     return false;
   }
 
@@ -109,14 +109,14 @@ bool validate_arguments(const T & input_points, const double resampling_interval
     RCLCPP_DEBUG(
       get_logger(), "invalid argument: resampling interval is less than %f",
       autoware::motion_utils::overlap_threshold);
-    autoware::universe_utils::print_backtrace();
+    autoware_utils::print_backtrace();
     return false;
   }
 
   // Check duplication
   if (!validate_points_duplication(input_points)) {
     RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
-    autoware::universe_utils::print_backtrace();
+    autoware_utils::print_backtrace();
     return false;
   }
 

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp

@@ -16,7 +16,7 @@
 #define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
 
 #include "autoware_planning_msgs/msg/detail/path__struct.hpp"
-#include "autoware_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
+#include "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
 #include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp"
 #include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp"
 #include "std_msgs/msg/header.hpp"
@@ -58,7 +58,7 @@ autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input
 
 template <>
 inline autoware_planning_msgs::msg::Path convertToPath(
-  const autoware_planning_msgs::msg::PathWithLaneId & input)
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & input)
 {
   autoware_planning_msgs::msg::Path output{};
   output.header = input.header;
@@ -80,7 +80,7 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input)
 
 template <>
 inline TrajectoryPoints convertToTrajectoryPoints(
-  const autoware_planning_msgs::msg::PathWithLaneId & input)
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & input)
 {
   TrajectoryPoints output{};
   for (const auto & p : input.points) {
@@ -95,20 +95,20 @@ inline TrajectoryPoints convertToTrajectoryPoints(
 }
 
 template <class T>
-autoware_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
+autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
   [[maybe_unused]] const T & input)
 {
   static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used.");
   throw std::logic_error("Only specializations of convertToPathWithLaneId can be used.");
 }
 
 template <>
-inline autoware_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
+inline autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
   const TrajectoryPoints & input)
 {
-  autoware_planning_msgs::msg::PathWithLaneId output{};
+  autoware_internal_planning_msgs::msg::PathWithLaneId output{};
   for (const auto & p : input) {
-    autoware_planning_msgs::msg::PathPointWithLaneId pp;
+    autoware_internal_planning_msgs::msg::PathPointWithLaneId pp;
     pp.point.pose = p.pose;
     pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
     output.points.emplace_back(pp);

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp

@@ -15,9 +15,9 @@
 #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
 #define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
 
-#include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware_utils/geometry/geometry.hpp"
 
-#include "autoware_planning_msgs/msg/path_with_lane_id.hpp"
+#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 
 #include <boost/optional.hpp>
@@ -51,8 +51,8 @@ autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint(
  * twist information
  * @return resampled path(poses)
  */
-autoware_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint(
-  const autoware_planning_msgs::msg::PathWithLaneId & path,
+autoware_internal_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint(
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & path,
   const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false,
   const double dist_threshold = std::numeric_limits<double>::max(),
   const double yaw_threshold = std::numeric_limits<double>::max());
@@ -73,22 +73,22 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar
   }
 
   if (points.size() < 2 || target_length < 0.0) {
-    return autoware::universe_utils::getPose(points.front());
+    return autoware_utils::get_pose(points.front());
   }
 
   double accumulated_length = 0;
   for (size_t i = 0; i < points.size() - 1; ++i) {
-    const auto & curr_pose = autoware::universe_utils::getPose(points.at(i));
-    const auto & next_pose = autoware::universe_utils::getPose(points.at(i + 1));
-    const double length = autoware::universe_utils::calcDistance3d(curr_pose, next_pose);
+    const auto & curr_pose = autoware_utils::get_pose(points.at(i));
+    const auto & next_pose = autoware_utils::get_pose(points.at(i + 1));
+    const double length = autoware_utils::calc_distance3d(curr_pose, next_pose);
     if (accumulated_length + length > target_length) {
       const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6);
-      return autoware::universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio);
+      return autoware_utils::calc_interpolated_pose(curr_pose, next_pose, ratio);
     }
     accumulated_length += length;
   }
 
-  return autoware::universe_utils::getPose(points.back());
+  return autoware_utils::get_pose(points.back());
 }
 
 }  // namespace autoware::motion_utils

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp

@@ -15,31 +15,31 @@
 #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
 #define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
 
-#include "autoware_planning_msgs/msg/path_with_lane_id.hpp"
+#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
 #include <geometry_msgs/msg/point.hpp>
 
 #include <optional>
 #include <utility>
 namespace autoware::motion_utils
 {
 std::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
-  const autoware_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);
 
 size_t findNearestIndexFromLaneId(
-  const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
   const int64_t lane_id);
 
 size_t findNearestSegmentIndexFromLaneId(
-  const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
   const int64_t lane_id);
 
 // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle
 // center follow the input path
 // @param [in] path with position to be followed by the center of the vehicle
 // @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle
 // center follow the input path NOTE: rear_to_cog is supposed to be positive
-autoware_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
-  const autoware_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
+autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
   const bool enable_last_point_compensation = true);
 }  // namespace autoware::motion_utils