YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Battery_nws_ros2.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#include "Battery_nws_ros2.h"
8#include <yarp/os/LogStream.h>
9#include <yarp/os/Stamp.h>
10#include <Ros2Utils.h>
11
12YARP_LOG_COMPONENT(BATTERY_NWS_ROS2, "yarp.devices.Battery_nws_ros2")
13
17
19{
20 m_battery_interface = nullptr;
21}
22
23
25{
26 if (driver->isValid())
27 {
28 driver->view(m_battery_interface);
29 } else {
30 yCError(BATTERY_NWS_ROS2) << "not valid driver";
31 }
32
33 if (m_battery_interface == nullptr)
34 {
35 yCError(BATTERY_NWS_ROS2, "Subdevice passed to attach method is invalid");
36 return false;
37 }
38 PeriodicThread::setPeriod(m_period);
39 return PeriodicThread::start();
40}
41
42
44{
45 if (PeriodicThread::isRunning())
46 {
47 PeriodicThread::stop();
48 }
49 m_battery_interface = nullptr;
50 return true;
51}
52
54{
55 return true;
56}
57
59{
60 parseParams(config);
61 rclcpp::NodeOptions node_options;
62 node_options.allow_undeclared_parameters(true);
63 node_options.automatically_declare_parameters_from_overrides(true);
64 if(m_namespace.empty()) {
65 m_node = NodeCreator::createNode(m_node_name, node_options);
66 } else {
67 m_node = NodeCreator::createNode(m_node_name, m_namespace, node_options);
68 }
69
70 if (m_node == nullptr) {
71 yCError(BATTERY_NWS_ROS2) << " opening " << m_node_name << " Node, check your yarp-ROS2 network configuration\n";
72 return false;
73 }
74
75 m_ros2Publisher = m_node->create_publisher<sensor_msgs::msg::BatteryState>(m_topic_name, 10);
76 return true;
77}
78
82
84{
85 if (m_battery_interface==nullptr)
86 {
87 yCError(BATTERY_NWS_ROS2) << "the interface is not valid";
88 }
89 else if (!m_ros2Publisher)
90 {
91 yCError(BATTERY_NWS_ROS2) << "the publisher is not ready";
92 }
93 else if(m_ros2Publisher->get_subscription_count()>0)
94 {
95 double voltage=0;
96 double current=0;
97 double charge=0;
98 double temperature=0;
100 std::string battery_info;
101 m_battery_interface->getBatteryVoltage(voltage);
102 m_battery_interface->getBatteryCurrent(current);
103 m_battery_interface->getBatteryCharge(charge);
104 m_battery_interface->getBatteryStatus(status);
105 m_battery_interface->getBatteryTemperature(temperature);
106 m_battery_interface->getBatteryInfo(battery_info);
107
108 m_timeStamp.update(yarp::os::Time::now());
109
110 battMsg.voltage = voltage;
111 battMsg.current = current;
112 battMsg.temperature = temperature;
113 battMsg.charge = std::numeric_limits<double>::quiet_NaN();//std::nan("");
114 // battMsg.capacity = std::nan("");
115 // battMsg.design_capacity = std::nan("");
116 battMsg.percentage = charge;
117 battMsg.power_supply_status = 0;
118 battMsg.power_supply_health = 0;
119 battMsg.power_supply_technology = 0;
120 battMsg.present=true;
121 battMsg.location="";
122 battMsg.serial_number="";
123
124 battMsg.header.frame_id = "" ;
125 battMsg.header.stamp.sec = int(m_timeStamp.getTime());
126 battMsg.header.stamp.nanosec = int(1000000000UL * (m_timeStamp.getTime() - int(m_timeStamp.getTime())));
127
128 m_ros2Publisher->publish(battMsg);
129 }
130}
131
133{
135 if (PeriodicThread::isRunning())
136 {
137 PeriodicThread::stop();
138 }
139
140 detach();
141 return true;
142}
const yarp::os::LogComponent & BATTERY_NWS_ROS2()
#define DEFAULT_THREAD_PERIOD
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
Battery_nws_ros2: A ros2 nws to get the status of a battery and publish it on a ros2 topic....
bool close() override
Close the DeviceDriver.
void run() override
Loop function.
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
void threadRelease() override
Release method.
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
bool threadInit() override
Initialization method.
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getBatteryTemperature(double &temperature)=0
get the battery temperature
virtual yarp::dev::ReturnValue getBatteryInfo(std::string &battery_info)=0
get the battery hardware characteristics (e.g.
virtual yarp::dev::ReturnValue getBatteryCharge(double &charge)=0
get the battery status of charge
virtual yarp::dev::ReturnValue getBatteryStatus(Battery_status &status)=0
get the battery status
virtual yarp::dev::ReturnValue getBatteryCurrent(double &current)=0
Get the instantaneous current measurement.
virtual yarp::dev::ReturnValue getBatteryVoltage(double &voltage)=0
Get the instantaneous voltage measurement.
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
A base class for nested structures that can be searched.
Definition Searchable.h:31
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
#define yCError(component,...)
#define yCTrace(component,...)
#define YARP_LOG_COMPONENT(name,...)
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
The main, catch-all namespace for YARP.
Definition dirs.h:16