YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FrameGrabber_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#include <yarp/dev/PolyDriver.h>
11
12#include <sensor_msgs/image_encodings.hpp>
13#include <Ros2Utils.h>
14
15namespace {
16YARP_LOG_COMPONENT(FRAMEGRABBER_NWS_ROS2, "yarp.device.frameGrabber_nws_ros2")
17
18// FIXME Copied from rgbdSensor_nws_ros2
19std::string yarp2RosPixelCode(int code)
20{
21 switch (code)
22 {
23 case VOCAB_PIXEL_BGR:
24 return sensor_msgs::image_encodings::BGR8;
26 return sensor_msgs::image_encodings::BGRA8;
28 return sensor_msgs::image_encodings::BAYER_BGGR16;
30 return sensor_msgs::image_encodings::BAYER_BGGR8;
32 return sensor_msgs::image_encodings::BAYER_GBRG16;
34 return sensor_msgs::image_encodings::BAYER_GBRG8;
36 return sensor_msgs::image_encodings::BAYER_GRBG16;
38 return sensor_msgs::image_encodings::BAYER_GRBG8;
40 return sensor_msgs::image_encodings::BAYER_RGGB16;
42 return sensor_msgs::image_encodings::BAYER_RGGB8;
44 return sensor_msgs::image_encodings::MONO8;
46 return sensor_msgs::image_encodings::MONO16;
47 case VOCAB_PIXEL_RGB:
48 return sensor_msgs::image_encodings::RGB8;
50 return sensor_msgs::image_encodings::RGBA8;
52 return sensor_msgs::image_encodings::TYPE_32FC1;
53 default:
54 return sensor_msgs::image_encodings::RGB8;
55 }
56}
57} // namespace
58
60 PeriodicThread(s_default_period)
61{
62}
63
64
69
70
72{
73 if (!m_active) {
74 return false;
75 }
76 m_active = false;
77
78 detach();
79
80 return true;
81}
82
83
85{
86 if (m_active) {
87 yCError(FRAMEGRABBER_NWS_ROS2, "Device is already opened");
88 return false;
89 }
90 parseParams(config);
91
92 if(m_namespace.empty())
93 {
95 } else {
97 }
98
99 if(m_node == nullptr) {
100 yCError(FRAMEGRABBER_NWS_ROS2) << " opening " << m_node_name << " Node, check your yarp-ROS2 network configuration\n";
101 return false;
102 }
103
104 publisher_image = m_node->create_publisher<sensor_msgs::msg::Image>(m_topic_name, 10);
105
106
107 // set "cameraInfoTopicName" and open publisher
108 std::string cameraInfoTopicName = m_topic_name.substr(0,m_topic_name.rfind('/')) + "/camera_info";
109 publisher_cameraInfo = m_node->create_publisher<sensor_msgs::msg::CameraInfo>(cameraInfoTopicName, 10);
110
111 yCInfo(FRAMEGRABBER_NWS_ROS2) << "Running, waiting for attach...";
112
113 m_active = true;
114
115 return true;
116}
117
119{
120 if (!poly->isValid()) {
121 yCError(FRAMEGRABBER_NWS_ROS2) << "Device " << poly << " to attach to is not valid ... cannot proceed";
122 return false;
123 }
124
125 PeriodicThread::setPeriod(m_period);
126
127 poly->view(iRgbVisualParams);
128 poly->view(iFrameGrabberImage);
129 poly->view(iPreciselyTimed);
130
131 if (iFrameGrabberImage == nullptr) {
132 yCError(FRAMEGRABBER_NWS_ROS2) << "IFrameGrabberImage interface is not available on the device";
133 return false;
134 }
135
136 if (iRgbVisualParams == nullptr) {
137 yCWarning(FRAMEGRABBER_NWS_ROS2) << "IRgbVisualParams interface is not available on the device";
138 }
139
140 return PeriodicThread::start();
141}
142
143
145{
148 }
149
150 iRgbVisualParams = nullptr;
151 iFrameGrabberImage = nullptr;
152 iPreciselyTimed = nullptr;
153
154 return true;
155}
156
158{
160 return true;
161}
162
164{
165 delete yarpimg;
166 yarpimg = nullptr;
167}
168
169
170// Publish the images on the buffered port
172{
173// if (false /* FIXME Can we check if there are subscribers connected in ROS2? */) {
174// // If no subscribers are connected, do not call getImage on the interface.
175// return;
176// }
177
178 if (iPreciselyTimed) {
179 m_stamp = iPreciselyTimed->getLastInputStamp();
180 } else {
181 m_stamp.update(yarp::os::Time::now());
182 }
183
184 if (iFrameGrabberImage)
185 {
186 if(publisher_image->get_subscription_count()>0){
187 if (iFrameGrabberImage->getImage(*yarpimg))
188 {
189 sensor_msgs::msg::Image rosimg;
190 rosimg.data.resize(yarpimg->getRawImageSize());
191 rosimg.width = yarpimg->width();
192 rosimg.height = yarpimg->height();
193 rosimg.encoding = yarp2RosPixelCode(yarpimg->getPixelCode());
194 rosimg.step = yarpimg->getRowSize();
195 rosimg.header.frame_id = m_frame_id;
196 // rosimg.header.stamp.sec = static_cast<int>(m_stamp.getTime()); // FIXME
197 // rosimg.header.stamp.nanosec = static_cast<int>(1000000000UL * (m_stamp.getTime() - int(m_stamp.getTime()))); // FIXME
198 rosimg.is_bigendian = 0;
199 memcpy(rosimg.data.data(), yarpimg->getRawImage(), yarpimg->getRawImageSize());
200 publisher_image->publish(rosimg);
201 }
202 else
203 {
204 yCError(FRAMEGRABBER_NWS_ROS2) << "Image not captured (getImage failed). Check hardware configuration.";
205 }
206 }
207 }
208 else
209 {
210 yCError(FRAMEGRABBER_NWS_ROS2) << "Invalid call to interface iFrameGrabberImage";
211 }
212
213 if (iRgbVisualParams && publisher_cameraInfo->get_subscription_count()>0)
214 {
215 sensor_msgs::msg::CameraInfo cameraInfo;
216 if (setCamInfo(cameraInfo)) {
217
218 publisher_cameraInfo->publish(cameraInfo);
219 }
220 }
221 else
222 {
223 yCError(FRAMEGRABBER_NWS_ROS2) << "Invalid call to interface iRgbVisualParams";
224 }
225}
226
227namespace {
228template <class T>
229struct param
230{
231 param(T& inVar, std::string inName) :
232 var(&inVar),
233 parname(std::move(inName))
234 {
235 }
236 T* var;
237 std::string parname;
238};
239} // namespace
240
241bool FrameGrabber_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo)
242{
243 yarp::os::Property camData;
244 if (!iRgbVisualParams->getRgbIntrinsicParam(camData)) {
245 yCErrorThreadOnce(FRAMEGRABBER_NWS_ROS2) << "Unable to get intrinsic param from rgb sensor!";
246 return false;
247 }
248
249 if (!camData.check("distortionModel")) {
250 yCWarning(FRAMEGRABBER_NWS_ROS2) << "Missing distortion model";
251 return false;
252 }
253
254 std::string distModel = camData.find("distortionModel").asString();
255 if (distModel != "plumb_bob") {
256 yCError(FRAMEGRABBER_NWS_ROS2) << "Distortion model not supported";
257 return false;
258 }
259
260 double phyF = 0.0;
261 double fx = 0.0;
262 double fy = 0.0;
263 double cx = 0.0;
264 double cy = 0.0;
265 double k1 = 0.0;
266 double k2 = 0.0;
267 double t1 = 0.0;
268 double t2 = 0.0;
269 double k3 = 0.0;
270
271 std::vector<param<double>> parVector;
272 parVector.emplace_back(phyF,"physFocalLength");
273 parVector.emplace_back(fx,"focalLengthX");
274 parVector.emplace_back(fy,"focalLengthY");
275 parVector.emplace_back(cx,"principalPointX");
276 parVector.emplace_back(cy,"principalPointY");
277 parVector.emplace_back(k1,"k1");
278 parVector.emplace_back(k2,"k2");
279 parVector.emplace_back(t1,"t1");
280 parVector.emplace_back(t2,"t2");
281 parVector.emplace_back(k3,"k3");
282
283 for(auto& par : parVector) {
284 if(!camData.check(par.parname)) {
285 yCWarning(FRAMEGRABBER_NWS_ROS2) << "Driver has not the param:" << par.parname;
286 return false;
287 }
288 *(par.var) = camData.find(par.parname).asFloat64();
289 }
290
291 cameraInfo.header.frame_id = m_frame_id;
292// cameraInfo.header.stamp.sec = static_cast<int>(m_stamp.getTime()); // FIXME
293// cameraInfo.header.stamp.nanosec = static_cast<int>(1000000000UL * (m_stamp.getTime() - int(m_stamp.getTime()))); // FIXME
294 cameraInfo.width = iRgbVisualParams->getRgbWidth();
295 cameraInfo.height = iRgbVisualParams->getRgbHeight();
296 cameraInfo.distortion_model = distModel;
297
298 cameraInfo.d.resize(5);
299 cameraInfo.d[0] = k1;
300 cameraInfo.d[1] = k2;
301 cameraInfo.d[2] = t1;
302 cameraInfo.d[3] = t2;
303 cameraInfo.d[4] = k3;
304
305 cameraInfo.k[0] = fx; cameraInfo.k[1] = 0; cameraInfo.k[2] = cx;
306 cameraInfo.k[3] = 0; cameraInfo.k[4] = fy; cameraInfo.k[5] = cy;
307 cameraInfo.k[6] = 0; cameraInfo.k[7] = 0; cameraInfo.k[8] = 1;
308
309 /*
310 * ROS documentation on cameraInfo message:
311 * "Rectification matrix (stereo cameras only)
312 * A rotation matrix aligning the camera coordinate system to the ideal
313 * stereo image plane so that epipolar lines in both stereo images are
314 * parallel."
315 * useless in our case, it will be an identity matrix
316 */
317
318 cameraInfo.r[0] = 1; cameraInfo.r[1] = 0; cameraInfo.r[2] = 0;
319 cameraInfo.r[3] = 0; cameraInfo.r[4] = 1; cameraInfo.r[5] = 0;
320 cameraInfo.r[6] = 0; cameraInfo.r[7] = 0; cameraInfo.r[8] = 1;
321
322 cameraInfo.p[0] = fx; cameraInfo.p[1] = 0; cameraInfo.p[2] = cx; cameraInfo.p[3] = 0;
323 cameraInfo.p[4] = 0; cameraInfo.p[5] = fy; cameraInfo.p[6] = cy; cameraInfo.p[7] = 0;
324 cameraInfo.p[8] = 0; cameraInfo.p[9] = 0; cameraInfo.p[10] = 1; cameraInfo.p[11] = 0;
325
326 cameraInfo.binning_x = cameraInfo.binning_y = 0;
327 cameraInfo.roi.height = cameraInfo.roi.width = cameraInfo.roi.x_offset = cameraInfo.roi.y_offset = 0;
328 cameraInfo.roi.do_rectify = false;
329 return true;
330}
@ 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
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
void run() override
Loop function.
bool threadInit() override
Initialization method.
bool detach() override
Detach the object (you must have first called attach).
void threadRelease() override
Release method.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
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 getImage(ImageType &image)=0
Get an image from the frame grabber.
virtual yarp::os::Stamp getLastInputStamp()=0
Return the time stamp relative to the last acquisition.
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
bool isValid() const
Check if device is valid.
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
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
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
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
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
#define yCErrorThreadOnce(component,...)
STL namespace.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121