diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index c2ede4da2f543..b7a70b8442458 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -35,7 +35,7 @@ link_directories(${PCL_LIBRARY_DIRS}) target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "NDTScanMatcher" + PLUGIN "autoware::ndt_scan_matcher::NDTScanMatcher" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR MultiThreadedExecutor ) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index 5374e598ba68f..8244bb7ab4b92 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -24,6 +24,9 @@ #include #include +namespace autoware::ndt_scan_matcher +{ + enum class ConvergedParamType { TRANSFORM_PROBABILITY = 0, NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 @@ -191,4 +194,6 @@ struct HyperParameters } }; +} // namespace autoware::ndt_scan_matcher + #endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index ddc5e32f782f7..24ecc17ff7af1 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -40,6 +40,9 @@ #include #include +namespace autoware::ndt_scan_matcher +{ + class MapUpdateModule { using PointSource = pcl::PointXYZ; @@ -95,4 +98,6 @@ class MapUpdateModule std::mutex last_update_position_mtx_; }; +} // namespace autoware::ndt_scan_matcher + #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index aa412fab317a2..eb49711fe9270 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -69,6 +69,9 @@ #include #include +namespace autoware::ndt_scan_matcher +{ + class NDTScanMatcher : public rclcpp::Node { using PointSource = pcl::PointXYZ; @@ -209,4 +212,6 @@ class NDTScanMatcher : public rclcpp::Node HyperParameters param_; }; +} // namespace autoware::ndt_scan_matcher + #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp index f1c05bfe98551..c5ecc7f2e9837 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp @@ -20,6 +20,9 @@ #include +namespace autoware::ndt_scan_matcher +{ + struct Particle { Particle( @@ -38,4 +41,6 @@ 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); +} // namespace autoware::ndt_scan_matcher + #endif // NDT_SCAN_MATCHER__PARTICLE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 012c72e6b3a46..611d15783ea38 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -14,6 +14,9 @@ #include "ndt_scan_matcher/map_update_module.hpp" +namespace autoware::ndt_scan_matcher +{ + MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr, HyperParameters::DynamicMapLoading param) @@ -312,3 +315,5 @@ void MapUpdateModule::publish_partial_pcd_map() loaded_pcd_pub_->publish(map_msg); } + +} // namespace autoware::ndt_scan_matcher diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index b59686c211b53..16f297e5a1d48 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -37,6 +37,9 @@ #include #include +namespace autoware::ndt_scan_matcher +{ + tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) { @@ -1111,5 +1114,7 @@ std::tuple NDTScanMatcher return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score); } +} // namespace autoware::ndt_scan_matcher + #include -RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::ndt_scan_matcher::NDTScanMatcher) diff --git a/localization/ndt_scan_matcher/src/particle.cpp b/localization/ndt_scan_matcher/src/particle.cpp index b30703c2761e8..fce6cdce3326c 100644 --- a/localization/ndt_scan_matcher/src/particle.cpp +++ b/localization/ndt_scan_matcher/src/particle.cpp @@ -15,6 +15,8 @@ #include "ndt_scan_matcher/particle.hpp" #include "localization_util/util_func.hpp" +namespace autoware::ndt_scan_matcher +{ void push_debug_markers( visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp, @@ -61,3 +63,5 @@ void push_debug_markers( marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); } + +} // namespace autoware::ndt_scan_matcher diff --git a/localization/ndt_scan_matcher/test/test_fixture.hpp b/localization/ndt_scan_matcher/test/test_fixture.hpp index aff227d4fcfc7..c14c0b2ffe087 100644 --- a/localization/ndt_scan_matcher/test/test_fixture.hpp +++ b/localization/ndt_scan_matcher/test/test_fixture.hpp @@ -53,7 +53,7 @@ class TestNDTScanMatcher : public ::testing::Test node_options.parameter_overrides().push_back(param); } } - node_ = std::make_shared(node_options); + node_ = std::make_shared(node_options); rcl_yaml_node_struct_fini(params_st); // prepare tf_static "base_link -> sensor_frame" @@ -79,7 +79,7 @@ class TestNDTScanMatcher : public ::testing::Test sensor_pcd_publisher_ = std::make_shared(); } - std::shared_ptr node_; + std::shared_ptr node_; std::shared_ptr tf_broadcaster_; std::shared_ptr pcd_loader_; std::shared_ptr initialpose_client_;