YARP
Yet Another Robot Platform
FrameGrabberOf_Responder-inl.h
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 #ifndef YARP_FRAMEGRABBER_PROTOCOL_FRAMEGRABBEROF_RESPONDER_INL_H
7 #define YARP_FRAMEGRABBER_PROTOCOL_FRAMEGRABBEROF_RESPONDER_INL_H
8 
9 #include <yarp/os/LogComponent.h>
10 #include <yarp/os/LogStream.h>
11 
13 #include <yarp/sig/ImageUtils.h>
14 
15 namespace {
16 YARP_LOG_COMPONENT(FRAMEGRABBEROF_RESPONDER, "yarp.proto.framegrabber.FrameGrabberOf_Responder")
17 }
18 
19 namespace yarp::proto::framegrabber {
20 
21 
22 template <typename ImageType,
23  yarp::conf::vocab32_t IfVocab,
24  yarp::conf::vocab32_t ImgVocab>
26 {
27  if (!interface) {
28  yCError(FRAMEGRABBEROF_RESPONDER) << "Invalid IFrameGrabberOf interface received";
29  return false;
30  }
31 
32  iFrameGrabberOf = interface;
33  return true;
34 }
35 
36 template <typename ImageType,
37  yarp::conf::vocab32_t IfVocab,
38  yarp::conf::vocab32_t ImgVocab>
40 {
41  if (!iFrameGrabberOf) {
42  reply.addVocab32(VOCAB_FAILED);
43  // FIXME C++17
44  if /* constexpr */ (std::is_same<ImageType, yarp::sig::ImageOf<yarp::sig::PixelRgb>>::value) {
45  reply.addString("Selected camera device has no IFrameGrabberImage interface");
46  yCError(FRAMEGRABBEROF_RESPONDER) << "Selected camera device has no IFrameGrabberImage interface";
47  } else if /* constexpr */ (std::is_same<ImageType, yarp::sig::ImageOf<yarp::sig::PixelMono>>::value) {
48  reply.addString("Selected camera device has no IFrameGrabberImageRaw interface");
49  yCError(FRAMEGRABBEROF_RESPONDER) << "Selected camera device has no IFrameGrabberImageRaw interface";
50  } else {
51  reply.addString("Selected camera device has no IFrameGrabberOf<ImageType> interface");
52  yCError(FRAMEGRABBEROF_RESPONDER) << "Selected camera device has no IFrameGrabberOf<ImageType> interface";
53  }
54  return false;
55  }
56 
57  if (cmd.get(0).asVocab32() != IfVocab) {
58  reply.addVocab32(VOCAB_FAILED);
59  yCError(FRAMEGRABBEROF_RESPONDER) << "Received a command not belonging to this interface.";
60  return false;
61  }
62 
63  switch (cmd.get(1).asVocab32()) {
64  case VOCAB_GET: {
65  switch (cmd.get(2).asVocab32()) {
66  case VOCAB_HEIGHT:
67  reply.clear();
68  reply.addVocab32(IfVocab);
69  reply.addVocab32(VOCAB_HEIGHT);
70  reply.addVocab32(VOCAB_IS);
71  reply.addInt32(iFrameGrabberOf->height());
72  break;
73  case VOCAB_WIDTH:
74  reply.clear();
75  reply.addVocab32(IfVocab);
76  reply.addVocab32(VOCAB_WIDTH);
77  reply.addVocab32(VOCAB_IS);
78  reply.addInt32(iFrameGrabberOf->width());
79  break;
80  case ImgVocab: {
81  reply.clear();
82  ImageType image;
83  iFrameGrabberOf->getImage(image);
84 
85  if (image.width() == 0 || image.height() == 0 || image.getRawImage() == nullptr) {
86  reply.addVocab32(VOCAB_FAILED);
87  reply.addString("Could not retrieve image from device.");
88  yCError(FRAMEGRABBEROF_RESPONDER) << "Could not retrieve image from device.";
89  return false;
90  }
91 
92  reply.addVocab32(IfVocab);
93  reply.addVocab32(ImgVocab);
94  reply.addVocab32(VOCAB_IS);
95  auto& b = reply.addList();
97  } break;
98  case VOCAB_CROP: {
99  reply.clear();
100  // If the device driver support it, use the device implementation, because it may be more efficient.
101  // If not, acquire the whole image and crop it here before sending it.
102 
103  if (cmd.size() != 5) {
104  reply.addVocab32(VOCAB_FAILED);
105  reply.addString("GetImageCrop failed: Invalid command received.");
106  yCError(FRAMEGRABBEROF_RESPONDER) << "GetImageCrop failed: Invalid command received, got " << cmd.toString();
107  return false;
108  }
109 
110  yarp::os::Bottle* list = cmd.get(4).asList();
111  if (!list) {
112  reply.addVocab32(VOCAB_FAILED);
113  reply.addString("GetImageCrop failed: Empty vertices list received.");
114  yCError(FRAMEGRABBEROF_RESPONDER) << "GetImageCrop failed: Empty vertices list received.";
115  return false;
116  }
117 
118  size_t nPoints = list->size() / 2; // divided by 2 because each pixel is identified by 2 numbers (u,v)
119 
121  vertices.resize(nPoints);
122 
123  for (size_t i = 0; i < nPoints; i++) {
124  vertices[i].first = list->get(i * 2).asInt32();
125  vertices[i].second = list->get(i * 2 + 1).asInt32();
126  }
127 
128  ImageType cropped;
129 
130  if (iFrameGrabberOf->getImageCrop(static_cast<cropType_id_t>(cmd.get(3).asVocab32()), vertices, cropped)) {
131  // use the device output
132  } else {
133  // In case the device has not yet implemented this feature, do it here (less efficient)
134  if (cmd.get(3).asVocab32() == YARP_CROP_RECT) {
135  if (nPoints != 2) {
136  reply.addVocab32(VOCAB_FAILED);
137  reply.addString("GetImageCrop failed: RECT mode requires 2 vertices.");
138  yCError(FRAMEGRABBEROF_RESPONDER) << "GetImageCrop failed: RECT mode requires 2 vertices, got " << nPoints;
139  return false;
140  }
141  ImageType full;
142  iFrameGrabberOf->getImage(full);
143 
144  if (!yarp::sig::utils::cropRect(full, vertices[0], vertices[1], cropped)) {
145  reply.addVocab32(VOCAB_FAILED);
146  reply.addString("GetImageCrop failed: utils::cropRect error.");
147  yCError(FRAMEGRABBEROF_RESPONDER, "GetImageCrop failed: utils::cropRect error: (%d, %d) (%d, %d)",
148  vertices[0].first,
149  vertices[0].second,
150  vertices[1].first,
151  vertices[1].second);
152  return false;
153  }
154  } else if (cmd.get(3).asVocab32() == YARP_CROP_LIST) {
155  reply.addVocab32(VOCAB_FAILED);
156  reply.addString("List type not yet implemented");
157  yCError(FRAMEGRABBEROF_RESPONDER) << "List type not yet implemented";
158  return false;
159  } else {
160  reply.addVocab32(VOCAB_FAILED);
161  reply.addString("Crop type unknown");
162  yCError(FRAMEGRABBEROF_RESPONDER) << "Crop type unknown";
163  return false;
164  }
165  }
166 
167  reply.addVocab32(IfVocab);
168  reply.addVocab32(VOCAB_CROP);
169  reply.addVocab32(VOCAB_IS);
170  auto& b = reply.addList();
172  return true;
173  } break;
174  }
175  break;
176 
177  case VOCAB_SET: // Nothing to do here yet
178  default:
179  reply.addVocab32(VOCAB_FAILED);
180  reply.addString("Received an unknown command");
181  yCError(FRAMEGRABBEROF_RESPONDER) << "Received an unknown command " << cmd.toString();
182  break;
183  }
184  }
185  return true;
186 }
187 
188 } // namespace yarp::proto::framegrabber
189 
190 
191 #endif // YARP_FRAMEGRABBER_PROTOCOL_FRAMEGRABBEROF_RESPONDER_INL_H
constexpr yarp::conf::vocab32_t VOCAB_CROP
Definition: CameraVocabs.h:38
constexpr yarp::conf::vocab32_t VOCAB_IS
Definition: GenericVocabs.h:14
constexpr yarp::conf::vocab32_t VOCAB_WIDTH
Definition: GenericVocabs.h:41
constexpr yarp::conf::vocab32_t VOCAB_GET
Definition: GenericVocabs.h:13
constexpr yarp::conf::vocab32_t VOCAB_FAILED
Definition: GenericVocabs.h:16
constexpr yarp::conf::vocab32_t VOCAB_SET
Definition: GenericVocabs.h:12
constexpr yarp::conf::vocab32_t VOCAB_HEIGHT
Definition: GenericVocabs.h:42
cropType_id_t
@ YARP_CROP_LIST
@ YARP_CROP_RECT
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:164
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:182
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:251
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:121
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:140
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:170
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:211
static bool copyPortable(const PortWriter &writer, PortReader &reader)
Copy one portable to another, via writing and reading.
Definition: Portable.cpp:16
virtual yarp::conf::vocab32_t asVocab32() const
Get vocabulary identifier as an integer.
Definition: Value.cpp:228
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:240
bool configure(yarp::dev::IFrameGrabberOf< ImageType > *interface)
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
Provides:
Definition: Vector.h:117
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:220
#define yCError(component,...)
Definition: LogComponent.h:213
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
std::int32_t vocab32_t
Definition: numeric.h:78
bool cropRect(const yarp::sig::Image &inImg, const std::pair< unsigned int, unsigned int > &vertex1, const std::pair< unsigned int, unsigned int > &vertex2, yarp::sig::Image &outImg)
Crop a rectangle area out of an image given two opposite vertices.
Definition: ImageUtils.cpp:113