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.