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