refactor(universe_utils/motion_utils)!: add autoware namespace (#59)

* refactor(universe_utils): add autoware namespace

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

style(pre-commit): autofix

* refactor(motion_utils): add autoware namespace

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

---------

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

Author: kosuke55

common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp

@@ -439,9 +439,9 @@ void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const
 
 void AutowareAutomaticGoalPanel::publishMarkers()
 {
-  using autoware_universe_utils::createDefaultMarker;
-  using autoware_universe_utils::createMarkerColor;
-  using autoware_universe_utils::createMarkerScale;
+  using autoware::universe_utils::createDefaultMarker;
+  using autoware::universe_utils::createMarkerColor;
+  using autoware::universe_utils::createMarkerScale;
 
   MarkerArray text_array;
   MarkerArray arrow_array;

common/tier4_debug_tools/src/lateral_error_publisher.cpp

@@ -74,7 +74,7 @@ void LateralErrorPublisher::onGroundTruthPose(
   }
 
   // Search closest trajectory point with vehicle pose
-  const auto closest_index = autoware_motion_utils::findNearestIndex(
+  const auto closest_index = autoware::motion_utils::findNearestIndex(
     current_trajectory_ptr_->points, current_vehicle_pose_ptr_->pose.pose,
     std::numeric_limits<double>::max(), yaw_threshold_to_search_closest_);
   if (!closest_index) {

control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp

@@ -30,7 +30,7 @@
 
 namespace stop_accel_evaluator
 {
-using autoware_universe_utils::SelfPoseListener;
+using autoware::universe_utils::SelfPoseListener;
 using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::Imu;

control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp

@@ -123,7 +123,7 @@ void StopAccelEvaluatorNode::calculateStopAccel()
           const double pitch =
             lpf_pitch_->filter(getPitchByQuaternion(current_pose_ptr->pose.orientation));
 
-          stop_accel_ = accel_sum / accel_num + autoware_universe_utils::gravity *
+          stop_accel_ = accel_sum / accel_num + autoware::universe_utils::gravity *
                                                   std::sin(pitch);  // consider removing gravity
           stop_accel_with_gravity_ = accel_sum / accel_num;         // not consider removing gravity
 

driving_environment_analyzer/src/utils.cpp

@@ -54,9 +54,9 @@ std::vector<double> calcElevationAngle(const T & points)
   }
 
   for (size_t i = 0; i < points.size() - 1; ++i) {
-    const auto p1 = autoware_universe_utils::getPoint(points.at(i));
-    const auto p2 = autoware_universe_utils::getPoint(points.at(i + 1));
-    elevation_vec.at(i) = autoware_universe_utils::calcElevationAngle(p1, p2);
+    const auto p1 = autoware::universe_utils::getPoint(points.at(i));
+    const auto p2 = autoware::universe_utils::getPoint(points.at(i + 1));
+    elevation_vec.at(i) = autoware::universe_utils::calcElevationAngle(p1, p2);
   }
   elevation_vec.at(elevation_vec.size() - 1) = elevation_vec.at(elevation_vec.size() - 2);
 
@@ -79,11 +79,11 @@ double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose)
     return 0.0;
   }
 
-  const size_t idx = autoware_motion_utils::findNearestSegmentIndex(points, pose.position);
+  const size_t idx = autoware::motion_utils::findNearestSegmentIndex(points, pose.position);
 
-  const auto p1 = autoware_universe_utils::getPoint(points.at(idx));
-  const auto p2 = autoware_universe_utils::getPoint(points.at(idx + 1));
-  return autoware_universe_utils::calcElevationAngle(p1, p2);
+  const auto p1 = autoware::universe_utils::getPoint(points.at(idx));
+  const auto p2 = autoware::universe_utils::getPoint(points.at(idx + 1));
+  return autoware::universe_utils::calcElevationAngle(p1, p2);
 }
 
 double getLaneWidth(const lanelet::ConstLanelet & lane)
@@ -96,7 +96,7 @@ double getLaneWidth(const lanelet::ConstLanelet & lane)
     return points;
   };
 
-  const auto lon_length = autoware_motion_utils::calcArcLength(to_ros_msg(lane.centerline()));
+  const auto lon_length = autoware::motion_utils::calcArcLength(to_ros_msg(lane.centerline()));
   return boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length;
 }
 
@@ -118,7 +118,7 @@ double getMaxCurvature(const lanelet::ConstLanelets & lanes)
   double max_value = 0.0;
 
   for (const auto & lane : lanes) {
-    const auto values = autoware_motion_utils::calcCurvature(to_ros_msg(lane.centerline()));
+    const auto values = autoware::motion_utils::calcCurvature(to_ros_msg(lane.centerline()));
     const auto max_value_itr = std::max_element(values.begin(), values.end());
     if (max_value_itr == values.end()) {
       continue;
@@ -146,7 +146,7 @@ std::pair<double, double> getLaneWidth(const lanelet::ConstLanelets & lanes)
   double max_value = 0.0;
 
   for (const auto & lane : lanes) {
-    const auto lon_length = autoware_motion_utils::calcArcLength(to_ros_msg(lane.centerline()));
+    const auto lon_length = autoware::motion_utils::calcArcLength(to_ros_msg(lane.centerline()));
     const auto width = boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length;
 
     if (min_value > width) {

localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/autoware_universe_utils.hpp

@@ -38,11 +38,11 @@ template <class Pose1, class Pose2>
 bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose)
 {
   // check the first point direction
-  const double src_yaw = tf2::getYaw(autoware_universe_utils::getPose(src_pose).orientation);
-  const double pose_direction_yaw = autoware_universe_utils::calcAzimuthAngle(
-    autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose));
-  return std::fabs(autoware_universe_utils::normalizeRadian(src_yaw - pose_direction_yaw)) <
-         autoware_universe_utils::pi / 2.0;
+  const double src_yaw = tf2::getYaw(autoware::universe_utils::getPose(src_pose).orientation);
+  const double pose_direction_yaw = autoware::universe_utils::calcAzimuthAngle(
+    autoware::universe_utils::getPoint(src_pose), autoware::universe_utils::getPoint(dst_pose));
+  return std::fabs(autoware::universe_utils::normalizeRadian(src_yaw - pose_direction_yaw)) <
+         autoware::universe_utils::pi / 2.0;
 }
 
 /**
@@ -56,8 +56,8 @@ template <class Point1, class Point2>
 geometry_msgs::msg::Point calcInterpolatedPoint(
   const Point1 & src, const Point2 & dst, const double ratio)
 {
-  const auto src_point = autoware_universe_utils::getPoint(src);
-  const auto dst_point = autoware_universe_utils::getPoint(dst);
+  const auto src_point = autoware::universe_utils::getPoint(src);
+  const auto dst_point = autoware::universe_utils::getPoint(dst);
 
   tf2::Vector3 src_vec;
   src_vec.setX(src_point.x);
@@ -101,35 +101,35 @@ geometry_msgs::msg::Pose calcInterpolatedPose(
 
   geometry_msgs::msg::Pose output_pose;
   output_pose.position = calcInterpolatedPoint(
-    autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose),
+    autoware::universe_utils::getPoint(src_pose), autoware::universe_utils::getPoint(dst_pose),
     clamped_ratio);
 
   if (set_orientation_from_position_direction) {
-    const double input_poses_dist = autoware_universe_utils::calcDistance2d(
-      autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose));
+    const double input_poses_dist = autoware::universe_utils::calcDistance2d(
+      autoware::universe_utils::getPoint(src_pose), autoware::universe_utils::getPoint(dst_pose));
     const bool is_driving_forward = isDrivingForward(src_pose, dst_pose);
 
     // Get orientation from interpolated point and src_pose
     if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) {
-      output_pose.orientation = autoware_universe_utils::getPose(dst_pose).orientation;
+      output_pose.orientation = autoware::universe_utils::getPose(dst_pose).orientation;
     } else if (!is_driving_forward && clamped_ratio < 1e-6) {
-      output_pose.orientation = autoware_universe_utils::getPose(src_pose).orientation;
+      output_pose.orientation = autoware::universe_utils::getPose(src_pose).orientation;
     } else {
       const auto & base_pose = is_driving_forward ? dst_pose : src_pose;
-      const double pitch = autoware_universe_utils::calcElevationAngle(
-        autoware_universe_utils::getPoint(output_pose),
-        autoware_universe_utils::getPoint(base_pose));
-      const double yaw = autoware_universe_utils::calcAzimuthAngle(
-        autoware_universe_utils::getPoint(output_pose),
-        autoware_universe_utils::getPoint(base_pose));
-      output_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw);
+      const double pitch = autoware::universe_utils::calcElevationAngle(
+        autoware::universe_utils::getPoint(output_pose),
+        autoware::universe_utils::getPoint(base_pose));
+      const double yaw = autoware::universe_utils::calcAzimuthAngle(
+        autoware::universe_utils::getPoint(output_pose),
+        autoware::universe_utils::getPoint(base_pose));
+      output_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw);
     }
   } else {
     // Get orientation by spherical linear interpolation
     tf2::Transform src_tf;
     tf2::Transform dst_tf;
-    tf2::fromMsg(autoware_universe_utils::getPose(src_pose), src_tf);
-    tf2::fromMsg(autoware_universe_utils::getPose(dst_pose), dst_tf);
+    tf2::fromMsg(autoware::universe_utils::getPose(src_pose), src_tf);
+    tf2::fromMsg(autoware::universe_utils::getPose(dst_pose), dst_tf);
     const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio);
     output_pose.orientation = tf2::toMsg(quaternion);
   }

localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp

@@ -103,7 +103,7 @@ class DeviationEstimator : public rclcpp::Node
   std::unique_ptr<VelocityCoefModule> vel_coef_module_;
   std::unique_ptr<ValidationModule> validation_module_;
 
-  std::shared_ptr<autoware_universe_utils::TransformListener> transform_listener_;
+  std::shared_ptr<autoware::universe_utils::TransformListener> transform_listener_;
 
   void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 

localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp

@@ -178,7 +178,7 @@ DeviationEstimator::DeviationEstimator(
     declare_parameter<double>("thres_coef_vx"), declare_parameter<double>("thres_stddev_vx"),
     declare_parameter<double>("thres_bias_gyro"), declare_parameter<double>("thres_stddev_gyro"),
     5);
-  transform_listener_ = std::make_shared<autoware_universe_utils::TransformListener>(this);
+  transform_listener_ = std::make_shared<autoware::universe_utils::TransformListener>(this);
 
   RCLCPP_INFO(this->get_logger(), "[Deviation Estimator] launch success");
 }

localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp

@@ -93,7 +93,7 @@ geometry_msgs::msg::Point integrate_position(
 {
   double t_prev = rclcpp::Time(vx_list.front().stamp).seconds();
   double yaw = yaw_init;
-  geometry_msgs::msg::Point d_pos = autoware_universe_utils::createPoint(0.0, 0.0, 0.0);
+  geometry_msgs::msg::Point d_pos = autoware::universe_utils::createPoint(0.0, 0.0, 0.0);
   for (std::size_t i = 0; i < vx_list.size() - 1; ++i) {
     const double t_cur = rclcpp::Time(vx_list[i + 1].stamp).seconds();
     const geometry_msgs::msg::Vector3 gyro_interpolated =
@@ -118,9 +118,9 @@ geometry_msgs::msg::Vector3 calculate_error_rpy(
   const geometry_msgs::msg::Vector3 & gyro_bias)
 {
   const geometry_msgs::msg::Vector3 rpy_0 =
-    autoware_universe_utils::getRPY(pose_list.front().pose.orientation);
+    autoware::universe_utils::getRPY(pose_list.front().pose.orientation);
   const geometry_msgs::msg::Vector3 rpy_1 =
-    autoware_universe_utils::getRPY(pose_list.back().pose.orientation);
+    autoware::universe_utils::getRPY(pose_list.back().pose.orientation);
   const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias);
 
   geometry_msgs::msg::Vector3 error_rpy = createVector3(

localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp

@@ -53,7 +53,7 @@ TEST(DeviationEstimatorGyroBias, SmokeTestDefault)
     for (int i = 0; i <= ndt_rate * dt; ++i) {
       geometry_msgs::msg::PoseStamped pose;
       pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate);
-      pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0);
+      pose.pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0);
       pose_list.push_back(pose);
     }
 

localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp

@@ -53,7 +53,7 @@ TEST(DeviationEstimatorGyroStddev, SmokeTestDefault)
     for (int i = 0; i <= ndt_rate * t_window; ++i) {
       geometry_msgs::msg::PoseStamped pose;
       pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate);
-      pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0);
+      pose.pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0);
       pose_list.push_back(pose);
     }
 
@@ -104,7 +104,7 @@ TEST(DeviationEstimatorGyroStddev, SmokeTestWithBias)
     for (int i = 0; i <= ndt_rate * t_window; ++i) {
       geometry_msgs::msg::PoseStamped pose;
       pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate);
-      pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0);
+      pose.pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0);
       pose_list.push_back(pose);
     }
 

localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/autoware_universe_utils.hpp

@@ -29,11 +29,11 @@ template <class Pose1, class Pose2>
 bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose)
 {
   // check the first point direction
-  const double src_yaw = tf2::getYaw(autoware_universe_utils::getPose(src_pose).orientation);
-  const double pose_direction_yaw = autoware_universe_utils::calcAzimuthAngle(
-    autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose));
-  return std::fabs(autoware_universe_utils::normalizeRadian(src_yaw - pose_direction_yaw)) <
-         autoware_universe_utils::pi / 2.0;
+  const double src_yaw = tf2::getYaw(autoware::universe_utils::getPose(src_pose).orientation);
+  const double pose_direction_yaw = autoware::universe_utils::calcAzimuthAngle(
+    autoware::universe_utils::getPoint(src_pose), autoware::universe_utils::getPoint(dst_pose));
+  return std::fabs(autoware::universe_utils::normalizeRadian(src_yaw - pose_direction_yaw)) <
+         autoware::universe_utils::pi / 2.0;
 }
 
 /**
@@ -47,8 +47,8 @@ template <class Point1, class Point2>
 geometry_msgs::msg::Point calcInterpolatedPoint(
   const Point1 & src, const Point2 & dst, const double ratio)
 {
-  const auto src_point = autoware_universe_utils::getPoint(src);
-  const auto dst_point = autoware_universe_utils::getPoint(dst);
+  const auto src_point = autoware::universe_utils::getPoint(src);
+  const auto dst_point = autoware::universe_utils::getPoint(dst);
 
   tf2::Vector3 src_vec;
   src_vec.setX(src_point.x);
@@ -92,35 +92,35 @@ geometry_msgs::msg::Pose calcInterpolatedPose(
 
   geometry_msgs::msg::Pose output_pose;
   output_pose.position = calcInterpolatedPoint(
-    autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose),
+    autoware::universe_utils::getPoint(src_pose), autoware::universe_utils::getPoint(dst_pose),
     clamped_ratio);
 
   if (set_orientation_from_position_direction) {
-    const double input_poses_dist = autoware_universe_utils::calcDistance2d(
-      autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose));
+    const double input_poses_dist = autoware::universe_utils::calcDistance2d(
+      autoware::universe_utils::getPoint(src_pose), autoware::universe_utils::getPoint(dst_pose));
     const bool is_driving_forward = isDrivingForward(src_pose, dst_pose);
 
     // Get orientation from interpolated point and src_pose
     if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) {
-      output_pose.orientation = autoware_universe_utils::getPose(dst_pose).orientation;
+      output_pose.orientation = autoware::universe_utils::getPose(dst_pose).orientation;
     } else if (!is_driving_forward && clamped_ratio < 1e-6) {
-      output_pose.orientation = autoware_universe_utils::getPose(src_pose).orientation;
+      output_pose.orientation = autoware::universe_utils::getPose(src_pose).orientation;
     } else {
       const auto & base_pose = is_driving_forward ? dst_pose : src_pose;
-      const double pitch = autoware_universe_utils::calcElevationAngle(
-        autoware_universe_utils::getPoint(output_pose),
-        autoware_universe_utils::getPoint(base_pose));
-      const double yaw = autoware_universe_utils::calcAzimuthAngle(
-        autoware_universe_utils::getPoint(output_pose),
-        autoware_universe_utils::getPoint(base_pose));
-      output_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw);
+      const double pitch = autoware::universe_utils::calcElevationAngle(
+        autoware::universe_utils::getPoint(output_pose),
+        autoware::universe_utils::getPoint(base_pose));
+      const double yaw = autoware::universe_utils::calcAzimuthAngle(
+        autoware::universe_utils::getPoint(output_pose),
+        autoware::universe_utils::getPoint(base_pose));
+      output_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw);
     }
   } else {
     // Get orientation by spherical linear interpolation
     tf2::Transform src_tf;
     tf2::Transform dst_tf;
-    tf2::fromMsg(autoware_universe_utils::getPose(src_pose), src_tf);
-    tf2::fromMsg(autoware_universe_utils::getPose(dst_pose), dst_tf);
+    tf2::fromMsg(autoware::universe_utils::getPose(src_pose), src_tf);
+    tf2::fromMsg(autoware::universe_utils::getPose(dst_pose), dst_tf);
     const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio);
     output_pose.orientation = tf2::toMsg(quaternion);
   }

localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp

@@ -81,7 +81,7 @@ class DeviationEvaluator : public rclcpp::Node
   PoseStamped::SharedPtr current_ekf_gt_pose_ptr_;
   PoseStamped::SharedPtr current_ndt_pose_ptr_;
 
-  std::shared_ptr<autoware_universe_utils::TransformListener> transform_listener_;
+  std::shared_ptr<autoware::universe_utils::TransformListener> transform_listener_;
 
   bool has_published_initial_pose_;
 

localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp

@@ -106,7 +106,7 @@ DeviationEvaluator::DeviationEvaluator(
   pub_init_pose_with_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "out_initial_pose_with_covariance", 1);
 
-  transform_listener_ = std::make_shared<autoware_universe_utils::TransformListener>(this);
+  transform_listener_ = std::make_shared<autoware::universe_utils::TransformListener>(this);
 
   current_ndt_pose_ptr_ = nullptr;
   has_published_initial_pose_ = false;

planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp

@@ -86,8 +86,8 @@ class TrajectoryAnalyzer
     TrajectoryDebugInfo data;
     data.stamp = node_->now();
     data.size = points.size();
-    data.curvature = autoware_motion_utils::calcCurvature(points);
-    const auto arclength_offset = autoware_motion_utils::calcSignedArcLength(points, 0, ego_p);
+    data.curvature = autoware::motion_utils::calcCurvature(points);
+    const auto arclength_offset = autoware::motion_utils::calcSignedArcLength(points, 0, ego_p);
     data.arclength = calcPathArcLengthArray(points, -arclength_offset);
     data.velocity = getVelocityArray(points);
     data.acceleration = getAccelerationArray(points);

planning/planning_debug_tools/include/planning_debug_tools/util.hpp

@@ -28,11 +28,11 @@
 namespace planning_debug_tools
 {
 
+using autoware::universe_utils::calcDistance2d;
+using autoware::universe_utils::getPoint;
+using autoware::universe_utils::getRPY;
 using autoware_planning_msgs::msg::PathPoint;
 using autoware_planning_msgs::msg::TrajectoryPoint;
-using autoware_universe_utils::calcDistance2d;
-using autoware_universe_utils::getPoint;
-using autoware_universe_utils::getRPY;
 using tier4_planning_msgs::msg::PathPointWithLaneId;
 
 double getVelocity(const PathPoint & p)
@@ -88,7 +88,7 @@ inline std::vector<double> getAccelerationArray(const T & points)
     const auto & prev_point = points.at(i);
     const auto & next_point = points.at(i + 1);
 
-    const double delta_s = autoware_universe_utils::calcDistance2d(prev_point, next_point);
+    const double delta_s = autoware::universe_utils::calcDistance2d(prev_point, next_point);
     if (delta_s == 0.0) {
       segment_wise_a_arr.push_back(0.0);
     } else {