refactor(localization_util)!: prefix package and namespace with autoware

.github/CODEOWNERS

@@ -91,7 +91,7 @@ localization/autoware_pose_initializer/** [email protected] isamu.takagi@tie
 localization/autoware_pose_instability_detector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_stop_filter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_twist2accel/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-localization/localization_util/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+localization/autoware_localization_util/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_common/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_image_processing/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]

localization/autoware_ekf_localizer/package.xml

@@ -23,11 +23,11 @@
   <build_depend>eigen</build_depend>
 
   <depend>autoware_kalman_filter</depend>
+  <depend>autoware_localization_util</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
-  <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

localization/autoware_ekf_localizer/src/ekf_localizer.cpp

@@ -17,7 +17,7 @@
 #include "autoware/ekf_localizer/diagnostics.hpp"
 #include "autoware/ekf_localizer/string.hpp"
 #include "autoware/ekf_localizer/warning_message.hpp"
-#include "localization_util/covariance_ellipse.hpp"
+#include "autoware/localization_util/covariance_ellipse.hpp"
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>

localization/autoware_gyro_odometer/package.xml

@@ -17,11 +17,11 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_localization_util</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
-  <depend>localization_util</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>

localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp

@@ -72,7 +72,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options)
   twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
     "twist_with_covariance", rclcpp::QoS{10});
 
-  diagnostics_ = std::make_unique<DiagnosticsModule>(this, "gyro_odometer_status");
+  diagnostics_ =
+    std::make_unique<autoware::localization_util::DiagnosticsModule>(this, "gyro_odometer_status");
 
   // TODO(YamatoAndo) createTimer
 }

localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp

@@ -15,10 +15,10 @@
 #ifndef GYRO_ODOMETER_CORE_HPP_
 #define GYRO_ODOMETER_CORE_HPP_
 
+#include "autoware/localization_util/diagnostics_module.hpp"
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 #include "autoware/universe_utils/ros/msg_covariance.hpp"
 #include "autoware/universe_utils/ros/transform_listener.hpp"
-#include "localization_util/diagnostics_module.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node
   std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_;
   std::deque<sensor_msgs::msg::Imu> gyro_queue_;
 
-  std::unique_ptr<DiagnosticsModule> diagnostics_;
+  std::unique_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_;
 };
 
 }  // namespace autoware::gyro_odometer

localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml

@@ -19,9 +19,9 @@
   <test_depend>ament_index_cpp</test_depend>
   <depend>aruco</depend>
   <depend>autoware_landmark_manager</depend>
+  <depend>autoware_localization_util</depend>
   <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>
-  <depend>localization_util</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2_eigen</depend>

localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp

@@ -44,7 +44,7 @@
 
 #include "ar_tag_based_localizer.hpp"
 
-#include "localization_util/util_func.hpp"
+#include "autoware/localization_util/util_func.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -94,7 +94,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
     RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode);
     return;
   }
-  ekf_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
+  ekf_pose_buffer_ = std::make_unique<autoware::localization_util::SmartPoseBuffer>(
     this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_);
 
   /*
@@ -168,8 +168,8 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
   const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp;
 
   // get self pose
-  const std::optional<SmartPoseBuffer::InterpolateResult> interpolate_result =
-    ekf_pose_buffer_->interpolate(sensor_stamp);
+  const std::optional<autoware::localization_util::SmartPoseBuffer::InterpolateResult>
+    interpolate_result = ekf_pose_buffer_->interpolate(sensor_stamp);
   if (!interpolate_result) {
     return;
   }

localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp

@@ -46,7 +46,7 @@
 #define AR_TAG_BASED_LOCALIZER_HPP_
 
 #include "autoware/landmark_manager/landmark_manager.hpp"
-#include "localization_util/smart_pose_buffer.hpp"
+#include "autoware/localization_util/smart_pose_buffer.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -122,7 +122,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   aruco::MarkerDetector detector_;
   aruco::CameraParameters cam_param_;
   bool cam_info_received_;
-  std::unique_ptr<SmartPoseBuffer> ekf_pose_buffer_;
+  std::unique_ptr<autoware::localization_util::SmartPoseBuffer> ekf_pose_buffer_;
   landmark_manager::LandmarkManager landmark_manager_;
 };
 

localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml

@@ -19,10 +19,10 @@
   <build_depend>eigen</build_depend>
 
   <depend>autoware_lanelet2_extension</depend>
+  <depend>autoware_localization_util</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_universe_utils</depend>
   <depend>geometry_msgs</depend>
-  <depend>localization_util</depend>
   <depend>rclcpp</depend>
   <depend>tf2_eigen</depend>
 

localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp

@@ -14,9 +14,9 @@
 
 #include "autoware/landmark_manager/landmark_manager.hpp"
 
+#include "autoware/localization_util/util_func.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware_lanelet2_extension/utility/message_conversion.hpp"
-#include "localization_util/util_func.hpp"
 
 #include <Eigen/Core>
 #include <tf2_eigen/tf2_eigen.hpp>
@@ -178,12 +178,14 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose(
       }
 
       if (consider_orientation) {
-        const Eigen::Affine3d landmark_pose = pose_to_affine3d(mapped_landmark_on_map);
+        const Eigen::Affine3d landmark_pose =
+          autoware::localization_util::pose_to_affine3d(mapped_landmark_on_map);
         const Eigen::Affine3d landmark_to_base_link =
-          pose_to_affine3d(detected_landmark_on_base_link).inverse();
+          autoware::localization_util::pose_to_affine3d(detected_landmark_on_base_link).inverse();
         const Eigen::Affine3d new_self_pose_eigen = landmark_pose * landmark_to_base_link;
 
-        const Pose new_self_pose = matrix4f_to_pose(new_self_pose_eigen.matrix().cast<float>());
+        const Pose new_self_pose =
+          autoware::localization_util::matrix4f_to_pose(new_self_pose_eigen.matrix().cast<float>());
 
         // update
         min_distance = curr_distance;

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml

@@ -15,10 +15,10 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_landmark_manager</depend>
+  <depend>autoware_localization_util</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_point_types</depend>
   <depend>autoware_universe_utils</depend>
-  <depend>localization_util</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp

@@ -81,48 +81,49 @@
   param_.save_file_name = this->declare_parameter<std::string>("save_file_name");
   param_.save_frame_id = this->declare_parameter<std::string>("save_frame_id");
 
-  ekf_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
+  ekf_pose_buffer_ = std::make_unique<autoware::localization_util::SmartPoseBuffer>(
     this->get_logger(), param_.self_pose_timeout_sec, param_.self_pose_distance_tolerance_m);
 
   rclcpp::CallbackGroup::SharedPtr points_callback_group;
   points_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   auto points_sub_opt = rclcpp::SubscriptionOptions();
   points_sub_opt.callback_group = points_callback_group;
   sub_points_ = this->create_subscription<PointCloud2>(
     "~/input/pointcloud", rclcpp::QoS(1).best_effort(),
     std::bind(&LidarMarkerLocalizer::points_callback, this, _1), points_sub_opt);
 
   rclcpp::CallbackGroup::SharedPtr self_pose_callback_group;
   self_pose_callback_group =
     this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   auto self_pose_sub_opt = rclcpp::SubscriptionOptions();
   self_pose_sub_opt.callback_group = self_pose_callback_group;
   sub_self_pose_ = this->create_subscription<PoseWithCovarianceStamped>(
     "~/input/ekf_pose", rclcpp::QoS(1),
     std::bind(&LidarMarkerLocalizer::self_pose_callback, this, _1), self_pose_sub_opt);
   sub_map_bin_ = this->create_subscription<HADMapBin>(
     "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal),
     std::bind(&LidarMarkerLocalizer::map_bin_callback, this, _1));
 
   pub_base_link_pose_with_covariance_on_map_ =
     this->create_publisher<PoseWithCovarianceStamped>("~/output/pose_with_covariance", 10);
   rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10));
   qos_marker.transient_local();
   qos_marker.reliable();
   pub_marker_mapped_ = this->create_publisher<MarkerArray>("~/debug/marker_mapped", qos_marker);
   pub_marker_detected_ = this->create_publisher<PoseArray>("~/debug/marker_detected", 10);
   pub_debug_pose_with_covariance_ =
     this->create_publisher<PoseWithCovarianceStamped>("~/debug/pose_with_covariance", 10);
   pub_marker_pointcloud_ = this->create_publisher<PointCloud2>("~/debug/marker_pointcloud", 10);
   service_trigger_node_ = this->create_service<SetBool>(
     "~/service/trigger_node_srv",
     std::bind(&LidarMarkerLocalizer::service_trigger_node, this, _1, _2),
     rclcpp::ServicesQoS().get_rmw_qos_profile(), points_callback_group);
 
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this, false);
 
-  diagnostics_module_.reset(new DiagnosticsModule(this, "marker_detection_status"));
+  diagnostics_module_.reset(
+    new autoware::localization_util::DiagnosticsModule(this, "marker_detection_status"));
 }
 
 void LidarMarkerLocalizer::initialize_diagnostics()
@@ -195,8 +196,8 @@
   }
 
   // (2) get Self Pose
-  const std::optional<SmartPoseBuffer::InterpolateResult> interpolate_result =
-    ekf_pose_buffer_->interpolate(sensor_ros_time);
+  const std::optional<autoware::localization_util::SmartPoseBuffer::InterpolateResult>
+    interpolate_result = ekf_pose_buffer_->interpolate(sensor_ros_time);
 
   const bool is_received_self_pose = interpolate_result != std::nullopt;
   diagnostics_module_->add_key_value("is_received_self_pose", is_received_self_pose);
@@ -607,7 +608,7 @@
   const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped =
     autoware::universe_utils::transform2pose(transform);
   const Eigen::Matrix4f base_to_sensor_matrix =
-    pose_to_matrix4f(target_to_source_pose_stamped.pose);
+    autoware::localization_util::pose_to_matrix4f(target_to_source_pose_stamped.pose);
   pcl_ros::transformPointCloud(
     base_to_sensor_matrix, *sensor_points_input_ptr, *sensor_points_output_ptr);
 }

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp

@@ -15,8 +15,8 @@
 #ifndef LIDAR_MARKER_LOCALIZER_HPP_
 #define LIDAR_MARKER_LOCALIZER_HPP_
 
-#include "localization_util/diagnostics_module.hpp"
-#include "localization_util/smart_pose_buffer.hpp"
+#include "autoware/localization_util/diagnostics_module.hpp"
+#include "autoware/localization_util/smart_pose_buffer.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -134,11 +134,11 @@ class LidarMarkerLocalizer : public rclcpp::Node
   rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_debug_pose_with_covariance_;
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_marker_pointcloud_;
 
-  std::shared_ptr<DiagnosticsModule> diagnostics_module_;
+  std::shared_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_module_;
 
   Param param_;
   bool is_activated_;
-  std::unique_ptr<SmartPoseBuffer> ekf_pose_buffer_;
+  std::unique_ptr<autoware::localization_util::SmartPoseBuffer> ekf_pose_buffer_;
 
   landmark_manager::LandmarkManager landmark_manager_;
 };

localization/autoware_localization_error_monitor/package.xml

@@ -19,9 +19,9 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen</buildtool_depend>
 
+  <depend>autoware_localization_util</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
-  <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>

localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp

@@ -58,7 +58,8 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o
 
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
 
-  diagnostics_error_monitor_ = std::make_unique<DiagnosticsModule>(this, "ellipse_error_status");
+  diagnostics_error_monitor_ =
+    std::make_unique<autoware::localization_util::DiagnosticsModule>(this, "ellipse_error_status");
 }
 
 void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)

localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp

@@ -15,8 +15,8 @@
 #ifndef LOCALIZATION_ERROR_MONITOR_HPP_
 #define LOCALIZATION_ERROR_MONITOR_HPP_
 
-#include "localization_util/covariance_ellipse.hpp"
-#include "localization_util/diagnostics_module.hpp"
+#include "autoware/localization_util/covariance_ellipse.hpp"
+#include "autoware/localization_util/diagnostics_module.hpp"
 
 #include <Eigen/Dense>
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
@@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
 
-  std::unique_ptr<DiagnosticsModule> diagnostics_error_monitor_;
+  std::unique_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_error_monitor_;
 
   double scale_;
   double error_ellipse_size_;

localization/localization_util/CMakeLists.txt → localization/autoware_localization_util/CMakeLists.txt

@@ -1,10 +1,10 @@
 cmake_minimum_required(VERSION 3.14)
-project(localization_util)
+project(autoware_localization_util)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(localization_util SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/util_func.cpp
   src/diagnostics_module.cpp
   src/smart_pose_buffer.cpp

localization/autoware_localization_util/README.md

@@ -0,0 +1,5 @@
+# autoware_localization_util
+
+`autoware_localization_util` is a localization utility package.
+
+This package does not have a node, it is just a library.

localization/localization_util/include/localization_util/covariance_ellipse.hpp → localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
-#define LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+#ifndef AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
 
 #include <Eigen/Dense>
 
@@ -41,4 +41,4 @@ visualization_msgs::msg::Marker create_ellipse_marker(
 
 }  // namespace autoware::localization_util
 
-#endif  // LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+#endif  // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_

localization/localization_util/include/localization_util/diagnostics_module.hpp → localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
-#define LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#ifndef AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -22,6 +22,8 @@
 #include <string>
 #include <vector>
 
+namespace autoware::localization_util
+{
 class DiagnosticsModule
 {
 public:
@@ -57,4 +59,6 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string
 template <>
 void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
 
-#endif  // LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_

localization/localization_util/include/localization_util/matrix_type.hpp → localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp

@@ -12,12 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
-#define LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+#ifndef AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
 
 #include <Eigen/Core>
 
+namespace autoware::localization_util
+{
 using Matrix6d = Eigen::Matrix<double, 6, 6>;
 using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+}  // namespace autoware::localization_util
 
-#endif  // LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+#endif  // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_