From d4a87e54d4d64777b7aa0b30511ac67311812efc Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Mon, 10 Feb 2025 15:18:08 -0600 Subject: [PATCH 1/3] Changes per https://github.com/ipa320/ros_battery_monitoring/issues/2 Added parameters generation for better handling of dynamic values and State interfaces --- battery_state_broadcaster/CMakeLists.txt | 11 +++ .../BatterySensor.hpp | 43 +++++++++-- .../BatteryStateBroadcaster.hpp | 12 ++- battery_state_broadcaster/package.xml | 3 +- .../src/BatteryStateBroadcaster.cpp | 76 ++++++++++++------- .../battery_state_broadcaster_parameters.yaml | 57 ++++++++++++++ 6 files changed, 164 insertions(+), 38 deletions(-) create mode 100644 battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt index 90c8f1a..ee5f447 100644 --- a/battery_state_broadcaster/CMakeLists.txt +++ b/battery_state_broadcaster/CMakeLists.txt @@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# add_compile_options(-v) + set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface pluginlib @@ -14,15 +16,23 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS # find dependencies find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) + foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${dependency} REQUIRED) endforeach() +# Parameters Library ================================================ +set(TARGET ${PROJECT_NAME}_parameters) +generate_parameter_library(${TARGET} src/${TARGET}.yaml) + add_library(battery_state_broadcaster SHARED src/BatteryStateBroadcaster.cpp) target_include_directories(battery_state_broadcaster PUBLIC $ $ ) + +target_link_libraries(${PROJECT_NAME} PUBLIC battery_state_broadcaster_parameters) ament_target_dependencies(battery_state_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml) @@ -33,6 +43,7 @@ install( ) install( TARGETS + battery_state_broadcaster_parameters battery_state_broadcaster EXPORT export_battery_state_broadcaster RUNTIME DESTINATION bin diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp index adb5ef7..3b352c4 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp @@ -4,15 +4,23 @@ #include #include +// +// inspired by https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/include/semantic_components/battery_state.hpp +// + namespace battery_state_broadcaster { class BatterySensor : public semantic_components::SemanticComponentInterface { public: - explicit BatterySensor(const std::string& name) - : semantic_components::SemanticComponentInterface(name, 1) + explicit BatterySensor(const std::string& name, const std::vector & interfaces, rclcpp::Logger logger) + : semantic_components::SemanticComponentInterface(name, interfaces.size()) { - interface_names_.emplace_back(name_ + "/" + "voltage"); + for (const auto & interface : interfaces) { + std::string interface_name = name_ + "/" + interface; + interface_names_.emplace_back(interface_name); + RCLCPP_INFO(logger, "Interface '%s' configured", interface_name.c_str()); + } } virtual ~BatterySensor() = default; @@ -23,11 +31,32 @@ class BatterySensor : public semantic_components::SemanticComponentInterface(voltage_); - return true; + // seed the message with dynamic values - only from existing interfaces: + for (const auto & state_interface : state_interfaces_) { + const auto & name = state_interface.get().get_interface_name(); + const auto & value = state_interface.get().get_value(); + if (name == "voltage") { + message.voltage = value; + } else if (name == "temperature") { + message.temperature = value; + } else if (name == "charge") { + message.charge = value; + } else if (name == "current") { + message.current = value; + } else if (name == "capacity") { + message.capacity = value; + } else if (name == "percentage") { + message.percentage = value; + } else if (name == "power_supply_health") { + message.power_supply_health = static_cast(round(value)); + } else if (name == "power_supply_status") { + message.power_supply_status = static_cast(round(value)); + } else if (name == "present") { + message.present = static_cast(round(value)) == 0 ? false : true; + } + } } private: diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp index 2008cb8..1cb489f 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp @@ -5,6 +5,9 @@ #include #include +// auto-generated by generate_parameter_library +#include "battery_state_broadcaster/battery_state_broadcaster_parameters.hpp" + #include "BatterySensor.hpp" namespace battery_state_broadcaster @@ -27,8 +30,13 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: - rclcpp::Publisher::SharedPtr battery_state_pub_; std::unique_ptr battery_sensor_; - std::unique_ptr> realtime_publisher_; + std::shared_ptr param_listener_; + Params params_; + + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr battery_state_pub_; + std::unique_ptr realtime_publisher_; + }; } // namespace battery_state_broadcaster diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml index b2f2a4f..3fcf3cc 100644 --- a/battery_state_broadcaster/package.xml +++ b/battery_state_broadcaster/package.xml @@ -3,13 +3,14 @@ battery_state_broadcaster 1.0.1 - ROS2 Control boradcaster for battery state sensors. + ROS2 Control broadcaster for battery state sensors. Jonas Otto MIT ament_cmake controller_interface + generate_parameter_library pluginlib realtime_tools sensor_msgs diff --git a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp index dec9844..5886bb5 100644 --- a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp +++ b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp @@ -3,13 +3,24 @@ #include #include +// +// Some code from https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/src/battery_state_broadcaster.cpp +// + namespace battery_state_broadcaster { controller_interface::CallbackReturn BatteryStateBroadcaster::on_init() { - get_node()->declare_parameter("sensor_name", "battery_state"); - get_node()->declare_parameter("power_supply_technology", -1); - get_node()->declare_parameter("design_capacity", 0.0); + try { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", + e.what()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; } @@ -18,35 +29,44 @@ BatteryStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_ { std::string sensor_name = get_node()->get_parameter("sensor_name").as_string(); - battery_sensor_ = std::make_unique(BatterySensor(sensor_name)); - - battery_state_pub_ = - get_node()->create_publisher("~/battery_state", rclcpp::SystemDefaultsQoS()); - realtime_publisher_ = - std::make_unique>(battery_state_pub_); + params_ = param_listener_->get_params(); + + battery_sensor_ = std::make_unique(BatterySensor(sensor_name, params_.state_interfaces, get_node()->get_logger())); + + try { + // register ft sensor data publisher + battery_state_pub_ = + get_node()->create_publisher("~/battery_state", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(battery_state_pub_); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return CallbackReturn::ERROR; + } - realtime_publisher_->msg_.temperature = std::numeric_limits::quiet_NaN(); - realtime_publisher_->msg_.current = std::numeric_limits::quiet_NaN(); - realtime_publisher_->msg_.charge = std::numeric_limits::quiet_NaN(); - realtime_publisher_->msg_.capacity = std::numeric_limits::quiet_NaN(); - realtime_publisher_->msg_.design_capacity = std::numeric_limits::quiet_NaN(); - realtime_publisher_->msg_.percentage = std::numeric_limits::quiet_NaN(); + realtime_publisher_->lock(); + + // seed the message with static values from parameters and defaults for dynamic members: + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->msg_.design_capacity = params_.design_capacity; + realtime_publisher_->msg_.voltage = std::numeric_limits::quiet_NaN(); + realtime_publisher_->msg_.temperature = std::numeric_limits::quiet_NaN(); + realtime_publisher_->msg_.charge = std::numeric_limits::quiet_NaN(); + realtime_publisher_->msg_.current = std::numeric_limits::quiet_NaN(); + realtime_publisher_->msg_.capacity = std::numeric_limits::quiet_NaN(); + realtime_publisher_->msg_.percentage = std::numeric_limits::quiet_NaN(); realtime_publisher_->msg_.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; realtime_publisher_->msg_.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; - realtime_publisher_->msg_.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; - realtime_publisher_->msg_.present = true; + realtime_publisher_->msg_.power_supply_technology = params_.power_supply_technology; + realtime_publisher_->msg_.present = false; + realtime_publisher_->msg_.location = params_.location; + realtime_publisher_->msg_.serial_number = params_.serial_number; - int64_t psu_tech = get_node()->get_parameter("power_supply_technology").as_int(); - if (psu_tech != -1) - { - realtime_publisher_->msg_.power_supply_technology = psu_tech; - } + realtime_publisher_->unlock(); - double design_capacity = get_node()->get_parameter("design_capacity").as_double(); - if (design_capacity != 0.0) - { - realtime_publisher_->msg_.design_capacity = static_cast(design_capacity); - } + RCLCPP_DEBUG(get_node()->get_logger(), "on_configure() successful"); return CallbackReturn::SUCCESS; } @@ -87,7 +107,7 @@ controller_interface::return_type BatteryStateBroadcaster::update(const rclcpp:: if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; - battery_sensor_->get_values_as_message(realtime_publisher_->msg_); + battery_sensor_->populate_message_from_interfaces(realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } diff --git a/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml b/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml new file mode 100644 index 0000000..304f248 --- /dev/null +++ b/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml @@ -0,0 +1,57 @@ +# +# inspired by https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml +# + +battery_state_broadcaster: + + # the following parameters can be defined in controllers.yaml: + sensor_name: + type: string + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined." + default_value: "battery_sensor" + frame_id: + type: string + description: "Sensor's frame_id in which values are published." + default_value: "base_link" + power_supply_technology: + type: int + description: "The battery chemistry." + default_value: 0 + validation: + bounds<>: [0, 6] + design_capacity: + type: double + description: "Capacity in in Ah (design capacity) (If unmeasured NaN)" + default_value: .NAN + location: + type: string + description: "The location into which the battery is inserted. (slot number or plug)" + default_value: "" + serial_number: + type: string + description: "The best approximation of the battery serial number" + default_value: "" + + # the Base driver can report dynamic parameters: + state_interfaces: + type: string_array + default_value: ["voltage"] # voltage is mandatory + validation: + not_empty<>: [] + size_lt<>: 10 + subset_of<>: + [ + [ + # Mandatory: + "voltage", + # Optional: + "temperature", + "current", + "charge", + "capacity", + "percentage", + "power_supply_status", + "power_supply_health", + "present" + ], + ] From 8e4a3890706b694a43d1f7c2643e80f1ccebf898 Mon Sep 17 00:00:00 2001 From: Jonas Otto Date: Tue, 4 Mar 2025 12:01:38 +0100 Subject: [PATCH 2/3] formatting, cleanup --- battery_state_broadcaster/CMakeLists.txt | 4 +- .../BatterySensor.hpp | 57 ++++++++++++------- .../src/BatteryStateBroadcaster.cpp | 30 +++++----- .../battery_state_broadcaster_parameters.yaml | 8 +-- 4 files changed, 58 insertions(+), 41 deletions(-) diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt index ee5f447..75b653a 100644 --- a/battery_state_broadcaster/CMakeLists.txt +++ b/battery_state_broadcaster/CMakeLists.txt @@ -5,8 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# add_compile_options(-v) - set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface pluginlib @@ -24,7 +22,7 @@ endforeach() # Parameters Library ================================================ set(TARGET ${PROJECT_NAME}_parameters) -generate_parameter_library(${TARGET} src/${TARGET}.yaml) +generate_parameter_library(${TARGET} src/battery_state_broadcaster_parameters.yaml) add_library(battery_state_broadcaster SHARED src/BatteryStateBroadcaster.cpp) target_include_directories(battery_state_broadcaster PUBLIC diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp index 3b352c4..abeaea6 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp @@ -5,7 +5,8 @@ #include // -// inspired by https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/include/semantic_components/battery_state.hpp +// inspired by +// https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/include/semantic_components/battery_state.hpp // namespace battery_state_broadcaster @@ -13,13 +14,13 @@ namespace battery_state_broadcaster class BatterySensor : public semantic_components::SemanticComponentInterface { public: - explicit BatterySensor(const std::string& name, const std::vector & interfaces, rclcpp::Logger logger) + explicit BatterySensor(const std::string& name, const std::vector& interfaces) : semantic_components::SemanticComponentInterface(name, interfaces.size()) { - for (const auto & interface : interfaces) { + for (const auto& interface : interfaces) + { std::string interface_name = name_ + "/" + interface; interface_names_.emplace_back(interface_name); - RCLCPP_INFO(logger, "Interface '%s' configured", interface_name.c_str()); } } @@ -34,27 +35,45 @@ class BatterySensor : public semantic_components::SemanticComponentInterface(round(value)); - } else if (name == "power_supply_status") { - message.power_supply_status = static_cast(round(value)); - } else if (name == "present") { - message.present = static_cast(round(value)) == 0 ? false : true; + } + else if (name == "power_supply_health") + { + message.power_supply_health = static_cast(std::round(value)); + } + else if (name == "power_supply_status") + { + message.power_supply_status = static_cast(std::round(value)); + } + else if (name == "present") + { + message.present = static_cast(std::round(value)) == 0 ? false : true; } } } diff --git a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp index 5886bb5..7c6ef80 100644 --- a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp +++ b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp @@ -4,20 +4,22 @@ #include // -// Some code from https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/src/battery_state_broadcaster.cpp +// Some code from +// https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/src/battery_state_broadcaster.cpp // namespace battery_state_broadcaster { controller_interface::CallbackReturn BatteryStateBroadcaster::on_init() { - try { + try + { param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - } catch (const std::exception & e) { - RCLCPP_ERROR( - get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", - e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } @@ -31,18 +33,20 @@ BatteryStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_ params_ = param_listener_->get_params(); - battery_sensor_ = std::make_unique(BatterySensor(sensor_name, params_.state_interfaces, get_node()->get_logger())); + battery_sensor_ = std::make_unique(BatterySensor(sensor_name, params_.state_interfaces)); - try { + try + { // register ft sensor data publisher battery_state_pub_ = get_node()->create_publisher("~/battery_state", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(battery_state_pub_); - } catch (const std::exception & e) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Exception thrown during publisher creation at configure stage with message : %s \n", - e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(get_node()->get_logger(), + "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); return CallbackReturn::ERROR; } diff --git a/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml b/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml index 304f248..0e0dc98 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml +++ b/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml @@ -3,8 +3,6 @@ # battery_state_broadcaster: - - # the following parameters can be defined in controllers.yaml: sensor_name: type: string description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined." @@ -18,7 +16,6 @@ battery_state_broadcaster: description: "The battery chemistry." default_value: 0 validation: - bounds<>: [0, 6] design_capacity: type: double description: "Capacity in in Ah (design capacity) (If unmeasured NaN)" @@ -39,8 +36,7 @@ battery_state_broadcaster: validation: not_empty<>: [] size_lt<>: 10 - subset_of<>: - [ + subset_of<>: [ [ # Mandatory: "voltage", @@ -52,6 +48,6 @@ battery_state_broadcaster: "percentage", "power_supply_status", "power_supply_health", - "present" + "present", ], ] From 79fc74e9da1e3eef54443ba0773b502b28b1a746 Mon Sep 17 00:00:00 2001 From: Jonas Otto Date: Tue, 4 Mar 2025 12:01:54 +0100 Subject: [PATCH 3/3] set "battery present" true by default --- battery_state_broadcaster/src/BatteryStateBroadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp index 7c6ef80..955658c 100644 --- a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp +++ b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp @@ -64,7 +64,7 @@ BatteryStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_ realtime_publisher_->msg_.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; realtime_publisher_->msg_.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; realtime_publisher_->msg_.power_supply_technology = params_.power_supply_technology; - realtime_publisher_->msg_.present = false; + realtime_publisher_->msg_.present = true; realtime_publisher_->msg_.location = params_.location; realtime_publisher_->msg_.serial_number = params_.serial_number;