Generate parameters for IMU Sensor Broadcaster

Signed-off-by: Tyler Weaver <tyler@picknik.ai>
ros-controls · Jul 29, 2022 · 63f1abd · 63f1abd
1 parent 4504e8d
commit 63f1abd
Show file tree

Hide file tree

Showing 7 changed files with 44 additions and 12 deletions.
diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt
@@ -13,19 +13,24 @@ endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
     controller_interface
-    sensor_msgs
+    generate_parameter_library
     hardware_interface
     pluginlib
     rclcpp
     rclcpp_lifecycle
     realtime_tools
+    sensor_msgs
 )
 
 find_package(ament_cmake REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+generate_parameter_library(imu_sensor_broadcaster_parameters
+  src/imu_sensor_broadcaster_parameters.yaml
+)
+
 add_library(
   ${PROJECT_NAME}
   SHARED
@@ -40,6 +45,9 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL")
+target_link_libraries(imu_sensor_broadcaster
+  imu_sensor_broadcaster_parameters
+)
 
 pluginlib_export_plugin_description_file(
   controller_interface imu_sensor_broadcaster.xml)

diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp
@@ -25,6 +25,7 @@
 
 #include "controller_interface/controller_interface.hpp"
 #include "imu_sensor_broadcaster/visibility_control.h"
+#include "imu_sensor_broadcaster_parameters.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_publisher.h"
@@ -61,8 +62,8 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 protected:
-  std::string sensor_name_;
-  std::string frame_id_;
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
 
   std::unique_ptr<semantic_components::IMUSensor> imu_sensor_;
 

diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>controller_interface</depend>
+  <depend>generate_parameter_library</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp
@@ -27,8 +27,8 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init()
 {
   try
   {
-    auto_declare<std::string>("sensor_name", "");
-    auto_declare<std::string>("frame_id", "");
+    param_listener_ = std::make_shared<ParamListener>(get_node());
+    params_ = param_listener_->get_params();
   }
   catch (const std::exception & e)
   {
@@ -43,22 +43,27 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init()
 controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  sensor_name_ = get_node()->get_parameter("sensor_name").as_string();
-  if (sensor_name_.empty())
+  if (!param_listener_)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
+    return CallbackReturn::ERROR;
+  }
+
+  params_ = param_listener_->get_params();
+  if (params_.sensor_name.empty())
   {
     RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified.");
     return CallbackReturn::ERROR;
   }
 
-  frame_id_ = get_node()->get_parameter("frame_id").as_string();
-  if (frame_id_.empty())
+  if (params_.frame_id.empty())
   {
     RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided.");
     return CallbackReturn::ERROR;
   }
 
-  imu_sensor_ =
-    std::make_unique<semantic_components::IMUSensor>(semantic_components::IMUSensor(sensor_name_));
+  imu_sensor_ = std::make_unique<semantic_components::IMUSensor>(
+    semantic_components::IMUSensor(params_.sensor_name));
   try
   {
     // register ft sensor data publisher
@@ -75,7 +80,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
   }
 
   realtime_publisher_->lock();
-  realtime_publisher_->msg_.header.frame_id = frame_id_;
+  realtime_publisher_->msg_.header.frame_id = params_.frame_id;
   realtime_publisher_->unlock();
 
   RCLCPP_DEBUG(get_node()->get_logger(), "configure successful");

diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml
@@ -0,0 +1,9 @@
+imu_sensor_broadcaster:
+  sensor_name: {
+    type: string,
+    default_value: "",
+  }
+  frame_id: {
+    type: string,
+    default_value: "",
+  }
diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos
@@ -1 +1,5 @@
 repositories:
+  generate_parameter_library:
+    type: git
+    url: https://github.com/picknikrobotics/generate_parameter_library.git
+    version: main
diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos
@@ -15,3 +15,7 @@ repositories:
     type: git
     url: https://github.com/ros/angles.git
     version: ros2
+  generate_parameter_library:
+    type: git
+    url: https://github.com/picknikrobotics/generate_parameter_library.git
+    version: main