YARP
Yet Another Robot Platform
RGBDRosConversionUtils.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: BSD-3-Clause
4  */
5 
6 #include <cmath>
7 #include <algorithm>
8 #include <iomanip>
9 #include <cstdint>
10 
11 #include <yarp/os/LogComponent.h>
12 #include <yarp/os/Value.h>
13 #include <yarp/sig/ImageUtils.h>
15 
16 #include "RGBDRosConversionUtils.h"
17 #include "rosPixelCode.h"
18 
19 using namespace yarp::dev;
20 using namespace yarp::sig;
21 using namespace yarp::os;
22 using namespace yarp::dev::RGBDRosConversionUtils;
23 using namespace std;
24 
25 namespace {
26 YARP_LOG_COMPONENT(RGBD_ROS, "yarp.device.RGBDRosConversion")
27 }
28 
29 commonImageProcessor::commonImageProcessor(string cameradata_topic_name, string camerainfo_topic_name)
30 {
31  if (this->topic(cameradata_topic_name)==false)
32  {
33  yCError(RGBD_ROS) << "Error opening topic:" << cameradata_topic_name;
34  }
35  if (m_subscriber_camera_info.topic(camerainfo_topic_name) == false)
36  {
37  yCError(RGBD_ROS) << "Error opening topic:" << camerainfo_topic_name;
38  }
39  m_cameradata_topic_name = cameradata_topic_name;
40  m_camerainfo_topic_name = camerainfo_topic_name;
41  m_contains_rgb_data = false;
42  m_contains_depth_data = false;
43 }
44 commonImageProcessor::~commonImageProcessor()
45 {
46  this->close();
47  m_subscriber_camera_info.close();
48 }
49 
50 bool commonImageProcessor::getLastRGBData(yarp::sig::FlexImage& data, yarp::os::Stamp& stmp)
51 {
52  if (m_contains_rgb_data == false) { return false;}
53 
54  //this blocks untils the first data is received;
55  /*size_t counter = 0;
56  while (m_contains_rgb_data == false)
57  {
58  yarp::os::Time::delay(0.1);
59  if (counter++ > 100) { yCDebug(RGBD_ROS) << "Waiting for incoming rgb data..."; counter = 0; }
60  }*/
61 
62  m_port_mutex.lock();
63  data = m_lastRGBImage;
64  stmp = m_lastStamp;
65  m_port_mutex.unlock();
66  return true;
67 }
68 
69 bool commonImageProcessor::getLastDepthData(yarp::sig::ImageOf<yarp::sig::PixelFloat>& data, yarp::os::Stamp& stmp)
70 {
71  if (m_contains_depth_data == false) { return false;}
72 
73  //this blocks untils the first data is received;
74  /*size_t counter = 0;
75  while (m_contains_depth_data == false)
76  {
77  yarp::os::Time::delay(0.1);
78  if (counter++ > 100) { yCDebug(RGBD_ROS) << "Waiting for incoming depth data..."; counter = 0; }
79  }*/
80 
81  m_port_mutex.lock();
82  data = m_lastDepthImage;
83  stmp = m_lastStamp;
84  m_port_mutex.unlock();
85  return true;
86 }
87 
88 size_t commonImageProcessor::getWidth() const
89 {
90  return m_lastDepthImage.width();
91 }
92 
93 size_t commonImageProcessor::getHeight() const
94 {
95  return m_lastDepthImage.height();
96 }
97 
98 void commonImageProcessor::onRead(yarp::rosmsg::sensor_msgs::Image& v)
99 {
100  m_port_mutex.lock();
102  if (yarp_pixcode == VOCAB_PIXEL_RGB ||
103  yarp_pixcode == VOCAB_PIXEL_BGR)
104  {
105  m_lastRGBImage.setPixelCode(yarp_pixcode);
106  m_lastRGBImage.resize(v.width, v.height);
107  size_t c = 0;
108  for (auto it = v.data.begin(); it != v.data.end(); it++)
109  {
110  m_lastRGBImage.getRawImage()[c++]=*it;
111  }
112  m_lastStamp.update();
113  m_contains_rgb_data = true;
114  }
115  else if (v.encoding == TYPE_16UC1)
116  {
117  m_lastDepthImage.resize(v.width, v.height);
118  size_t c = 0;
119  uint16_t* p = (uint16_t*)(v.data.data());
120  uint16_t* siz = (uint16_t*)(v.data.data()) + (v.data.size() / sizeof(uint16_t));
121  int count = 0;
122  for (; p < siz; p++)
123  {
124  float value = float(*p) / 1000.0;
125  ((float*)(m_lastDepthImage.getRawImage()))[c++] = value;
126  count++;
127  }
128  m_lastStamp.update();
129  m_contains_depth_data = true;
130  }
131  else if (v.encoding == TYPE_32FC1)
132  {
133  m_lastDepthImage.resize(v.width, v.height);
134  size_t c = 0;
135  for (auto it = v.data.begin(); it != v.data.end(); it++)
136  {
137  m_lastDepthImage.getRawImage()[c++] = *it;
138  }
139  m_lastStamp.update();
140  m_contains_depth_data = true;
141  }
142  else
143  {
144  yCError(RGBD_ROS) << "Unsupported rgb/depth format:" << v.encoding;
145  }
146  m_port_mutex.unlock();
147 }
148 
149 bool commonImageProcessor::getFOV(double& horizontalFov, double& verticalFov) const
150 {
151  yarp::rosmsg::sensor_msgs::CameraInfo* tmp = m_subscriber_camera_info.read(true);
152  m_lastCameraInfo = *tmp;
154  params.focalLengthX = m_lastCameraInfo.K[0];
155  params.focalLengthY = m_lastCameraInfo.K[4];
156  params.principalPointX = m_lastCameraInfo.K[2];
157  params.principalPointY = m_lastCameraInfo.K[5];
158  yCError(RGBD_ROS) << "getIntrinsicParam not yet implemented";
159  return false;
160 }
161 
162 bool commonImageProcessor::getIntrinsicParam(yarp::os::Property& intrinsic) const
163 {
164  yarp::rosmsg::sensor_msgs::CameraInfo* tmp = m_subscriber_camera_info.read(true);
165  m_lastCameraInfo = *tmp;
166  intrinsic.clear();
168  params.focalLengthX = m_lastCameraInfo.K[0];
169  params.focalLengthY = m_lastCameraInfo.K[4];
170  params.principalPointX = m_lastCameraInfo.K[2];
171  params.principalPointY = m_lastCameraInfo.K[5];
172  // distortion model
173  if (m_lastCameraInfo.distortion_model=="plumb_bob")
174  {
175  params.distortionModel.type = YarpDistortion::YARP_PLUMB_BOB;
176  params.distortionModel.k1 = m_lastCameraInfo.D[0];
177  params.distortionModel.k2 = m_lastCameraInfo.D[1];
178  params.distortionModel.t1 = m_lastCameraInfo.D[2];
179  params.distortionModel.t2 = m_lastCameraInfo.D[3];
180  params.distortionModel.k3 = m_lastCameraInfo.D[4];
181  }
182  else
183  {
184  yCError(RGBD_ROS) << "Unsupported distortion model";
185  }
186  params.toProperty(intrinsic);
187  return false;
188 }
189 
190 
192 {
193  dest.setPixelCode(src.getPixelCode());
194  dest.setQuantum(src.getQuantum());
195  dest.setExternal(src.getRawImage(), src.width(), src.height());
196 }
197 
199 {
200  dest.setQuantum(src.getQuantum());
201  dest.setExternal(src.getRawImage(), src.width(), src.height());
202 }
203 
206  const string& frame_id,
207  const yarp::rosmsg::TickTime& timeStamp,
208  const unsigned int& seq)
209 {
210  dest.data.resize(src.getRawImageSize());
211  dest.width = src.width();
212  dest.height = src.height();
213  memcpy(dest.data.data(), src.getRawImage(), src.getRawImageSize());
215  dest.step = src.getRowSize();
216  dest.header.frame_id = frame_id;
217  dest.header.stamp = timeStamp;
218  dest.header.seq = seq;
219  dest.is_bigendian = 0;
220 }
221 
224  const string& frame_id,
225  const yarp::rosmsg::TickTime& timeStamp,
226  const unsigned int& seq)
227 {
228  dest.data.resize(src.getRawImageSize());
229 
230  dest.width = src.width();
231  dest.height = src.height();
232 
233  memcpy(dest.data.data(), src.getRawImage(), src.getRawImageSize());
234 
236  dest.step = src.getRowSize();
237  dest.header.frame_id = frame_id;
238  dest.header.stamp = timeStamp;
239  dest.header.seq = seq;
240  dest.is_bigendian = 0;
241 }
A class for storing options and configuration information.
Definition: Property.h:34
void clear()
Remove all associations.
Definition: Property.cpp:1057
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:22
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: CameraInfo.h:371
std::vector< std::uint8_t > data
Definition: Image.h:62
yarp::rosmsg::std_msgs::Header header
Definition: Image.h:56
yarp::rosmsg::TickTime stamp
Definition: Header.h:45
Image class with user control of representation details.
Definition: Image.h:414
void setQuantum(size_t imgQuantum)
Definition: Image.h:429
void setPixelCode(int imgPixelCode)
Definition: Image.h:417
int getPixelCode() const override
Gets pixel type identifier.
Definition: Image.h:775
void setQuantum(size_t imgQuantum)
Definition: Image.cpp:501
size_t width() const
Gets width of image in pixels.
Definition: Image.h:166
void setExternal(const void *data, size_t imgWidth, size_t imgHeight)
Use this to wrap an external image.
Definition: Image.cpp:903
size_t getRowSize() const
Size of the underlying image buffer rows.
Definition: Image.h:192
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition: Image.cpp:541
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:550
size_t getQuantum() const
The size of a row is constrained to be a multiple of the "quantum".
Definition: Image.h:199
size_t height() const
Gets height of image in pixels.
Definition: Image.h:172
virtual int getPixelCode() const
Gets pixel type identifier.
Definition: Image.cpp:440
#define yCError(component,...)
Definition: LogComponent.h:154
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:77
@ VOCAB_PIXEL_BGR
Definition: Image.h:52
@ VOCAB_PIXEL_RGB
Definition: Image.h:47
void shallowCopyImages(const yarp::sig::FlexImage &src, yarp::sig::FlexImage &dest)
void deepCopyImages(const yarp::sig::FlexImage &src, yarp::rosmsg::sensor_msgs::Image &dest, const std::string &frame_id, const yarp::rosmsg::TickTime &timeStamp, const unsigned int &seq)
std::string yarp2RosPixelCode(int code)
int Ros2YarpPixelCode(const std::string &roscode)
An interface for the device drivers.
An interface to the operating system, including Port based communication.
Signal processing.
Definition: Image.h:22
#define TYPE_16UC1
Definition: rosPixelCode.h:38
#define TYPE_32FC1
Definition: rosPixelCode.h:42
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).
double focalLengthY
Result of the product of the physical focal length(mm) and the size sy of the individual imager eleme...
DistortionModel distortionModel
Distortion model of the image.
double focalLengthX
Result of the product of the physical focal length(mm) and the size sx of the individual imager eleme...
void toProperty(yarp::os::Property &intrinsic) const
toProperty, convert the struct to a Property.
double principalPointX
Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge.
double principalPointY
Vertical coordinate of the principal point of the image, as a pixel offset from the top edge.