YARP
Yet Another Robot Platform
FrameGrabber_nws_yarp.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
8 
9 #include <yarp/os/LogComponent.h>
10 #include <yarp/os/LogStream.h>
11 
12 #include <yarp/dev/PolyDriver.h>
13 
15 
16 namespace {
17 YARP_LOG_COMPONENT(FRAMEGRABBER_NWS_YARP, "yarp.device.frameGrabber_nws_yarp")
18 } // namespace
19 
20 
22  PeriodicThread(DEFAULT_THREAD_PERIOD)
23 {
24 }
25 
26 
28 {
29  close();
30 }
31 
32 
34 {
35  if (!active) {
36  return false;
37  }
38  active = false;
39 
40  detach();
41 
42  pImg.interrupt();
43  pImg.close();
44 
45  rpcPort.interrupt();
46  rpcPort.close();
47 
48  delete img;
49  img = nullptr;
50 
51  delete img_Raw;
52  img_Raw = nullptr;
53 
54  if (subdevice) {
55  subdevice->close();
56  delete subdevice;
57  subdevice = nullptr;
58  }
59 
60  isSubdeviceOwned = false;
61 
62  return true;
63 }
64 
65 
67 {
68  if (active) {
69  yCError(FRAMEGRABBER_NWS_YARP, "Device is already opened");
70  return false;
71  }
72 
73 
74  // Check "period" option
75  if (config.check("period", "refresh period(in s) of the broadcasted values through yarp ports") && config.find("period").isFloat64()) {
76  period = config.find("period").asFloat64();
77  } else {
78  yCInfo(FRAMEGRABBER_NWS_YARP)
79  << "Period parameter not found, using default of"
80  << DEFAULT_THREAD_PERIOD
81  << "seconds";
82  }
83  PeriodicThread::setPeriod(period);
84 
85 
86  // Check "capabilities" option
87  if (config.check("capabilities", "two capabilities supported, COLOR and RAW respectively for rgb and raw streaming")) {
88  if (config.find("capabilities").asString() == "COLOR") {
89  cap = COLOR;
90  } else if (config.find("capabilities").asString() == "RAW") {
91  cap = RAW;
92  }
93  } else {
94  yCWarning(FRAMEGRABBER_NWS_YARP) << "'capabilities' parameter not found or misspelled, the option available are COLOR(default) and RAW, using default";
95  }
96 
97 
98  // Check "no_drop" option
99  noDrop = config.check("no_drop", "if present, use strict policy for sending data"); // FIXME DRDANZ
100 
101 
102  // Check "name" option and open ports
103  std::string pImg_Name = config.check("name", yarp::os::Value("/grabber"), "name of port to send data on").asString();
104  std::string rpcPort_Name = pImg_Name + "/rpc";
105 
106  if (!rpcPort.open(rpcPort_Name)) {
107  yCError(FRAMEGRABBER_NWS_YARP) << "Unable to open rpc Port" << rpcPort_Name.c_str();
108  return false;
109  }
110  rpcPort.setReader(*this);
111 
112  pImg.promiseType(yarp::os::Type::byName("yarp/image"));
113  pImg.setWriteOnly();
114  pImg.setStrict(noDrop);
115  if (!pImg.open(pImg_Name)) {
116  yCError(FRAMEGRABBER_NWS_YARP) << "Unable to open image streaming Port" << pImg_Name.c_str();
117  return false;
118  }
119  pImg.setReader(*this);
120 
121 
122  // Check "subdevice" option and eventually open the device
123  isSubdeviceOwned = config.check("subdevice");
124  if (isSubdeviceOwned) {
126  subdevice = new yarp::dev::PolyDriver;
127  p.fromString(config.toString());
128  if (cap == COLOR) {
129  p.put("pixelType", VOCAB_PIXEL_RGB);
130  } else {
131  p.put("pixelType", VOCAB_PIXEL_MONO);
132  }
133 
134  p.setMonitor(config.getMonitor(), "subdevice"); // pass on any monitoring
135  p.unput("device");
136  p.put("device", config.find("subdevice").asString()); // subdevice was already checked before
137 
138  // if errors occurred during open, quit here.
139  subdevice->open(p);
140 
141  if (!(subdevice->isValid())) {
142  yCError(FRAMEGRABBER_NWS_YARP, "Unable to open subdevice");
143  return false;
144  }
145  if (!attach(subdevice)) {
146  yCError(FRAMEGRABBER_NWS_YARP, "Unable to attach subdevice");
147  return false;
148  }
149  } else {
150  yCInfo(FRAMEGRABBER_NWS_YARP) << "Running, waiting for attach...";
151  }
152 
153  active = true;
154 
155  return true;
156 }
157 
159 {
160  if (!poly->isValid()) {
161  yCError(FRAMEGRABBER_NWS_YARP) << "Device " << poly << " to attach to is not valid ... cannot proceed";
162  return false;
163  }
164 
165  poly->view(iRgbVisualParams);
166  poly->view(iFrameGrabberImage);
167  poly->view(iFrameGrabberImageRaw);
168  poly->view(iFrameGrabberControls);
169  poly->view(iFrameGrabberControlsDC1394);
170  poly->view(iPreciselyTimed);
171 
172  switch (cap) {
173  case COLOR: {
174  if (iFrameGrabberImage == nullptr) {
175  yCError(FRAMEGRABBER_NWS_YARP) << "Capability \"COLOR\" required not supported";
176  return false;
177  }
178  } break;
179  case RAW: {
180  if (iFrameGrabberImageRaw == nullptr) {
181  yCError(FRAMEGRABBER_NWS_YARP) << "Capability \"RAW\" required not supported";
182  return false;
183  }
184  }
185  }
186 
187  if (iRgbVisualParams == nullptr) {
188  yCWarning(FRAMEGRABBER_NWS_YARP) << "Targets has not IVisualParamInterface, some features cannot be available";
189  }
190 
191  // Configuring parsers
192  if (iFrameGrabberImage != nullptr) {
193  if (!(frameGrabberImage_Responder.configure(iFrameGrabberImage))) {
194  yCError(FRAMEGRABBER_NWS_YARP) << "Error configuring interfaces for parsers";
195  return false;
196  }
197  }
198 
199  if (iFrameGrabberImageRaw != nullptr) {
200  if (!(frameGrabberImageRaw_Responder.configure(iFrameGrabberImageRaw))) {
201  yCError(FRAMEGRABBER_NWS_YARP) << "Error configuring interfaces for parsers";
202  return false;
203  }
204  }
205 
206  if (iRgbVisualParams != nullptr) {
207  if (!(rgbVisualParams_Responder.configure(iRgbVisualParams))) {
208  yCError(FRAMEGRABBER_NWS_YARP) << "Error configuring interfaces for parsers";
209  return false;
210  }
211  }
212  if (iFrameGrabberControls != nullptr) {
213  if (!(frameGrabberControls_Responder.configure(iFrameGrabberControls))) {
214  yCError(FRAMEGRABBER_NWS_YARP) << "Error configuring interfaces for parsers";
215  return false;
216  }
217  }
218 
219  if (iFrameGrabberControlsDC1394 != nullptr) {
220  if (!(frameGrabberControlsDC1394_Responder.configure(iFrameGrabberControlsDC1394))) {
221  yCError(FRAMEGRABBER_NWS_YARP) << "Error configuring interfaces for parsers";
222  return false;
223  }
224  }
225 
226  return PeriodicThread::start();
227 }
228 
229 
231 {
234  }
235 
236  iRgbVisualParams = nullptr;
237  iFrameGrabberImage = nullptr;
238  iFrameGrabberImageRaw = nullptr;
239  iFrameGrabberControls = nullptr;
240  iFrameGrabberControlsDC1394 = nullptr;
241  iPreciselyTimed = nullptr;
242 
243  return true;
244 }
245 
247 {
248  if (cap == COLOR) {
250  } else {
252  }
253 
254  return true;
255 }
256 
257 // Publish the images on the buffered port
259 {
260  if (pImg.getOutputCount() == 0) {
261  // If no ports are connected, do not call getImage on the interface.
262  return;
263  }
264 
265  yarp::sig::FlexImage& flex_i = pImg.prepare();
266 
267  if (cap == COLOR) {
268  if (iFrameGrabberImage != nullptr) {
269  iFrameGrabberImage->getImage(*img);
270  flex_i.swap(*img);
271  } else {
272  yCError(FRAMEGRABBER_NWS_YARP) << "Image not captured.. check hardware configuration";
273  }
274  }
275 
276  if (cap == RAW) {
277  if (iFrameGrabberImageRaw != nullptr) {
278  iFrameGrabberImageRaw->getImage(*img_Raw);
279  flex_i.swap(*img_Raw);
280  } else {
281  yCError(FRAMEGRABBER_NWS_YARP) << "Image not captured.. check hardware configuration";
282  }
283  }
284 
285  if (iPreciselyTimed) {
286  stamp = iPreciselyTimed->getLastInputStamp();
287  } else {
288  stamp.update(yarp::os::Time::now());
289  }
290  pImg.setEnvelope(stamp);
291 
292  pImg.write();
293 }
294 
295 // Respond to the RPC calls
297  yarp::os::Bottle& reply)
298 {
299  yarp::conf::vocab32_t code = command.get(0).asVocab32();
300  switch (code) {
302  return frameGrabberImage_Responder.respond(command, reply);
304  return frameGrabberImageRaw_Responder.respond(command, reply);
306  return frameGrabberControls_Responder.respond(command, reply);
308  return rgbVisualParams_Responder.respond(command, reply);
310  return frameGrabberControlsDC1394_Responder.respond(command, reply);
311  default:
312  yCError(FRAMEGRABBER_NWS_YARP) << "Command not recognized" << command.toString();
313  return false;
314  }
315 
316  return false;
317 }
constexpr yarp::conf::vocab32_t VOCAB_RGB_VISUAL_PARAMS
Definition: CameraVocabs.h:18
constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_CONTROL_DC1394
Definition: CameraVocabs.h:40
constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_IMAGERAW
Definition: CameraVocabs.h:17
constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_IMAGE
Definition: CameraVocabs.h:16
constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_CONTROL
Definition: CameraVocabs.h:39
constexpr double DEFAULT_THREAD_PERIOD
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
bool threadInit() override
Initialization method.
void run() override
Loop function.
bool close() override
Close the DeviceDriver.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply) override
Respond to a message.
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:74
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.
A container for a device driver.
Definition: PolyDriver.h:24
bool close() override
Close the DeviceDriver.
Definition: PolyDriver.cpp:173
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
bool open(const std::string &txt)
Construct and configure a device by its common name.
Definition: PolyDriver.cpp:140
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void setReader(PortReader &reader) override
Set an external reader for port data.
void close() override
Stop port activity.
void interrupt() override
Interrupt any current reads or writes attached to the port.
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:74
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:211
void promiseType(const Type &typ) override
Commit the port to a particular type of data.
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
void setReader(PortReader &reader) override
Set an external reader for port data.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
void setStrict(bool strict=true) override
Call this to strictly keep all messages, or allow old ones to be quietly dropped.
int getOutputCount() override
Determine how many output connections this port has.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
void setWriteOnly()
Shorthand for setInputMode(false), setOutputMode(true), setRpcMode(false)
Definition: Contactable.cpp:26
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:34
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
void unput(const std::string &key)
Remove the association from the given key to a value, if present.
Definition: Property.cpp:1046
A base class for nested structures that can be searched.
Definition: Searchable.h:66
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition: Stamp.cpp:124
static Type byName(const char *name)
Definition: Type.cpp:171
A single value (typically within a Bottle).
Definition: Value.h:45
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
virtual yarp::conf::vocab32_t asVocab32() const
Get vocabulary identifier as an integer.
Definition: Value.cpp:228
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
Definition: Value.cpp:150
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
bool configure(yarp::dev::IFrameGrabberControls *interface)
bool configure(yarp::dev::IFrameGrabberOf< ImageType > *interface)
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
bool configure(yarp::dev::IRgbVisualParams *interface)
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
Image class with user control of representation details.
Definition: Image.h:414
bool swap(Image &alt)
swap operator.
Definition: Image.cpp:888
#define yCInfo(component,...)
Definition: LogComponent.h:132
#define yCError(component,...)
Definition: LogComponent.h:154
#define yCWarning(component,...)
Definition: LogComponent.h:143
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:77
@ VOCAB_PIXEL_MONO
Definition: Image.h:45
@ VOCAB_PIXEL_RGB
Definition: Image.h:47
std::int32_t vocab32_t
Definition: numeric.h:78
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121