YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
RgbdSensor_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
7
9#include <yarp/os/LogStream.h>
10
11#include <string>
12#include <vector>
13#include <iostream>
14#include <Ros2Utils.h>
15
16#include <sensor_msgs/image_encodings.hpp>
17
18using namespace std::chrono_literals;
19
20namespace
21{
22YARP_LOG_COMPONENT(RGBDSENSOR_NWS_ROS2, "yarp.ros2.rgbdSensor_nws_ros2", yarp::os::Log::TraceType);
23
24constexpr double DEFAULT_THREAD_PERIOD = 0.03; // s
25
26// FIXME should be possible to set different frame_id for rgb and depth
27const std::string frameId_param = "frame_Id";
28const std::string nodeName_param = "nodeName";
29const std::string colorTopicName_param = "colorTopicName";
30const std::string depthTopicName_param = "depthTopicName";
31const std::string depthInfoTopicName_param = "depthInfoTopicName";
32const std::string colorInfoTopicName_param = "colorInfoTopicName";
33
34
35
36std::string yarp2RosPixelCode(int code)
37{
38 switch (code)
39 {
40 case VOCAB_PIXEL_BGR:
41 return sensor_msgs::image_encodings::BGR8;
43 return sensor_msgs::image_encodings::BGRA8;
45 return sensor_msgs::image_encodings::BAYER_BGGR16;
47 return sensor_msgs::image_encodings::BAYER_BGGR8;
49 return sensor_msgs::image_encodings::BAYER_GBRG16;
51 return sensor_msgs::image_encodings::BAYER_GBRG8;
53 return sensor_msgs::image_encodings::BAYER_GRBG16;
55 return sensor_msgs::image_encodings::BAYER_GRBG8;
57 return sensor_msgs::image_encodings::BAYER_RGGB16;
59 return sensor_msgs::image_encodings::BAYER_RGGB8;
61 return sensor_msgs::image_encodings::MONO8;
63 return sensor_msgs::image_encodings::MONO16;
64 case VOCAB_PIXEL_RGB:
65 return sensor_msgs::image_encodings::RGB8;
67 return sensor_msgs::image_encodings::RGBA8;
69 return sensor_msgs::image_encodings::TYPE_32FC1;
70 default:
71 return sensor_msgs::image_encodings::RGB8;
72 }
73}
74
75} // namespace
76
77
82
83
84// DeviceDriver
86{
87 yCDebug(RGBDSENSOR_NWS_ROS2) << "Parameters are: " << config.toString();
88
89 if(!fromConfig(config)) {
90 yCError(RGBDSENSOR_NWS_ROS2) << "Failed to open, check previous log for error messages.";
91 return false;
92 }
93
94 if(!initialize_ROS2(config)) {
95 yCError(RGBDSENSOR_NWS_ROS2) << "Error initializing ROS topic";
96 return false;
97 }
98
99 return true;
100}
101
102
103
104bool RgbdSensor_nws_ros2::fromConfig(yarp::os::Searchable &config)
105{
106 parseParams(config);
107 if(m_node_name[0] == '/'){
108 yCError(RGBDSENSOR_NWS_ROS2) << "node_name cannot begin with an initial /";
109 return false;
110 }
111
112 if(m_depth_topic_name[0] != '/'){
113 yCError(RGBDSENSOR_NWS_ROS2) << "depth_topic_name must begin with an initial /";
114 return false;
115 }
116 m_depth_info_topic_name = m_depth_topic_name.substr(0, m_depth_topic_name.rfind('/')) + "/camera_info";
117
118 if(m_color_topic_name[0] != '/'){
119 yCError(RGBDSENSOR_NWS_ROS2) << "color_topic_name must begin with an initial /";
120 return false;
121 }
122 m_color_info_topic_name = m_color_topic_name.substr(0, m_color_topic_name.rfind('/')) + "/camera_info";
123
124 m_forceInfoSync = m_force_info_synch == 1;
125
126 return true;
127}
128
129
130bool RgbdSensor_nws_ros2::initialize_ROS2(yarp::os::Searchable &params)
131{
132 if(m_namespace.empty()) {
134 }
135 else {
137 }
138 if(!m_node) {
139 yCError(RGBDSENSOR_NWS_ROS2) << "Failed to create node";
140 return false;
141 }
142 rosPublisher_color = m_node->create_publisher<sensor_msgs::msg::Image>(m_color_topic_name, 10);
143 rosPublisher_depth = m_node->create_publisher<sensor_msgs::msg::Image>(m_depth_topic_name, 10);
144 rosPublisher_colorCaminfo = m_node->create_publisher<sensor_msgs::msg::CameraInfo>(m_color_info_topic_name, 10);
145 rosPublisher_depthCaminfo = m_node->create_publisher<sensor_msgs::msg::CameraInfo>(m_depth_info_topic_name, 10);
146 if(m_pub_status_topic == 1) {
147 std::string status_topic_name = "/" + m_node_name + "/publisher_status";
148 m_rosPublisher_status = m_node->create_publisher<std_msgs::msg::UInt8>(status_topic_name, 10);
149 }
150 return true;
151}
152
153
155{
156 yCTrace(RGBDSENSOR_NWS_ROS2, "Close");
157 detach();
158
159 sensor_p = nullptr;
160 fgCtrl = nullptr;
161
162 return true;
163}
164
165// PeriodicThread
166
168{
169 if (sensor_p!=nullptr) {
170 static int i = 0;
171 switch (sensor_p->getSensorStatus()) {
173 if (!writeData()) {
174 yCError(RGBDSENSOR_NWS_ROS2, "Image not captured.. check hardware configuration");
175 }
176 i = 0;
177 break;
179 if(i < 1000) {
180 if((i % 30) == 0) {
181 yCInfo(RGBDSENSOR_NWS_ROS2) << "Device not ready, waiting...";
182 }
183 } else {
184 yCWarning(RGBDSENSOR_NWS_ROS2) << "Device is taking too long to start..";
185 }
186 i++;
187 break;
188 default:
189 yCError(RGBDSENSOR_NWS_ROS2, "Sensor returned error");
190 }
191 } else {
192 yCError(RGBDSENSOR_NWS_ROS2, "Sensor interface is not valid");
193 }
194}
195
196
197/*
198 * WrapperSingle interface
199 */
200
202{
203 if(poly)
204 {
205 poly->view(sensor_p);
206 poly->view(fgCtrl);
207 }
208
209 if(sensor_p == nullptr)
210 {
211 yCError(RGBDSENSOR_NWS_ROS2) << "Attached device has no valid IRGBDSensor interface.";
212 return false;
213 }
214
215 if(fgCtrl == nullptr)
216 {
217 yCWarning(RGBDSENSOR_NWS_ROS2) << "Attached device has no valid IFrameGrabberControls interface.";
218 }
219
220 return PeriodicThread::start();
221}
222
223
225{
228
229 sensor_p = nullptr;
230 if (fgCtrl)
231 {
232 fgCtrl = nullptr;
233 }
234 return true;
235}
236
237bool RgbdSensor_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo,
238 const std::string& frame_id,
239 const SensorType& sensorType)
240{
241 double phyF = 0.0;
242 double fx = 0.0;
243 double fy = 0.0;
244 double cx = 0.0;
245 double cy = 0.0;
246 double k1 = 0.0;
247 double k2 = 0.0;
248 double t1 = 0.0;
249 double t2 = 0.0;
250 double k3 = 0.0;
251
252 std::string distModel;
253 std::string currentSensor;
254 yarp::os::Property camData;
255 std::vector<param<double> > parVector;
256 bool ok;
257
258 currentSensor = sensorType == COLOR_SENSOR ? "Rgb" : "Depth";
259 ok = sensorType == COLOR_SENSOR ? sensor_p->getRgbIntrinsicParam(camData) : sensor_p->getDepthIntrinsicParam(camData);
260
261 if (!ok)
262 {
263 yCError(RGBDSENSOR_NWS_ROS2) << "Unable to get intrinsic param from" << currentSensor << "sensor!";
264 return false;
265 }
266
267 if(!camData.check("distortionModel"))
268 {
269 yCWarning(RGBDSENSOR_NWS_ROS2) << "Missing distortion model";
270 return false;
271 }
272
273 distModel = camData.find("distortionModel").asString();
274
275 parVector.emplace_back(phyF,"physFocalLength");
276 parVector.emplace_back(fx,"focalLengthX");
277 parVector.emplace_back(fy,"focalLengthY");
278 parVector.emplace_back(cx,"principalPointX");
279 parVector.emplace_back(cy,"principalPointY");
280
281 if (distModel != "none")
282 {
283 parVector.emplace_back(k1,"k1");
284 parVector.emplace_back(k2,"k2");
285 parVector.emplace_back(t1,"t1");
286 parVector.emplace_back(t2,"t2");
287 parVector.emplace_back(k3,"k3");
288 }
289
290 for(auto& par : parVector) {
291 if(!camData.check(par.parname)) {
292 yCWarning(RGBDSENSOR_NWS_ROS2) << "Driver has not the param:" << par.parname;
293 return false;
294 }
295 *(par.var) = camData.find(par.parname).asFloat64();
296 }
297
298 cameraInfo.header.frame_id = frame_id;
299 cameraInfo.width = sensorType == COLOR_SENSOR ? sensor_p->getRgbWidth() : sensor_p->getDepthWidth();
300 cameraInfo.height = sensorType == COLOR_SENSOR ? sensor_p->getRgbHeight() : sensor_p->getDepthHeight();
301 cameraInfo.distortion_model = distModel;
302
303 if (distModel != "none")
304 {
305 cameraInfo.d.resize(5);
306 cameraInfo.d[0] = k1;
307 cameraInfo.d[1] = k2;
308 cameraInfo.d[2] = t1;
309 cameraInfo.d[3] = t2;
310 cameraInfo.d[4] = k3;
311 }
312
313 cameraInfo.k[0] = fx; cameraInfo.k[1] = 0; cameraInfo.k[2] = cx;
314 cameraInfo.k[3] = 0; cameraInfo.k[4] = fy; cameraInfo.k[5] = cy;
315 cameraInfo.k[6] = 0; cameraInfo.k[7] = 0; cameraInfo.k[8] = 1;
316
317 /*
318 * ROS documentation on cameraInfo message:
319 * "Rectification matrix (stereo cameras only)
320 * A rotation matrix aligning the camera coordinate system to the ideal
321 * stereo image plane so that epipolar lines in both stereo images are
322 * parallel."
323 * useless in our case, it will be an identity matrix
324 */
325
326 cameraInfo.r[0] = 1; cameraInfo.r[1] = 0; cameraInfo.r[2] = 0;
327 cameraInfo.r[3] = 0; cameraInfo.r[4] = 1; cameraInfo.r[5] = 0;
328 cameraInfo.r[6] = 0; cameraInfo.r[7] = 0; cameraInfo.r[8] = 1;
329
330 cameraInfo.p[0] = fx; cameraInfo.p[1] = 0; cameraInfo.p[2] = cx; cameraInfo.p[3] = 0;
331 cameraInfo.p[4] = 0; cameraInfo.p[5] = fy; cameraInfo.p[6] = cy; cameraInfo.p[7] = 0;
332 cameraInfo.p[8] = 0; cameraInfo.p[9] = 0; cameraInfo.p[10] = 1; cameraInfo.p[11] = 0;
333
334 cameraInfo.binning_x = cameraInfo.binning_y = 0;
335 cameraInfo.roi.height = cameraInfo.roi.width = cameraInfo.roi.x_offset = cameraInfo.roi.y_offset = 0;
336 cameraInfo.roi.do_rectify = false;
337 return true;
338}
339
340
341bool RgbdSensor_nws_ros2::writeData()
342{
343 yarp::sig::FlexImage colorImage;
345 yarp::os::Stamp colorStamp;
346 yarp::os::Stamp depthStamp;
347 builtin_interfaces::msg::Time ros2Stamp;
348
349 if (!sensor_p->getImages(colorImage, depthImage, &colorStamp, &depthStamp)) {
350 return false;
351 }
352
353 static yarp::os::Stamp oldColorStamp = yarp::os::Stamp(0, 0);
354 static yarp::os::Stamp oldDepthStamp = yarp::os::Stamp(0, 0);
355 bool rgb_data_ok = true;
356 bool depth_data_ok = true;
357
358 if (((colorStamp.getTime() - oldColorStamp.getTime()) > 0) == false) {
359 rgb_data_ok=false;
360 //return true;
361 } else {
362 oldColorStamp = colorStamp;
363 }
364
365 if (((depthStamp.getTime() - oldDepthStamp.getTime()) > 0) == false) {
366 depth_data_ok=false;
367 //return true;
368 } else {
369 oldDepthStamp = depthStamp;
370 }
371
372 // Call setCamInfo only the first time
373 if (m_firstTime) {
374 setCamInfo(m_camInfoData.colorCamInfo, m_color_frame_id, COLOR_SENSOR);
375 setCamInfo(m_camInfoData.depthCamInfo, m_depth_frame_id, DEPTH_SENSOR);
376 m_firstTime = false;
377 }
378
379 // Use m_camInfoData for subsequent calls
380 if (rgb_data_ok) {
381 ros2Stamp.sec = static_cast<int>(colorStamp.getTime());
382 ros2Stamp.nanosec = static_cast<int>(1000000000UL * (colorStamp.getTime() - int(colorStamp.getTime())));
383 if(rosPublisher_color->get_subscription_count() > 0)
384 {
385 sensor_msgs::msg::Image rColorImage;
386 rColorImage.data.resize(colorImage.getRawImageSize());
387 rColorImage.width = colorImage.width();
388 rColorImage.height = colorImage.height();
389 memcpy(rColorImage.data.data(), colorImage.getRawImage(), colorImage.getRawImageSize());
390 rColorImage.encoding = yarp2RosPixelCode(colorImage.getPixelCode());
391 rColorImage.step = colorImage.getRowSize();
392 rColorImage.header.frame_id = m_color_frame_id;
393 rColorImage.header.stamp = ros2Stamp;
394 rColorImage.is_bigendian = 0;
395
396 rosPublisher_color->publish(rColorImage);
397 }
398
399 if (m_forceInfoSync) {
400 m_camInfoData.colorCamInfo.header.stamp = ros2Stamp;
401 }
402 if(rosPublisher_colorCaminfo->get_subscription_count() > 0)
403 {
404 rosPublisher_colorCaminfo->publish(m_camInfoData.colorCamInfo);
405 }
406 }
407
408 if (depth_data_ok)
409 {
410 ros2Stamp.sec = static_cast<int>(depthStamp.getTime());
411 ros2Stamp.nanosec = static_cast<int>(1000000000UL * (depthStamp.getTime() - int(depthStamp.getTime())));
412 if(rosPublisher_depth->get_subscription_count() > 0)
413 {
414 sensor_msgs::msg::Image rDepthImage;
415 rDepthImage.data.resize(depthImage.getRawImageSize());
416 rDepthImage.width = depthImage.width();
417 rDepthImage.height = depthImage.height();
418 memcpy(rDepthImage.data.data(), depthImage.getRawImage(), depthImage.getRawImageSize());
419 rDepthImage.encoding = yarp2RosPixelCode(depthImage.getPixelCode());
420 rDepthImage.step = depthImage.getRowSize();
421 rDepthImage.header.frame_id = m_depth_frame_id;
422 rDepthImage.header.stamp = ros2Stamp;
423 rDepthImage.is_bigendian = 0;
424
425 rosPublisher_depth->publish(rDepthImage);
426 }
427
428 if (m_forceInfoSync) {
429 m_camInfoData.depthCamInfo.header.stamp = ros2Stamp;
430 }
431 if(rosPublisher_depthCaminfo->get_subscription_count() > 0)
432 {
433 rosPublisher_depthCaminfo->publish(m_camInfoData.depthCamInfo);
434 }
435 }
436
437 if (m_pub_status_topic == 1 && m_rosPublisher_status) {
438 std_msgs::msg::UInt8 status_msg;
439 status_msg.data = rgb_data_ok*1 + depth_data_ok*2;
440 if(m_rosPublisher_status->get_subscription_count() > 0)
441 m_rosPublisher_status->publish(status_msg);
442 }
443
444 return true;
445}
#define DEFAULT_THREAD_PERIOD
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR16
Definition Image.h:59
@ VOCAB_PIXEL_RGBA
Definition Image.h:45
@ VOCAB_PIXEL_MONO16
Definition Image.h:43
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR8
Definition Image.h:58
@ VOCAB_PIXEL_BGRA
Definition Image.h:46
@ VOCAB_PIXEL_BGR
Definition Image.h:49
@ VOCAB_PIXEL_MONO_FLOAT
Definition Image.h:53
@ VOCAB_PIXEL_ENCODING_BAYER_RGGB8
Definition Image.h:62
@ VOCAB_PIXEL_ENCODING_BAYER_GRBG8
Definition Image.h:56
@ VOCAB_PIXEL_ENCODING_BAYER_GBRG16
Definition Image.h:61
@ VOCAB_PIXEL_ENCODING_BAYER_GRBG16
Definition Image.h:57
@ VOCAB_PIXEL_ENCODING_BAYER_GBRG8
Definition Image.h:60
@ VOCAB_PIXEL_MONO
Definition Image.h:42
@ VOCAB_PIXEL_ENCODING_BAYER_RGGB16
Definition Image.h:63
@ VOCAB_PIXEL_RGB
Definition Image.h:44
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
void run() override
Loop function.
bool close() override
Close the DeviceDriver.
bool view(T *&x)
Get an interface to the device driver.
virtual int getDepthWidth()=0
Return the height of each frame.
virtual yarp::dev::ReturnValue getDepthIntrinsicParam(yarp::os::Property &intrinsic)=0
Get the intrinsic parameters of the depth camera.
virtual int getDepthHeight()=0
Return the height of each frame.
virtual yarp::dev::ReturnValue 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 yarp::dev::ReturnValue getSensorStatus(RGBDSensor_status &status)=0
Get the current status of the sensor, using enum type.
virtual yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::os::Property &intrinsic)=0
Get the intrinsic parameters of the rgb camera.
virtual int getRgbHeight()=0
Return the height of each frame.
virtual int getRgbWidth()=0
Return the width of each frame.
A container for a device driver.
Definition PolyDriver.h:23
@ TraceType
Definition Log.h:92
bool isRunning() const
Returns true when the thread is started, false otherwise.
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
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
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
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:228
virtual std::string asString() const
Get string value.
Definition Value.cpp:246
Image class with user control of representation details.
Definition Image.h:361
Typed image class.
Definition Image.h:603
int getPixelCode() const override
Gets pixel type identifier.
Definition Image.h:719
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
size_t getRowSize() const
Size of the underlying image buffer rows.
Definition Image.h:197
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:462
size_t getRawImageSize() const
Access to the internal buffer size information (this is how much memory has been allocated for the im...
Definition Image.cpp:471
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
virtual int getPixelCode() const
Gets pixel type identifier.
Definition Image.cpp:375
yarp::sig::ImageOf< yarp::sig::PixelFloat > depthImage
This class is an utility.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
The main, catch-all namespace for YARP.
Definition dirs.h:16
sensor_msgs::msg::CameraInfo depthCamInfo
sensor_msgs::msg::CameraInfo colorCamInfo