YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
GenericSensor_nws_ros2.h
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#ifndef YARP_DEV_GENERICSENSORS_NWS_ROS2_H
7#define YARP_DEV_GENERICSENSORS_NWS_ROS2_H
8
13#include <yarp/os/Log.h>
15#include <yarp/os/LogStream.h>
17
18#include <rclcpp/rclcpp.hpp>
19#include <Ros2Utils.h>
20
21
22// The log component is defined in each device, with a specialized name
24
25
26
39template <class ROS_MSG>
45{
46protected:
47 rclcpp::Node::SharedPtr m_node;
48 typename rclcpp::Publisher<ROS_MSG>::SharedPtr m_publisher;
51 std::string m_framename;
52 const size_t m_sens_index = 0;
54
55public:
58
59 /* DevideDriver methods */
60 bool open(yarp::os::Searchable &params) override;
61 bool close() override;
62
63 /* IMultipleWrapper methods */
64 bool attachAll(const yarp::dev::PolyDriverList &p) override;
65 bool detachAll() override;
66
67 /* PeriodicRateThread methods */
68 void threadRelease() override;
69 void run() override;
70
71protected:
72 virtual bool viewInterfaces() = 0;
73};
74
75template <class ROS_MSG>
77 PeriodicThread(0.02)
78{
79 m_node = nullptr;
80 m_publisher=nullptr;
81 m_poly = nullptr;
83}
84
85template <class ROS_MSG>
87
88template <class ROS_MSG>
90{
91 parseParams(config);
92 if (m_node_name.c_str()[0] == '/') {
93 yCError(GENERICSENSOR_NWS_ROS2) << "node name cannot begin with /";
94 return false;
95 }
96
97 if (m_topic_name.c_str()[0] != '/') {
98 yCError(GENERICSENSOR_NWS_ROS2) << "Missing '/' in topic_name parameter";
99 return false;
100 }
101
102 if(m_namespace.empty()) {
103 m_node = NodeCreator::createNode(m_node_name);
104 } else {
105 m_node = NodeCreator::createNode(m_node_name, m_namespace);
106 }
107 if( m_node == nullptr) {
108 yCError(GENERICSENSOR_NWS_ROS2) << "Opening " << m_node_name << " Node creation failed, check your yarp-ROS network configuration\n";
109 return false;
110 }
111 m_publisher = m_node->create_publisher<ROS_MSG>(m_topic_name,rclcpp::QoS(10));
112
113 if (m_node == nullptr) {
114 yCError(GENERICSENSOR_NWS_ROS2) << "Opening " << m_node_name << " Node, check your yarp-ROS network configuration\n";
115 return false;
116 }
117
118 if (m_publisher == nullptr) {
119 yCError(GENERICSENSOR_NWS_ROS2) << "Opening " << m_topic_name << " Topic, check your yarp-ROS network configuration\n";
120 return false;
121 }
122
123 yCInfo(GENERICSENSOR_NWS_ROS2) << "Running, waiting for attach...";
124
125 return true;
126}
127
128template <class ROS_MSG>
130{
131 return this->detachAll();
132}
133
134template <class ROS_MSG>
136{
137 // Attach the device
138 if (p.size() > 1)
139 {
140 yCError(GENERICSENSOR_NWS_ROS2, "This device only supports exposing a "
141 "single MultipleAnalogSensors device on YARP ports, but %d devices have been passed in attachAll.",
142 p.size());
143 yCError(GENERICSENSOR_NWS_ROS2, "Please use the multipleanalogsensorsremapper device to combine several device in a new device.");
144 detachAll();
145 return false;
146 }
147
148 if (p.size() == 0)
149 {
150 yCError(GENERICSENSOR_NWS_ROS2, "No device passed to attachAll, please pass a device to expose on YARP ports.");
151 return false;
152 }
153
154 m_poly = p[0]->poly;
155
156 if (!m_poly)
157 {
158 yCError(GENERICSENSOR_NWS_ROS2, "Null pointer passed to attachAll.");
159 return false;
160 }
161
162 // View all the interfaces
163 bool ok = viewInterfaces();
164
165 // Set rate period
166 ok &= this->setPeriod(m_period);
167 ok &= this->start();
168
169 return ok;
170}
171
172template <class ROS_MSG>
174{
175 // Stop the thread on detach
176 if (this->isRunning()) {
177 this->stop();
178 }
179 return true;
180}
181
182template <class ROS_MSG>
186
187template <class ROS_MSG>
192
193#endif
const yarp::os::LogComponent & GENERICSENSOR_NWS_ROS2()
This class is the parameters parser for class GenericSensor_nws_ros2.
This abstract template needs to be specialized in a ROS Publisher, for a specific ROS mesagge/sensor ...
rclcpp::Publisher< ROS_MSG >::SharedPtr m_publisher
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
yarp::dev::PolyDriver * m_poly
bool detachAll() override
Detach the object (you must have first called attach).
virtual bool viewInterfaces()=0
rclcpp::Node::SharedPtr m_node
virtual ~GenericSensor_nws_ros2()
bool attachAll(const yarp::dev::PolyDriverList &p) override
Attach to a list of objects.
yarp::dev::PolyDriver m_subdevicedriver
bool close() override
Close the DeviceDriver.
void run() override
Loop function.
void threadRelease() override
Release method.
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
Interface implemented by all device drivers.
Interface for an object that can wrap/attach to to another.
A container for a device driver.
Definition PolyDriver.h:23
An abstraction for a periodic thread.
A base class for nested structures that can be searched.
Definition Searchable.h:31
#define yCInfo(component,...)
#define yCError(component,...)
#define YARP_DECLARE_LOG_COMPONENT(name)