YARP
Yet Another Robot Platform
Localization2D_nws_yarp.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: LGPL-2.1-or-later
4  */
5 
6 #define _USE_MATH_DEFINES
7 
9 
10 #include <yarp/os/Bottle.h>
11 #include <yarp/os/LogComponent.h>
12 #include <yarp/os/LogStream.h>
13 #include <yarp/os/Network.h>
14 #include <yarp/os/Port.h>
15 #include <yarp/os/RFModule.h>
16 #include <yarp/os/Time.h>
17 
18 #include <yarp/sig/Vector.h>
19 
22 #include <yarp/dev/IMap2D.h>
23 #include <yarp/dev/PolyDriver.h>
24 
25 #include <yarp/math/Math.h>
26 
27 #include <cmath>
28 
31 using namespace yarp::os;
32 using namespace yarp::dev;
33 using namespace yarp::dev::Nav2D;
34 
35 #define DEFAULT_THREAD_PERIOD 0.01
36 
37 namespace
38 {
39  YARP_LOG_COMPONENT(LOCALIZATION2D_NWS_YARP, "yarp.device.localization2D_nws_yarp")
40 }
41 
42 //------------------------------------------------------------------------------------------------------------------------------
43 
45  m_period(DEFAULT_THREAD_PERIOD)
46 {
48 }
49 
51 {
52  if (driver->isValid())
53  {
54  driver->view(iLoc);
56  }
57 
58  if (nullptr == iLoc)
59  {
60  yCError(LOCALIZATION2D_NWS_YARP, "Subdevice passed to attach method is invalid");
61  return false;
62  }
63 
64  //initialize m_current_position and m_current_status, if available
65  bool ret = true;
67  Map2DLocation loc;
68  ret &= iLoc->getLocalizationStatus(status);
69  ret &= iLoc->getCurrentPosition(loc);
70  if (ret)
71  {
72  m_RPC.m_current_status = status;
74  }
75  else
76  {
77  yCWarning(LOCALIZATION2D_NWS_YARP) << "Localization data not yet available during server initialization";
78  }
79 
80  PeriodicThread::setPeriod(m_period);
81  return PeriodicThread::start();
82 }
83 
85 {
86  if (PeriodicThread::isRunning())
87  {
88  PeriodicThread::stop();
89  }
90  iLoc = nullptr;
91  return true;
92 }
93 
95 {
96  Property params;
97  params.fromString(config.toString().c_str());
98  yCDebug(LOCALIZATION2D_NWS_YARP) << "Configuration: \n" << config.toString().c_str();
99 
100  if (config.check("GENERAL") == false)
101  {
102  yCWarning(LOCALIZATION2D_NWS_YARP) << "Missing GENERAL group, assuming default options";
103  }
104 
105  Bottle& general_group = config.findGroup("GENERAL");
106  if (!general_group.check("period"))
107  {
108  yCInfo(LOCALIZATION2D_NWS_YARP) << "Missing 'period' parameter. Using default value: " << DEFAULT_THREAD_PERIOD;
110  }
111  else
112  {
113  m_period = general_group.find("period").asFloat64();
114  yCInfo(LOCALIZATION2D_NWS_YARP) << "Period requested: " << m_period;
115  }
116 
117  if (!general_group.check("publish_odometry"))
118  {
119  m_enable_publish_odometry = general_group.find("publish_odometry").asBool();
120  yCInfo(LOCALIZATION2D_NWS_YARP) << "publish_odometry=" << m_enable_publish_odometry;
121  }
122  if (!general_group.check("publish_location"))
123  {
124  m_enable_publish_location = general_group.find("publish_location").asBool();
125  yCInfo(LOCALIZATION2D_NWS_YARP) << "publish_location=" << m_enable_publish_location;
126  }
127 
128  if (!general_group.check("retrieve_position_periodically"))
129  {
130  yCInfo(LOCALIZATION2D_NWS_YARP) << "Missing 'retrieve_position_periodically' parameter. Using default value: true. Period:" << m_period ;
132  }
133  else
134  {
135  m_RPC.m_getdata_using_periodic_thread = general_group.find("retrieve_position_periodically").asBool();
137  { yCInfo(LOCALIZATION2D_NWS_YARP) << "retrieve_position_periodically requested, Period:" << m_period; }
138  else
139  { yCInfo(LOCALIZATION2D_NWS_YARP) << "retrieve_position_periodically NOT requested. Localization data obtained asynchronously."; }
140  }
141 
142  if (!general_group.check("name"))
143  {
144  yCInfo(LOCALIZATION2D_NWS_YARP) << "Missing 'name' parameter. Using default value:" << m_local_name;
145  }
146  else
147  {
148  m_local_name = general_group.find("name").asString();
149  if (m_local_name.c_str()[0] != '/') { yCError(LOCALIZATION2D_NWS_YARP) << "Missing '/' in name parameter" ; return false; }
150  yCInfo(LOCALIZATION2D_NWS_YARP) << "Using local name:" << m_local_name;
151  }
152 
153  m_rpcPortName = m_local_name + "/rpc";
154  m_2DLocationPortName = m_local_name + "/streaming:o";
155  m_odometryPortName = m_local_name + "/odometry:o";
156 
157  if (config.check("subdevice"))
158  {
159  Property p;
160  p.fromString(config.toString(), false);
161  p.put("device", config.find("subdevice").asString());
162 
163  if (!pLoc.open(p) || !pLoc.isValid())
164  {
165  yCError(LOCALIZATION2D_NWS_YARP) << "Failed to open subdevice.. check params";
166  return false;
167  }
168 
169  if (!attach(&pLoc))
170  {
171  yCError(LOCALIZATION2D_NWS_YARP) << "Failed to attach subdevice.. check params";
172  return false;
173  }
174  }
175  else
176  {
177  yCInfo(LOCALIZATION2D_NWS_YARP) << "Waiting for device to attach";
178  }
180 
181  if (!initialize_YARP(config))
182  {
183  yCError(LOCALIZATION2D_NWS_YARP) << "Error initializing YARP ports";
184  return false;
185  }
186 
187  return true;
188 }
189 
191 {
193  {
194  yCError(LOCALIZATION2D_NWS_YARP, "Failed to open port %s", m_2DLocationPortName.c_str());
195  return false;
196  }
197 
198  if (!m_odometryPort.open(m_odometryPortName.c_str()))
199  {
200  yCError(LOCALIZATION2D_NWS_YARP, "Failed to open port %s", m_odometryPortName.c_str());
201  return false;
202  }
203 
204  if (!m_rpcPort.open(m_rpcPortName.c_str()))
205  {
206  yCError(LOCALIZATION2D_NWS_YARP, "Failed to open port %s", m_rpcPortName.c_str());
207  return false;
208  }
209  m_rpcPort.setReader(*this);
210 
211  return true;
212 }
213 
215 {
216  yCTrace(LOCALIZATION2D_NWS_YARP, "Close");
217  if (PeriodicThread::isRunning())
218  {
219  PeriodicThread::stop();
220  }
221 
222  detach();
223 
229  m_rpcPort.close();
230 
231  yCDebug(LOCALIZATION2D_NWS_YARP) << "Execution terminated";
232  return true;
233 }
234 
236 {
237  bool b = m_RPC.read(connection);
238  if (b)
239  {
240  return true;
241  }
242  else
243  {
244  yCDebug(LOCALIZATION2D_NWS_YARP) << "read() Command failed";
245  return false;
246  }
247 }
248 
250 {
251  double m_stats_time_curr = yarp::os::Time::now();
252  if (m_stats_time_curr - m_stats_time_last > 5.0)
253  {
254  yCInfo(LOCALIZATION2D_NWS_YARP) << "Running";
256  }
257 
258  //enter the critical section
259  m_RPC.getMutex()->lock();
260  {
262  {
264  if (ret == false)
265  {
266  yCError(LOCALIZATION2D_NWS_YARP) << "getLocalizationStatus() failed";
267  }
268 
270  {
272  if (ret2 == false)
273  {
274  yCError(LOCALIZATION2D_NWS_YARP) << "getCurrentPosition() failed";
275  }
276  else
277  {
279  }
281  if (ret3 == false)
282  {
283  //yCError(LOCALIZATION2D_NWS_YARP) << "getEstimatedOdometry() failed";
284  }
285  else
286  {
288  }
289  }
290  else
291  {
292  yCWarning(LOCALIZATION2D_NWS_YARP, "The system is not properly localized!");
293  }
294  }
295  }
296  m_RPC.getMutex()->unlock();
297 
299  publish_odometry_on_yarp_port();
300  }
302  publish_2DLocation_on_yarp_port();
303  }
304 }
305 
306 void Localization2D_nws_yarp::publish_odometry_on_yarp_port()
307 {
308  if (m_odometryPort.getOutputCount() > 0)
309  {
311  odom = m_RPC.m_current_odometry;
312 
313  //send data to port
316  }
317 }
318 
319 void Localization2D_nws_yarp::publish_2DLocation_on_yarp_port()
320 {
322  {
325  {
327  }
328  else
329  {
330  Map2DLocation temp_loc;
331  temp_loc.x = std::nan("");
332  temp_loc.y = std::nan("");
333  temp_loc.theta = std::nan("");
334  loc = temp_loc;
335  }
336 
337  //send data to port
340  }
341 }
define control board standard interfaces
bool ret
#define DEFAULT_THREAD_PERIOD
contains the definition of a Vector type
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::dev::OdometryData m_current_odometry
void setInterface(yarp::dev::Nav2D::ILocalization2D *_iloc)
yarp::dev::Nav2D::LocalizationStatusEnum m_current_status
yarp::dev::Nav2D::Map2DLocation m_current_position
void run() override
Loop function.
yarp::os::BufferedPort< yarp::dev::OdometryData > m_odometryPort
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::dev::PolyDriver pLoc
yarp::dev::Nav2D::ILocalization2D * iLoc
bool initialize_YARP(yarp::os::Searchable &config)
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
bool close() override
Close the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
yarp::os::BufferedPort< yarp::dev::Nav2D::Map2DLocation > m_2DLocationPort
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
double theta
orientation [deg] in the map reference frame
double y
y position of the location [m], expressed in the map reference frame
double x
x position of the location [m], expressed in the map reference frame
virtual bool getEstimatedOdometry(yarp::dev::OdometryData &odom)=0
Gets the estimated odometry the robot, including its velocity expressed in the world and in the local...
virtual bool getCurrentPosition(yarp::dev::Nav2D::Map2DLocation &loc)=0
Gets the current position of the robot w.r.t world reference frame.
virtual bool getLocalizationStatus(LocalizationStatusEnum &status)=0
Gets the current status of the localization task.
A container for a device driver.
Definition: PolyDriver.h:23
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
bool open(const std::string &txt)
Construct and configure a device by its common name.
Definition: PolyDriver.cpp:140
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Bottle.cpp:277
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition: Bottle.cpp:287
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
int getOutputCount() override
Determine how many output connections this port has.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
An interface for reading from a network connection.
An abstraction for a periodic thread.
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition: Port.cpp:511
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition: Port.cpp:383
void close() override
Stop port activity.
Definition: Port.cpp:363
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:33
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
A base class for nested structures that can be searched.
Definition: Searchable.h:63
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition: Stamp.cpp:124
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
virtual bool asBool() const
Get boolean value.
Definition: Value.cpp:186
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCTrace(component,...)
Definition: LogComponent.h:84
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
An interface to the operating system, including Port based communication.