YARP
Yet Another Robot Platform
zfpPortmonitor.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #include "zfpPortmonitor.h"
10 
11 #include <yarp/os/LogComponent.h>
12 #include <yarp/sig/Image.h>
13 
14 #include <cstring>
15 #include <cmath>
16 #include <algorithm>
17 
18 extern "C" {
19  #include "zfp.h"
20 }
21 
22 using namespace yarp::os;
23 using namespace yarp::sig;
24 
25 namespace {
26 YARP_LOG_COMPONENT(ZFPMONITOR,
27  "yarp.carrier.portmonitor.zfp",
31  nullptr)
32 }
33 
34 
36 {
37  shouldCompress = (options.find("sender_side").asBool());
38  compressed = nullptr;
39  decompressed = nullptr;
40  buffer = nullptr;
41  sizeToAllocate = 0;
42  sizeToAllocateB = 0;
43  return true;
44 }
45 
47 {
48  if(compressed){
49  free(compressed);
50  compressed = nullptr;
51  }
52 
53  if(buffer){
54  free(buffer);
55  buffer = nullptr;
56  }
57 
58  if(decompressed){
59  free(decompressed);
60  decompressed = nullptr;
61  }
62 }
63 
65 {
66  return false;
67 }
68 
70 {
71  return false;
72 }
73 
75 {
76  if(shouldCompress){
78  if(img == nullptr) {
79  yCError(ZFPMONITOR, "Expected type ImageOf<PixelFloat> in sender side, but got wrong data type!");
80  return false;
81  }
82  }
83  else{
84  Bottle* bt= thing.cast_as<Bottle>();
85  if(bt == nullptr){
86  yCError(ZFPMONITOR, "Expected type Bottle in receiver side, but got wrong data type!");
87  return false;
88  }
89 
90  }
91 
92 
93  return true;
94 }
95 
97 {
98 
99  if(shouldCompress) {
101  // .... buffer, len
102  int sizeCompressed;
103  compress((float*)img->getRawImage(), compressed, sizeCompressed, img->width(),img->height(),1e-3);
104  if(!compressed){
105  yCError(ZFPMONITOR, "Failed to compress, exiting...");
106  return thing;
107  }
108  data.clear();
109  data.addInt32(img->width());
110  data.addInt32(img->height());
111  data.addInt32(sizeCompressed);
112  Value v(compressed, sizeCompressed);
113  data.add(v);
114  th.setPortWriter(&data);
115  }
116  else
117  {
118 
119  Bottle* compressedbt= thing.cast_as<Bottle>();
120 
121  int width=compressedbt->get(0).asInt32();
122  int height=compressedbt->get(1).asInt32();
123  int sizeCompressed=compressedbt->get(2).asInt32();
124  // cast thing to compressed.
125  decompress((float*)compressedbt->get(3).asBlob(), decompressed, sizeCompressed, width, height,1e-3);
126 
127  if(!decompressed){
128  yCError(ZFPMONITOR, "Failed to decompress, exiting...");
129  return thing;
130  }
131  imageOut.resize(width,height);
132  memcpy(imageOut.getRawImage(),decompressed,width*height*4);
133  th.setPortWriter(&imageOut);
134 
135  }
136 
137  return th;
138 }
139 
140 void ZfpMonitorObject::resizeF(float *&array, int newSize){
141  if(newSize>sizeToAllocate){
142  sizeToAllocate=newSize;
143  free(array);
144  array=(float*) malloc(sizeToAllocate);
145  }
146 
147 
148 }
149 void ZfpMonitorObject::resizeV(void *&array, int newSize){
150  if(newSize>sizeToAllocateB){
151  sizeToAllocateB=newSize;
152  free(array);
153  array=(void*) malloc(sizeToAllocateB);
154  }
155 
156 
157 }
158 
159 int ZfpMonitorObject::compress(float* array, float* &compressed, int &zfpsize, int nx, int ny, float tolerance){
160  int status = 0; /* return value: 0 = success */
161  zfp_type type; /* array scalar type */
162  zfp_field* field; /* array meta data */
163  zfp_stream* zfp; /* compressed stream */
164  size_t bufsize; /* byte size of compressed buffer */
165  bitstream* stream; /* bit stream to write to or read from */
166 
167  type = zfp_type_float;
168  field = zfp_field_2d(array, type, nx, ny);
169 
170  /* allocate meta data for a compressed stream */
171  zfp = zfp_stream_open(nullptr);
172 
173  /* set compression mode and parameters via one of three functions */
174  /* zfp_stream_set_rate(zfp, rate, type, 3, 0); */
175  /* zfp_stream_set_precision(zfp, precision); */
176  zfp_stream_set_accuracy(zfp, tolerance);
177 
178  /* allocate buffer for compressed data */
179  bufsize = zfp_stream_maximum_size(zfp, field);
180 
181  resizeV(buffer,bufsize);
182 
183  /* associate bit stream with allocated buffer */
184  stream = stream_open(buffer, bufsize);
185  zfp_stream_set_bit_stream(zfp, stream);
186  zfp_stream_rewind(zfp);
187 
188  /* compress entire array */
189  /* compress array and output compressed stream */
190  zfpsize = zfp_compress(zfp, field);
191  if (!zfpsize) {
192  yCError(ZFPMONITOR, "compression failed");
193  status = 1;
194  }
195 
196  resizeF(compressed,zfpsize);
197  memcpy(compressed,(float*) stream_data(zfp->stream),zfpsize);
198 
199  /* clean up */
200  zfp_field_free(field);
201  zfp_stream_close(zfp);
202  stream_close(stream);
203 
204  return status;
205 }
206 
207 int ZfpMonitorObject::decompress(float* array, float* &decompressed, int zfpsize, int nx, int ny, float tolerance){
208  int status = 0; /* return value: 0 = success */
209  zfp_type type; /* array scalar type */
210  zfp_field* field; /* array meta data */
211  zfp_stream* zfp; /* compressed stream */
212  size_t bufsize; /* byte size of compressed buffer */
213  bitstream* stream; /* bit stream to write to or read from */
214 
215  type = zfp_type_float;
216  resizeF(decompressed,nx*ny*sizeof(float));
217  field = zfp_field_2d(decompressed, type, nx, ny);
218 
219  /* allocate meta data for a compressed stream */
220  zfp = zfp_stream_open(nullptr);
221 
222  /* set compression mode and parameters via one of three functions */
223  /* zfp_stream_set_rate(zfp, rate, type, 3, 0); */
224  /* zfp_stream_set_precision(zfp, precision, type); */
225  zfp_stream_set_accuracy(zfp, tolerance);
226 
227  /* allocate buffer for compressed data */
228  bufsize = zfp_stream_maximum_size(zfp, field);
229  resizeV(buffer,bufsize);
230  memcpy(buffer,array,zfpsize);
231 
232  /* associate bit stream with allocated buffer */
233  stream = stream_open(buffer, zfpsize);
234  zfp_stream_set_bit_stream(zfp, stream);
235  zfp_stream_rewind(zfp);
236 
237  /* read compressed stream and decompress array */
238  if (!zfp_decompress(zfp, field)) {
239  yCError(ZFPMONITOR, "decompression failed");
240  status = 1;
241  }
242 
243  /* clean up */
244  zfp_field_free(field);
245  zfp_stream_close(zfp);
246  stream_close(stream);
247 
248  return status;
249 
250 }
bool setparam(const yarp::os::Property &params) override
This will be called when the portmonitor carrier parameters are set via Yarp admin port.
int decompress(float *array, float *&decompressed, int zfpsize, int nx, int ny, float tolerance)
int compress(float *array, float *&compressed, int &zfpsize, int nx, int ny, float tolerance)
bool getparam(yarp::os::Property &params) override
This will be called when the portmonitor carrier parameters are requested via Yarp admin port.
void resizeV(void *&array, int newSize)
void resizeF(float *&array, int newSize)
bool accept(yarp::os::Things &thing) override
This will be called when the data reach the portmonitor object.
void destroy() override
This will be called when the portmonitor object destroyes.
yarp::os::Things & update(yarp::os::Things &thing) override
After data get accpeted in the accept() callback, an instance of that is given to the update function...
bool create(const yarp::os::Property &options) override
This will be called when the dll is properly loaded by the portmonitor carrier.
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:76
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:249
static LogCallback printCallback()
Get current print callback.
Definition: Log.cpp:852
static LogType minimumPrintLevel()
Get current minimum print level.
Definition: Log.cpp:805
@ LogTypeReserved
Definition: Log.h:83
A class for storing options and configuration information.
Definition: Property.h:37
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition: Property.cpp:1054
Base class for generic things.
Definition: Things.h:22
T * cast_as()
Definition: Things.h:57
A single value (typically within a Bottle).
Definition: Value.h:47
virtual bool asBool() const
Get boolean value.
Definition: Value.cpp:189
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:207
virtual const char * asBlob() const
Get binary data value.
Definition: Value.cpp:264
Typed image class.
Definition: Image.h:647
size_t width() const
Gets width of image in pixels.
Definition: Image.h:153
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition: Image.cpp:534
size_t height() const
Gets height of image in pixels.
Definition: Image.h:159
#define yCError(component,...)
Definition: LogComponent.h:157
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:80
An interface to the operating system, including Port based communication.
Signal processing.
Definition: Image.h:25