Merge pull request #40 from robotology/battery_nws

battery_nws_ros2

src/devices/CMakeLists.txt

@@ -20,3 +20,4 @@ add_subdirectory(rgbdSensor_nwc_ros2)
 add_subdirectory(multipleAnalogSensors_nws_ros2)
 add_subdirectory(multipleAnalogSensors_nwc_ros2)
 add_subdirectory(rangefinder2D_controlBoard_nws_ros2)
+add_subdirectory(battery_nws_ros2)

src/devices/battery_nws_ros2/Battery_nws_ros2.cpp

@@ -0,0 +1,162 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#include "Battery_nws_ros2.h"
+#include <yarp/os/LogComponent.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Stamp.h>
+#include <Ros2Utils.h>
+
+YARP_LOG_COMPONENT(BATTERY_NWS_ROS2, "yarp.devices.Battery_nws_ros2")
+
+Battery_nws_ros2::Battery_nws_ros2() : yarp::os::PeriodicThread(DEFAULT_THREAD_PERIOD)
+{
+}
+
+Battery_nws_ros2::~Battery_nws_ros2()
+{
+    m_battery_interface = nullptr;
+}
+
+
+bool Battery_nws_ros2::attach(yarp::dev::PolyDriver* driver)
+{
+    if (driver->isValid())
+    {
+        driver->view(m_battery_interface);
+    } else {
+        yCError(BATTERY_NWS_ROS2) << "not valid driver";
+    }
+
+    if (m_battery_interface == nullptr)
+    {
+        yCError(BATTERY_NWS_ROS2, "Subdevice passed to attach method is invalid");
+        return false;
+    }
+    PeriodicThread::setPeriod(m_period);
+    return PeriodicThread::start();
+}
+
+
+bool Battery_nws_ros2::detach()
+{
+    if (PeriodicThread::isRunning())
+    {
+        PeriodicThread::stop();
+    }
+    m_battery_interface = nullptr;
+    return true;
+}
+
+bool Battery_nws_ros2::threadInit()
+{
+    return true;
+}
+
+bool Battery_nws_ros2::open(yarp::os::Searchable &config)
+{
+    if (!config.check("period")) {
+        yCWarning(BATTERY_NWS_ROS2) << "missing 'period' parameter, using default value of" << DEFAULT_THREAD_PERIOD;
+    } else {
+        m_period = config.find("period").asFloat64();
+    }
+
+    if (!config.check("node_name")) {
+        yCError(BATTERY_NWS_ROS2) << "missing node_name parameter";
+        return false;
+    }
+    m_nodeName = config.find("node_name").asString();
+    if (m_nodeName[0] == '/') {
+        yCError(BATTERY_NWS_ROS2) << "node_name parameter cannot begin with '/'";
+        return false;
+    }
+
+    if (!config.check("topic_name")) {
+        yCError(BATTERY_NWS_ROS2) << "missing topic_name parameter";
+        return false;
+    }
+    m_topicName = config.find("topic_name").asString();
+    if (m_topicName[0] != '/') {
+        yCError(BATTERY_NWS_ROS2) << "missing initial / in topic_name parameter";
+        return false;
+    }
+
+    rclcpp::NodeOptions node_options;
+    node_options.allow_undeclared_parameters(true);
+    node_options.automatically_declare_parameters_from_overrides(true);
+    m_node = NodeCreator::createNode(m_nodeName, node_options);
+    if (m_node == nullptr) {
+        yCError(BATTERY_NWS_ROS2) << " opening " << m_nodeName << " Node, check your yarp-ROS2 network configuration\n";
+        return false;
+    }
+
+    m_ros2Publisher = m_node->create_publisher<sensor_msgs::msg::BatteryState>(m_topicName, 10);
+    return true;
+}
+
+void Battery_nws_ros2::threadRelease()
+{
+}
+
+void Battery_nws_ros2::run()
+{
+    if (m_battery_interface==nullptr)
+    {
+        yCError(BATTERY_NWS_ROS2) << "the interface is not valid";
+    }
+    else if (!m_ros2Publisher)
+    {
+        yCError(BATTERY_NWS_ROS2) << "the publisher is not ready";
+    }
+    else
+    {
+        double voltage=0;
+        double current=0;
+        double charge=0;
+        double temperature=0;
+        yarp::dev::IBattery::Battery_status status;
+        std::string battery_info;
+        m_battery_interface->getBatteryVoltage(voltage);
+        m_battery_interface->getBatteryCurrent(current);
+        m_battery_interface->getBatteryCharge(charge);
+        m_battery_interface->getBatteryStatus(status);
+        m_battery_interface->getBatteryTemperature(temperature);
+        m_battery_interface->getBatteryInfo(battery_info);
+
+        m_timeStamp.update(yarp::os::Time::now());
+
+        battMsg.voltage = voltage;
+        battMsg.current = current;
+        battMsg.temperature = temperature;
+        battMsg.charge = std::numeric_limits<double>::quiet_NaN();//std::nan("");
+     //   battMsg.capacity = std::nan("");
+      //  battMsg.design_capacity = std::nan("");
+        battMsg.percentage = charge;
+        battMsg.power_supply_status = 0;
+        battMsg.power_supply_health = 0;
+        battMsg.power_supply_technology = 0;
+        battMsg.present=true;
+        battMsg.location="";
+        battMsg.serial_number="";
+
+        battMsg.header.frame_id = "" ;
+        battMsg.header.stamp.sec = int(m_timeStamp.getTime());
+        battMsg.header.stamp.nanosec = int(1000000000UL * (m_timeStamp.getTime() - int(m_timeStamp.getTime())));
+
+        m_ros2Publisher->publish(battMsg);
+    }
+}
+
+bool Battery_nws_ros2::close()
+{
+    yCTrace(BATTERY_NWS_ROS2);
+    if (PeriodicThread::isRunning())
+    {
+        PeriodicThread::stop();
+    }
+
+    detach();
+    return true;
+}

src/devices/battery_nws_ros2/Battery_nws_ros2.h

@@ -0,0 +1,81 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2022 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+
+#ifndef YARP_ROS2_BATTERY_NWS_ROS2_H
+#define YARP_ROS2_BATTERY_NWS_ROS2_H
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+
+#include <yarp/dev/IBattery.h>
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/os/Stamp.h>
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/WrapperSingle.h>
+
+#define DEFAULT_THREAD_PERIOD 0.02 //s
+
+/**
+ * @ingroup dev_impl_nws_ros2
+ *
+ * \section Battery_nws_ros2_parameters Device description
+ * \brief `Battery_nws_ros2`: A ros2 nws to get the status of a battery and publish it on a ros2 topic.
+ * The attached device must implement a `yarp::dev::IBattery` interface.
+ *
+ * Parameters required by this device are:
+ * | Parameter name      | SubParameter            | Type    | Units          | Default Value | Required                       | Description                                             | Notes |
+ * |:-------------------:|:-----------------------:|:-------:|:--------------:|:-------------:|:-----------------------------: |:-------------------------------------------------------:|:-----:|
+ * | period              |      -                  | double  | s              |   0.02        | No                             | refresh period of the broadcasted values in s           | default 0.02s |
+ * | node_name           |      -                  | string  | -              |   -           | Yes                            | name of the ros2 node                                   |      |
+ * | topic_name          |      -                  | string  | -              |   -           | Yes                            | name of the topic where the device must publish the data| must begin with an initial '/'     |
+ *
+ */
+
+
+class Battery_nws_ros2 :
+        public yarp::os::PeriodicThread,
+        public yarp::dev::DeviceDriver,
+        public yarp::dev::WrapperSingle
+{
+public:
+    Battery_nws_ros2();
+    ~Battery_nws_ros2();
+
+    // DeviceDriver
+    bool open(yarp::os::Searchable &params) override;
+    bool close() override;
+
+    // WrapperSingle
+    bool attach(yarp::dev::PolyDriver* driver) override;
+    bool detach() override;
+
+    // PeriodicThread
+    bool threadInit() override;
+    void threadRelease() override;
+    void run() override;
+
+
+private:
+    // parameters from configuration
+    std::string m_topicName;
+    std::string m_nodeName;
+
+    // stamp count for timestamp
+    yarp::os::Stamp m_timeStamp;
+
+    // period for thread
+    double m_period{DEFAULT_THREAD_PERIOD};
+
+    //ros2 node
+    sensor_msgs::msg::BatteryState  battMsg;
+    rclcpp::Node::SharedPtr m_node;
+    rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr m_ros2Publisher =nullptr;
+
+    //interfaces
+    yarp::dev::PolyDriver m_driver;
+    yarp::dev::IBattery *m_battery_interface{nullptr};
+};
+
+#endif // YARP_ROS2_BATTERY_NWS_ROS2_H

src/devices/battery_nws_ros2/CMakeLists.txt

@@ -0,0 +1,56 @@
+# SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+yarp_prepare_plugin(battery_nws_ros2
+  CATEGORY device
+  TYPE Battery_nws_ros2
+  INCLUDE Battery_nws_ros2.h
+  DEFAULT ON
+)
+
+if(NOT SKIP_battery_nws_ros2)
+  yarp_add_plugin(yarp_battery_nws_ros2)
+
+  target_sources(yarp_battery_nws_ros2
+    PRIVATE
+      Battery_nws_ros2.cpp
+      Battery_nws_ros2.h
+  )
+  target_sources(yarp_battery_nws_ros2 PRIVATE $<TARGET_OBJECTS:Ros2Utils>)
+
+  target_include_directories(yarp_battery_nws_ros2 PRIVATE $<TARGET_PROPERTY:Ros2Utils,INTERFACE_INCLUDE_DIRECTORIES>)
+  target_link_libraries(yarp_battery_nws_ros2
+    PRIVATE
+      YARP::YARP_os
+      YARP::YARP_sig
+      YARP::YARP_dev
+      rclcpp::rclcpp
+      sensor_msgs::sensor_msgs__rosidl_typesupport_cpp
+      Ros2Utils
+  )
+  list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS
+    YARP_os
+    YARP_sig
+    YARP_dev
+    rclcpp::rclcpp
+    sensor_msgs::sensor_msgs__rosidl_typesupport_cpp
+    Ros2Utils
+  )
+
+  yarp_install(
+    TARGETS yarp_battery_nws_ros2
+    EXPORT YARP_${YARP_PLUGIN_MASTER}
+    COMPONENT ${YARP_PLUGIN_MASTER}
+    LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+    ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
+    YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
+  )
+
+  set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE)
+
+  set_property(TARGET yarp_battery_nws_ros2 PROPERTY FOLDER "Plugins/Device/NWS")
+
+  if(YARP_COMPILE_TESTS)
+    add_subdirectory(tests)
+  endif()
+endif()

src/devices/battery_nws_ros2/tests/CMakeLists.txt

@@ -0,0 +1,4 @@
+# SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+create_device_test (battery_nws_ros2)

src/devices/battery_nws_ros2/tests/battery_nws_ros2_test.cpp

@@ -0,0 +1,80 @@
+/*
+ * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <yarp/os/Network.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/WrapperSingle.h>
+
+#include <catch2/catch_amalgamated.hpp>
+#include <harness.h>
+
+using namespace yarp::dev;
+using namespace yarp::os;
+
+TEST_CASE("dev::battery_nws_ros2_test", "[yarp::dev]")
+{
+    YARP_REQUIRE_PLUGIN("battery_nws_ros2", "device");
+    YARP_REQUIRE_PLUGIN("fakeBattery", "device");
+
+    Network::setLocalMode(true);
+
+    SECTION("Checking the nws alone")
+    {
+        PolyDriver ddnws;
+
+        ////////"Checking opening nws"
+        {
+            Property pcfg;
+            pcfg.put("device", "battery_nws_ros2");
+            pcfg.put("node_name", "battery_node");
+            pcfg.put("topic_name","/robot_battery");
+            REQUIRE(ddnws.open(pcfg));
+        }
+
+        //"Close all polydrivers and check"
+        {
+            CHECK(ddnws.close());
+        }
+    }
+
+    SECTION("Checking the nws attached to device")
+    {
+        PolyDriver ddnws;
+        PolyDriver ddfake;
+        yarp::dev::WrapperSingle* ww_nws = nullptr;
+
+        ////////"Checking opening nws"
+        {
+            Property pcfg;
+            pcfg.put("device", "battery_nws_ros2");
+            pcfg.put("node_name", "battery_node");
+            pcfg.put("topic_name","/robot_battery");
+            REQUIRE(ddnws.open(pcfg));
+        }
+
+        ////////"Checking opening device"
+        {
+            Property pcfg_fake;
+            pcfg_fake.put("device", "fakeBattery");
+            REQUIRE(ddfake.open(pcfg_fake));
+        }
+
+        //attach the nws to the fake device
+        {
+            ddnws.view(ww_nws);
+            bool result_att = ww_nws->attach(&ddfake);
+            REQUIRE(result_att);
+        }
+
+        //"Close all polydrivers and check"
+        {
+            CHECK(ddnws.close());
+            CHECK(ddfake.close());
+        }
+    }
+
+    Network::setLocalMode(false);
+}