YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FrameGrabber_nws_ros.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
8
10#include <yarp/os/LogStream.h>
11
12#include <yarp/dev/PolyDriver.h>
13
14#include <rosPixelCode.h>
15
16namespace {
17YARP_LOG_COMPONENT(FRAMEGRABBER_NWS_ROS, "yarp.device.frameGrabber_nws_ros")
18} // namespace
19
20
22 PeriodicThread(s_default_period)
23{
24}
25
26
31
32
34{
35 if (!m_active) {
36 return false;
37 }
38 m_active = false;
39
40 detach();
41
42 publisherPort_image.interrupt();
43 publisherPort_image.close();
44
45 publisherPort_cameraInfo.interrupt();
46 publisherPort_cameraInfo.close();
47
48 if (node != nullptr) {
49 node->interrupt();
50 delete node;
51 node = nullptr;
52 }
53
54 return true;
55}
56
57
59{
60 if (m_active) {
61 yCError(FRAMEGRABBER_NWS_ROS, "Device is already opened");
62 return false;
63 }
64
65 // Check "period" option
66 if (config.check("period", "refresh period(in s) of the broadcasted values through yarp ports") && config.find("period").isFloat64()) {
67 m_period = config.find("period").asFloat64();
68 } else {
69 yCInfo(FRAMEGRABBER_NWS_ROS)
70 << "Period parameter not found, using default of"
71 << s_default_period
72 << "seconds";
73 }
74 PeriodicThread::setPeriod(m_period);
75
76 // Check "node_name" option and open node
77 if (!config.check("node_name"))
78 {
79 yCError(FRAMEGRABBER_NWS_ROS) << "Missing node_name parameter";
80 return false;
81 }
82 std::string nodeName = config.find("node_name").asString();
83 if (nodeName.c_str()[0] != '/') {
84 yCError(FRAMEGRABBER_NWS_ROS) << "Missing '/' in node_name parameter";
85 return false;
86 }
87
88 node = new yarp::os::Node(nodeName);
89
90 // Check "topic_name" option and open publisher
91 if (!config.check("topic_name"))
92 {
93 yCError(FRAMEGRABBER_NWS_ROS) << "Missing topic_name parameter";
94 return false;
95 }
96 std::string topicName = config.find("topic_name").asString();
97 if (topicName.c_str()[0] != '/') {
98 yCError(FRAMEGRABBER_NWS_ROS) << "Missing '/' in topic_name parameter";
99 return false;
100 }
101
102 // set "imageTopicName" and open publisher
103 if (!publisherPort_image.topic(topicName)) {
104 yCError(FRAMEGRABBER_NWS_ROS) << "Unable to publish data on " << topicName << " topic, check your yarp-ROS network configuration";
105 return false;
106 }
107
108 // set "cameraInfoTopicName" and open publisher
109
110
111 std::string cameraInfoTopicName = topicName.substr(0,topicName.rfind('/')) + "/camera_info";
112 if (!publisherPort_cameraInfo.topic(cameraInfoTopicName)) {
113 yCError(FRAMEGRABBER_NWS_ROS) << "Unable to publish data on" << cameraInfoTopicName << "topic, check your yarp-ROS network configuration";
114 return false;
115 }
116
117 // Check "frame_id" option
118 if (!config.check("frame_id"))
119 {
120 yCError(FRAMEGRABBER_NWS_ROS) << "Missing frame_id parameter";
121 return false;
122 }
123 m_frameId = config.find("frame_id").asString();
124
125 yCInfo(FRAMEGRABBER_NWS_ROS) << "Running, waiting for attach...";
126
127 m_active = true;
128
129 return true;
130}
131
133{
134 if (!poly->isValid()) {
135 yCError(FRAMEGRABBER_NWS_ROS) << "Device " << poly << " to attach to is not valid ... cannot proceed";
136 return false;
137 }
138
139 poly->view(iRgbVisualParams);
140 poly->view(iFrameGrabberImage);
141 poly->view(iPreciselyTimed);
142
143 if (iFrameGrabberImage == nullptr) {
144 yCError(FRAMEGRABBER_NWS_ROS) << "IFrameGrabberImage interface is not available on the device";
145 return false;
146 }
147
148 if (iRgbVisualParams == nullptr) {
149 yCWarning(FRAMEGRABBER_NWS_ROS) << "IRgbVisualParams interface is not available on the device";
150 }
151
152 return PeriodicThread::start();
153}
154
155
157{
160 }
161
162 iRgbVisualParams = nullptr;
163 iFrameGrabberImage = nullptr;
164 iPreciselyTimed = nullptr;
165
166 return true;
167}
168
170{
172 return true;
173}
174
176{
177 delete img;
178 img = nullptr;
179}
180
181
182// Publish the images on the buffered port
184{
185 if (publisherPort_image.getOutputCount() == 0 && publisherPort_cameraInfo.getOutputCount() == 0) {
186 // If no ports are connected, do not call getImage on the interface.
187 return;
188 }
189
190 if (iPreciselyTimed) {
191 m_stamp = iPreciselyTimed->getLastInputStamp();
192 } else {
193 m_stamp.update(yarp::os::Time::now());
194 }
195
196 if (iFrameGrabberImage && publisherPort_image.getOutputCount() > 0) {
197 iFrameGrabberImage->getImage(*img);
198 auto& image = publisherPort_image.prepare();
199
200 image.data.resize(img->getRawImageSize());
201 image.width = img->width();
202 image.height = img->height();
204 image.step = img->getRowSize();
205 image.header.frame_id = m_frameId;
206 image.header.stamp = m_stamp.getTime();
207 image.header.seq = m_stamp.getCount();
208 image.is_bigendian = 0;
209
210 memcpy(image.data.data(), img->getRawImage(), img->getRawImageSize());
211
212 publisherPort_image.setEnvelope(m_stamp);
213 publisherPort_image.write();
214 }
215
216 if (iRgbVisualParams && publisherPort_cameraInfo.getOutputCount() > 0) {
217 auto& cameraInfo = publisherPort_cameraInfo.prepare();
218
219 if (setCamInfo(cameraInfo)) {
220 publisherPort_cameraInfo.setEnvelope(m_stamp);
221 publisherPort_cameraInfo.write();
222 } else {
223 publisherPort_cameraInfo.unprepare();
224 }
225 }
226}
227
228namespace {
229template <class T>
230struct param
231{
232 param(T& inVar, std::string inName) :
233 var(&inVar),
234 parname(std::move(inName))
235 {
236 }
237 T* var;
238 std::string parname;
239};
240} // namespace
241
242bool FrameGrabber_nws_ros::setCamInfo(yarp::rosmsg::sensor_msgs::CameraInfo& cameraInfo)
243{
244 yCAssert(FRAMEGRABBER_NWS_ROS, iRgbVisualParams);
245
246 yarp::os::Property camData;
247 if (!iRgbVisualParams->getRgbIntrinsicParam(camData)) {
248 yCErrorThreadOnce(FRAMEGRABBER_NWS_ROS) << "Unable to get intrinsic param from rgb sensor!";
249 return false;
250 }
251
252 if (!camData.check("distortionModel")) {
253 yCWarning(FRAMEGRABBER_NWS_ROS) << "Missing distortion model";
254 return false;
255 }
256
257 std::string distModel = camData.find("distortionModel").asString();
258 if (distModel != "plumb_bob") {
259 yCError(FRAMEGRABBER_NWS_ROS) << "Distortion model not supported";
260 return false;
261 }
262
263 double phyF = 0.0;
264 double fx = 0.0;
265 double fy = 0.0;
266 double cx = 0.0;
267 double cy = 0.0;
268 double k1 = 0.0;
269 double k2 = 0.0;
270 double t1 = 0.0;
271 double t2 = 0.0;
272 double k3 = 0.0;
273
274 std::vector<param<double>> parVector;
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 parVector.emplace_back(k1,"k1");
281 parVector.emplace_back(k2,"k2");
282 parVector.emplace_back(t1,"t1");
283 parVector.emplace_back(t2,"t2");
284 parVector.emplace_back(k3,"k3");
285
286 for(auto& par : parVector) {
287 if(!camData.check(par.parname)) {
288 yCWarning(FRAMEGRABBER_NWS_ROS) << "Driver has not the param:" << par.parname;
289 return false;
290 }
291 *(par.var) = camData.find(par.parname).asFloat64();
292 }
293
294 cameraInfo.header.frame_id = m_frameId;
295 cameraInfo.header.seq = m_stamp.getCount();
296 cameraInfo.header.stamp = m_stamp.getTime();
297 cameraInfo.width = iRgbVisualParams->getRgbWidth();
298 cameraInfo.height = iRgbVisualParams->getRgbHeight();
299 cameraInfo.distortion_model = distModel;
300
301 cameraInfo.D.resize(5);
302 cameraInfo.D[0] = k1;
303 cameraInfo.D[1] = k2;
304 cameraInfo.D[2] = t1;
305 cameraInfo.D[3] = t2;
306 cameraInfo.D[4] = k3;
307
308 cameraInfo.K.resize(9);
309 cameraInfo.K[0] = fx; cameraInfo.K[1] = 0; cameraInfo.K[2] = cx;
310 cameraInfo.K[3] = 0; cameraInfo.K[4] = fy; cameraInfo.K[5] = cy;
311 cameraInfo.K[6] = 0; cameraInfo.K[7] = 0; cameraInfo.K[8] = 1;
312
313 /*
314 * ROS documentation on cameraInfo message:
315 * "Rectification matrix (stereo cameras only)
316 * A rotation matrix aligning the camera coordinate system to the ideal
317 * stereo image plane so that epipolar lines in both stereo images are
318 * parallel."
319 * useless in our case, it will be an identity matrix
320 */
321
322 cameraInfo.R.assign(9, 0);
323 cameraInfo.R.at(0) = cameraInfo.R.at(4) = cameraInfo.R.at(8) = 1;
324
325 cameraInfo.P.resize(12);
326 cameraInfo.P[0] = fx; cameraInfo.P[1] = 0; cameraInfo.P[2] = cx; cameraInfo.P[3] = 0;
327 cameraInfo.P[4] = 0; cameraInfo.P[5] = fy; cameraInfo.P[6] = cy; cameraInfo.P[7] = 0;
328 cameraInfo.P[8] = 0; cameraInfo.P[9] = 0; cameraInfo.P[10] = 1; cameraInfo.P[11] = 0;
329
330 cameraInfo.binning_x = cameraInfo.binning_y = 0;
331 cameraInfo.roi.height = cameraInfo.roi.width = cameraInfo.roi.x_offset = cameraInfo.roi.y_offset = 0;
332 cameraInfo.roi.do_rectify = false;
333 return true;
334}
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
void run() override
Loop function.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
bool close() override
Close the DeviceDriver.
bool threadInit() override
Initialization method.
void threadRelease() override
Release method.
bool view(T *&x)
Get an interface to the device driver.
virtual bool 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 bool 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.
int getOutputCount() override
Determine how many output connections this port has.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
The Node class.
Definition Node.h:23
void interrupt()
interrupt delegates the call to the Node port interrupt.
Definition Node.cpp:597
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.
void close() override
Stop port activity.
Definition Publisher.h:84
bool topic(const std::string &name)
Set topic to publish to.
Definition Publisher.h:63
bool unprepare()
Give the last prepared object back to YARP without writing it.
Definition Publisher.h:133
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition Publisher.h:90
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.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
int getCount() const
Get the sequence number.
Definition Stamp.cpp:29
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:222
virtual std::string asString() const
Get string value.
Definition Value.cpp:234
Typed image class.
Definition Image.h:616
int getPixelCode() const override
Gets pixel type identifier.
Definition Image.h:732
size_t width() const
Gets width of image in pixels.
Definition Image.h:163
size_t getRowSize() const
Size of the underlying image buffer rows.
Definition Image.h:189
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:542
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:551
size_t height() const
Gets height of image in pixels.
Definition Image.h:169
#define yCInfo(component,...)
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
#define yCErrorThreadOnce(component,...)
STL namespace.
std::string yarp2RosPixelCode(int code)
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121