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..2b086ff --- /dev/null +++ b/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 +#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_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(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::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; +} 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..3be17cd --- /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 '/' | + * + */ + + +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 + sensor_msgs::msg::BatteryState battMsg; + rclcpp::Node::SharedPtr m_node; + rclcpp::Publisher::SharedPtr m_ros2Publisher =nullptr; + + //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..1cf83b3 --- /dev/null +++ b/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_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 + 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() 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); +}