diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 4beecc2625fe3..bd424fb027684 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -26,16 +26,21 @@ find_package(glog REQUIRED) find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) -ament_auto_add_executable(ndt_scan_matcher +ament_auto_add_library(ndt_scan_matcher_node SHARED src/debug.cpp - src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp src/map_module.cpp src/map_update_module.cpp ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) +target_link_libraries(ndt_scan_matcher_node ${PCL_LIBRARIES} glog::glog) + +rclcpp_components_register_node(ndt_scan_matcher_node + PLUGIN "NDTScanMatcher" + EXECUTABLE ndt_scan_matcher + EXECUTOR MultiThreadedExecutor +) if(BUILD_TESTING) add_launch_test( 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 f0d90fdfa0e08..fd74e6568622a 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 @@ -77,7 +77,7 @@ class NDTScanMatcher : public rclcpp::Node pclomp::MultiGridNormalDistributionsTransform; public: - NDTScanMatcher(); + explicit NDTScanMatcher(const rclcpp::NodeOptions & node_options); private: void service_ndt_align( diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index dcfdd77cb5a18..2ba8c70b8b1cd 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -25,6 +25,7 @@ ndt_omp pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs std_srvs 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 a2e89540a172f..3366cb8ae2016 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -108,8 +108,8 @@ bool validate_local_optimal_solution_oscillation( } // cspell: ignore degrounded -NDTScanMatcher::NDTScanMatcher() -: Node("ndt_scan_matcher"), +NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & node_options) +: Node("ndt_scan_matcher", node_options), tf2_broadcaster_(*this), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), @@ -1009,3 +1009,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( return result_pose_with_cov_msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp deleted file mode 100644 index d5ea544d3c5e5..0000000000000 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - - rclcpp::init(argc, argv); - auto ndt_scan_matcher = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(ndt_scan_matcher); - exec.spin(); - rclcpp::shutdown(); - return 0; -}