Merge pull request #438 from husarion/ros2-param-gen

Generate node parameters using generate_parameter_library

husarion_ugv_battery/CMakeLists.txt

@@ -5,8 +5,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs
-                         husarion_ugv_utils rclcpp sensor_msgs)
+set(PACKAGE_DEPENDENCIES
+    ament_cmake
+    diagnostic_updater
+    generate_parameter_library
+    panther_msgs
+    husarion_ugv_utils
+    rclcpp
+    sensor_msgs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
@@ -25,6 +31,9 @@ add_executable(
   src/battery_publisher/single_battery_publisher.cpp)
 ament_target_dependencies(battery_driver_node ${PACKAGE_DEPENDENCIES})
 
+generate_parameter_library(battery_parameters config/battery_parameters.yaml)
+target_link_libraries(battery_driver_node battery_parameters)
+
 install(TARGETS battery_driver_node DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
@@ -76,6 +85,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_publisher
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_single_battery_publisher
@@ -89,6 +100,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_single_battery_publisher
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_dual_battery_publisher
@@ -102,6 +115,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_dual_battery_publisher
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_battery_driver_node_adc_dual
@@ -118,6 +133,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_dual
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_dual
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_battery_driver_node_adc_single
@@ -134,6 +151,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_single
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_single
+                        battery_parameters)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_battery_driver_node_roboteq
@@ -150,6 +169,8 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_roboteq
                             ${PACKAGE_DEPENDENCIES})
+  target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_roboteq
+                        battery_parameters)
 
 endif()
 

husarion_ugv_battery/README.md

@@ -8,6 +8,10 @@ This package contains:
 
 - `battery.launch.py`: Responsible for activating battery node, which dealing with reading and publishing battery data.
 
+## Configuration Files
+
+- [`battery_parameters.yaml`](./config/battery_parameters.yaml): Defines parameters for `battery_driver_node`.
+
 ## ROS Nodes
 
 ### battery_driver_node

husarion_ugv_battery/config/battery_parameters.yaml

@@ -0,0 +1,52 @@
+battery:
+  adc:
+    device0:
+      type: string
+      default_value: "/sys/bus/iio/devices/iio:device0"
+      description: "Internal ADC0 IIO device."
+      validation: { not_empty<> }
+
+    device1:
+      type: string
+      default_value: "/sys/bus/iio/devices/iio:device1"
+      description: "Internal ADC1 IIO device."
+      validation: { not_empty<> }
+
+    ma_window_len:
+      charge:
+        type: int
+        default_value: 10
+        description: "Window length of a moving average, used to smooth out battery charge readings."
+        validation: { gt<>: 0 }
+
+      temp:
+        type: int
+        default_value: 10
+        description: "Window length of a moving average, used to smooth out battery temperature readings."
+        validation: { gt<>: 0 }
+
+  ma_window_len:
+    voltage:
+      type: int
+      default_value: 10
+      description: "Window length of a moving average, used to smooth out battery voltage readings."
+      validation: { gt<>: 0 }
+
+    current:
+      type: int
+      default_value: 10
+      description: "Window length of a moving average, used to smooth out battery current readings."
+      validation: { gt<>: 0 }
+
+  roboteq:
+    driver_state_timeout:
+      type: double
+      default_value: 0.2
+      description: "Timeout in seconds after which driver state messages will be considered old. Used as a fallback when ADC data is not available."
+      validation: { gt<>: 0.0 }
+
+  battery_timeout:
+    type: double
+    default_value: 1.0
+    description: "Timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state."
+    validation: { gt<>: 0.0 }

husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp

@@ -23,6 +23,8 @@
 
 #include "panther_msgs/msg/robot_driver_state.hpp"
 
+#include "battery_parameters.hpp"
+
 #include "husarion_ugv_battery/adc_data_reader.hpp"
 #include "husarion_ugv_battery/battery/battery.hpp"
 #include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp"
@@ -54,6 +56,9 @@ class BatteryDriverNode : public rclcpp::Node
   std::shared_ptr<Battery> battery_2_;
   std::shared_ptr<BatteryPublisher> battery_publisher_;
 
+  std::shared_ptr<battery::ParamListener> param_listener_;
+  battery::Params params_;
+
   rclcpp::Subscription<RobotDriverStateMsg>::SharedPtr driver_state_sub_;
   rclcpp::TimerBase::SharedPtr battery_pub_timer_;
 

husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp

@@ -37,7 +37,8 @@ class BatteryPublisher
 public:
   BatteryPublisher(
     const rclcpp::Node::SharedPtr & node,
-    const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater);
+    const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
+    const double battery_timeout);
 
   ~BatteryPublisher() {}
 

husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp

@@ -32,7 +32,8 @@ class DualBatteryPublisher : public BatteryPublisher
   DualBatteryPublisher(
     const rclcpp::Node::SharedPtr & node,
     const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-    const std::shared_ptr<Battery> & battery_1, const std::shared_ptr<Battery> & battery_2);
+    const double battery_timeout, const std::shared_ptr<Battery> & battery_1,
+    const std::shared_ptr<Battery> & battery_2);
 
   ~DualBatteryPublisher() {}
 

husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp

@@ -31,7 +31,7 @@ class SingleBatteryPublisher : public BatteryPublisher
   SingleBatteryPublisher(
     const rclcpp::Node::SharedPtr & node,
     const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-    const std::shared_ptr<Battery> & battery);
+    const double battery_timeout, const std::shared_ptr<Battery> & battery);
 
   ~SingleBatteryPublisher() {}
 

husarion_ugv_battery/package.xml

@@ -17,6 +17,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>diagnostic_updater</depend>
+  <depend>generate_parameter_library</depend>
   <depend>husarion_ugv_utils</depend>
   <depend>panther_msgs</depend>
   <depend>rclcpp</depend>

husarion_ugv_battery/src/battery_driver_node.cpp

@@ -41,8 +41,9 @@ BatteryDriverNode::BatteryDriverNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
-  this->declare_parameter<int>("ma_window_len/voltage", 10);
-  this->declare_parameter<int>("ma_window_len/current", 10);
+  this->param_listener_ =
+    std::make_shared<battery::ParamListener>(this->get_node_parameters_interface());
+  this->params_ = this->param_listener_->get_params();
 
   // Running at 10 Hz
   battery_pub_timer_ = this->create_wall_timer(
@@ -75,22 +76,17 @@ void BatteryDriverNode::InitializeWithADCBattery()
 {
   RCLCPP_DEBUG(this->get_logger(), "Initializing with ADC data.");
 
-  this->declare_parameter<std::string>("adc/device0", "/sys/bus/iio/devices/iio:device0");
-  this->declare_parameter<std::string>("adc/device1", "/sys/bus/iio/devices/iio:device1");
-  this->declare_parameter<int>("adc/ma_window_len/temp", 10);
-  this->declare_parameter<int>("adc/ma_window_len/charge", 10);
-
-  const std::string adc0_device_path = this->get_parameter("adc/device0").as_string();
-  const std::string adc1_device_path = this->get_parameter("adc/device1").as_string();
+  const std::string adc0_device_path = this->params_.adc.device0;
+  const std::string adc1_device_path = this->params_.adc.device1;
 
   adc0_reader_ = std::make_shared<ADCDataReader>(adc0_device_path);
   adc1_reader_ = std::make_shared<ADCDataReader>(adc1_device_path);
 
   const ADCBatteryParams battery_params = {
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
-    static_cast<std::size_t>(this->get_parameter("adc/ma_window_len/temp").as_int()),
-    static_cast<std::size_t>(this->get_parameter("adc/ma_window_len/charge").as_int()),
+    static_cast<std::size_t>(this->params_.ma_window_len.voltage),
+    static_cast<std::size_t>(this->params_.ma_window_len.current),
+    static_cast<std::size_t>(this->params_.adc.ma_window_len.temp),
+    static_cast<std::size_t>(this->params_.adc.ma_window_len.charge),
   };
 
   battery_2_ = std::make_shared<ADCBattery>(
@@ -106,7 +102,8 @@ void BatteryDriverNode::InitializeWithADCBattery()
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0),
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params);
     battery_publisher_ = std::make_shared<DualBatteryPublisher>(
-      this->shared_from_this(), diagnostic_updater_, battery_1_, battery_2_);
+      this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_,
+      battery_2_);
   } else {
     battery_2_.reset();
     battery_1_ = std::make_shared<ADCBattery>(
@@ -118,7 +115,7 @@ void BatteryDriverNode::InitializeWithADCBattery()
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0),
       std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params);
     battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
-      this->shared_from_this(), diagnostic_updater_, battery_1_);
+      this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_);
   }
 
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data.");
@@ -128,12 +125,10 @@ void BatteryDriverNode::InitializeWithRoboteqBattery()
 {
   RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data.");
 
-  this->declare_parameter<float>("roboteq/driver_state_timeout", 0.2);
-
   const RoboteqBatteryParams battery_params = {
-    static_cast<float>(this->get_parameter("roboteq/driver_state_timeout").as_double()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
+    static_cast<float>(this->params_.roboteq.driver_state_timeout),
+    static_cast<std::size_t>(this->params_.ma_window_len.voltage),
+    static_cast<std::size_t>(this->params_.ma_window_len.current),
   };
 
   driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
@@ -143,7 +138,7 @@ void BatteryDriverNode::InitializeWithRoboteqBattery()
   battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
 
   battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
-    this->shared_from_this(), diagnostic_updater_, battery_1_);
+    this->shared_from_this(), diagnostic_updater_, this->params_.battery_timeout, battery_1_);
 
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data.");
 }

husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp

@@ -26,12 +26,12 @@ namespace husarion_ugv_battery
 
 BatteryPublisher::BatteryPublisher(
   const rclcpp::Node::SharedPtr & node,
-  const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater)
-: node_(std::move(node)), diagnostic_updater_(std::move(diagnostic_updater))
+  const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
+  const double battery_timeout)
+: node_(std::move(node)),
+  diagnostic_updater_(std::move(diagnostic_updater)),
+  battery_timeout_(battery_timeout)
 {
-  node->declare_parameter<float>("battery_timeout", 1.0);
-  battery_timeout_ = node->get_parameter("battery_timeout").as_double();
-
   charger_connected_ = false;
   last_battery_info_time_ = rclcpp::Time(std::int64_t(0), RCL_ROS_TIME);
 

husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp

@@ -34,8 +34,9 @@ namespace husarion_ugv_battery
 DualBatteryPublisher::DualBatteryPublisher(
   const rclcpp::Node::SharedPtr & node,
   const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-  const std::shared_ptr<Battery> & battery_1, const std::shared_ptr<Battery> & battery_2)
-: BatteryPublisher(std::move(node), std::move(diagnostic_updater)),
+  const double battery_timeout, const std::shared_ptr<Battery> & battery_1,
+  const std::shared_ptr<Battery> & battery_2)
+: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout),
   battery_1_(std::move(battery_1)),
   battery_2_(std::move(battery_2))
 {

husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp

@@ -32,8 +32,9 @@ namespace husarion_ugv_battery
 SingleBatteryPublisher::SingleBatteryPublisher(
   const rclcpp::Node::SharedPtr & node,
   const std::shared_ptr<diagnostic_updater::Updater> & diagnostic_updater,
-  const std::shared_ptr<Battery> & battery)
-: BatteryPublisher(std::move(node), std::move(diagnostic_updater)), battery_(std::move(battery))
+  const double battery_timeout, const std::shared_ptr<Battery> & battery)
+: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout),
+  battery_(std::move(battery))
 {
   battery_pub_ = node->create_publisher<BatteryStateMsg>("battery/battery_status", 5);
   battery_1_pub_ = node->create_publisher<BatteryStateMsg>("_battery/battery_1_status_raw", 5);

husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp

@@ -35,7 +35,7 @@ class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher
   BatteryPublisherWrapper(
     const rclcpp::Node::SharedPtr & node,
     std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater)
-  : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater)
+  : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater, kBatteryTimeout)
   {
   }
 
@@ -62,6 +62,9 @@ class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher
   {
     status.summary(0, "");  // Avoid unused parameter compiler warning
   };
+
+protected:
+  static constexpr double kBatteryTimeout = 0.2;
 };
 
 class TestBatteryPublisher : public testing::Test

husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp

@@ -40,7 +40,7 @@ class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPubl
     std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater,
     std::shared_ptr<husarion_ugv_battery::Battery> & battery_1,
     std::shared_ptr<husarion_ugv_battery::Battery> & battery_2)
-  : DualBatteryPublisher(node, diagnostic_updater, battery_1, battery_2)
+  : DualBatteryPublisher(node, diagnostic_updater, kBatteryTimeout, battery_1, battery_2)
   {
   }
 
@@ -71,6 +71,9 @@ class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPubl
     return DualBatteryPublisher::MergeChargingStatusMsgs(
       charging_status_msg_1, charging_status_msg_2);
   }
+
+private:
+  static constexpr double kBatteryTimeout = 0.2;
 };
 
 class TestDualBatteryPublisher : public testing::Test

husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp

@@ -44,6 +44,8 @@ class TestSingleBatteryPublisher : public testing::Test
   std::shared_ptr<husarion_ugv_battery::BatteryPublisher> battery_publisher_;
   BatteryStateMsg::SharedPtr battery_state_;
   BatteryStateMsg::SharedPtr battery_1_state_;
+
+  static constexpr double kBatteryTimeout = 0.2f;
 };
 
 TestSingleBatteryPublisher::TestSingleBatteryPublisher()
@@ -62,7 +64,7 @@ TestSingleBatteryPublisher::TestSingleBatteryPublisher()
     "/_battery/battery_1_status_raw", 10,
     [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
   battery_publisher_ = std::make_shared<husarion_ugv_battery::SingleBatteryPublisher>(
-    node_, diagnostic_updater_, battery_);
+    node_, diagnostic_updater_, kBatteryTimeout, battery_);
 }
 
 TEST_F(TestSingleBatteryPublisher, CorrectTopicPublished)