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