diff --git a/battery_state_broadcaster/CHANGELOG.rst b/battery_state_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..ffc7e8f7bf --- /dev/null +++ b/battery_state_broadcaster/CHANGELOG.rst @@ -0,0 +1,24 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package battery_state_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2025-09-26) +------------------ +* address deprecations in ros2_control for kilted +* Don't make a temporary copy of semantic component (https://github.com/ipa320/ros_battery_monitoring/pull/9) +* Contributors: Christoph Froehlich, Jonas Otto + +1.0.2 (2025-06-01) +------------------ +* Replace ament_target_dependencies with target_link_libraries ([#6](https://github.com/ipa320/ros_battery_monitoring/issues/6)) +* Contributors: Alejandro Hernandez Cordero, Jonas Otto + +1.0.1 (2025-02-06) +------------------ +* fix realtime_tools include +* Contributors: Jonas Otto + +1.0.0 (2024-08-14) +------------------ +* initial release +* Contributors: Jonas Otto diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..3543845c70 --- /dev/null +++ b/battery_state_broadcaster/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(battery_state_broadcaster) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + pluginlib + realtime_tools + sensor_msgs +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_library(battery_state_broadcaster SHARED src/BatteryStateBroadcaster.cpp) +target_include_directories(battery_state_broadcaster PUBLIC + $ + $ +) +target_link_libraries(battery_state_broadcaster + PUBLIC + controller_interface::controller_interface + pluginlib::pluginlib + realtime_tools::realtime_tools + ${sensor_msgs_TARGETS} +) + +pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml) + +install( + DIRECTORY include/ + DESTINATION include/battery_state_broadcaster +) +install( + TARGETS + battery_state_broadcaster + EXPORT export_battery_state_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_battery_state_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_package() diff --git a/battery_state_broadcaster/battery_state_broadcaster.xml b/battery_state_broadcaster/battery_state_broadcaster.xml new file mode 100644 index 0000000000..e4d371b578 --- /dev/null +++ b/battery_state_broadcaster/battery_state_broadcaster.xml @@ -0,0 +1,8 @@ + + + + This controller publishes the readings of a battery sensor as sensor_msgs/BatteryState message. + + + \ No newline at end of file diff --git a/battery_state_broadcaster/doc/userdoc.rst b/battery_state_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..85c091c5d7 --- /dev/null +++ b/battery_state_broadcaster/doc/userdoc.rst @@ -0,0 +1,55 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/battery_state_broadcaster/doc/userdoc.rst + +.. _battery_state_broadcaster_userdoc: + +Battery State Broadcaster +========================= +This broadcaster publishes `sensor_msgs/BatteryState `__ messages from appropriate state interfaces. + + +Required State Interfaces +--------------------------------- +This broadcaster requires the robot to have a sensor component which contains the battery state interfaces: + +.. code-block:: xml + + + + + + + + + + +Parameters +--------------------------------- +To use this broadcaster, declare it in the controller manager and set its parameters: + +.. code-block:: yaml + + controller_manager: + ros__parameters: + battery_state_broadcaster: + type: battery_state_broadcaster/BatteryStateBroadcaster + + battery_state_broadcaster: + ros__parameters: + sensor_name: "battery_state" + design_capacity: 100.0 + # https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg + power_supply_technology: 2 + +And spawn it in the launch file: + +.. code-block:: python + + battery_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["battery_state_broadcaster"] + +Topics +--------------------------------- +The battery state is published on ``~/battery_state``. +Since it's a plugin within the controller manager, add a remapping of the form ``("~/battery_state", "/my_battery_state")`` to the *controller manager*, not the spawner, to change the topic name. diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp new file mode 100644 index 0000000000..eb3330c172 --- /dev/null +++ b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp @@ -0,0 +1,36 @@ + +#include +#include +#include +#include + +namespace battery_state_broadcaster +{ +class BatterySensor : public semantic_components::SemanticComponentInterface +{ +public: + explicit BatterySensor(const std::string& name) + : semantic_components::SemanticComponentInterface(name, 1) + { + interface_names_.emplace_back(name_ + "/" + "voltage"); + } + + virtual ~BatterySensor() = default; + + double get_voltage() + { + voltage_ = state_interfaces_[0].get().get_optional().value(); + return voltage_; + } + + bool get_values_as_message(sensor_msgs::msg::BatteryState& message) + { + get_voltage(); + message.voltage = static_cast(voltage_); + return true; + } + +private: + double voltage_ = 0.0; +}; +} // namespace battery_state_broadcaster diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp new file mode 100644 index 0000000000..16fb6a8022 --- /dev/null +++ b/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include +#include + +#include "BatterySensor.hpp" + +namespace battery_state_broadcaster +{ +class BatteryStateBroadcaster : public controller_interface::ControllerInterface +{ +public: + [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + 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_; + sensor_msgs::msg::BatteryState msg_; +}; +} // namespace battery_state_broadcaster diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml new file mode 100644 index 0000000000..fae83a820b --- /dev/null +++ b/battery_state_broadcaster/package.xml @@ -0,0 +1,20 @@ + + + + battery_state_broadcaster + 1.1.0 + ROS2 Control boradcaster for battery state sensors. + Jonas Otto + MIT + + ament_cmake + + controller_interface + pluginlib + realtime_tools + sensor_msgs + + + ament_cmake + + diff --git a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp new file mode 100644 index 0000000000..f6432b3960 --- /dev/null +++ b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp @@ -0,0 +1,101 @@ +#include "battery_state_broadcaster/BatteryStateBroadcaster.hpp" +#include +#include +#include + +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); + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +BatteryStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) +{ + std::string sensor_name = get_node()->get_parameter("sensor_name").as_string(); + + battery_sensor_ = std::make_unique(sensor_name); + + battery_state_pub_ = + get_node()->create_publisher("~/battery_state", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = + std::make_unique>(battery_state_pub_); + + msg_.temperature = std::numeric_limits::quiet_NaN(); + msg_.current = std::numeric_limits::quiet_NaN(); + msg_.charge = std::numeric_limits::quiet_NaN(); + msg_.capacity = std::numeric_limits::quiet_NaN(); + msg_.design_capacity = std::numeric_limits::quiet_NaN(); + msg_.percentage = std::numeric_limits::quiet_NaN(); + msg_.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; + msg_.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; + msg_.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; + msg_.present = true; + + int64_t psu_tech = get_node()->get_parameter("power_supply_technology").as_int(); + if (psu_tech != -1) + { + msg_.power_supply_technology = psu_tech; + } + + double design_capacity = get_node()->get_parameter("design_capacity").as_double(); + if (design_capacity != 0.0) + { + msg_.design_capacity = static_cast(design_capacity); + } + + return CallbackReturn::SUCCESS; +} + +[[nodiscard]] controller_interface::InterfaceConfiguration +BatteryStateBroadcaster::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +[[nodiscard]] controller_interface::InterfaceConfiguration BatteryStateBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = battery_sensor_->get_state_interface_names(); + return state_interfaces_config; +} + +controller_interface::CallbackReturn +BatteryStateBroadcaster::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + battery_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +BatteryStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + battery_sensor_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type BatteryStateBroadcaster::update(const rclcpp::Time& time, + const rclcpp::Duration& /*period*/) +{ + if (realtime_publisher_) + { + msg_.header.stamp = time; + battery_sensor_->get_values_as_message(msg_); + realtime_publisher_->try_publish(msg_); + } + + return controller_interface::return_type::OK; +} + +} // namespace battery_state_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(battery_state_broadcaster::BatteryStateBroadcaster, controller_interface::ControllerInterface) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index b8e9f8a8ca..f87f3dadb5 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -66,6 +66,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> GPS Sensor Broadcaster <../gps_sensor_broadcaster/doc/userdoc.rst> State Interfaces Broadcaster <../state_interfaces_broadcaster/doc/userdoc.rst> + Battery State Broadcaster <../battery_state_broadcaster/doc/userdoc.rst> Filters **********************