feat(imu_corrector): componentize ImuCorrector and GyroBiasEstimator (a…

…utowarefoundation#7129) * 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]> * change log output from screen to both Signed-off-by: a-maumau <[email protected]> * Update sensing/imu_corrector/CMakeLists.txt add namespace Co-authored-by: Yamato Ando <[email protected]> Signed-off-by: a-maumau <[email protected]> * Update sensing/imu_corrector/CMakeLists.txt add namespace Co-authored-by: Yamato Ando <[email protected]> --------- Signed-off-by: a-maumau <[email protected]> Co-authored-by: Yamato Ando <[email protected]>
shtokuda · Jun 7, 2024 · e6b56a4 · e6b56a4
1 parent 4aeb69f
commit e6b56a4
Show file tree

Hide file tree

Showing 9 changed files with 30 additions and 70 deletions.
diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt
@@ -4,27 +4,32 @@ project(imu_corrector)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(gyro_bias_estimation_module SHARED
-  src/gyro_bias_estimation_module.cpp
-)
-
-ament_auto_add_executable(imu_corrector
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/imu_corrector_core.cpp
-  src/imu_corrector_node.cpp
 )
 
-ament_auto_add_executable(gyro_bias_estimator
+ament_auto_add_library(gyro_bias_estimator SHARED
   src/gyro_bias_estimator.cpp
-  src/gyro_bias_estimator_node.cpp
+  src/gyro_bias_estimation_module.cpp
 )
 
-target_link_libraries(gyro_bias_estimator gyro_bias_estimation_module)
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "imu_corrector::ImuCorrector"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+rclcpp_components_register_node(gyro_bias_estimator
+  PLUGIN "imu_corrector::GyroBiasEstimator"
+  EXECUTABLE gyro_bias_estimator_node
+  EXECUTOR SingleThreadedExecutor
+)
 
 function(add_testcase filepath)
   get_filename_component(filename ${filepath} NAME)
   string(REGEX REPLACE ".cpp" "" test_name ${filename})
   ament_add_gmock(${test_name} ${filepath})
-  target_link_libraries("${test_name}" gyro_bias_estimation_module)
+  target_link_libraries("${test_name}" gyro_bias_estimator)
   ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
 endfunction()
 

diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
@@ -6,7 +6,7 @@
   <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
   <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
 
-  <node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
+  <node pkg="imu_corrector" exec="gyro_bias_estimator_node" output="both">
     <remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
     <remap from="~/input/odom" to="$(var input_odom)"/>
     <remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>

diff --git a/sensing/imu_corrector/launch/imu_corrector.launch.xml b/sensing/imu_corrector/launch/imu_corrector.launch.xml
@@ -4,7 +4,7 @@
   <arg name="output_topic" default="imu_data"/>
   <arg name="param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
 
-  <node pkg="imu_corrector" exec="imu_corrector" name="imu_corrector" output="screen">
+  <node pkg="imu_corrector" exec="imu_corrector_node" output="both">
     <remap from="input" to="$(var input_topic)"/>
     <remap from="output" to="$(var output_topic)"/>
     <param from="$(var param_file)"/>

diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp
@@ -23,8 +23,8 @@
 
 namespace imu_corrector
 {
-GyroBiasEstimator::GyroBiasEstimator()
-: Node("gyro_bias_validator"),
+GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options)
+: rclcpp::Node("gyro_bias_validator", options),
   gyro_bias_threshold_(declare_parameter<double>("gyro_bias_threshold")),
   angular_velocity_offset_x_(declare_parameter<double>("angular_velocity_offset_x")),
   angular_velocity_offset_y_(declare_parameter<double>("angular_velocity_offset_y")),
@@ -244,3 +244,6 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW
 }
 
 }  // namespace imu_corrector
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator)
diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp
@@ -42,7 +42,7 @@ class GyroBiasEstimator : public rclcpp::Node
   using Odometry = nav_msgs::msg::Odometry;
 
 public:
-  GyroBiasEstimator();
+  explicit GyroBiasEstimator(const rclcpp::NodeOptions & options);
 
 private:
   void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);

diff --git a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp
diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp
@@ -53,8 +53,9 @@ geometry_msgs::msg::Vector3 transformVector3(
 
 namespace imu_corrector
 {
-ImuCorrector::ImuCorrector()
-: Node("imu_corrector"), output_frame_(declare_parameter<std::string>("base_link", "base_link"))
+ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options)
+: rclcpp::Node("imu_corrector", options),
+  output_frame_(declare_parameter<std::string>("base_link", "base_link"))
 {
   transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
 
@@ -123,3 +124,6 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
 }
 
 }  // namespace imu_corrector
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector)
diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp
@@ -34,7 +34,7 @@ class ImuCorrector : public rclcpp::Node
   using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
 
 public:
-  ImuCorrector();
+  explicit ImuCorrector(const rclcpp::NodeOptions & options);
 
 private:
   void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);

diff --git a/sensing/imu_corrector/src/imu_corrector_node.cpp b/sensing/imu_corrector/src/imu_corrector_node.cpp