From 12bda9c4c6bbb5f3e7f1ee8e1586c629a0ad66f8 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Fri, 19 Aug 2022 16:35:53 +0200 Subject: [PATCH 1/3] added new nws device battery_nws_ros2 --- src/devices/CMakeLists.txt | 1 + .../battery_nws_ros2/Battery_nws_ros2.cpp | 174 ++++++++++++++++++ .../battery_nws_ros2/Battery_nws_ros2.h | 81 ++++++++ src/devices/battery_nws_ros2/CMakeLists.txt | 50 +++++ 4 files changed, 306 insertions(+) create mode 100644 src/devices/battery_nws_ros2/Battery_nws_ros2.cpp create mode 100644 src/devices/battery_nws_ros2/Battery_nws_ros2.h create mode 100644 src/devices/battery_nws_ros2/CMakeLists.txt diff --git a/src/devices/CMakeLists.txt b/src/devices/CMakeLists.txt index dac0002..e67a0ff 100644 --- a/src/devices/CMakeLists.txt +++ b/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) diff --git a/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp b/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp new file mode 100644 index 0000000..ca6cfb4 --- /dev/null +++ b/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp @@ -0,0 +1,174 @@ +/* + * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + +#include "Battery_nws_ros2.h" +#include +#include +#include +#include + +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_odometry2D_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; + } + + if (config.check("subdevice")) { + yarp::os::Property p; + p.fromString(config.toString(), false); + p.put("device", config.find("subdevice").asString()); + + if (!m_driver.open(p) || !m_driver.isValid()) { + yCError(BATTERY_NWS_ROS2) << "failed to open subdevice.. check params"; + return false; + } + + if (!attach(&m_driver)) { + yCError(BATTERY_NWS_ROS2) << "failed to open subdevice.. check params"; + 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(m_topicName, 10); + return true; +} + +void Battery_nws_ros2::threadRelease() +{ +} + +void Battery_nws_ros2::run() +{ + if (m_battery_interface!=nullptr && m_ros2Publisher) + { + double voltage=0; + double current=0; + double charge=0; + double temperature=0; + Battery_status status; + std::string battery_info; + m_odometry2D_interface->getBatteryVoltage(voltage); + m_odometry2D_interface->getBatteryCurrent(current); + m_odometry2D_interface->getBatteryCharge(charge); + m_odometry2D_interface->getBatteryStatus(status); + m_odometry2D_interface->getBatteryTemperature(temperature); + m_odometry2D_interface->getBatteryInfo(battery_info); + + m_timeStamp.update(yarp::os::Time::now()); + + sensor_msgs::msg::BatteryState battMsg; + battMsg.voltage = voltage; + battMsg.current = current; + battMsg.temperature = temperature; + battMsg.charge = 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); + } + else + { + yCError(BATTERY_NWS_ROS2) << "the interface is not valid"; + } +} + +bool Battery_nws_ros2::close() +{ + yCTrace(BATTERY_NWS_ROS2); + if (PeriodicThread::isRunning()) + { + PeriodicThread::stop(); + } + + detach(); + return true; +} diff --git a/src/devices/battery_nws_ros2/Battery_nws_ros2.h b/src/devices/battery_nws_ros2/Battery_nws_ros2.h new file mode 100644 index 0000000..8f0bcdc --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include +#include + +#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 '/' | + * | subdevice | - | string | - | - | alternative to 'attach' action | name of the subdevice to use as a data source | when used, parameters for the subdevice must be provided as well | + * + */ + + +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 ¶ms) 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 + rclcpp::Node::SharedPtr m_node; + rclcpp::Publisher::SharedPtr m_ros2Publisher; + + //interfaces + yarp::dev::PolyDriver m_driver; + yarp::dev::IBattery *m_battery_interface{nullptr}; +}; + +#endif // YARP_ROS2_BATTERY_NWS_ROS2_H diff --git a/src/devices/battery_nws_ros2/CMakeLists.txt b/src/devices/battery_nws_ros2/CMakeLists.txt new file mode 100644 index 0000000..857fc83 --- /dev/null +++ b/src/devices/battery_nws_ros2/CMakeLists.txt @@ -0,0 +1,50 @@ +# SPDX-FileCopyrightText: 2006-2021 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_include_directories(yarp_battery_nws_ros2 PRIVATE $) + target_link_libraries(yarp_battery_nws_ros2 + PRIVATE + YARP::YARP_os + YARP::YARP_sig + YARP::YARP_dev + rclcpp::rclcpp + Ros2Utils + ) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS + YARP_os + YARP_sig + YARP_dev + rclcpp::rclcpp + 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") +endif() From dbe762b11551b1ee6ae8befa4a3f94f722c1a000 Mon Sep 17 00:00:00 2001 From: Marco Radazzo Date: Tue, 6 Sep 2022 15:31:16 +0000 Subject: [PATCH 2/3] fix --- .../battery_nws_ros2/Battery_nws_ros2.cpp | 37 ++++++++++--------- .../battery_nws_ros2/Battery_nws_ros2.h | 5 ++- src/devices/battery_nws_ros2/CMakeLists.txt | 2 + 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp b/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp index ca6cfb4..b661fba 100644 --- a/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp +++ b/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp @@ -46,7 +46,7 @@ bool Battery_nws_ros2::detach() { PeriodicThread::stop(); } - m_odometry2D_interface = nullptr; + m_battery_interface = nullptr; return true; } @@ -117,30 +117,37 @@ void Battery_nws_ros2::threadRelease() void Battery_nws_ros2::run() { - if (m_battery_interface!=nullptr && m_ros2Publisher) + 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; - Battery_status status; + yarp::dev::IBattery::Battery_status status; std::string battery_info; - m_odometry2D_interface->getBatteryVoltage(voltage); - m_odometry2D_interface->getBatteryCurrent(current); - m_odometry2D_interface->getBatteryCharge(charge); - m_odometry2D_interface->getBatteryStatus(status); - m_odometry2D_interface->getBatteryTemperature(temperature); - m_odometry2D_interface->getBatteryInfo(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()); - sensor_msgs::msg::BatteryState battMsg; battMsg.voltage = voltage; battMsg.current = current; battMsg.temperature = temperature; - battMsg.charge = std::nan; - battMsg.capacity = std::nan; - battMsg.design_capacity = std::nan; + battMsg.charge = std::numeric_limits::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; @@ -155,10 +162,6 @@ void Battery_nws_ros2::run() m_ros2Publisher->publish(battMsg); } - else - { - yCError(BATTERY_NWS_ROS2) << "the interface is not valid"; - } } bool Battery_nws_ros2::close() diff --git a/src/devices/battery_nws_ros2/Battery_nws_ros2.h b/src/devices/battery_nws_ros2/Battery_nws_ros2.h index 8f0bcdc..95f3f3d 100644 --- a/src/devices/battery_nws_ros2/Battery_nws_ros2.h +++ b/src/devices/battery_nws_ros2/Battery_nws_ros2.h @@ -7,7 +7,7 @@ #ifndef YARP_ROS2_BATTERY_NWS_ROS2_H #define YARP_ROS2_BATTERY_NWS_ROS2_H #include -#include +#include #include #include @@ -70,8 +70,9 @@ class Battery_nws_ros2 : double m_period{DEFAULT_THREAD_PERIOD}; //ros2 node + sensor_msgs::msg::BatteryState battMsg; rclcpp::Node::SharedPtr m_node; - rclcpp::Publisher::SharedPtr m_ros2Publisher; + rclcpp::Publisher::SharedPtr m_ros2Publisher =nullptr; //interfaces yarp::dev::PolyDriver m_driver; diff --git a/src/devices/battery_nws_ros2/CMakeLists.txt b/src/devices/battery_nws_ros2/CMakeLists.txt index 857fc83..17c1c45 100644 --- a/src/devices/battery_nws_ros2/CMakeLists.txt +++ b/src/devices/battery_nws_ros2/CMakeLists.txt @@ -25,6 +25,7 @@ if(NOT SKIP_battery_nws_ros2) YARP::YARP_sig YARP::YARP_dev rclcpp::rclcpp + sensor_msgs::sensor_msgs__rosidl_typesupport_cpp Ros2Utils ) list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS @@ -32,6 +33,7 @@ if(NOT SKIP_battery_nws_ros2) YARP_sig YARP_dev rclcpp::rclcpp + sensor_msgs::sensor_msgs__rosidl_typesupport_cpp Ros2Utils ) From 6f6df5ff777c33011f73ec70e4b8a197fd75354a Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Tue, 22 Aug 2023 11:36:37 +0200 Subject: [PATCH 3/3] added test --- .../battery_nws_ros2/Battery_nws_ros2.cpp | 27 ++----- .../battery_nws_ros2/Battery_nws_ros2.h | 1 - src/devices/battery_nws_ros2/CMakeLists.txt | 6 +- .../battery_nws_ros2/tests/CMakeLists.txt | 4 + .../tests/battery_nws_ros2_test.cpp | 80 +++++++++++++++++++ 5 files changed, 95 insertions(+), 23 deletions(-) create mode 100644 src/devices/battery_nws_ros2/tests/CMakeLists.txt create mode 100644 src/devices/battery_nws_ros2/tests/battery_nws_ros2_test.cpp diff --git a/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp b/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp index b661fba..2b086ff 100644 --- a/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp +++ b/src/devices/battery_nws_ros2/Battery_nws_ros2.cpp @@ -83,21 +83,6 @@ bool Battery_nws_ros2::open(yarp::os::Searchable &config) return false; } - if (config.check("subdevice")) { - yarp::os::Property p; - p.fromString(config.toString(), false); - p.put("device", config.find("subdevice").asString()); - - if (!m_driver.open(p) || !m_driver.isValid()) { - yCError(BATTERY_NWS_ROS2) << "failed to open subdevice.. check params"; - return false; - } - - if (!attach(&m_driver)) { - yCError(BATTERY_NWS_ROS2) << "failed to open subdevice.. check params"; - return false; - } - } rclcpp::NodeOptions node_options; node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); @@ -120,12 +105,12 @@ void Battery_nws_ros2::run() if (m_battery_interface==nullptr) { yCError(BATTERY_NWS_ROS2) << "the interface is not valid"; - } - else if (!m_ros2Publisher) - { + } + else if (!m_ros2Publisher) + { yCError(BATTERY_NWS_ROS2) << "the publisher is not ready"; - } - else + } + else { double voltage=0; double current=0; @@ -155,7 +140,7 @@ void Battery_nws_ros2::run() 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()))); diff --git a/src/devices/battery_nws_ros2/Battery_nws_ros2.h b/src/devices/battery_nws_ros2/Battery_nws_ros2.h index 95f3f3d..3be17cd 100644 --- a/src/devices/battery_nws_ros2/Battery_nws_ros2.h +++ b/src/devices/battery_nws_ros2/Battery_nws_ros2.h @@ -30,7 +30,6 @@ * | 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 '/' | - * | subdevice | - | string | - | - | alternative to 'attach' action | name of the subdevice to use as a data source | when used, parameters for the subdevice must be provided as well | * */ diff --git a/src/devices/battery_nws_ros2/CMakeLists.txt b/src/devices/battery_nws_ros2/CMakeLists.txt index 17c1c45..1cf83b3 100644 --- a/src/devices/battery_nws_ros2/CMakeLists.txt +++ b/src/devices/battery_nws_ros2/CMakeLists.txt @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) +# SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause yarp_prepare_plugin(battery_nws_ros2 @@ -49,4 +49,8 @@ if(NOT SKIP_battery_nws_ros2) 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() diff --git a/src/devices/battery_nws_ros2/tests/CMakeLists.txt b/src/devices/battery_nws_ros2/tests/CMakeLists.txt new file mode 100644 index 0000000..4dcf806 --- /dev/null +++ b/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) diff --git a/src/devices/battery_nws_ros2/tests/battery_nws_ros2_test.cpp b/src/devices/battery_nws_ros2/tests/battery_nws_ros2_test.cpp new file mode 100644 index 0000000..7d62f39 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include + +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); +}