YARP
Yet Another Robot Platform
yarpros.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 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 <cstdio>
10 #include <yarp/os/all.h>
11 #include "RosSlave.h"
12 #include "RosLookup.h"
13 #include "TcpRosStream.h"
14 
15 #include <string>
16 
17 
18 using namespace yarp::os;
19 using namespace std;
20 
21 bool verbose = false;
22 
23 string addPart(string t, string name, int code, Value *val, string orig, string mode="") {
24  char buf[5000];
25  if (mode=="length") {
26  sprintf(buf,"%s %s # suggested length: %d", t.c_str(), name.c_str(), code);
27  } else if (mode=="string") {
28  sprintf(buf,"%s %s # value seen: \"%s\"", t.c_str(), name.c_str(), orig.c_str());
29  } else if (mode=="vocab") {
30  char char4 = (code>>24)%256;
31  char char3 = (code>>16)%256;
32  char char2 = (code>>8)%256;
33  char char1 = code%256;
34  string r;
35  if (char1!=0) {
36  r += '\''; r += char1; r += "\'*256^3";
37  }
38  if (char2!=0) {
39  if (r!="") r += "+";
40  r += '\''; r += char2; r += "\'*256^2";
41  }
42  if (char3!=0) {
43  if (r!="") r += "+";
44  r += '\''; r += char3; r += "\'*256";
45  }
46  if (char4!=0) {
47  if (r!="") r += "+";
48  r += '\''; r += char4; r += '\'';
49  }
50  if (r.length()==0) {
51  r = "0";
52  }
53  sprintf(buf,"%s %s # set to %d (=%s=%s)", t.c_str(), name.c_str(), code, r.c_str(), orig.c_str());
54  } else {
55  if (val) {
56  sprintf(buf,"%s %s # set to %s (%s)", t.c_str(), name.c_str(), val->toString().c_str(), orig.c_str());
57  } else {
58  sprintf(buf,"%s %s # set to %d (%s)", t.c_str(), name.c_str(), code, orig.c_str());
59  }
60  }
61  return buf;
62 }
63 
64 string showFormat(Bottle& b, string root) {
65  string r;
66  int code = b.getSpecialization();
67  r += addPart("int32",root + "_tag",BOTTLE_TAG_LIST+code,nullptr,"BOTTLE_TAG_LIST+code");
68  r += "\n";
69  bool specialized = (code>0);
70  if (code==BOTTLE_TAG_INT32) {
71  r += addPart("int32[]",root,b.size(),nullptr,"length","length");
72  r += "\n";
73  if (b.size()<50) {
74  r += " # integers seen: ";
75  for (size_t i=0; i<b.size(); i++) {
76  char buf[1000];
77  sprintf(buf," %d",b.get(i).asInt32());
78  r += buf;
79  }
80  r += "\n";
81  }
82  return r;
83  }
84  if (code==BOTTLE_TAG_FLOAT64) {
85  r += addPart("float64[]",root,b.size(),nullptr,"length","length");
86  r += "\n";
87  if (b.size()<50) {
88  r += " # floats seen: ";
89  for (size_t i=0; i<b.size(); i++) {
90  char buf[1000];
91  sprintf(buf," %g",b.get(i).asFloat64());
92  r += buf;
93  }
94  r += "\n";
95  }
96  return r;
97  }
98  r += addPart("int32",root + "_len",b.size(),nullptr,"elements in list");
99  r += "\n";
100  for (size_t i=0; i<b.size(); i++) {
101  Value& v = b.get(i);
102  char tag_name[1000];
103  char val_name[1000];
104  sprintf(tag_name,"%s%zu_tag", root.c_str(), i);
105  sprintf(val_name,"%s%zu", root.c_str(), i);
106  if (v.isVocab()) {
107  if (!specialized) {
108  r += addPart("int32",tag_name,BOTTLE_TAG_VOCAB,nullptr,
109  "BOTTLE_TAG_VOCAB");
110  r += "\n";
111  }
112  r += addPart("int32",val_name,v.asInt32(),nullptr,v.toString().c_str(),"vocab");
113  r += "\n";
114  } else if (v.isInt32()) {
115  if (!specialized) {
116  r += addPart("int32",tag_name,BOTTLE_TAG_INT32,nullptr,
117  "BOTTLE_TAG_INT32");
118  r += "\n";
119  }
120  r += addPart("int32",val_name,v.asInt32(),&v,v.toString().c_str());
121  r += "\n";
122  } else if (v.isFloat64()) {
123  if (!specialized) {
124  r += addPart("int32",tag_name,BOTTLE_TAG_FLOAT64,nullptr,
125  "BOTTLE_TAG_FLOAT64");
126  r += "\n";
127  }
128  r += addPart("float64",val_name,v.asInt32(),&v,v.toString().c_str());
129  r += "\n";
130  } else if (v.isList()) {
131  r += showFormat(*v.asList(), val_name);
132  } else if (v.isBlob()) {
133  if (!specialized) {
134  r += addPart("int32",tag_name,BOTTLE_TAG_BLOB,nullptr,
135  "BOTTLE_TAG_BLOB");
136  r += "\n";
137  }
138  r += addPart("int8[]",val_name,v.asBlobLength(),nullptr,"length","length");
139  } else if (v.isString()) {
140  if (!specialized) {
141  r += addPart("int32",tag_name,BOTTLE_TAG_STRING,nullptr,
142  "BOTTLE_TAG_STRING");
143  r += "\n";
144  }
145  r += addPart("string",val_name,0,nullptr,v.asString().c_str(),"string");
146  r += "\n";
147  } else {
148  r += "IGNORED ";
149  r += v.toString().c_str();
150  r += "\n";
151  }
152  }
153  return r;
154 }
155 
156 void usage(const char *action,
157  const char *msg,
158  const char *example = nullptr,
159  const char *explanation = nullptr) {
160  printf("\n yarpros %s\n", action);
161  printf(" %s\n", msg);
162  if (example!=nullptr) {
163  printf(" $ yarpros %s\n", example);
164  }
165  if (explanation!=nullptr) {
166  printf(" # %s\n", explanation);
167  }
168 }
169 
170 void show_usage() {
171  printf("Welcome to yarpros. Here are the most useful commands available:\n");
172  usage("sniff out <port>","suggest .msg for output from <port> ","sniff out /grabber");
173  usage("sniff in <port>","suggest .msg for input to <port> ","sniff in /grabber");
174  usage("type <name>","(MOVED to yarpidl_rosmsg) generate YARP header files from <name>.msg","type PointCloud2");
175  usage("help","show this help",nullptr);
176 
177  printf("\nYARP clients can use the ROS name server. If you'd prefer to stick\n");
178  printf("with the native YARP name server, the following commands are useful:\n");
179  usage("roscore","register port /roscore to refer to ROS_MASTER_URI","roscore");
180  usage("roscore <hostname> <port number>","manually register port /roscore to point to the ros master","roscore 192.168.0.1 11311");
181  usage("pub[lisher] <node> <topic>","register a ROS publisher <node>/<topic> pair as a port called <node><topic>","publisher /talker /chatter","this registers a port called /talker/chatter");
182  usage("pub[lisher] <port> <node> <topic>","register a ROS publisher <node>/<topic> pair as a port called <port>","publisher /talker /talker /chatter");
183  usage("sub[scriber] <node> <topic>","register a ROS subscriber <node>/<topic> pair as a port called <node><topic>","subscriber /listener /chatter","this registers a port called /listener/chatter");
184  usage("sub[scriber] <yarp> <node> <topic>","register a ROS subscriber <node>/<topic> pair as a port called <port>","subscriber /listener /listener /chatter");
185  usage("service <yarp> <node> <service>","register a ROS service <node>/<service> pair as a port called <port>","service /adder /add_two_ints_server /add_two_ints");
186  usage("node <name>","register a ROS node name with YARP","node /talker");
187 
188  printf("\nHere are some general options:\n");
189  usage("--verbose","give verbose output for debugging",nullptr);
190 }
191 
192 bool announce_port(const char *name,
193  PortReader& reply) {
194  Bottle req;
195  req.addString("announce");
196  req.addString(name);
198  req,
199  reply);
200 }
201 
202 bool register_port(const char *name,
203  const char *carrier,
204  const char *hostname,
205  int portnum,
206  PortReader& reply) {
207  std::string ip = Contact::convertHostToIp(hostname);
208  Bottle req;
209  req.addString("register");
210  req.addString(name);
211  req.addString(carrier);
212  req.addString(ip);
213  req.addInt32(portnum);
215  req,
216  reply);
217  if (ok) {
218  Bottle reply2;
219  announce_port(name,reply2);
220  }
221  return ok;
222 }
223 
224 
225 int main(int argc, char *argv[]) {
226  if (argc<=1) {
227  show_usage();
228  return 0;
229  }
230  if (std::string(argv[1])=="help" ||
231  std::string(argv[1])=="--help") {
232  show_usage();
233  return 0;
234  }
235 
236  Network yarp;
237 
238  // Read in the command sequence
239  Bottle cmd;
240  for (int i=1; i<argc; i++) {
241  Value v;
242  v.fromString(argv[i]);
243  if (argv[i][0]!='-') {
244  cmd.add(v);
245  }
246  }
247 
248  // Check for flags
249  Property options;
250  options.fromCommand(argc,argv);
251  verbose = false;
252  if (options.check("verbose")) {
253  verbose = true;
254  }
255 
256  // Get the command tag
257  std::string tag = cmd.get(0).asString();
258 
259  // Process the command
260  if (tag=="roscore") {
261  if (cmd.size()>1) {
262  if (!(cmd.get(1).isString()&&cmd.get(2).isInt32())) {
263  fprintf(stderr,"wrong syntax, run with no arguments for help\n");
264  return 1;
265  }
266  Bottle reply;
267  register_port("/roscore", "xmlrpc",
268  cmd.get(1).asString().c_str(), cmd.get(2).asInt32(),
269  reply);
270  printf("%s\n", reply.toString().c_str());
271  } else {
272  Bottle reply;
274  if (!c.isValid()) {
275  fprintf(stderr,"cannot find roscore, is ROS_MASTER_URI set?\n");
276  return 1;
277  }
278  register_port("/roscore", "xmlrpc",
279  c.getHost().c_str(), c.getPort(),
280  reply);
281  printf("%s\n", reply.toString().c_str());
282  }
283  return 0;
284  } else if (tag=="node") {
285  Bottle req, reply;
286  if (cmd.size()!=2) {
287  fprintf(stderr,"wrong syntax, run with no arguments for help\n");
288  return 1;
289  }
290  if (!cmd.get(1).isString()) {
291  fprintf(stderr,"wrong syntax, run with no arguments for help\n");
292  return 1;
293  }
294  RosLookup lookup(verbose);
295  bool ok = lookup.lookupCore(cmd.get(1).asString());
296  if (ok) {
297  register_port(cmd.get(1).asString().c_str(),
298  "xmlrpc",
299  lookup.hostname.c_str(),
300  lookup.portnum,
301  reply);
302  printf("%s\n",reply.toString().c_str());
303  }
304  return ok?0:1;
305  } else if (tag=="publisher"||tag=="pub"||tag=="service"||tag=="srv") {
306  bool service = (tag=="service"||tag=="srv");
307  Bottle req, reply;
308  if (cmd.size()!=3 && cmd.size()!=4) {
309  fprintf(stderr,"wrong syntax, run with no arguments for help\n");
310  return 1;
311  }
312  int offset = 0;
313  if (cmd.size()==3) {
314  offset = -1;
315  }
316  std::string yarp_port = cmd.get(1+offset).asString();
317  std::string ros_port = cmd.get(2+offset).asString();
318  std::string topic = cmd.get(3+offset).asString();
319  if (cmd.size()==3) {
320  yarp_port = ros_port + topic;
321  }
322  RosLookup lookup(verbose);
323  if (verbose) printf(" * looking up ros node %s\n", ros_port.c_str());
324  bool ok = lookup.lookupCore(ros_port.c_str());
325  if (!ok) return 1;
326  if (verbose) printf(" * found ros node %s\n", ros_port.c_str());
327  if (verbose) printf(" * looking up topic %s\n", topic.c_str());
328  ok = lookup.lookupTopic(topic.c_str());
329  if (!ok) return 1;
330  if (verbose) printf(" * found topic %s\n", topic.c_str());
331  string carrier = "tcpros+role.pub+topic.";
332  if (service) {
333  carrier = "rossrv+service.";
334  }
335  register_port(yarp_port.c_str(),
336  (carrier+topic.c_str()).c_str(),
337  lookup.hostname.c_str(),
338  lookup.portnum,
339  reply);
340  printf("%s\n", reply.toString().c_str());
341  return 0;
342  } else if (tag=="subscriber"||tag=="sub") {
343  Bottle req, reply;
344  if (cmd.size()!=3 && cmd.size()!=4) {
345  fprintf(stderr,"wrong syntax, run with no arguments for help\n");
346  return 1;
347  }
348  int offset = 0;
349  if (cmd.size()==3) {
350  offset = -1;
351  }
352  std::string yarp_port = cmd.get(1+offset).asString();
353  std::string ros_port = cmd.get(2+offset).asString();
354  std::string topic = cmd.get(3+offset).asString();
355  if (cmd.size()==3) {
356  yarp_port = ros_port + topic;
357  }
358  RosLookup lookup(verbose);
359  if (verbose) printf(" * looking up ros node %s\n", ros_port.c_str());
360  bool ok = lookup.lookupCore(ros_port.c_str());
361  if (!ok) return 1;
362  if (verbose) printf(" * found ros node %s\n", ros_port.c_str());
363  ok = register_port(yarp_port.c_str(),
364  (string("tcpros+role.sub+topic.")+topic.c_str()).c_str(),
365  lookup.hostname.c_str(),
366  lookup.portnum,
367  reply);
368  printf("%s\n", reply.toString().c_str());
369  return ok?0:1;
370  } else if (tag=="type") {
371  fprintf(stderr, "MOVED: 'yarpros type' is now 'yarpidl_rosmsg'\n");
372  return 1;
373  } else if (tag=="sniff") {
374  if (cmd.size()<2) {
375  fprintf(stderr,"Show the format of a YARP bottle-compatible message in ROS syntax.\n");
376  return 1;
377  }
378  std::string dir = cmd.get(1).asString();
379  bool in = false;
380  if (dir=="in") in = true;
381  else if (dir=="out") in = false;
382  else {
383  fprintf(stderr,"Please specify one of 'in' or 'out'.\n");
384  return 1;
385  }
386  std::string pname = cmd.get(2).asString();
387  Port p;
388  if (!p.open("...")) return 1;
389  if (in) {
390  if (!Network::connect(pname,p.getName(),"tcp+log.in")) return 1;
391  } else {
392  if (!Network::connect(pname,p.getName())) return 1;
393  }
394  Bottle b;
395  p.read(b);
396  string r;
397  if (in&&b.get(0).asVocab()==yarp::os::createVocab('r','p','c')&&b.get(1).isList()) {
398 
399  r = showFormat(*b.get(1).asList(),"v");
400  } else {
401  r = showFormat(b,"v");
402  }
403  printf("Got message:\n%s\n",r.c_str());
404  return 0;
405  } else {
406  fprintf(stderr,"unknown command, run with no arguments for help\n");
407  return 1;
408  }
409  return 0;
410 }
411 
std::string hostname
Definition: RosLookup.h:17
bool isValid() const
Checks if a Contact is tagged as valid.
Definition: Contact.cpp:307
string showFormat(Bottle &b, string root)
Definition: yarpros.cpp:64
#define BOTTLE_TAG_STRING
Definition: Bottle.h:28
#define BOTTLE_TAG_INT32
Definition: Bottle.h:23
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Value.cpp:343
virtual bool isBlob() const
Checks if value is a binary object.
Definition: Value.cpp:182
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Property.cpp:957
A class for storing options and configuration information.
Definition: Property.h:35
static Contact getNameServerContact()
Get the contact information for the port associated with the nameserver (usually "/root", but this can be overwritten by the "yarp namespace" command).
Definition: Network.cpp:1304
int main(int argc, char *argv[])
Definition: yarpros.cpp:225
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:129
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:206
int getSpecialization()
Get numeric bottle code for this bottle.
Definition: Bottle.cpp:220
STL namespace.
A mini-server for network communication.
Definition: Port.h:50
void usage(const char *action, const char *msg, const char *example=nullptr, const char *explanation=nullptr)
Definition: yarpros.cpp:156
bool announce_port(const char *name, PortReader &reply)
Definition: yarpros.cpp:192
int portnum
Definition: RosLookup.h:18
virtual bool isVocab() const
Checks if value is a vocabulary identifier.
Definition: Value.cpp:176
void add(const Value &value)
Add a Value to the bottle, at the end of the list.
Definition: Bottle.cpp:295
The main, catch-all namespace for YARP.
Definition: numeric.h:47
std::string getHost() const
Get the host name associated with this Contact for socket communication.
Definition: Contact.cpp:231
virtual std::string asString() const
Get string value.
Definition: Value.cpp:236
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: Port.cpp:79
#define BOTTLE_TAG_VOCAB
Definition: Bottle.h:25
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
Definition: Value.cpp:152
virtual bool isString() const
Checks if value is a string.
Definition: Value.cpp:158
float t
Interface implemented by all objects that can read themselves from the network, such as Bottle object...
Definition: PortReader.h:27
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:99
string addPart(string t, string name, int code, Value *val, string orig, string mode="")
Definition: yarpros.cpp:23
virtual std::string getName() const
Get name of port.
Definition: Contactable.cpp:18
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:242
A simple collection of objects that can be described and transmitted in a portable way...
Definition: Bottle.h:70
#define BOTTLE_TAG_BLOB
Definition: Bottle.h:29
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:27
bool verbose
Definition: yarpros.cpp:21
Utilities for manipulating the YARP network, including initialization and shutdown.
Definition: Network.h:688
constexpr yarp::conf::vocab32_t createVocab(char a, char b=0, char c=0, char d=0)
Definition: Vocab.h:43
static std::string convertHostToIp(const char *name)
If the host is a machine name, convert it to a plausible IP address.
Definition: Contact.cpp:340
bool lookupCore(const std::string &name)
Definition: RosLookup.cpp:32
void show_usage()
Definition: yarpros.cpp:170
bool register_port(const char *name, const char *carrier, const char *hostname, int portnum, PortReader &reply)
Definition: yarpros.cpp:202
void fromString(const char *str)
Set value to correspond to a textual representation.
Definition: Value.cpp:339
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:170
static yarp::os::Contact getRosCoreAddressFromEnv()
Definition: RosLookup.cpp:110
A single value (typically within a Bottle).
Definition: Value.h:40
int getPort() const
Get the port number associated with this Contact for socket communication.
Definition: Contact.cpp:243
Represents how to reach a part of a YARP network.
Definition: Contact.h:34
virtual size_t asBlobLength() const
Get binary data length.
Definition: Value.cpp:267
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:224
static bool connect(const std::string &src, const std::string &dest, const std::string &carrier="", bool quiet=true)
Request that an output port connect to an input port.
Definition: Network.cpp:648
static bool write(const Contact &contact, PortWriter &cmd, PortReader &reply, bool admin=false, bool quiet=false, double timeout=-1)
Send a single command to a port and await a single response.
Definition: Network.cpp:1169
bool lookupTopic(const std::string &name)
Definition: RosLookup.cpp:69
#define BOTTLE_TAG_LIST
Definition: Bottle.h:30
An interface to the operating system, including Port based communication.
virtual std::int32_t asVocab() const
Get vocabulary identifier as an integer.
Definition: Value.cpp:230
bool read(PortReader &reader, bool willReply=false) override
Read an object from the port.
Definition: Port.cpp:480
virtual bool isList() const
Checks if value is a list.
Definition: Value.cpp:164
virtual bool isInt32() const
Checks if value is a 32-bit integer.
Definition: Value.cpp:134
size_t size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:210
void fromCommand(int argc, char *argv[], bool skipFirst=true, bool wipe=true)
Interprets a list of command arguments as a list of properties.
Definition: Property.cpp:992
Value & get(size_t index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:205