YARP
Yet Another Robot Platform
WireTwiddler.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 "WireTwiddler.h"
10 
11 #include <vector>
12 
13 #include <cstdio>
14 #include <cstring>
15 #include <cstdlib>
16 
19 #include <yarp/os/Route.h>
20 #include <yarp/os/InputStream.h>
22 #include <yarp/os/NetType.h>
23 #include <yarp/os/Log.h>
24 #include <yarp/os/Vocab.h>
25 #include <yarp/os/NetFloat32.h>
26 #include <yarp/os/NetFloat64.h>
27 #include <yarp/os/Os.h>
28 #include <yarp/sig/Image.h>
29 
30 using namespace std;
31 using namespace yarp::os;
32 using namespace yarp::wire_rep_utils;
33 
34 #define dbg_flag 0
35 #define dbg_printf if (dbg_flag) printf
36 
37 WireTwiddlerWriter::~WireTwiddlerWriter() = default;
38 
39 
40 int WireTwiddler::configure(Bottle& desc, int offset, bool& ignored,
41  const std::string& vtag) {
42  int start = offset;
43  // example: list 4 int32 * float64 * vector int32 * vector int32 3 *
44  bool is_vector = false;
45  bool is_list = false;
46  bool ignore = false;
47  bool saving = false;
48  bool loading = false;
49  bool computing = false;
50  std::string kind = desc.get(offset).asString();
51  std::string var;
52  offset++;
53  if (kind=="---") {
54  offset = desc.size();
55  return offset;
56  }
57  if (kind=="skip"||kind=="compute") {
58  if (kind=="compute") computing = true;
59  ignore = true;
60  var = kind = desc.get(offset).asString();
61  offset++;
62  }
63  if (kind.length()>0 && (kind[0]=='>'||kind[0]=='<')) {
64  saving = (kind[0]=='>');
65  loading = (kind[0]=='<');
66  ignore = saving;
67  var = kind.substr(1,kind.length());
68  kind = desc.get(offset).asString();
69  offset++;
70  }
71 
72  is_vector = (kind=="vector");
73  is_list = (kind=="list");
74  if (kind=="item_vector") {
75  is_vector = true;
76  is_list = true;
77  }
78  if (is_vector) {
79  kind = desc.get(offset).asString();
80  offset++;
81  }
82  int len = 1;
83  bool data = false;
84  if (computing) data = true;
85  if (is_vector||is_list) {
86  Value v = desc.get(offset);
87  offset++;
88  if (v.isInt32()) {
89  len = v.asInt32();
90  } else {
91  if (v.asString()!="*") {
92  fprintf(stderr,"Does not look like length: %s\n",
93  v.toString().c_str());
94  }
95  len = -1;
96  if (is_list) {
97  fprintf(stderr,"List should have fixed length\n");
98  len = 0;
99  }
100  data = true;
101  }
102  }
103  if (!is_list) {
104  if (!data) {
105  Value v = desc.get(offset);
106  offset++;
107  if (v.asString()!="*") {
108  fprintf(stderr,"Does not look like data: %s\n",
109  v.toString().c_str());
110  }
111  data = true;
112  }
113  }
114 
115  int tag = 0;
116  int unit_length = 0;
117  int wire_unit_length = -1;
118  bool item = false;
119  if (kind=="item") {
120  kind = vtag;
121  item = true;
122  }
123  if (kind=="int8"||kind=="uint8"||kind=="bool") {
124  tag = BOTTLE_TAG_INT8;
125  unit_length = 1;
126  } else if (kind=="int16"||kind=="uint16") {
127  tag = BOTTLE_TAG_INT16;
128  unit_length = 2;
129  } else if (kind=="int32"||kind=="uint32") {
130  tag = BOTTLE_TAG_INT32;
131  unit_length = 4;
132  } else if (kind=="int64"||kind=="uint64") {
133  tag = BOTTLE_TAG_INT64;
134  unit_length = 8;
135  } else if (kind=="float32") {
136  tag = BOTTLE_TAG_FLOAT32;
137  unit_length = 4;
138  } else if (kind=="float64") {
139  tag = BOTTLE_TAG_FLOAT64;
140  unit_length = 8;
141  } else if (kind=="vocab") {
142  tag = BOTTLE_TAG_VOCAB32;
143  unit_length = 4;
144  } else if (kind=="string") {
145  tag = BOTTLE_TAG_STRING;
146  unit_length = -1;
147  //len = -1;
148  } else if (kind=="blob") {
149  tag = BOTTLE_TAG_BLOB;
150  unit_length = -1;
151  //len = -1;
152  } else if (kind=="list"||kind=="vector"||computing) {
153  //pass
154  } else {
155  fprintf(stderr,"%s does not know about %s\n", __FILE__, kind.c_str());
156  std::exit(1);
157  }
158 
159  dbg_printf("Type %s (%s) len %d unit %d %s\n",
160  kind.c_str(),
161  is_list?"LIST":(is_vector?"VECTOR":"PRIMITIVE"), len,
162  unit_length,
163  ignore?"SKIP":"");
164 
165  if (!ignore) {
166  if (is_vector) {
167  buffer.push_back(BOTTLE_TAG_LIST+tag);
168  if (len!=-1) {
169  buffer.push_back(len);
170  }
171  } else if (is_list) {
172  buffer.push_back(BOTTLE_TAG_LIST);
173  buffer.push_back(len);
174  } else {
175  if (!item) buffer.push_back(tag);
176  }
177  }
178 
179  if (data) {
180  WireTwiddlerGap gap;
181  if (!ignore) {
182  gap.buffer_start = buffer_start;
183  gap.buffer_length = (int)buffer.size()-buffer_start;
184  buffer_start = (int)buffer.size();
185  } else {
186  gap.buffer_start = 0;
187  gap.buffer_length = 0;
188  }
189  gap.unit_length = unit_length;
190  gap.wire_unit_length = (wire_unit_length!=-1)?wire_unit_length:unit_length;
191  gap.length = len;
192  gap.ignore_external = ignore;
193  gap.save_external = saving;
194  gap.load_external = loading;
195  gap.computing = computing;
196  gap.var_name = var;
197  Bottle tmp;
198  tmp.copy(desc,start,offset-start-1);
199  gap.origin = tmp.toString();
200  gap.flavor = tag;
201  gaps.push_back(gap);
202  }
203  ignored = ignore;
204 
205  if (is_list) {
206  int i=0;
207  while (i<len) {
208  bool ign = false;
209  offset = configure(desc,offset,ign,kind);
210  if (!ign) i++;
211  }
212  }
213  return offset;
214 }
215 
216 bool WireTwiddler::configure(const char *txt, const char *prompt) {
217  this->prompt = prompt;
218  clear();
219  std::string str(txt);
220  char *cstr = (char *)str.c_str();
221  for (size_t i=0; i<str.length(); i++) {
222  if (cstr[i]==',') {
223  cstr[i] = ' ';
224  }
225  }
226 
227  if (str.find("list") == std::string::npos &&
228  str.find("vector") == std::string::npos) {
229  str += " list 0";
230  }
231 
232  dbg_printf("Configure as %s\n", str.c_str());
233 
234  Bottle desc(str);
235 
236  buffer_start = 0;
237  buffer.clear();
238  size_t at = 0;
239  size_t next = 0;
240  do {
241  bool ign = false;
242  at = next;
243  dbg_printf("Configuring, length %zu, at %zu\n", desc.size(), at);
244  next = configure(desc,at,ign,"");
245  } while (next>at&&next<desc.size());
246  if (buffer_start!=(int)buffer.size()) {
247  WireTwiddlerGap gap;
248  gap.buffer_start = buffer_start;
249  gap.buffer_length = (int)buffer.size()-buffer_start;
250  buffer_start = (int)buffer.size();
251  gaps.push_back(gap);
252  }
253  dbg_printf("buffer has %zu items\n", buffer.size());
254  dbg_printf("gaps has %zu items\n", gaps.size());
255  for (auto& gap : gaps) {
256  if (gap.buffer_length!=0) {
257  gap.byte_start = (char *) (&buffer[gap.buffer_start]);
258  gap.byte_length = gap.buffer_length*4;
259  } else {
260  gap.byte_start = nullptr;
261  gap.byte_length = 0;
262  }
263  }
264  if (dbg_flag) show();
265  return at == desc.size();
266 }
267 
268 std::string nameThatCode(int code) {
269  switch (code) {
270  case BOTTLE_TAG_INT8:
271  return "int8";
272  break;
273  case BOTTLE_TAG_INT16:
274  return "int16";
275  break;
276  case BOTTLE_TAG_INT32:
277  return "int32";
278  break;
279  case BOTTLE_TAG_INT64:
280  return "int64";
281  break;
282  case BOTTLE_TAG_FLOAT32:
283  return "float32";
284  break;
285  case BOTTLE_TAG_FLOAT64:
286  return "float64";
287  break;
288  case BOTTLE_TAG_VOCAB32:
289  return "vocab";
290  break;
291  case BOTTLE_TAG_STRING:
292  return "string";
293  break;
294  case BOTTLE_TAG_LIST:
295  return "list";
296  break;
297  case BOTTLE_TAG_BLOB:
298  return "blob";
299  break;
300  }
301  return "unsupported";
302 }
303 
304 std::string WireTwiddler::fromTemplate(const yarp::os::Bottle& msg) {
305  string result;
306 
307  // assume we want to remove any meta-information
308 
309  int len = msg.size();
310 
311  int code = -1;
312  for (int i=0; i<len; i++) {
313  Value&v = msg.get(i);
314  int icode = v.getCode();
315  if (i==0) code = icode;
316  if (icode!=code) code = -1;
317  }
318  string codeName = nameThatCode(code);
319  if (code == -1) {
320  result += "list ";
321  } else {
322  result += "vector ";
323  result += codeName;
324  result += " ";
325  }
326  result += yarp::conf::numeric::to_string(len);
327  result += " ";
328  for (int i=0; i<len; i++) {
329  Value&v = msg.get(i);
330  if (!v.isList()) {
331  if (code == -1) {
332  result += nameThatCode(v.getCode());
333  result += " ";
334  }
335  result += "* ";
336  } else {
337  result += fromTemplate(*v.asList());
338  }
339  }
340  return result;
341 }
342 
343 
344 void WireTwiddler::show() {
345  for (size_t i=0; i<gaps.size(); i++) {
346  WireTwiddlerGap& gap = gaps[i];
347  printf("Block #%zu (%s)\n", i, gap.getOrigin().c_str());
348  if (gap.buffer_length!=0) {
349  printf(" Buffer from %d to %d\n", gap.buffer_start,
350  gap.buffer_start+gap.buffer_length-1);
351  }
352  if (gap.ignore_external) {
353  printf(" External data will be ignored\n");
354  } else {
355  if (gap.unit_length!=0) {
356  printf(" Expect %d x %d\n", gap.length, gap.unit_length);
357  }
358  }
359  }
360 }
361 
362 std::string WireTwiddler::toString() const {
363  std::string result;
364  for (const auto& gap : gaps) {
365  std::string item;
366  item += gap.origin;
367  //if (gap.buffer_length!=0) {
368  // char buf[1024];
369  //sprintf(buf," (%d to %d)", gap.buffer_start,
370  // gap.buffer_start+gap.buffer_length-1);
371  //item += buf;
372  //}
373  if (gap.unit_length!=0) {
374  char buf[1024];
375  if (gap.length==-1 && gap.unit_length==-1) {
376  sprintf(buf," [4-byte length] [<length> instances of 4-byte-length bytes followed by specified number of bytes]");
377  } else if (gap.length==-1) {
378  sprintf(buf," [4-byte length] [<length>*%d bytes]", gap.unit_length);
379  } else if (gap.length==1) {
380  sprintf(buf," [%d bytes]", gap.unit_length);
381  } else {
382  sprintf(buf," [%d*%d bytes]", gap.length, gap.unit_length);
383  }
384  item += buf;
385  }
386  if (result!="") result += " ";
387  result += item;
388  }
389  return result;
390 }
391 
392 
393 bool WireTwiddler::read(Bottle& bot, const Bytes& data) {
394  StringInputStream sis;
395  sis.add(data);
396  WireTwiddlerReader twiddled_input(sis,*this);
397  return ConnectionReader::readFromStream(bot,twiddled_input);
398 }
399 
400 
402  yarp::os::ManagedBytes& data) {
403  StringOutputStream sos;
404  if (!writer) {
405  writer = ConnectionWriter::createBufferedConnectionWriter();
406  }
407  if (!writer) {
408  return false;
409  }
410  SizedWriter *buf = writer->getBuffer();
411  if (!buf) {
412  return false;
413  }
414  buf->clear();
415  bot.write(*writer);
416  WireTwiddlerWriter twiddled_output(*buf,*this);
417  twiddled_output.write(sos);
418  std::string result = sos.toString();
419  data = ManagedBytes(Bytes((char*)result.c_str(),result.length()),false);
420  data.copy();
421  return true;
422 }
423 
424 
425 void WireTwiddlerReader::compute(const WireTwiddlerGap& gap) {
426  if (gap.var_name == "image_params") {
427  int w = prop.find("width").asInt32();
428  int h = prop.find("height").asInt32();
429  int step = prop.find("step").asInt32();
430  bool bigendian = prop.find("is_bigendian").asInt32()==1;
431  if (bigendian) {
432  fprintf(stderr,"Sorry, cannot handle bigendian images yet.\n");
433  std::exit(1);
434  }
435  std::string encoding = prop.find("encoding").asString();
436  int bpp = 1;
437  int translated_encoding = 0;
438  switch (Vocab::encode(encoding)) {
439  case yarp::os::createVocab('b','g','r','8'):
440  bpp = 3;
441  translated_encoding = VOCAB_PIXEL_BGR;
442  break;
443  case yarp::os::createVocab('r','g','b','8'):
444  bpp = 3;
445  translated_encoding = VOCAB_PIXEL_RGB;
446  break;
447  case yarp::os::createVocab('3','2','F','C'):
448  yAssert(encoding=="32FC1");
449  bpp = 4;
450  translated_encoding = VOCAB_PIXEL_MONO_FLOAT;
451  break;
452  case yarp::os::createVocab('1','6','U','C'):
453  yAssert(encoding=="16UC1");
454  bpp = 2;
455  translated_encoding = VOCAB_PIXEL_MONO16;
456  break;
457  case yarp::os::createVocab('m','o','n','o'):
458  yAssert(encoding=="mono8"||encoding=="mono16");
459  if (encoding == "mono8") {
460  bpp = 1;
461  translated_encoding = VOCAB_PIXEL_MONO;
462  } else {
463  bpp = 2;
464  translated_encoding = VOCAB_PIXEL_MONO16;
465  }
466  break;
467  case yarp::os::createVocab('b', 'a', 'y', 'e'):
468  bpp = 1;
469  translated_encoding = VOCAB_PIXEL_MONO;
470  if (encoding == "bayer_grbg8")
471  translated_encoding = VOCAB_PIXEL_ENCODING_BAYER_GRBG8;
472  else
473  {
474  fprintf(stderr, "Warning automatic debayering not yet supported, keeping raw format.\n");
475  }
476 
477  break;
478  default:
479  fprintf(stderr, "Sorry, cannot handle [%s] images yet.\n",
480  encoding.c_str());
481  std::exit(1);
482  break;
483  }
484  int quantum = 1;
485  if (step!=w*bpp) {
486  bool ok = false;
487  while (quantum<=256) {
488  quantum *= 2;
489  if (((w*bpp+quantum)/quantum)*quantum == step) {
490  ok = true;
491  break;
492  }
493  }
494  if (!ok) {
495  quantum = step;
496  }
497  }
498  int img_size = step*h;
499  prop.put("depth",3);
500  prop.put("img_size",img_size);
501  prop.put("quantum",quantum);
502  prop.put("translated_encoding",translated_encoding);
503  }
504 }
505 
507  dbg_printf("Want %zu bytes\n", b.length());
508  if (index==-1) {
509  dbg_printf("WireTwidderReader::read getting started\n");
510  }
511  int ct = twiddler.getGapCount();
512  if (ct<=0) {
513  fprintf(stderr,"WireTwidderReader, nothing available\n");
514  return -1;
515  }
516  if (index==-1) {
517  index = 0;
518  sent = 0;
519  consumed = 0;
520  }
521  if (index>=ct) {
522  fprintf(stderr,"WireTwidderReader, nothing left\n");
523  return -1;
524  }
525  bool more = false;
526  do {
527  const WireTwiddlerGap& gap = twiddler.getGap(index);
528  if (gap.computing) {
529  compute(gap);
530  }
531  dbg_printf("*** index %d sent %d consumed %d / len %d unit %d/%d\n", index, sent, consumed, gap.length, gap.unit_length, gap.wire_unit_length);
532  char *byte_start = gap.getStart();
533  int byte_length = gap.getLength();
534  if (byte_start != nullptr) {
535  byte_start += consumed;
536  byte_length -= consumed;
537  }
538  if (byte_start != nullptr && byte_length > 0) {
539  int len = b.length();
540  if (len>byte_length) {
541  len = byte_length;
542  }
543  memcpy(b.get(),byte_start,len);
544  sent += len;
545  consumed += len;
546  auto* nn = reinterpret_cast<NetInt32 *> (b.get());
547  dbg_printf("WireTwidderReader sending %d boilerplate bytes:\n",len);
548  dbg_printf(" [[[%d]]]\n", (int)(*nn));
549  return len;
550  }
551  if ((gap.length==-1||gap.unit_length==-1) && override_length==-1) {
552  dbg_printf("LOOKING TO EXTERNAL\n");
553  Bytes bytes(reinterpret_cast<char*>(&lengthBuffer), sizeof(NetInt32));
554  int r = is.readFull(bytes);
555  if (r!=sizeof(NetInt32)) return -1;
556  dbg_printf("Read length %d\n", lengthBuffer);
557  pending_length = sizeof(lengthBuffer);
558  if (gap.length==-1) {
559  dbg_printf("Expect 4 + %d x %d\n", lengthBuffer, gap.unit_length);
560  if (gap.unit_length<0) {
561  pending_strings = lengthBuffer;
562  pending_string_length = 0;
563  pending_string_data = 0;
564  override_length = 0;
565  } else {
566  override_length = lengthBuffer*gap.unit_length;
567  }
568  } else {
569  override_length = lengthBuffer;
570  dbg_printf("Expect 1 x %d\n", lengthBuffer);
571  }
572  }
573  if (gap.shouldIgnoreExternal()) {
574  pending_length = 0;
575  }
576  if (pending_length) {
577  int len = b.length();
578  if (len>pending_length) {
579  len = pending_length;
580  }
581  Bytes b2(b.get(),len);
582  memcpy(b.get(),
583  (char*)(&lengthBuffer)+sizeof(lengthBuffer)-pending_length,
584  len);
585  pending_length -= len;
586  dbg_printf("WireTwidderReader sending %d length bytes:\n",len);
587  dbg_printf(" (((%d)))\n", lengthBuffer);
588  return len;
589  }
590  while (pending_strings) {
591  dbg_printf("### %d pending strings\n", pending_strings);
592  if (pending_string_length==0&&pending_string_data==0) {
593  dbg_printf("Checking string length\n");
594  Bytes bytes(reinterpret_cast<char*>(&lengthBuffer), sizeof(NetInt32));
595  int r = is.readFull(bytes);
596  if (r!=sizeof(NetInt32)) return -1;
597  dbg_printf("Read length %d\n", lengthBuffer);
598  pending_string_length = sizeof(lengthBuffer);
599  pending_string_data = lengthBuffer;
600  }
601  if (pending_string_length) {
602  int len = b.length();
603  if (len>pending_string_length) {
604  len = pending_string_length;
605  }
606  Bytes b2(b.get(),len);
607  memcpy(b.get(),
608  (char*)(&lengthBuffer)+sizeof(lengthBuffer)-pending_string_length,
609  len);
610  pending_string_length -= len;
611  dbg_printf("WireTwidderReader sending %d string length bytes:\n",len);
612  dbg_printf(" (((%d)))\n", lengthBuffer);
613  if (pending_string_length==0&&pending_string_data==0) { pending_strings--; }
614  return len;
615  }
616  if (pending_string_data) {
617  int len = b.length();
618  if (len>pending_string_data) {
619  len = pending_string_data;
620  }
621  Bytes b2(b.get(),len);
622  int r = is.readFull(b2);
623  if (r<0) {
624  fprintf(stderr,"No string payload bytes available\n");
625  return r;
626  }
627  pending_string_data -= r;
628  dbg_printf("WireTwidderReader sending %d string payload bytes\n",r);
629  if (pending_string_data==0) { pending_strings--; }
630  return r;
631  }
632  }
633  int extern_length = gap.length * gap.unit_length;
634  if (gap.unit_length<0||gap.length<0) extern_length = override_length;
635  dbg_printf("extern_length %d\n", extern_length);
636 
637  if (extern_length>sent-consumed) {
638  int len = b.length();
639  if (len>extern_length) {
640  len = extern_length;
641  }
642  Bytes b2(b.get(),len);
643  int r = 0;
644  if (!gap.shouldIgnoreExternal()) {
645  r = readMapped(is,b2,gap);
646  auto* nn = reinterpret_cast<NetInt32 *> (b.get());
647  dbg_printf("WireTwidderReader sending %d payload bytes:\n",r);
648  dbg_printf(" [[[%d]]]\n", (int)(*nn));
649  if (r>0) {
650  sent += r;
651  }
652  if (r<0) {
653  fprintf(stderr,"No payload bytes available\n");
654  return r;
655  }
656  return r;
657  } else {
658  int len2 = extern_length;
659  if (gap.wire_unit_length>=0 && gap.wire_unit_length!=len2) {
660  len2 = gap.wire_unit_length;
661  }
662  dump.allocateOnNeed(len2,len2);
663  Bytes b3(dump.get(),len2);
664  r = is.readFull(b3);
665  auto* nn = reinterpret_cast<NetInt32 *> (dump.get());
666  if (gap.save_external) {
667  if (override_length>=0) {
668  prop.put(gap.var_name,
669  std::string((char *)(dump.get()),
670  len2));
671  dbg_printf("Saved %s: %s\n",
672  gap.var_name.c_str(),
673  prop.find(gap.var_name).asString().c_str());
674  } else if (len2>=4) {
675  prop.put(gap.var_name,(int)(*nn));
676  dbg_printf("Saved %s: is %d\n",
677  gap.var_name.c_str(),
678  prop.find(gap.var_name).asInt32());
679  }
680  }
681  dbg_printf("WireTwidderReader sending %d payload bytes:\n",r);
682  dbg_printf(" [[[%d]]]\n", (int)(*nn));
683  dbg_printf(" (ignoring %d payload bytes)\n",r);
684  if (r>0) {
685  sent += r;
686  }
687  }
688  }
689  more = false;
690  if (index<ct-1) {
691  index++;
692  consumed = 0;
693  sent = 0;
694  override_length = -1;
695  pending_length = 0;
696  pending_strings = 0;
697  more = true;
698  }
699  } while (more);
700  fprintf(stderr,"WireTwidderReader shrugging\n");
701  return -1;
702 }
703 
704 
705 bool WireTwiddlerWriter::update() {
706  scratchOffset = 0;
707  errorState = false;
708  activeGap = nullptr;
709  codeExpected = 0;
710  codeReceived = 0;
711  srcs.clear();
712 
713  lengthBytes = Bytes((char*)(&lengthBuffer),sizeof(yarp::os::NetInt32));
714  offset = 0;
715  blockPtr = nullptr;
716  blockLen = 0;
717  block = parent->headerLength();
718  lastBlock = parent->length()-block-1;
719  activeEmit = nullptr;
720  activeEmitLength = 0;
721  activeEmitOffset = -1;
722 
723  dbg_printf("Parent headers %zu blocks %zu\n", parent->headerLength(), parent->length());
724 
725  for (int i=0; i<twiddler->getGapCount(); i++) {
726  if (errorState) return false;
727  std::string item;
728  const WireTwiddlerGap& gap = twiddler->getGap(i);
729  if (gap.buffer_length!=0) {
730  dbg_printf("Skip %d bytes\n", gap.byte_length);
731  skip(gap.byte_start,gap.byte_length);
732  if (gap.unit_length!=0) {
733  dbg_printf("Length %d unit_length %d\n", gap.length,
734  gap.unit_length);
735  if (gap.length==-1 && gap.unit_length==-1) {
736  dbg_printf("Pass [4-byte length] [<length> instances of 4-byte-length bytes followed by specified number of bytes]\n");
737  readLengthAndPass(-1);
738  } else if (gap.length==1&&gap.unit_length==1) {
739  dbg_printf("Pass [%d bytes]\n", gap.unit_length);
740  pass(gap.unit_length);
741  } else if (gap.length==1&&gap.unit_length==-1) {
742  dbg_printf("Pass [4-byte length] [<length> bytes]\n");
743  readLengthAndPass(1);
744  } else if (gap.length==-1) {
745  dbg_printf("Pass [4-byte length] [<length>*%d bytes]\n", gap.unit_length);
746  readLengthAndPass(gap.unit_length,&gap);
747  } else {
748  dbg_printf("Pass [%d*%d bytes]\n", gap.length, gap.unit_length);
749  if (gap.unit_length!=gap.wire_unit_length) {
750  dbg_printf("Need to tweak length (not correct for neg numbers yet)...\n");
751  for (int i=0; i<gap.length; i++) {
752  if (errorState) return false;
753  transform(gap);
754  }
755  } else {
756  pass(gap.length*gap.unit_length);
757  }
758  }
759  }
760  }
761  }
762  emit(nullptr, 0);
763  dbg_printf("%zu write blocks\n", srcs.size());
764  if (dbg_flag) {
765  for (size_t i=0; i<srcs.size(); i++) {
766  dbg_printf(" write block %zu: len %d offset %d ptr %p\n", i, srcs[i].len, srcs[i].offset, srcs[i].src);
767  }
768  }
769  return !errorState;
770 }
771 
772 
773 bool WireTwiddlerWriter::skip(const char *start, int len) {
774  activeCheck = start;
775  return advance(len,false,false,true);
776 }
777 
778 bool WireTwiddlerWriter::pass(int len) {
779  return advance(len,true);
780 }
781 
782 bool WireTwiddlerWriter::pad(size_t len) {
783  if (zeros.length()<len) {
784  zeros.allocate(len);
785  for (size_t i=0; i<len; i++) {
786  zeros.get()[i] = '\0';
787  }
788  }
789  return emit(zeros.get(),len);
790 }
791 
792 bool WireTwiddlerWriter::readLengthAndPass(int unitLength,
793  const WireTwiddlerGap *gap) {
794  int len = readLength();
795  if (len==0) return false;
796  if (unitLength!=-1) {
797  if (gap == nullptr || gap->wire_unit_length == unitLength) {
798  advance(unitLength*len,true);
799  } else {
800  for (int i=0; i<len; i++) {
801  if (!transform(*gap)) return false;
802  }
803  }
804  } else {
805  for (int i=0; i<len; i++) {
806  bool ok = readLengthAndPass(1,gap);
807  if (!ok) return false;
808  }
809  }
810  return true;
811 }
812 
813 void WireTwiddlerWriter::showBrokenExpectation(const yarp::os::NetInt32& expected,
814  const yarp::os::NetInt32& received,
815  int evidence) {
816  if (!errorState) {
817  yError("Structure of message is unexpected (expected %s)", twiddler->getPrompt().c_str());
818  if (evidence>=4) {
819  if (expected!=received) {
820  yError("Expected '%s', got '%s'\n",
821  Bottle::describeBottleCode(expected).c_str(),
822  Bottle::describeBottleCode(received).c_str());
823  }
824  }
825  }
826  errorState = true;
827 }
828 
829 
830 int WireTwiddlerWriter::readLength() {
831  advance(4,true,true);
832  if (accumOffset==4) {
833  if (codeExpected!=codeReceived) {
834  if (lengthBuffer!=0) {
835  showBrokenExpectation(codeExpected,codeReceived,4);
836  }
837  codeExpected = codeReceived = 0;
838  }
839  return lengthBuffer;
840  }
841  return 0;
842 }
843 
844 bool WireTwiddlerWriter::advance(int length, bool shouldEmit,
845  bool shouldAccum, bool shouldCheck) {
846  accumOffset = 0;
847  if (length==0) return true;
848  if (length<0) return false;
849  while (length>0) {
850  if (blockPtr == nullptr) {
851  blockPtr = parent->data(block);
852  blockLen = parent->length(block);
853  dbg_printf(" block %d is at %ld, length %d\n",block,
854  (long int)blockPtr, blockLen);
855  offset = 0;
856  }
857  int rem = blockLen-offset;
858  if (rem==0) {
859  block++;
860  blockPtr = nullptr;
861  if (block>lastBlock) {
862  return false;
863  }
864  dbg_printf(" moved on to block %d\n",block);
865  continue;
866  }
867  if (rem>length) rem = length;
868  if (shouldCheck) {
869  dbg_printf("Type check against %ld\n", (long int)activeCheck);
870  int result = memcmp(activeCheck,blockPtr+offset,rem);
871  activeCheck += rem;
872  if (result!=0) {
873  NetInt32 t1 = 0;
874  NetInt32 t2 = 0;
875  if (rem>=4) {
876  t1 = *(reinterpret_cast<const NetInt32*>(blockPtr+offset));
877  t2 = *(reinterpret_cast<const NetInt32*>(activeCheck-rem));
878  if (t1!=t2 && (t1&BOTTLE_TAG_LIST) && (t2&BOTTLE_TAG_LIST)) {
879  // delay checking of codes until list length is read
880  // since list may be empty
881  codeExpected = t2;
882  codeReceived = t1;
883  result = 0;
884  }
885  }
886  if (result!=0) {
887  dbg_printf("Type check failed! >>>\n");
888  showBrokenExpectation(t2,t1,rem);
889  return false;
890  }
891  }
892  }
893  if (shouldEmit) {
894  emit(blockPtr+offset, rem);
895  }
896  if (shouldAccum) {
897  if (accumOffset+rem>4) {
898  fprintf(stderr,"ACCUMULATION GONE WRONG %d %d\n",
899  accumOffset, rem);
900  ::exit(1);
901  }
902  memcpy(lengthBytes.get()+accumOffset,blockPtr+offset,rem);
903  accumOffset += rem;
904  }
905  offset += rem;
906  length -= rem;
907  }
908  return true;
909 }
910 
911 bool WireTwiddlerWriter::emit(const char *src, int len) {
912  int noffset = -1;
913  if (src != nullptr) {
914  if (activeGap) {
915  const WireTwiddlerGap& gap = *activeGap;
916  if (gap.wire_unit_length==4 && gap.unit_length==8 &&
917  len == 8 &&
918  gap.flavor == BOTTLE_TAG_FLOAT64) {
919  const auto* x = reinterpret_cast<const NetFloat64*>(src);
920  if (scratchOffset+4>scratch.length()) {
921  scratch.allocateOnNeed(scratchOffset+4,scratchOffset+4);
922  }
923  auto* y = reinterpret_cast<NetFloat32*>(scratch.get()+scratchOffset);
924  *y = (NetFloat32) *x;
925  src = nullptr;
926  noffset = scratchOffset;
927  len = 4;
928  scratchOffset += 4;
929  } else {
930  fprintf(stderr,"WireTwidder::emit needs to be extended to deal with this case\n");
931  ::exit(1);
932  }
933  activeGap = nullptr;
934  }
935  }
936  dbg_printf(" cache %ld len %d offset %d /// activeEmit %ld %d %d\n", (long int)src, len, noffset, (long int) activeEmit, activeEmitLength, activeEmitOffset);
937  if (activeEmit != nullptr || activeEmitOffset >= 0) {
938  bool push = false;
939  if (activeEmitOffset>=0 && noffset<0) {
940  push = true;
941  } else if (activeEmitOffset<0 && noffset>=0) {
942  push = true;
943  } else if (noffset==-1 && activeEmit+activeEmitLength!=src) {
944  push = true;
945  }
946  if (push) {
947  dbg_printf(" ** emit %ld len %d offset %d\n", (long int)activeEmit,
948  activeEmitLength, activeEmitOffset);
949  srcs.emplace_back((char*)activeEmit,activeEmitLength,activeEmitOffset);
950  activeEmit = nullptr;
951  activeEmitLength = 0;
952  activeEmitOffset = -1;
953  } else {
954  activeEmitLength += len;
955  dbg_printf(" ** extend %ld len %d offset %d\n", (long int)activeEmit,
956  activeEmitLength, activeEmitOffset);
957  return true;
958  }
959  }
960  if (src != nullptr || noffset >= 0) {
961  activeEmit = src;
962  activeEmitLength = len;
963  activeEmitOffset = noffset;
964  }
965  return true;
966 }
967 
968 bool WireTwiddlerWriter::transform(const WireTwiddlerGap& gap) {
969  // So far, very crude, does not cover all cases
970  if (gap.wire_unit_length>gap.unit_length) {
971  pass(gap.unit_length);
972  pad(gap.wire_unit_length-gap.unit_length);
973  return true;
974  }
975  if (gap.wire_unit_length==4 && gap.unit_length==8 &&
976  gap.flavor == BOTTLE_TAG_FLOAT64) {
977  activeGap = &gap;
978  pass(gap.unit_length);
979  return true;
980  }
981  fprintf(stderr,"WireTwidder::transform needs to be extended to deal with this case\n");
982  ::exit(1);
983  return false;
984 }
985 
986 yarp::conf::ssize_t WireTwiddlerReader::readMapped(yarp::os::InputStream& is,
987  yarp::os::Bytes& b,
988  const WireTwiddlerGap& gap) {
989  if (gap.load_external) {
990  int v = 0;
991  if (gap.var_name[0]=='=') {
992  Bottle bot;
993  bot.fromString(gap.var_name.substr(1,gap.var_name.length()));
994  v = bot.get(0).asInt32();
995  } else {
996  v = prop.find(gap.var_name).asInt32();
997  }
998  dbg_printf("Read %s: %d\n", gap.var_name.c_str(), v);
999  for (size_t i=0; i<b.length(); i++) {
1000  b.get()[i] = 0;
1001  }
1002  auto* nn = reinterpret_cast<NetInt32 *> (b.get());
1003  if (b.length()>=4) {
1004  *nn = v;
1005  }
1006  return gap.unit_length;
1007  }
1008  if (gap.wire_unit_length==gap.unit_length) {
1009  return is.read(b);
1010  }
1011  for (size_t i=0; i<b.length(); i++) {
1012  b.get()[i] = 0;
1013  }
1014  size_t len = gap.wire_unit_length;
1015  if (len>b.length()) {
1016  len = b.length();
1017  }
1018  Bytes b2(b.get(),len);
1019  yarp::conf::ssize_t r = is.readFull(b2);
1020  if (r==(yarp::conf::ssize_t)len) {
1021  if (gap.flavor==BOTTLE_TAG_FLOAT64) {
1022  if (gap.wire_unit_length==4 &&
1023  gap.unit_length==8) {
1024  NetFloat32 x = *(reinterpret_cast<NetFloat32 *> (b2.get()));
1025  auto* y = reinterpret_cast<NetFloat64 *> (b2.get());
1026  *y = x;
1027  }
1028  }
1029  int len2 = gap.wire_unit_length-b.length();
1030  if (len2>0) {
1031  dump.allocateOnNeed(len2,len2);
1032  Bytes b3(dump.get(),len2);
1033  is.readFull(b3);
1034  }
1035  return gap.unit_length;
1036  }
1037  return -1;
1038 }
1039 
1040 
1041 
1042 //void write(OutputStream& os) {
1043 //}
#define BOTTLE_TAG_INT8
Definition: Bottle.h:21
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:27
#define BOTTLE_TAG_INT64
Definition: Bottle.h:24
#define BOTTLE_TAG_INT16
Definition: Bottle.h:22
#define BOTTLE_TAG_INT32
Definition: Bottle.h:23
#define BOTTLE_TAG_STRING
Definition: Bottle.h:28
#define BOTTLE_TAG_BLOB
Definition: Bottle.h:29
#define BOTTLE_TAG_LIST
Definition: Bottle.h:30
#define BOTTLE_TAG_VOCAB32
Definition: Bottle.h:25
#define BOTTLE_TAG_FLOAT32
Definition: Bottle.h:26
#define yError(...)
Definition: Log.h:282
#define yAssert(x)
Definition: Log.h:297
std::string nameThatCode(int code)
#define dbg_printf
#define dbg_flag
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:76
void fromString(const std::string &text)
Initializes bottle from a string.
Definition: Bottle.cpp:207
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:254
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:249
void copy(const Bottle &alt, size_type first=0, size_type len=npos)
Copy all or part of another Bottle.
Definition: Bottle.cpp:269
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition: Bottle.cpp:233
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:214
A simple abstraction for a block of bytes.
Definition: Bytes.h:28
size_t length() const
Definition: Bytes.cpp:25
const char * get() const
Definition: Bytes.cpp:30
Simple specification of the minimum functions needed from input streams.
Definition: InputStream.h:29
virtual int read()
Read and return a single byte.
Definition: InputStream.cpp:23
yarp::conf::ssize_t readFull(Bytes &b)
Keep reading until buffer is full.
Definition: InputStream.cpp:99
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.
Minimal requirements for an efficient Writer.
Definition: SizedWriter.h:36
virtual void write(OutputStream &os)
Definition: SizedWriter.cpp:19
virtual void clear()
Definition: SizedWriter.cpp:35
An InputStream that reads from a string.
void add(const std::string &txt)
An OutputStream that produces a string.
A single value (typically within a Bottle).
Definition: Value.h:47
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:207
virtual bool isList() const
Checks if value is a list.
Definition: Value.cpp:165
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:243
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Value.cpp:359
virtual bool isInt32() const
Checks if value is a 32-bit integer.
Definition: Value.cpp:135
virtual std::int32_t getCode() const
Get standard type code of value.
Definition: Value.cpp:377
virtual std::string asString() const
Get string value.
Definition: Value.cpp:237
size_t length() const
Get the length of the vector.
Definition: Vector.h:334
const std::string & getOrigin() const
Definition: WireTwiddler.h:67
std::string toString(const T &value)
convert an arbitrary type to string.
@ VOCAB_PIXEL_MONO16
Definition: Image.h:49
@ VOCAB_PIXEL_BGR
Definition: Image.h:55
@ VOCAB_PIXEL_MONO_FLOAT
Definition: Image.h:59
@ VOCAB_PIXEL_ENCODING_BAYER_GRBG8
Definition: Image.h:62
@ VOCAB_PIXEL_MONO
Definition: Image.h:48
@ VOCAB_PIXEL_RGB
Definition: Image.h:50
std::string to_string(Integer x)
Definition: numeric.h:118
::ssize_t ssize_t
Definition: numeric.h:89
yarp::sig::Vector zeros(int s)
Creates a vector of zeros (defined in Math.h).
Definition: math.cpp:552
NetInt32 encode(const std::string &str)
Convert a string into a vocabulary identifier.
Definition: Vocab.cpp:14
An interface to the operating system, including Port based communication.
yarp::conf::float32_t NetFloat32
Definition of the NetFloat32 type.
Definition: NetFloat32.h:45
yarp::conf::float64_t NetFloat64
Definition of the NetFloat64 type.
Definition: NetFloat64.h:45
std::int32_t NetInt32
Definition of the NetInt32 type.
Definition: NetInt32.h:33
constexpr yarp::conf::vocab32_t createVocab(char a, char b=0, char c=0, char d=0)
Definition: Vocab.h:22
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
Definition: ImageFile.cpp:920
bool write(const ImageOf< PixelRgb > &src, const std::string &dest, image_fileformat format=FORMAT_PPM)
Definition: ImageFile.cpp:1096