diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index d009a73809074..07b35e3e33045 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -91,7 +91,7 @@ localization/autoware_pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tie
localization/autoware_pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
diff --git a/localization/autoware_ekf_localizer/package.xml b/localization/autoware_ekf_localizer/package.xml
index 7f33fb0b946d1..e0f4c9319d0d5 100644
--- a/localization/autoware_ekf_localizer/package.xml
+++ b/localization/autoware_ekf_localizer/package.xml
@@ -23,11 +23,11 @@
eigen
autoware_kalman_filter
+ autoware_localization_util
autoware_universe_utils
diagnostic_msgs
fmt
geometry_msgs
- localization_util
nav_msgs
rclcpp
rclcpp_components
diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp
index f678412a25438..1b747c750977c 100644
--- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp
+++ b/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
#include
diff --git a/localization/autoware_gyro_odometer/package.xml b/localization/autoware_gyro_odometer/package.xml
index 0e78d0dea51b6..77de1d582fc0f 100644
--- a/localization/autoware_gyro_odometer/package.xml
+++ b/localization/autoware_gyro_odometer/package.xml
@@ -17,11 +17,11 @@
ament_cmake_auto
autoware_cmake
+ autoware_localization_util
autoware_universe_utils
diagnostic_msgs
fmt
geometry_msgs
- localization_util
rclcpp
rclcpp_components
sensor_msgs
diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp
index bc2abb4a8a321..9511f168f346f 100644
--- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp
+++ b/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(
"twist_with_covariance", rclcpp::QoS{10});
- diagnostics_ = std::make_unique(this, "gyro_odometer_status");
+ diagnostics_ =
+ std::make_unique(this, "gyro_odometer_status");
// TODO(YamatoAndo) createTimer
}
diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp
index 1b2c4246a037a..036b3d7cab527 100644
--- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp
+++ b/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
@@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node
std::deque vehicle_twist_queue_;
std::deque gyro_queue_;
- std::unique_ptr diagnostics_;
+ std::unique_ptr diagnostics_;
};
} // namespace autoware::gyro_odometer
diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml
index e8597a6fc9a4e..4f6726abb33ef 100644
--- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml
+++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml
@@ -19,9 +19,9 @@
ament_index_cpp
aruco
autoware_landmark_manager
+ autoware_localization_util
cv_bridge
diagnostic_msgs
- localization_util
rclcpp
rclcpp_components
tf2_eigen
diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
index 80845402a59f3..2418cf3f5dd1c 100644
--- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
+++ b/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
#include
@@ -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(
+ ekf_pose_buffer_ = std::make_unique(
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 interpolate_result =
- ekf_pose_buffer_->interpolate(sensor_stamp);
+ const std::optional
+ interpolate_result = ekf_pose_buffer_->interpolate(sensor_stamp);
if (!interpolate_result) {
return;
}
diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp
index 9826c04e3a86f..ddfdf852be2b5 100644
--- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp
+++ b/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
@@ -122,7 +122,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
aruco::MarkerDetector detector_;
aruco::CameraParameters cam_param_;
bool cam_info_received_;
- std::unique_ptr ekf_pose_buffer_;
+ std::unique_ptr ekf_pose_buffer_;
landmark_manager::LandmarkManager landmark_manager_;
};
diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml
index d69adc7904411..43ef73b8c5226 100644
--- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml
+++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml
@@ -19,10 +19,10 @@
eigen
autoware_lanelet2_extension
+ autoware_localization_util
autoware_map_msgs
autoware_universe_utils
geometry_msgs
- localization_util
rclcpp
tf2_eigen
diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp
index 488f250e325d2..04823a71febe9 100644
--- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp
+++ b/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
#include
@@ -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());
+ const Pose new_self_pose =
+ autoware::localization_util::matrix4f_to_pose(new_self_pose_eigen.matrix().cast());
// update
min_distance = curr_distance;
diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml
index bf60d96311ff1..2b19b46b55424 100755
--- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml
+++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml
@@ -15,10 +15,10 @@
autoware_cmake
autoware_landmark_manager
+ autoware_localization_util
autoware_map_msgs
autoware_point_types
autoware_universe_utils
- localization_util
pcl_conversions
pcl_ros
rclcpp
diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp
index 83fcae2352f51..06d2acf8a55fc 100644
--- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp
+++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp
@@ -81,7 +81,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti
param_.save_file_name = this->declare_parameter("save_file_name");
param_.save_frame_id = this->declare_parameter("save_frame_id");
- ekf_pose_buffer_ = std::make_unique(
+ ekf_pose_buffer_ = std::make_unique(
this->get_logger(), param_.self_pose_timeout_sec, param_.self_pose_distance_tolerance_m);
rclcpp::CallbackGroup::SharedPtr points_callback_group;
@@ -122,7 +122,8 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti
tf_buffer_ = std::make_shared(this->get_clock());
tf_listener_ = std::make_shared(*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 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin
}
// (2) get Self Pose
- const std::optional interpolate_result =
- ekf_pose_buffer_->interpolate(sensor_ros_time);
+ const std::optional
+ 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 @@ void LidarMarkerLocalizer::transform_sensor_measurement(
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);
}
diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp
index 11a6e0b34abdc..d1482c6912592 100644
--- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp
+++ b/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
@@ -134,11 +134,11 @@ class LidarMarkerLocalizer : public rclcpp::Node
rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_;
rclcpp::Publisher::SharedPtr pub_marker_pointcloud_;
- std::shared_ptr diagnostics_module_;
+ std::shared_ptr diagnostics_module_;
Param param_;
bool is_activated_;
- std::unique_ptr ekf_pose_buffer_;
+ std::unique_ptr ekf_pose_buffer_;
landmark_manager::LandmarkManager landmark_manager_;
};
diff --git a/localization/autoware_localization_error_monitor/package.xml b/localization/autoware_localization_error_monitor/package.xml
index 426b0d6b6c432..463e7aee347d7 100644
--- a/localization/autoware_localization_error_monitor/package.xml
+++ b/localization/autoware_localization_error_monitor/package.xml
@@ -19,9 +19,9 @@
autoware_cmake
eigen
+ autoware_localization_util
autoware_universe_utils
diagnostic_msgs
- localization_util
nav_msgs
rclcpp_components
tf2
diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp
index 3f9a02834c74d..63edbe5f6a9c7 100644
--- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp
+++ b/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(this);
- diagnostics_error_monitor_ = std::make_unique(this, "ellipse_error_status");
+ diagnostics_error_monitor_ =
+ std::make_unique(this, "ellipse_error_status");
}
void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)
diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp
index 49abacfa60f38..7b26573964b38 100644
--- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp
+++ b/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
#include
@@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node
std::unique_ptr logger_configure_;
- std::unique_ptr diagnostics_error_monitor_;
+ std::unique_ptr diagnostics_error_monitor_;
double scale_;
double error_ellipse_size_;
diff --git a/localization/localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt
similarity index 87%
rename from localization/localization_util/CMakeLists.txt
rename to localization/autoware_localization_util/CMakeLists.txt
index 2e4d4b321ec54..dd18f3cbad932 100644
--- a/localization/localization_util/CMakeLists.txt
+++ b/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
diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md
new file mode 100644
index 0000000000000..f7fddd9eebf05
--- /dev/null
+++ b/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.
diff --git a/localization/localization_util/include/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp
similarity index 87%
rename from localization/localization_util/include/localization_util/covariance_ellipse.hpp
rename to localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp
index 1f187d411dc63..abd0af46856b0 100644
--- a/localization/localization_util/include/localization_util/covariance_ellipse.hpp
+++ b/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
@@ -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_
diff --git a/localization/localization_util/include/localization_util/diagnostics_module.hpp b/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp
similarity index 87%
rename from localization/localization_util/include/localization_util/diagnostics_module.hpp
rename to localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp
index 0ec52cfe814af..8c19c94127630 100644
--- a/localization/localization_util/include/localization_util/diagnostics_module.hpp
+++ b/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
@@ -22,6 +22,8 @@
#include
#include
+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_
diff --git a/localization/localization_util/include/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp
similarity index 75%
rename from localization/localization_util/include/localization_util/matrix_type.hpp
rename to localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp
index d9ec9b369127a..bda6ff19f2867 100644
--- a/localization/localization_util/include/localization_util/matrix_type.hpp
+++ b/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
+namespace autoware::localization_util
+{
using Matrix6d = Eigen::Matrix;
using RowMatrixXd = Eigen::Matrix;
+} // namespace autoware::localization_util
-#endif // LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
diff --git a/localization/localization_util/include/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp
similarity index 86%
rename from localization/localization_util/include/localization_util/smart_pose_buffer.hpp
rename to localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp
index 2a2a775a9280c..8c10506c36753 100644
--- a/localization/localization_util/include/localization_util/smart_pose_buffer.hpp
+++ b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp
@@ -12,10 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
-#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
+#ifndef AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
-#include "localization_util/util_func.hpp"
+#include "autoware/localization_util/util_func.hpp"
#include
@@ -23,6 +23,8 @@
#include
+namespace autoware::localization_util
+{
class SmartPoseBuffer
{
private:
@@ -64,5 +66,6 @@ class SmartPoseBuffer
const geometry_msgs::msg::Point & target_point,
const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const;
};
+} // namespace autoware::localization_util
-#endif // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
+#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
diff --git a/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp
similarity index 87%
rename from localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp
rename to localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp
index ee25ac175c0b4..ddf7625c202ec 100644
--- a/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp
+++ b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
-#define LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+#ifndef AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
/*
A implementation of tree-structured parzen estimator (TPE)
@@ -28,6 +28,8 @@ Optuna is also used as a reference for implementation.
#include
#include
+namespace autoware::localization_util
+{
class TreeStructuredParzenEstimator
{
public:
@@ -80,5 +82,6 @@ class TreeStructuredParzenEstimator
const std::vector sample_stddev_;
Input base_stddev_;
};
+} // namespace autoware::localization_util
-#endif // LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp
similarity index 92%
rename from localization/localization_util/include/localization_util/util_func.hpp
rename to localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp
index 0b31333da44d3..bef9968f34b6f 100644
--- a/localization/localization_util/include/localization_util/util_func.hpp
+++ b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef LOCALIZATION_UTIL__UTIL_FUNC_HPP_
-#define LOCALIZATION_UTIL__UTIL_FUNC_HPP_
+#ifndef AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_
#include
#include
@@ -36,6 +36,8 @@
#include
#include
+namespace autoware::localization_util
+{
// ref by http://takacity.blog.fc2.com/blog-entry-69.html
std_msgs::msg::ColorRGBA exchange_color_crc(double x);
@@ -81,4 +83,6 @@ void output_pose_with_cov_to_log(
const rclcpp::Logger & logger, const std::string & prefix,
const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);
-#endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_
+} // namespace autoware::localization_util
+
+#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_
diff --git a/localization/localization_util/package.xml b/localization/autoware_localization_util/package.xml
similarity index 92%
rename from localization/localization_util/package.xml
rename to localization/autoware_localization_util/package.xml
index 86c74bb92f2f8..48abf542ddd07 100644
--- a/localization/localization_util/package.xml
+++ b/localization/autoware_localization_util/package.xml
@@ -1,9 +1,9 @@
- localization_util
+ autoware_localization_util
0.1.0
- The localization_util package
+ The autoware_localization_util package
Yamato Ando
Masahiro Sakamoto
Shintaro Sakoda
diff --git a/localization/localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp
similarity index 98%
rename from localization/localization_util/src/covariance_ellipse.cpp
rename to localization/autoware_localization_util/src/covariance_ellipse.cpp
index 885ce2dcee19c..4a7d71909fb7a 100644
--- a/localization/localization_util/src/covariance_ellipse.cpp
+++ b/localization/autoware_localization_util/src/covariance_ellipse.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "localization_util/covariance_ellipse.hpp"
+#include "autoware/localization_util/covariance_ellipse.hpp"
#include
#ifdef ROS_DISTRO_GALACTIC
diff --git a/localization/localization_util/src/diagnostics_module.cpp b/localization/autoware_localization_util/src/diagnostics_module.cpp
similarity index 95%
rename from localization/localization_util/src/diagnostics_module.cpp
rename to localization/autoware_localization_util/src/diagnostics_module.cpp
index fb9e122a71e24..2b68dbf4f5890 100644
--- a/localization/localization_util/src/diagnostics_module.cpp
+++ b/localization/autoware_localization_util/src/diagnostics_module.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "localization_util/diagnostics_module.hpp"
+#include "autoware/localization_util/diagnostics_module.hpp"
#include
@@ -21,6 +21,8 @@
#include
#include
+namespace autoware::localization_util
+{
DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
: clock_(node->get_clock())
{
@@ -103,3 +105,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra
return diagnostics_msg;
}
+} // namespace autoware::localization_util
diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp
similarity index 97%
rename from localization/localization_util/src/smart_pose_buffer.cpp
rename to localization/autoware_localization_util/src/smart_pose_buffer.cpp
index 201c743471efd..3b529d68cf6c5 100644
--- a/localization/localization_util/src/smart_pose_buffer.cpp
+++ b/localization/autoware_localization_util/src/smart_pose_buffer.cpp
@@ -12,8 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "localization_util/smart_pose_buffer.hpp"
+#include "autoware/localization_util/smart_pose_buffer.hpp"
+namespace autoware::localization_util
+{
SmartPoseBuffer::SmartPoseBuffer(
const rclcpp::Logger & logger, const double & pose_timeout_sec,
const double & pose_distance_tolerance_meters)
@@ -153,3 +155,4 @@ bool SmartPoseBuffer::validate_position_difference(
}
return success;
}
+} // namespace autoware::localization_util
diff --git a/localization/localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp
similarity index 97%
rename from localization/localization_util/src/tree_structured_parzen_estimator.cpp
rename to localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp
index 8d594310d79bc..064b33ebe3ad9 100644
--- a/localization/localization_util/src/tree_structured_parzen_estimator.cpp
+++ b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp
@@ -12,13 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "localization_util/tree_structured_parzen_estimator.hpp"
+#include "autoware/localization_util/tree_structured_parzen_estimator.hpp"
#include
#include
#include
#include
+namespace autoware::localization_util
+{
// random number generator
std::mt19937_64 TreeStructuredParzenEstimator::engine(0);
@@ -177,3 +179,4 @@ double TreeStructuredParzenEstimator::log_gaussian_pdf(
}
return result;
}
+} // namespace autoware::localization_util
diff --git a/localization/localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp
similarity index 97%
rename from localization/localization_util/src/util_func.cpp
rename to localization/autoware_localization_util/src/util_func.cpp
index 133442df68dcd..6b019f7750471 100644
--- a/localization/localization_util/src/util_func.cpp
+++ b/localization/autoware_localization_util/src/util_func.cpp
@@ -12,10 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "localization_util/util_func.hpp"
+#include "autoware/localization_util/util_func.hpp"
-#include "localization_util/matrix_type.hpp"
+#include "autoware/localization_util/matrix_type.hpp"
+namespace autoware::localization_util
+{
// ref by http://takacity.blog.fc2.com/blog-entry-69.html
std_msgs::msg::ColorRGBA exchange_color_crc(double x)
{
@@ -249,3 +251,4 @@ void output_pose_with_cov_to_log(
<< covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3)
<< "," << covariance(4, 4) << "," << covariance(5, 5));
}
+} // namespace autoware::localization_util
diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp
similarity index 88%
rename from localization/localization_util/test/test_smart_pose_buffer.cpp
rename to localization/autoware_localization_util/test/test_smart_pose_buffer.cpp
index a8ed6d98b6064..d55555682be84 100644
--- a/localization/localization_util/test/test_smart_pose_buffer.cpp
+++ b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "localization_util/smart_pose_buffer.hpp"
-#include "localization_util/util_func.hpp"
+#include "autoware/localization_util/smart_pose_buffer.hpp"
+#include "autoware/localization_util/util_func.hpp"
#include
#include
@@ -26,6 +26,7 @@
#include
using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer;
bool compare_pose(
const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b)
@@ -56,7 +57,8 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT
old_pose_ptr->pose.pose.position.x = 10.0;
old_pose_ptr->pose.pose.position.y = 20.0;
old_pose_ptr->pose.pose.position.z = 0.0;
- old_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 0.0);
+ old_pose_ptr->pose.pose.orientation =
+ autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0);
smart_pose_buffer.push_back(old_pose_ptr);
// second data
@@ -66,7 +68,8 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT
new_pose_ptr->pose.pose.position.x = 20.0;
new_pose_ptr->pose.pose.position.y = 40.0;
new_pose_ptr->pose.pose.position.z = 0.0;
- new_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 90.0);
+ new_pose_ptr->pose.pose.orientation =
+ autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0);
smart_pose_buffer.push_back(new_pose_ptr);
// interpolate
@@ -90,7 +93,7 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT
EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0);
EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0);
EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0);
- const auto rpy = get_rpy(result.interpolated_pose.pose.pose);
+ const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose);
EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6);
EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6);
EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6);
diff --git a/localization/localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp
similarity index 93%
rename from localization/localization_util/test/test_tpe.cpp
rename to localization/autoware_localization_util/test/test_tpe.cpp
index fd9afe8b2a75f..6cbe3c2a62571 100644
--- a/localization/localization_util/test/test_tpe.cpp
+++ b/localization/autoware_localization_util/test/test_tpe.cpp
@@ -12,10 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "localization_util/tree_structured_parzen_estimator.hpp"
+#include "autoware/localization_util/tree_structured_parzen_estimator.hpp"
#include
+using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator;
+
TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function)
{
auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) {
diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp
index 8e7bf78845674..9b140977f4971 100644
--- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp
+++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp
@@ -15,10 +15,10 @@
#ifndef AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
#define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
+#include "autoware/localization_util/diagnostics_module.hpp"
+#include "autoware/localization_util/util_func.hpp"
#include "autoware/ndt_scan_matcher/hyper_parameters.hpp"
#include "autoware/ndt_scan_matcher/particle.hpp"
-#include "localization_util/diagnostics_module.hpp"
-#include "localization_util/util_func.hpp"
#include
#include
@@ -42,6 +42,7 @@
namespace autoware::ndt_scan_matcher
{
+using DiagnosticsModule = autoware::localization_util::DiagnosticsModule;
class MapUpdateModule
{
diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp
index b4062535503c1..c47af70f4c189 100644
--- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp
+++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -17,10 +17,10 @@
#define FMT_HEADER_ONLY
+#include "autoware/localization_util/diagnostics_module.hpp"
+#include "autoware/localization_util/smart_pose_buffer.hpp"
#include "autoware/ndt_scan_matcher/hyper_parameters.hpp"
#include "autoware/ndt_scan_matcher/map_update_module.hpp"
-#include "localization_util/diagnostics_module.hpp"
-#include "localization_util/smart_pose_buffer.hpp"
#include
#include
@@ -202,13 +202,13 @@ class NDTScanMatcher : public rclcpp::Node
Eigen::Matrix4f base_to_sensor_matrix_;
std::mutex ndt_ptr_mtx_;
- std::unique_ptr initial_pose_buffer_;
+ std::unique_ptr initial_pose_buffer_;
// Keep latest position for dynamic map loading
std::mutex latest_ekf_position_mtx_;
std::optional latest_ekf_position_ = std::nullopt;
- std::unique_ptr regularization_pose_buffer_;
+ std::unique_ptr regularization_pose_buffer_;
std::atomic is_activated_;
std::unique_ptr diagnostics_scan_points_;
diff --git a/localization/autoware_ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml
index f27cd63ff523e..a4e5d71fa5c59 100644
--- a/localization/autoware_ndt_scan_matcher/package.xml
+++ b/localization/autoware_ndt_scan_matcher/package.xml
@@ -17,13 +17,13 @@
ament_cmake_auto
autoware_cmake
+ autoware_localization_util
autoware_map_msgs
autoware_universe_utils
diagnostic_msgs
fmt
geometry_msgs
libpcl-all-dev
- localization_util
nav_msgs
ndt_omp
pcl_conversions
diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
index e9619375dc60a..8cc397c4531b5 100644
--- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -14,10 +14,10 @@
#include "autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp"
+#include "autoware/localization_util/matrix_type.hpp"
+#include "autoware/localization_util/tree_structured_parzen_estimator.hpp"
+#include "autoware/localization_util/util_func.hpp"
#include "autoware/ndt_scan_matcher/particle.hpp"
-#include "localization_util/matrix_type.hpp"
-#include "localization_util/tree_structured_parzen_estimator.hpp"
-#include "localization_util/util_func.hpp"
#include
#include
@@ -39,6 +39,14 @@
namespace autoware::ndt_scan_matcher
{
+using autoware::localization_util::exchange_color_crc;
+using autoware::localization_util::matrix4f_to_pose;
+using autoware::localization_util::point_to_vector3d;
+using autoware::localization_util::pose_to_matrix4f;
+
+using autoware::localization_util::DiagnosticsModule;
+using autoware::localization_util::SmartPoseBuffer;
+using autoware::localization_util::TreeStructuredParzenEstimator;
tier4_debug_msgs::msg::Float32Stamped make_float32_stamped(
const builtin_interfaces::msg::Time & stamp, const float data)
@@ -593,8 +601,8 @@ bool NDTScanMatcher::callback_sensor_points_main(
}
// check distance_initial_to_result
- const auto distance_initial_to_result = static_cast(
- norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position));
+ const auto distance_initial_to_result = static_cast(autoware::localization_util::norm(
+ interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position));
diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result);
if (distance_initial_to_result > param_.validation.initial_to_result_distance_tolerance_m) {
std::stringstream message;
@@ -798,18 +806,18 @@ void NDTScanMatcher::publish_initial_to_result(
initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame;
initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped);
- const auto initial_to_result_distance =
- static_cast(norm(initial_pose_cov_msg.pose.pose.position, result_pose_msg.position));
+ const auto initial_to_result_distance = static_cast(autoware::localization_util::norm(
+ initial_pose_cov_msg.pose.pose.position, result_pose_msg.position));
initial_to_result_distance_pub_->publish(
make_float32_stamped(sensor_ros_time, initial_to_result_distance));
- const auto initial_to_result_distance_old =
- static_cast(norm(initial_pose_old_msg.pose.pose.position, result_pose_msg.position));
+ const auto initial_to_result_distance_old = static_cast(autoware::localization_util::norm(
+ initial_pose_old_msg.pose.pose.position, result_pose_msg.position));
initial_to_result_distance_old_pub_->publish(
make_float32_stamped(sensor_ros_time, initial_to_result_distance_old));
- const auto initial_to_result_distance_new =
- static_cast(norm(initial_pose_new_msg.pose.pose.position, result_pose_msg.position));
+ const auto initial_to_result_distance_new = static_cast(autoware::localization_util::norm(
+ initial_pose_new_msg.pose.pose.position, result_pose_msg.position));
initial_to_result_distance_new_pub_->publish(
make_float32_stamped(sensor_ros_time, initial_to_result_distance_new));
}
@@ -1010,7 +1018,8 @@ void NDTScanMatcher::service_ndt_align_main(
diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", true);
// transform pose_frame to map_frame
- auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t);
+ auto initial_pose_msg_in_map_frame =
+ autoware::localization_util::transform(req->pose_with_covariance, transform_s2t);
initial_pose_msg_in_map_frame.header.stamp = req->pose_with_covariance.header.stamp;
map_update_module_->update_map(
initial_pose_msg_in_map_frame.pose.pose.position, diagnostics_ndt_align_);
@@ -1062,10 +1071,11 @@ void NDTScanMatcher::service_ndt_align_main(
std::tuple NDTScanMatcher::align_pose(
const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov)
{
- output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov);
+ autoware::localization_util::output_pose_with_cov_to_log(
+ get_logger(), "align_pose_input", initial_pose_with_cov);
- const auto base_rpy = get_rpy(initial_pose_with_cov);
- const Eigen::Map covariance = {
+ const auto base_rpy = autoware::localization_util::get_rpy(initial_pose_with_cov);
+ const Eigen::Map covariance = {
initial_pose_with_cov.pose.covariance.data(), 6, 6};
const double stddev_x = std::sqrt(covariance(0, 0));
const double stddev_y = std::sqrt(covariance(1, 1));
@@ -1127,7 +1137,7 @@ std::tuple NDTScanMatcher
}
const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose);
- const geometry_msgs::msg::Vector3 rpy = get_rpy(pose);
+ const geometry_msgs::msg::Vector3 rpy = autoware::localization_util::get_rpy(pose);
TreeStructuredParzenEstimator::Input result(6);
result[0] = pose.position.x;
@@ -1154,7 +1164,8 @@ std::tuple NDTScanMatcher
result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame;
result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose;
- output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg);
+ autoware::localization_util::output_pose_with_cov_to_log(
+ get_logger(), "align_pose_output", result_pose_with_cov_msg);
diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score);
return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score);
diff --git a/localization/autoware_ndt_scan_matcher/src/particle.cpp b/localization/autoware_ndt_scan_matcher/src/particle.cpp
index 238e0ea14a65f..a5db875fc3ff7 100644
--- a/localization/autoware_ndt_scan_matcher/src/particle.cpp
+++ b/localization/autoware_ndt_scan_matcher/src/particle.cpp
@@ -14,10 +14,12 @@
#include "autoware/ndt_scan_matcher/particle.hpp"
-#include "localization_util/util_func.hpp"
+#include "autoware/localization_util/util_func.hpp"
namespace autoware::ndt_scan_matcher
{
+using autoware::localization_util::exchange_color_crc;
+
void push_debug_markers(
visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp,
const std::string & map_frame_, const Particle & particle, const size_t i)
diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml
index 67079c476ac8a..d1ffc4ca4aef5 100644
--- a/localization/autoware_pose_initializer/package.xml
+++ b/localization/autoware_pose_initializer/package.xml
@@ -19,13 +19,13 @@
ament_cmake
autoware_cmake
+ autoware_localization_util
autoware_map_height_fitter
autoware_motion_utils
autoware_universe_utils
component_interface_specs
component_interface_utils
geometry_msgs
- localization_util
rclcpp
rclcpp_components
std_srvs
diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp
index 777f92826de73..912c6ec9b57a2 100644
--- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp
+++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp
@@ -39,7 +39,8 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options)
output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance");
gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance");
- diagnostics_pose_reliable_ = std::make_unique(this, "pose_initializer_status");
+ diagnostics_pose_reliable_ = std::make_unique(
+ this, "pose_initializer_status");
if (declare_parameter("ekf_enabled")) {
ekf_localization_trigger_ = std::make_unique(this);
diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp
index e8fef885cf577..a44f7f70b4b43 100644
--- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp
+++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp
@@ -15,7 +15,7 @@
#ifndef POSE_INITIALIZER_CORE_HPP_
#define POSE_INITIALIZER_CORE_HPP_
-#include "localization_util/diagnostics_module.hpp"
+#include "autoware/localization_util/diagnostics_module.hpp"
#include
#include
@@ -59,7 +59,7 @@ class PoseInitializer : public rclcpp::Node
std::unique_ptr ekf_localization_trigger_;
std::unique_ptr ndt_localization_trigger_;
std::unique_ptr logger_configure_;
- std::unique_ptr diagnostics_pose_reliable_;
+ std::unique_ptr diagnostics_pose_reliable_;
double stop_check_duration_;
void change_node_trigger(bool flag, bool need_spin = false);
diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md
deleted file mode 100644
index a7daea33e0273..0000000000000
--- a/localization/localization_util/README.md
+++ /dev/null
@@ -1,5 +0,0 @@
-# localization_util
-
-`localization_util` is a localization utility package.
-
-This package does not have a node, it is just a library.