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