YARP
Yet Another Robot Platform
WireImage.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms of the
7  * BSD-3-Clause license. See the accompanying LICENSE file for details.
8  */
9 
10 #ifndef YARP2_WIREIMAGE
11 #define YARP2_WIREIMAGE
12 
13 #include <yarp/conf/system.h>
14 #include <yarp/os/SizedWriter.h>
17 #include <yarp/os/ManagedBytes.h>
18 #include <yarp/sig/Image.h>
19 #include <cstdlib>
20 #include <cstring>
21 
23 
24 namespace yarp {
25 namespace wire_rep_utils {
26 
27  /*
28  // layout for ROS:
29  // Header header
30  // uint32 seq --> +1
31  // time stamp --> int32 secs, int32 nsecs (sync by time)
32  // string frame_id --> string! argh / just pass it along
33  // uint32 height
34  // uint32 width
35  // string encoding --> string! argh
36  // uint8 is_bigendian
37  // uint32 step
38  // uint8[] data --> real payload
39  */
40 
43 public:
47 };
49 
62 private:
63  RosImageStamp ros_seq_stamp;
64  yarp::os::ManagedBytes ros_const_header;
65  const yarp::sig::FlexImage *image;
66 public:
68  ros_seq_stamp({0,0,0}),
69  image(nullptr)
70  {}
71 
72  void init(const yarp::sig::FlexImage& img,
73  const std::string& frame) {
74  image = &img;
77  if (!pbuf) ::exit(1);
78  yarp::os::ConnectionWriter& buf = *pbuf;
80  // probably need to translate encoding format better, but at
81  // a guess "rgb" and "bgr" will work ok.
82  std::string encoding =
83  yarp::os::Vocab::decode(image->getPixelCode()).c_str();
84  switch (image->getPixelCode()) {
85  case VOCAB_PIXEL_BGR:
86  encoding = "bgr8";
87  break;
88  case VOCAB_PIXEL_RGB:
89  encoding = "rgb8";
90  break;
91  case VOCAB_PIXEL_MONO:
92  encoding = "mono8";
93  break;
94  case VOCAB_PIXEL_MONO16:
95  encoding = "mono16";
96  break;
98  encoding = "32FC1";
99  break;
100  }
101  buf.appendString(frame);
102  buf.appendInt32(image->height());
103  buf.appendInt32(image->width());
104  buf.appendString(encoding);
105  char is_bigendian = 0;
106  buf.appendBlock(&is_bigendian,1);
107  buf.appendInt32((image->width()*image->getPixelSize())+image->getPadding());
108  buf.appendInt32(image->getRawImageSize());
109  buf.getBuffer()->write(ss);
110  std::string hdr = ss.toString();
111  yarp::os::Bytes hdr_wrap((char*)hdr.c_str(),hdr.length());
112  ros_const_header = yarp::os::ManagedBytes(hdr_wrap);
113  ros_const_header.copy();
114  delete pbuf;
115  pbuf = 0 /*NULL*/;
116  }
117 
118  void update(const yarp::sig::FlexImage *img, int seq, double t) {
119  // We should check if img properties have changed. But we don't.
120  ros_seq_stamp.seq = seq;
121  ros_seq_stamp.sec = (int)(t);
122  ros_seq_stamp.nsec = (int)((t-(int)t)*1e9);
123  }
124 
125  size_t length() const override { return 3; }
126 
127  size_t headerLength() const override { return 0; }
128 
129  size_t length(size_t index) const override {
130  size_t result = 0;
131  switch (index) {
132  case 0:
133  result = sizeof(ros_seq_stamp);
134  break;
135  case 1:
136  result = ros_const_header.length();
137  break;
138  case 2:
139  result = image->getRawImageSize();
140  break;
141  default:
142  result = 0;
143  break;
144  }
145  return result;
146  }
147 
148  const char *data(size_t index) const override {
149  const char *result = 0 /*NULL*/;
150  switch (index) {
151  case 0:
152  result = (const char *)(&ros_seq_stamp);
153  break;
154  case 1:
155  result = ros_const_header.get();
156  break;
157  case 2:
158  result = (const char *)(image->getRawImage());
159  break;
160  }
161  return result;
162  }
163 
164  yarp::os::PortReader *getReplyHandler() override { return NULL; }
165 
166  yarp::os::Portable *getReference() override { return NULL; }
167 
168  bool dropRequested() override { return false; }
169 
170  void startWrite() const override {}
171 
172  void stopWrite() const override {}
173 };
174 
176 private:
178 public:
179  yarp::sig::FlexImage *checkForImage(yarp::os::SizedWriter& writer);
180 };
181 
182 } // namespace wire_rep_utils
183 } // namespace yarp
184 
185 #endif
float t
A simple abstraction for a block of bytes.
Definition: Bytes.h:28
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:25
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:28
This is a base class for objects that can be both read from and be written to the YARP network.
Definition: Portable.h:29
Minimal requirements for an efficient Writer.
Definition: SizedWriter.h:36
virtual void write(OutputStream &os)
Definition: SizedWriter.cpp:19
An OutputStream that produces a string.
Image class with user control of representation details.
Definition: Image.h:403
size_t width() const
Gets width of image in pixels.
Definition: Image.h:153
size_t getPadding() const
Returns the number of padding bytes.
Definition: Image.h:192
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition: Image.cpp:534
virtual size_t getPixelSize() const
Gets pixel size in memory in bytes.
Definition: Image.cpp:449
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:543
size_t height() const
Gets height of image in pixels.
Definition: Image.h:159
virtual int getPixelCode() const
Gets pixel type identifier.
Definition: Image.cpp:454
As far as YARP is concerned, on the wire to/from ROS a raw image has:
Definition: WireImage.h:61
yarp::os::Portable * getReference() override
Definition: WireImage.h:166
void stopWrite() const override
Call when all writing is finished.
Definition: WireImage.h:172
yarp::os::PortReader * getReplyHandler() override
Definition: WireImage.h:164
void init(const yarp::sig::FlexImage &img, const std::string &frame)
Definition: WireImage.h:72
void update(const yarp::sig::FlexImage *img, int seq, double t)
Definition: WireImage.h:118
size_t length(size_t index) const override
Definition: WireImage.h:129
size_t headerLength() const override
Definition: WireImage.h:127
void startWrite() const override
Call when writing is about to begin.
Definition: WireImage.h:170
const char * data(size_t index) const override
Definition: WireImage.h:148
size_t length() const override
Definition: WireImage.h:125
@ VOCAB_PIXEL_MONO16
Definition: Image.h:49
@ VOCAB_PIXEL_BGR
Definition: Image.h:55
@ VOCAB_PIXEL_MONO_FLOAT
Definition: Image.h:59
@ VOCAB_PIXEL_MONO
Definition: Image.h:48
@ VOCAB_PIXEL_RGB
Definition: Image.h:50
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition: Vocab.cpp:36
std::int32_t NetInt32
Definition of the NetInt32 type.
Definition: NetInt32.h:33
The main, catch-all namespace for YARP.
Definition: environment.h:25
#define YARP_END_PACK
Ends 1 byte packing for structs/classes.
Definition: system.h:194
#define YARP_BEGIN_PACK
Starts 1 byte packing for structs/classes.
Definition: system.h:193
#define YARP_wire_rep_utils_API
Definition: api.h:15