diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt index 90c8f1a..75b653a 100644 --- a/battery_state_broadcaster/CMakeLists.txt +++ b/battery_state_broadcaster/CMakeLists.txt @@ -14,15 +14,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/battery_state_broadcaster_parameters.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 +41,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..abeaea6 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp @@ -4,15 +4,24 @@ #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) + : 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); + } } virtual ~BatterySensor() = default; @@ -23,11 +32,50 @@ 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(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; + } + } } 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..955658c 100644 --- a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp +++ b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp @@ -3,13 +3,26 @@ #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 +31,46 @@ 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)); + params_ = param_listener_->get_params(); - battery_state_pub_ = - get_node()->create_publisher("~/battery_state", rclcpp::SystemDefaultsQoS()); - realtime_publisher_ = - std::make_unique>(battery_state_pub_); + battery_sensor_ = std::make_unique(BatterySensor(sensor_name, params_.state_interfaces)); - 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(); + 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_->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_.power_supply_technology = params_.power_supply_technology; realtime_publisher_->msg_.present = true; + 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 +111,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..0e0dc98 --- /dev/null +++ b/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml @@ -0,0 +1,53 @@ +# +# inspired by https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml +# + +battery_state_broadcaster: + 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: + 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", + ], + ]