YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
RGBDToPointCloudSensor_nws_ros.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7#include <sstream>
9#include <yarp/os/LogStream.h>
10#include "rosPixelCode.h"
12#include <yarp/rosmsg/std_msgs/Header.h>
13#include <yarp/rosmsg/sensor_msgs/PointField.h>
14
15using namespace RGBDToPointCloudImpl;
16using namespace yarp::sig;
17using namespace yarp::dev;
18using namespace yarp::os;
19
20YARP_LOG_COMPONENT(RGBDTOPOINTCLOUDSENSORNWSROS, "yarp.devices.RGBDToPointCloudSensor_nws_ros");
21
26
27
28/* destructor of the wrapper */
33
37{
38 // check period
39 if (!config.check("period", "refresh period of the broadcasted values in s")) {
40 if(verbose >= 3) {
41 yCInfo(RGBDTOPOINTCLOUDSENSORNWSROS) << "Using default 'period' parameter of " << DEFAULT_THREAD_PERIOD << "s";
42 }
43 }
44 else {
45 period = config.find("period").asFloat64();
46 }
47
48 // nodename parameter
49 if (!config.check("node_name", "the name of the ros node")) {
50 yCError(RGBDTOPOINTCLOUDSENSORNWSROS) << "missing node_name parameter";
51 return false;
52 }
53 nodeName = config.find("node_name").asString();
54
55
56 // baseTopicName parameter
57 if (!config.check("topic_name", "the name of the ros node")) {
58 yCError(RGBDTOPOINTCLOUDSENSORNWSROS) << "missing topic_name parameter, using default one";
59 return false;
60 }
61 pointCloudTopicName = config.find("topic_name").asString();
62
63 // frame_id parameter
64 if (!config.check("frame_id", "the name of the ros node")) {
65 yCError(RGBDTOPOINTCLOUDSENSORNWSROS) << "missing frame_id parameter";
66 return false;
67 }
68 frameId = config.find("frame_id").asString();
69
70 // open topics here if needed
71 m_node = new yarp::os::Node(nodeName);
72 nodeSeq = 0;
73 if (!publisherPort_pointCloud.topic(pointCloudTopicName))
74 {
75 yCError(RGBDTOPOINTCLOUDSENSORNWSROS) << "Unable to publish data on " << pointCloudTopicName.c_str() << " topic, check your yarp-ROS network configuration";
76 return false;
77 }
78 return true;
79}
80
82{
84 detach();
85
86 if(m_node !=nullptr)
87 {
88 m_node->interrupt();
89 delete m_node;
90 m_node = nullptr;
91 }
92
93 return true;
94}
95
101{
103 {
105 }
106
107 sensor_p = nullptr;
108 return true;
109}
110
112{
113 if(poly)
114 {
115 poly->view(sensor_p);
116 poly->view(fgCtrl);
117 }
118
119 if(sensor_p == nullptr)
120 {
121 yCError(RGBDTOPOINTCLOUDSENSORNWSROS) << "Attached device has no valid IRGBDSensor interface.";
122 return false;
123 }
124
126 return PeriodicThread::start();
127}
128
129/* IRateThread interface */
130
132{
133 // Get interface from attached device if any.
134 return true;
135}
136
138{
139 // Detach() calls stop() which in turns calls this functions, therefore no calls to detach here!
140}
141
142
143bool RGBDToPointCloudSensor_nws_ros::writeData()
144{
145 //colorImage.setPixelCode(VOCAB_PIXEL_RGB);
146 // depthImage.setPixelCode(VOCAB_PIXEL_MONO_FLOAT);
147
148 // colorImage.resize(hDim, vDim); // Has this to be done each time? If size is the same what it does?
149 // depthImage.resize(hDim, vDim);
150 if (!sensor_p->getImages(colorImage, depthImage, &colorStamp, &depthStamp))
151 {
152 return false;
153 }
154
155 static Stamp oldColorStamp = Stamp(0, 0);
156 static Stamp oldDepthStamp = Stamp(0, 0);
158 bool rgb_data_ok = true;
159 bool depth_data_ok = true;
160
161 if ((colorStamp.getTime() - oldColorStamp.getTime()) <= 0)
162 {
163 rgb_data_ok=false;
164 //return true;
165 }
166 else { oldColorStamp = colorStamp; }
167
168 if ((depthStamp.getTime() - oldDepthStamp.getTime()) <= 0)
169 {
170 depth_data_ok=false;
171 //return true;
172 }
173 else { oldDepthStamp = depthStamp; }
175
176
177 // TBD: We should check here somehow if the timestamp was correctly updated and, if not, update it ourselves.
178 if (rgb_data_ok)
179 {
180 if (depth_data_ok)
181 {
182 if (intrinsic_ok)
183 {
186 colorImagePixelRGB.setExternal(colorImage.getRawImage(), colorImage.width(), colorImage.height());
187 // create point cloud in yarp format
189 yarp::sig::PixelRgb>(depthImage,
193 PointCloud2Type& pc2Ros = publisherPort_pointCloud.prepare();
194 // filling ros header
195 yarp::rosmsg::std_msgs::Header headerRos;
196 headerRos.clear();
197 headerRos.seq = nodeSeq;
198 headerRos.frame_id = frameId;
199 headerRos.stamp = depthStamp.getTime();
200
201 // filling ros point field
202 std::vector<yarp::rosmsg::sensor_msgs::PointField> pointFieldRos;
203 pointFieldRos.push_back(yarp::rosmsg::sensor_msgs::PointField());
204 pointFieldRos.push_back(yarp::rosmsg::sensor_msgs::PointField());
205 pointFieldRos.push_back(yarp::rosmsg::sensor_msgs::PointField());
206 pointFieldRos.push_back(yarp::rosmsg::sensor_msgs::PointField());
207 pointFieldRos[0].name = "x";
208 pointFieldRos[0].offset = 0;
209 pointFieldRos[0].datatype = 7;
210 pointFieldRos[0].count = 1;
211 pointFieldRos[1].name = "y";
212 pointFieldRos[1].offset = 4;
213 pointFieldRos[1].datatype = 7;
214 pointFieldRos[1].count = 1;
215 pointFieldRos[2].name = "z";
216 pointFieldRos[2].offset = 8;
217 pointFieldRos[2].datatype = 7;
218 pointFieldRos[2].count = 1;
219 pointFieldRos[3].name = "rgb";
220 pointFieldRos[3].offset = 16;
221 pointFieldRos[3].datatype = 7;
222 pointFieldRos[3].count = 1;
223 pc2Ros.fields = pointFieldRos;
224
225 std::vector<unsigned char> vec(yarpCloud.getRawData(), yarpCloud.getRawData() + yarpCloud.dataSizeBytes() );
226 pc2Ros.data = vec;
227 pc2Ros.header = headerRos;
228 pc2Ros.width = yarpCloud.width() * yarpCloud.height();
229 pc2Ros.height = 1;
230 pc2Ros.is_dense = yarpCloud.isDense();
231
232 pc2Ros.point_step = sizeof (yarp::sig::DataXYZRGBA);
233 pc2Ros.row_step = static_cast<std::uint32_t> (sizeof (yarp::sig::DataXYZRGBA) * pc2Ros.width);
234
235 publisherPort_pointCloud.write();
236 }
237 }
238 }
239
240 nodeSeq++;
241
242 return true;
243}
244
246{
247 if (sensor_p!=nullptr)
248 {
249 static int i = 0;
250 sensorStatus = sensor_p->getSensorStatus();
251 switch (sensorStatus)
252 {
254 {
255 if (!writeData()) {
256 yCError(RGBDTOPOINTCLOUDSENSORNWSROS, "Image not captured.. check hardware configuration");
257 }
258 i = 0;
259 }
260 break;
262 {
263 if(i < 1000) {
264 if((i % 30) == 0) {
265 yCInfo(RGBDTOPOINTCLOUDSENSORNWSROS) << "Device not ready, waiting...";
266 }
267 } else {
268 yCWarning(RGBDTOPOINTCLOUDSENSORNWSROS) << "Device is taking too long to start..";
269 }
270 i++;
271 }
272 break;
273 default:
274 {
275 if (verbose >= 1) { // better not to print it every cycle anyway, too noisy
276 yCError(RGBDTOPOINTCLOUDSENSORNWSROS, "%s: Sensor returned error", nodeName.c_str());
277 }
278 }
279 }
280 }
281 else
282 {
283 if(verbose >= 6) {
284 yCError(RGBDTOPOINTCLOUDSENSORNWSROS, "%s: Sensor interface is not valid", nodeName.c_str());
285 }
286 }
287}
#define DEFAULT_THREAD_PERIOD
const yarp::os::LogComponent & RGBDTOPOINTCLOUDSENSORNWSROS()
constexpr double DEFAULT_THREAD_PERIOD
bool open(yarp::os::Searchable &params) override
Device driver interface.
bool attach(yarp::dev::PolyDriver *poly) override
Specify which sensor this thread has to read from.
bool threadInit() override
Initialization method.
bool close() override
Close the DeviceDriver.
bool detach() override
WrapperSingle interface.
bool view(T *&x)
Get an interface to the device driver.
bool getRgbIntrinsicParam(yarp::os::Property &intrinsic) override=0
Get the intrinsic parameters of the rgb camera.
virtual bool getImages(yarp::sig::FlexImage &colorFrame, yarp::sig::ImageOf< yarp::sig::PixelFloat > &depthFrame, yarp::os::Stamp *colorStamp=nullptr, yarp::os::Stamp *depthStamp=nullptr)=0
Get the both the color and depth frame in a single call.
virtual RGBDSensor_status getSensorStatus()=0
Get the surrent status of the sensor, using enum type.
A container for a device driver.
Definition PolyDriver.h:23
A mini-server for performing network communication in the background.
The Node class.
Definition Node.h:23
void interrupt()
interrupt delegates the call to the Node port interrupt.
Definition Node.cpp:597
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A class for storing options and configuration information.
Definition Property.h:33
bool topic(const std::string &name)
Set topic to publish to.
Definition Publisher.h:63
void write(bool forceStrict=false)
Write the current object being returned by Publisher::prepare.
Definition Publisher.h:148
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::Publisher::write.
Definition Publisher.h:123
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
size_t width() const
Gets width of image in pixels.
Definition Image.h:163
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:542
size_t height() const
Gets height of image in pixels.
Definition Image.h:169
The PointCloud class.
Definition PointCloud.h:22
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
yarp::sig::PointCloud< T1 > depthRgbToPC(const yarp::sig::ImageOf< yarp::sig::PixelFloat > &depth, const yarp::sig::ImageOf< T2 > &color, const yarp::sig::IntrinsicParams &intrinsic, const yarp::sig::utils::OrganizationType organizationType=yarp::sig::utils::OrganizationType::Organized)
depthRgbToPC, compute the colored PointCloud given depth image, color image and the intrinsic paramet...
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).