YARP
Yet Another Robot Platform
WireImage.h
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 
7 #ifndef YARP2_WIREIMAGE
8 #define YARP2_WIREIMAGE
9 
10 #include <yarp/conf/system.h>
11 #include <yarp/os/SizedWriter.h>
14 #include <yarp/os/ManagedBytes.h>
15 #include <yarp/sig/Image.h>
16 #include <cstdlib>
17 #include <cstring>
18 
20 
21 namespace yarp {
22 namespace wire_rep_utils {
23 
24  /*
25  // layout for ROS:
26  // Header header
27  // uint32 seq --> +1
28  // time stamp --> int32 secs, int32 nsecs (sync by time)
29  // string frame_id --> string! argh / just pass it along
30  // uint32 height
31  // uint32 width
32  // string encoding --> string! argh
33  // uint8 is_bigendian
34  // uint32 step
35  // uint8[] data --> real payload
36  */
37 
40 public:
44 };
46 
59 private:
60  RosImageStamp ros_seq_stamp;
61  yarp::os::ManagedBytes ros_const_header;
62  const yarp::sig::FlexImage *image;
63 public:
65  ros_seq_stamp({0,0,0}),
66  image(nullptr)
67  {}
68 
69  void init(const yarp::sig::FlexImage& img,
70  const std::string& frame) {
71  image = &img;
74  if (!pbuf) {
75  ::exit(1);
76  }
77  yarp::os::ConnectionWriter& buf = *pbuf;
79  // probably need to translate encoding format better, but at
80  // a guess "rgb" and "bgr" will work ok.
81  std::string encoding =
82  yarp::os::Vocab32::decode(image->getPixelCode()).c_str();
83  switch (image->getPixelCode()) {
84  case VOCAB_PIXEL_BGR:
85  encoding = "bgr8";
86  break;
87  case VOCAB_PIXEL_RGB:
88  encoding = "rgb8";
89  break;
90  case VOCAB_PIXEL_MONO:
91  encoding = "mono8";
92  break;
93  case VOCAB_PIXEL_MONO16:
94  encoding = "mono16";
95  break;
97  encoding = "32FC1";
98  break;
99  }
100  buf.appendString(frame);
101  buf.appendInt32(image->height());
102  buf.appendInt32(image->width());
103  buf.appendString(encoding);
104  char is_bigendian = 0;
105  buf.appendBlock(&is_bigendian,1);
106  buf.appendInt32((image->width()*image->getPixelSize())+image->getPadding());
107  buf.appendInt32(image->getRawImageSize());
108  buf.getBuffer()->write(ss);
109  std::string hdr = ss.toString();
110  yarp::os::Bytes hdr_wrap((char*)hdr.c_str(),hdr.length());
111  ros_const_header = yarp::os::ManagedBytes(hdr_wrap);
112  ros_const_header.copy();
113  delete pbuf;
114  pbuf = 0 /*NULL*/;
115  }
116 
117  void update(const yarp::sig::FlexImage *img, int seq, double t) {
118  // We should check if img properties have changed. But we don't.
119  ros_seq_stamp.seq = seq;
120  ros_seq_stamp.sec = (int)(t);
121  ros_seq_stamp.nsec = (int)((t-(int)t)*1e9);
122  }
123 
124  size_t length() const override { return 3; }
125 
126  size_t headerLength() const override { return 0; }
127 
128  size_t length(size_t index) const override {
129  size_t result = 0;
130  switch (index) {
131  case 0:
132  result = sizeof(ros_seq_stamp);
133  break;
134  case 1:
135  result = ros_const_header.length();
136  break;
137  case 2:
138  result = image->getRawImageSize();
139  break;
140  default:
141  result = 0;
142  break;
143  }
144  return result;
145  }
146 
147  const char *data(size_t index) const override {
148  const char *result = 0 /*NULL*/;
149  switch (index) {
150  case 0:
151  result = (const char *)(&ros_seq_stamp);
152  break;
153  case 1:
154  result = ros_const_header.get();
155  break;
156  case 2:
157  result = (const char *)(image->getRawImage());
158  break;
159  }
160  return result;
161  }
162 
163  yarp::os::PortReader *getReplyHandler() override { return NULL; }
164 
165  yarp::os::Portable *getReference() override { return NULL; }
166 
167  bool dropRequested() override { return false; }
168 
169  void startWrite() const override {}
170 
171  void stopWrite() const override {}
172 };
173 
175 private:
177 public:
178  yarp::sig::FlexImage *checkForImage(yarp::os::SizedWriter& writer);
179 };
180 
181 } // namespace wire_rep_utils
182 } // namespace yarp
183 
184 #endif
float t
A simple abstraction for a block of bytes.
Definition: Bytes.h:25
An interface for writing to a network connection.
static ConnectionWriter * createBufferedConnectionWriter()
Create a connection writer implementation that stores to a buffer which can be read later using getBu...
virtual SizedWriter * getBuffer() const =0
virtual void appendInt32(std::int32_t data)=0
Send a representation of a 32-bit integer to the network connection.
virtual void appendBlock(const char *data, size_t len)=0
Send a block of data to the network connection.
virtual void appendString(const char *str, const char terminate='\n') final
Send a character sequence to the network connection.
An abstraction for a block of bytes, with optional responsibility for allocating/destroying that bloc...
Definition: ManagedBytes.h:22
void copy()
Makes sure data block is owned, making a copy if necessary.
const char * get() const
Interface implemented by all objects that can read themselves from the network, such as Bottle object...
Definition: PortReader.h:25
This is a base class for objects that can be both read from and be written to the YARP network.
Definition: Portable.h:26
Minimal requirements for an efficient Writer.
Definition: SizedWriter.h:33
virtual void write(OutputStream &os)
Definition: SizedWriter.cpp:16
An OutputStream that produces a string.
Image class with user control of representation details.
Definition: Image.h:414
size_t width() const
Gets width of image in pixels.
Definition: Image.h:166
size_t getPadding() const
Returns the number of padding bytes.
Definition: Image.h:205
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition: Image.cpp:541
virtual size_t getPixelSize() const
Gets pixel size in memory in bytes.
Definition: Image.cpp:435
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 height() const
Gets height of image in pixels.
Definition: Image.h:172
virtual int getPixelCode() const
Gets pixel type identifier.
Definition: Image.cpp:440
As far as YARP is concerned, on the wire to/from ROS a raw image has:
Definition: WireImage.h:58
yarp::os::Portable * getReference() override
Definition: WireImage.h:165
void stopWrite() const override
Call when all writing is finished.
Definition: WireImage.h:171
yarp::os::PortReader * getReplyHandler() override
Definition: WireImage.h:163
void init(const yarp::sig::FlexImage &img, const std::string &frame)
Definition: WireImage.h:69
void update(const yarp::sig::FlexImage *img, int seq, double t)
Definition: WireImage.h:117
size_t length(size_t index) const override
Definition: WireImage.h:128
size_t headerLength() const override
Definition: WireImage.h:126
void startWrite() const override
Call when writing is about to begin.
Definition: WireImage.h:169
const char * data(size_t index) const override
Definition: WireImage.h:147
size_t length() const override
Definition: WireImage.h:124
@ VOCAB_PIXEL_MONO16
Definition: Image.h:46
@ VOCAB_PIXEL_BGR
Definition: Image.h:52
@ VOCAB_PIXEL_MONO_FLOAT
Definition: Image.h:56
@ VOCAB_PIXEL_MONO
Definition: Image.h:45
@ VOCAB_PIXEL_RGB
Definition: Image.h:47
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition: Vocab.cpp:33
std::int32_t NetInt32
Definition of the NetInt32 type.
Definition: NetInt32.h:30
The main, catch-all namespace for YARP.
Definition: dirs.h:16
#define YARP_END_PACK
Ends 1 byte packing for structs/classes.
Definition: system.h:191
#define YARP_BEGIN_PACK
Starts 1 byte packing for structs/classes.
Definition: system.h:190
#define YARP_wire_rep_utils_API
Definition: api.h:12