Merge branch 'ros2-lynx-devel' into ros2-ugv-description

README.md

@@ -104,7 +104,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | ✅   | ✅   | `localization_mode`          | Specifies the localization mode:  <br/>- 'relative' `odometry/filtered` data is relative to the initial position and orientation. <br/>- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation. <br/> ***string:*** `relative` (choices: `relative`, `enu`)           |
 | ✅   | ✅   | `namespace`                  | Add namespace to all launched nodes. <br/> ***string:*** `env(ROBOT_NAMESPACE)`                                                                                                                                                                                                                                    |
 | ✅   | ✅   | `publish_robot_state`        | Whether to publish the default URDF of specified robot. <br/> ***bool:*** `True`                                                                                                                                                                                                                                   |
-| ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL)` (choices: `lynx`, `panther`)                                                                                                                                                                                                                      |
+| ❌   | ✅   | `robot_model`                | Specify robot model type. <br/> ***string:*** `env(ROBOT_MODEL_NAME)` (choices: `lynx`, `panther`)                                                                                                                                                                                                                      |
 | ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`SafetyBT.btproj`](./husarion_ugv_manager/behavior_trees/SafetyBT.btproj)                                                                                                                    |
 | ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./husarion_ugv_manager/config/shutdown_hosts.yaml)                                                                                                                                                                |
 | ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |

husarion_ugv/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package husarion_ugv
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.2 (2024-12-02)
+------------------
+* Merge branch 'ros2-devel' into ros2-lights-tests
+* Contributors: pawelirh
+
 2.1.1 (2024-09-05)
 ------------------
 * Merge branch 'ros2-devel' into ros2-ns-refactor

husarion_ugv/hardware_deps.repos

@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: ros2-lynx-devel
+    version: ros2-devel
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git

husarion_ugv/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>husarion_ugv</name>
-  <version>2.1.1</version>
+  <version>2.1.2</version>
   <description>Meta package that contains all packages of Husarion UGV (Unmanned Ground Vehicle)</description>
   <maintainer email="[email protected]">Husarion</maintainer>
   <license>Apache License 2.0</license>

husarion_ugv/simulation_deps.repos

@@ -10,7 +10,7 @@ repositories:
   panther_msgs:
     type: git
     url: https://github.com/husarion/panther_msgs.git
-    version: ros2-lynx-devel
+    version: ros2-devel
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git

husarion_ugv_battery/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package husarion_ugv_battery
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.2 (2024-12-02)
+------------------
+* Merge branch 'ros2-devel' into ros2-lights-tests
+* Contributors: pawelirh
+
 2.1.1 (2024-09-05)
 ------------------
 * LEDStrip plugin to Gazebo (`#391 <https://github.com/husarion/panther_ros/issues/391>`_)

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
@@ -29,11 +33,11 @@ Publishes battery state read from ADC unit.
 
 #### Parameters
 
-- `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device.
-- `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device.
-- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings.
-- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings.
+- `~/adc.device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device.
+- `~/adc.device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device.
+- `~/adc.ma_window_len.charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings.
+- `~/adc.ma_window_len.temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings.
 - `~/battery_timeout` [*float*, default: **1.0**]: Specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state.
-- `~/ma_window_len/voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings.
-- `~/ma_window_len/current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings.
-- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old (deprecated).
+- `~/ma_window_len.voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings.
+- `~/ma_window_len.current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings.
+- `~/roboteq.driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old (deprecated).

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/adc_battery.hpp

@@ -77,7 +77,12 @@ class ADCBattery : public Battery
   static constexpr float kR1 = 10000.0;
   static constexpr float kR0 = 10000.0;
   static constexpr float kUSupply = 3.28;
-  static constexpr float kKelvinToCelciusOffset = 273.15;
+  static constexpr float kKelvinToCelsiusOffset = 273.15;
+
+  // Threshold used for diagnostics. At this voltage level, and below, the battery shall draw a
+  // significant current from the charger if the charger is connected. If not, the charging circuit
+  // may be broken.
+  static constexpr float kBatteryCCCheckTresh = 41.2;
 
   float voltage_raw_;
   float current_raw_;

husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp

@@ -57,14 +57,14 @@ class Battery
 
   float GetBatteryPercent(const float voltage) const
   {
-    for (int i = 0; i < 4; i++) {
+    for (int i = 0; i < 5; i++) {
       if (voltage > battery_approx_ranges[i]) {
         return std::clamp(
           (battery_approx_a_values[i] * voltage + battery_approx_b_values[i]) / 100, 0.0f, 1.0f);
       }
     }
     return std::clamp(
-      (battery_approx_a_values[4] * voltage + battery_approx_b_values[4]) / 100, 0.0f, 1.0f);
+      (battery_approx_a_values[5] * voltage + battery_approx_b_values[5]) / 100, 0.0f, 1.0f);
   }
 
   void ResetBatteryMsgs(const rclcpp::Time & header_stamp)
@@ -117,9 +117,10 @@ class Battery
   static constexpr float kDesignedCapacity = 20.0;
   static constexpr std::string_view kLocation = "user_compartment";
 
-  static constexpr float battery_approx_ranges[4] = {41.25, 37, 35, 33.7};
-  static constexpr float battery_approx_a_values[5] = {1.733, 9.153, 19.8, 10.538, 0.989};
-  static constexpr float battery_approx_b_values[5] = {27.214, -278.861, -672.6, -351.47, -29.67};
+  static constexpr float battery_approx_ranges[5] = {41.25, 37.0, 36.0, 35.0, 33.7};
+  static constexpr float battery_approx_a_values[6] = {8.665, 9.153, 19.8, 22.84, 10.538, 0.989};
+  static constexpr float battery_approx_b_values[6] = {-258.73, -278.861, -672.6,
+                                                       -782.04, -351.47,  -29.669};
 
   std::string error_msg_;
   BatteryStateMsg battery_state_;

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

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>husarion_ugv_battery</name>
-  <version>2.1.1</version>
+  <version>2.1.2</version>
   <description>Nodes monitoring the battery state of Husarion Panhter robot</description>
   <maintainer email="[email protected]">Husarion</maintainer>
   <license>Apache License 2.0</license>
@@ -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/adc_battery.cpp

@@ -107,7 +107,7 @@ float ADCBattery::ADCToBatteryTemp(const float adc_data) const
 
   const float R_therm = (adc_data * kR1) / (kUSupply - adc_data);
   return (kTempCoeffA * kTempCoeffB / (kTempCoeffA * logf(R_therm / kR0) + kTempCoeffB)) -
-         kKelvinToCelciusOffset;
+         kKelvinToCelsiusOffset;
 }
 
 void ADCBattery::UpdateBatteryMsgs(const rclcpp::Time & header_stamp, const bool charger_connected)
@@ -167,7 +167,7 @@ std::uint8_t ADCBattery::GetBatteryStatus(const float charge, const bool charger
   if (charger_connected) {
     if (fabs(battery_state_.percentage - 1.0f) < std::numeric_limits<float>::epsilon()) {
       return BatteryStateMsg::POWER_SUPPLY_STATUS_FULL;
-    } else if (charge > kChargingCurrentTresh) {
+    } else if (charge > kChargingCurrentTresh || battery_state_.voltage > kBatteryCCCheckTresh) {
       return BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING;
     } else {
       return BatteryStateMsg::POWER_SUPPLY_STATUS_NOT_CHARGING;