YARP
Yet Another Robot Platform
RosLookup.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 "RosLookup.h"
7 #include "TcpRosLogComponent.h"
8 
10 
11 #include <yarp/os/Bottle.h>
12 #include <yarp/os/ContactStyle.h>
13 #include <yarp/os/Network.h>
14 
15 #include <cstdlib>
16 
17 using namespace yarp::os;
18 
19 static bool rpc(const Contact& c,
20  const char *carrier,
21  Bottle& writer,
22  Bottle& reader)
23 {
24  ContactStyle style;
25  style.quiet = false;
26  style.timeout = 4;
27  style.carrier = carrier;
29  " > sending to [%s] %s",
30  c.toString().c_str(),
31  writer.toString().c_str());
32  bool ok = Network::write(c,writer,reader,style);
33  return ok;
34 }
35 
36 bool RosLookup::lookupCore(const std::string& name) {
37  Bottle req, reply;
38  req.addString("lookupNode");
39  req.addString("dummy_id");
40  req.addString(name);
41  rpc(getRosCoreAddress(), "xmlrpc", req, reply);
42  if (reply.get(0).asInt32()!=1) {
43  yCError(TCPROSCARRIER, "Failure: %s", reply.toString().c_str());
44  return false;
45  }
46  std::string url = reply.get(2).asString();
47  std::string::size_type break1 = url.find("://",0);
48  if (break1==std::string::npos) {
49  yCError(TCPROSCARRIER, "url not understood: %s", url.c_str());
50  return false;
51  }
52  std::string::size_type break2 = url.find(':',break1+3);
53  if (break2==std::string::npos) {
54  yCError(TCPROSCARRIER, "url not understood: %s", url.c_str());
55  return false;
56  }
57  std::string::size_type break3 = url.find('/',break2+1);
58  if (break3==std::string::npos) {
59  yCError(TCPROSCARRIER, "url not understood: %s", url.c_str());
60  return false;
61  }
62  hostname = url.substr(break1+3,break2-break1-3);
63  Value vportnum;
64  vportnum.fromString(url.substr(break2+1,break3-break2-1).c_str());
65  portnum = vportnum.asInt32();
66  yCDebug(TCPROSCARRIER, "%s", reply.toString().c_str());
67  valid = (portnum!=0);
68  rpc(getRosCoreAddress(), "xmlrpc", req, reply);
69  return valid;
70 }
71 
72 
73 bool RosLookup::lookupTopic(const std::string& name) {
74  if (!valid) {
75  yCError(TCPROSCARRIER, "Need a node");
76  return false;
77  }
78  Bottle req, reply;
79  req.addString("requestTopic");
80  req.addString("dummy_id");
81  req.addString(name);
82  Bottle& lst = req.addList();
83  Bottle& sublst = lst.addList();
84  sublst.addString("TCPROS");
86  "Sending [%s] to %s",
87  req.toString().c_str(),
88  toString().c_str());
90  rpc(c,"xmlrpc",req,reply);
91  if (reply.get(0).asInt32()!=1) {
92  yCError(TCPROSCARRIER, "Failure looking up topic %s: %s", name.c_str(), reply.toString().c_str());
93  return false;
94  }
95  Bottle *pref = reply.get(2).asList();
96  if (pref==nullptr) {
97  yCError(TCPROSCARRIER, "Failure looking up topic %s: expected list of protocols", name.c_str());
98  return false;
99  }
100  if (pref->get(0).asString()!="TCPROS") {
101  yCError(TCPROSCARRIER, "Failure looking up topic %s: unsupported protocol %s", name.c_str(),
102  pref->get(0).asString().c_str());
103  return false;
104  }
105  Value hostname2 = pref->get(1);
106  Value portnum2 = pref->get(2);
107  hostname = hostname2.asString();
108  portnum = portnum2.asInt32();
109  protocol = "tcpros";
111  "topic %s available at %s:%d",
112  name.c_str(),
113  hostname.c_str(),
114  portnum);
115  return true;
116 }
117 
119  std::string addr = yarp::conf::environment::get_string("ROS_MASTER_URI");
120  Contact c = Contact::fromString(addr);
121  if (c.isValid()) {
122  c.setCarrier("xmlrpc");
123  }
124  return c;
125 }
126 
128  static bool checkedEnv = false;
129  static Contact addr;
130  if (!checkedEnv) {
131  Contact c = getRosCoreAddressFromEnv();
132  addr = c;
133  checkedEnv = true;
134  }
135  if (!addr.isValid()) {
136  addr = NetworkBase::queryName("/roscore");
137  }
138  if (!addr.isValid()) {
139  yCError(TCPROSCARRIER, "cannot find roscore, is ROS_MASTER_URI set?");
140  ::exit(1);
141  }
142  return addr;
143 }
static bool rpc(const Contact &c, const char *carrier, Bottle &writer, Bottle &reader)
Definition: RosLookup.cpp:19
const yarp::os::LogComponent & TCPROSCARRIER()
static yarp::os::Contact getRosCoreAddress()
Definition: RosLookup.cpp:127
bool lookupTopic(const std::string &name)
Definition: RosLookup.cpp:73
bool lookupCore(const std::string &name)
Definition: RosLookup.cpp:36
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:73
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:182
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
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
Preferences for how to communicate with a contact.
Definition: ContactStyle.h:23
double timeout
Set a timeout for communication (in units of seconds, fractional seconds allowed).
Definition: ContactStyle.h:46
bool quiet
Suppress all outputs and warnings.
Definition: ContactStyle.h:35
std::string carrier
Request that communication be made using a particular carrier.
Definition: ContactStyle.h:52
Represents how to reach a part of a YARP network.
Definition: Contact.h:33
void setCarrier(const std::string &carrier)
Set the carrier to use for this Contact.
Definition: Contact.cpp:255
bool isValid() const
Checks if a Contact is tagged as valid.
Definition: Contact.cpp:298
std::string toString() const
Get a textual representation of the Contact.
Definition: Contact.cpp:303
static Contact fromString(const std::string &txt)
Factory method.
Definition: Contact.cpp:139
static Contact queryName(const std::string &name)
Find out information about a registered name.
Definition: Network.cpp:995
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
A single value (typically within a Bottle).
Definition: Value.h:43
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:240
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
std::string toString(const T &value)
convert an arbitrary type to string.
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCDebug(component,...)
Definition: LogComponent.h:128
std::string get_string(const std::string &key, bool *found=nullptr)
Read a string from an environment variable.
Definition: environment.h:66
An interface to the operating system, including Port based communication.