feat(pose_initializer): componentize PoseInitializer (autowarefoundat…

…ion#7134) * remove unusing main func file Signed-off-by: a-maumau <[email protected]> * mod to componentize and use glog Signed-off-by: a-maumau <[email protected]> * add log output both Signed-off-by: a-maumau <[email protected]> --------- Signed-off-by: a-maumau <[email protected]>
Interplai · Jun 3, 2024 · aa4ef7d · aa4ef7d
1 parent 573c8eb
commit aa4ef7d
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 35 deletions.
diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt
@@ -4,8 +4,7 @@ project(pose_initializer)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_executable(pose_initializer_node
-  src/pose_initializer/pose_initializer_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/pose_initializer/pose_initializer_core.cpp
   src/pose_initializer/gnss_module.cpp
   src/pose_initializer/ndt_module.cpp
@@ -15,6 +14,12 @@ ament_auto_add_executable(pose_initializer_node
   src/pose_initializer/ndt_localization_trigger_module.cpp
 )
 
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "PoseInitializer"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR MultiThreadedExecutor
+)
+
 if(BUILD_TESTING)
   function(add_testcase filepath)
     get_filename_component(filename ${filepath} NAME)
@@ -30,7 +35,8 @@ if(BUILD_TESTING)
   add_testcase(test/test_copy_vector_to_array.cpp)
 endif()
 
-ament_auto_package(INSTALL_TO_SHARE
+ament_auto_package(
+  INSTALL_TO_SHARE
   launch
   config
 )
diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml
@@ -9,7 +9,7 @@
   <arg name="sub_gnss_pose_cov" default="sub_gnss_pose_cov"/>
   <arg name="gnss_initial_pose_auto_fix_target" default="pointcloud_map"/>
 
-  <node pkg="pose_initializer" exec="pose_initializer_node" name="pose_initializer_node">
+  <node pkg="pose_initializer" exec="pose_initializer_node" output="both">
     <param from="$(var config_file)" allow_substs="true"/>
     <remap from="yabloc_align" to="/localization/pose_estimator/yabloc/initializer/yabloc_align_srv"/>
     <remap from="ndt_align" to="/localization/pose_estimator/ndt_align_srv"/>

diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml
@@ -25,6 +25,7 @@
   <depend>map_height_fitter</depend>
   <depend>motion_utils</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>std_srvs</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_localization_msgs</depend>

diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp
@@ -26,7 +26,8 @@
 #include <sstream>
 #include <vector>
 
-PoseInitializer::PoseInitializer() : Node("pose_initializer")
+PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options)
+: rclcpp::Node("pose_initializer", options)
 {
   const auto node = component_interface_utils::NodeAdaptor(this);
   group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -209,3 +210,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializer::get_gnss_pose()
   throw ServiceException(
     Initialize::Service::Response::ERROR_GNSS_SUPPORT, "GNSS is not supported.");
 }
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(PoseInitializer)
diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp
@@ -34,7 +34,7 @@ class NdtLocalizationTriggerModule;
 class PoseInitializer : public rclcpp::Node
 {
 public:
-  PoseInitializer();
+  explicit PoseInitializer(const rclcpp::NodeOptions & options);
   ~PoseInitializer();
 
 private:

diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp