YARP
Yet Another Robot Platform
navigation2DServer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3  *
4  * This library is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU Lesser General Public
6  * License as published by the Free Software Foundation; either
7  * version 2.1 of the License, or (at your option) any later version.
8  *
9  * This library is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12  * Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public
15  * License along with this library; if not, write to the Free Software
16  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
17  */
18 
19 #define _USE_MATH_DEFINES
20 
21 #include <yarp/os/Network.h>
22 #include <yarp/os/RFModule.h>
23 #include <yarp/os/Time.h>
24 #include <yarp/os/Port.h>
25 #include <yarp/os/LogStream.h>
27 #include <yarp/dev/INavigation2D.h>
28 #include <yarp/dev/MapGrid2D.h>
29 #include <math.h>
30 #include <cmath>
31 #include "navigation2DServer.h"
32 
33 using namespace yarp::os;
34 using namespace yarp::dev;
35 using namespace yarp::dev::Nav2D;
36 using namespace std;
37 
38 #ifndef RAD2DEG
39 #define RAD2DEG 180.0/M_PI
40 #endif
41 
43 {
44  iNav_target = nullptr;
45  iNav_ctrl = nullptr;
47 }
48 
50 {
51  if (device2attach.size() != 1)
52  {
53  yError("Navigation2DServer: cannot attach more than one device");
54  return false;
55  }
56 
57  yarp::dev::PolyDriver * Idevice2attach = device2attach[0]->poly;
58 
59  if (Idevice2attach->isValid())
60  {
61  Idevice2attach->view(iNav_target);
62  Idevice2attach->view(iNav_ctrl);
63  }
64 
65  if (nullptr == iNav_target ||
66  nullptr == iNav_ctrl)
67  {
68  yError("Navigation2DServer: subdevice passed to attach method is invalid");
69  return false;
70  }
71 
72  PeriodicThread::setPeriod(m_period);
73  return PeriodicThread::start();
74 }
75 
77 {
78  if (PeriodicThread::isRunning())
79  {
80  PeriodicThread::stop();
81  }
82  iNav_target = nullptr;
83  iNav_ctrl = nullptr;
84  return true;
85 }
86 
88 {
89  Property params;
90  params.fromString(config.toString().c_str());
91  yDebug() << "navigation2DServer configuration: \n" << config.toString().c_str();
92 
93  if (!config.check("period"))
94  {
95  yInfo() << "navigation2DServer: missing 'period' parameter. Using default value: 0.010";
96  m_period = 0.010;
97  }
98  else
99  {
100  m_period = config.find("period").asFloat64();
101  }
102 
103  string local_name = "/navigationServer";
104  if (!config.check("name"))
105  {
106  yInfo() << "navigation2DServer: missing 'name' parameter. Using default value: /navigationServer";
107  }
108  else
109  {
110  local_name = config.find("name").asString();
111  }
112  m_rpcPortName = local_name + "/rpc";
113  m_streamingPortName = local_name + "/streaming:o";
114 
115  if (config.check("subdevice"))
116  {
117  Property p;
118  PolyDriverList driverlist;
119  p.fromString(config.toString(), false);
120  p.put("device", config.find("subdevice").asString());
121 
122  if (!pNav.open(p) || !pNav.isValid())
123  {
124  yError() << "navigation2DServer: failed to open subdevice.. check params";
125  return false;
126  }
127 
128  driverlist.push(&pNav, "1");
129  if (!attachAll(driverlist))
130  {
131  yError() << "navigation2DServer: failed to open subdevice.. check params";
132  return false;
133  }
134  }
136 
137  if (!initialize_YARP(config))
138  {
139  yError() << "navigation2DServer: Error initializing YARP ports";
140  return false;
141  }
142 
143  return true;
144 }
145 
147 {
148  if (!m_rpcPort.open(m_rpcPortName.c_str()))
149  {
150  yError("Navigation2DServer: failed to open port %s", m_rpcPortName.c_str());
151  return false;
152  }
153  m_rpcPort.setReader(*this);
154  return true;
155 }
156 
158 {
159  yTrace("navigation2DServer::Close");
160  if (PeriodicThread::isRunning())
161  {
162  PeriodicThread::stop();
163  }
164 
165  detachAll();
166  return true;
167 }
168 
169 bool navigation2DServer::parse_respond_string(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
170 {
171  if (command.get(0).isString() == false)
172  {
173  yError() << "General error in navigation2DServer::parse_respond_string()";
174  return false;
175  }
176 
177  if (command.get(0).asString() == "help")
178  {
179  reply.addVocab(Vocab::encode("many"));
180  reply.addString("Navigation2DServer does not support rpc commands in plain text format, only vocabs.");
181  reply.addString("Please use the rpc port of Navigation2DClient.");
182  }
183  else
184  {
185  reply.addString("Unknown command. Type 'help'.");
186  }
187  return true;
188 }
189 
190 bool navigation2DServer::parse_respond_vocab(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
191 {
192  if (command.get(0).isVocab() == false)
193  {
194  yError() << "General error in navigation2DServer::parse_respond_vocab()";
195  return false;
196  }
197 
198  if (command.get(0).asVocab() != VOCAB_INAVIGATION ||
199  command.get(1).isVocab() == false)
200  {
201  yError() << "Invalid vocab received";
202  reply.addVocab(VOCAB_ERR);
203  return true;
204  }
205 
206  int request = command.get(1).asVocab();
207  if (request == VOCAB_NAV_GOTOABS)
208  {
209  Map2DLocation loc;
210  loc.map_id = command.get(2).asString();
211  loc.x = command.get(3).asFloat64();
212  loc.y = command.get(4).asFloat64();
213  loc.theta = command.get(5).asFloat64();
215  if (ret)
216  {
217  reply.addVocab(VOCAB_OK);
218  }
219  else
220  {
221  yError() << "gotoTargetByAbsoluteLocation() failed";
222  reply.addVocab(VOCAB_ERR);
223  }
224  }
225  else if (request == VOCAB_NAV_RECOMPUTE_PATH)
226  {
228  if (ret)
229  {
230  reply.addVocab(VOCAB_OK);
231  }
232  else
233  {
234  yError() << "recomputeCurrentNavigationPath() failed";
235  reply.addVocab(VOCAB_ERR);
236  }
237  reply.addVocab(VOCAB_OK);
238  }
239  else if (request == VOCAB_NAV_GOTOREL)
240  {
241  if (command.size() == 5)
242  {
243  double x = command.get(2).asFloat64();
244  double y = command.get(3).asFloat64();
245  double theta = command.get(4).asFloat64();
246  bool ret = iNav_target->gotoTargetByRelativeLocation(x, y, theta);
247  if (ret)
248  {
249  reply.addVocab(VOCAB_OK);
250  }
251  else
252  {
253  yError() << "gotoTargetByRelativeLocation() failed";
254  reply.addVocab(VOCAB_ERR);
255  }
256  }
257  else if (command.size() == 4)
258  {
259  double x = command.get(2).asFloat64();
260  double y = command.get(3).asFloat64();
262  if (ret)
263  {
264  reply.addVocab(VOCAB_OK);
265  }
266  else
267  {
268  yError() << "gotoTargetByRelativeLocation() failed";
269  reply.addVocab(VOCAB_ERR);
270  }
271  }
272  else
273  {
274  yError() << "Invalid number of params";
275  reply.addVocab(VOCAB_ERR);
276  }
277  }
278  else if (request == VOCAB_NAV_VELOCITY_CMD)
279  {
280  double x_vel = command.get(2).asFloat64();
281  double y_vel = command.get(3).asFloat64();
282  double t_vel = command.get(4).asFloat64();
283  double timeout = command.get(5).asFloat64();
284  bool ret = iNav_target->applyVelocityCommand(x_vel,y_vel,t_vel,timeout);
285  if (ret)
286  {
287  reply.addVocab(VOCAB_OK);
288  reply.addInt32(VOCAB_OK);
289  }
290  else
291  {
292  yError() << "applyVelocityCommand() failed";
293  reply.addVocab(VOCAB_ERR);
294  }
295  }
296  else if (request == VOCAB_NAV_GET_NAVIGATION_STATUS)
297  {
299  bool ret = iNav_ctrl->getNavigationStatus(nav_status);
300  if (ret)
301  {
302  reply.addVocab(VOCAB_OK);
303  reply.addInt32(nav_status);
304  }
305  else
306  {
307  yError() << "getNavigationStatus() failed";
308  reply.addVocab(VOCAB_ERR);
309  }
310  }
311  else if (request == VOCAB_NAV_STOP)
312  {
313  bool ret = iNav_ctrl->stopNavigation();
314  if (ret)
315  {
316  reply.addVocab(VOCAB_OK);
317  }
318  else
319  {
320  yError() << "stopNavigation() failed";
321  reply.addVocab(VOCAB_ERR);
322  }
323  }
324  else if (request == VOCAB_NAV_SUSPEND)
325  {
326  double time = -1;
327  if (command.size() > 1)
328  {
329  time = command.get(1).asFloat64();
330  bool ret = iNav_ctrl->suspendNavigation(time);
331  if (ret)
332  {
333  reply.addVocab(VOCAB_OK);
334  }
335  else
336  {
337  yError() << "suspendNavigation() failed";
338  reply.addVocab(VOCAB_ERR);
339  }
340  }
341  else
342  {
343  bool ret = iNav_ctrl->suspendNavigation();
344  if (ret)
345  {
346  reply.addVocab(VOCAB_OK);
347  }
348  else
349  {
350  yError() << "suspendNavigation() failed";
351  reply.addVocab(VOCAB_ERR);
352  }
353  }
354  }
355  else if (request == VOCAB_NAV_RESUME)
356  {
357  bool ret = iNav_ctrl->resumeNavigation();
358  if (ret)
359  {
360  reply.addVocab(VOCAB_OK);
361  }
362  else
363  {
364  yError() << "resumeNavigation failed()";
365  reply.addVocab(VOCAB_ERR);
366  }
367  }
368  else if (request == VOCAB_NAV_GET_NAVIGATION_WAYPOINTS)
369  {
370  Map2DPath locs;
372  if (ret)
373  {
374  reply.addVocab(VOCAB_OK);
375  Bottle& waypoints = reply.addList();
376  for (auto it = locs.begin(); it!=locs.end(); it++)
377  {
378  Bottle& the_waypoint = waypoints.addList();
379  the_waypoint.addString(it->map_id);
380  the_waypoint.addFloat64(it->x);
381  the_waypoint.addFloat64(it->y);
382  the_waypoint.addFloat64(it->theta);
383  }
384  }
385  else
386  {
387  yError() << "getAllNavigationWaypoints() failed";
388  reply.addVocab(VOCAB_ERR);
389  }
390  }
391  else if (request == VOCAB_NAV_GET_CURRENT_WAYPOINT)
392  {
393  Map2DLocation loc;
395  if (ret)
396  {
397  reply.addVocab(VOCAB_OK);
398  reply.addString(loc.map_id);
399  reply.addFloat64(loc.x);
400  reply.addFloat64(loc.y);
401  reply.addFloat64(loc.theta);
402  }
403  else
404  {
405  yError() << "getCurrentNavigationWaypoint() failed";
406  reply.addVocab(VOCAB_ERR);
407  }
408  }
409  else if (request == VOCAB_NAV_GET_NAV_MAP)
410  {
411  MapGrid2D map;
413  {
414  reply.addVocab(VOCAB_OK);
415  yarp::os::Bottle& mapbot = reply.addList();
416  Property::copyPortable(map, mapbot);
417  }
418  else
419  {
420  yError() << "getCurrentNavigationMap() failed";
421  reply.addVocab(VOCAB_ERR);
422  }
423  }
424  else if (request == VOCAB_NAV_GET_ABS_TARGET)
425  {
426  Map2DLocation loc;
427  bool ret;
429  if (ret)
430  {
431  reply.addVocab(VOCAB_OK);
432  reply.addString(loc.map_id);
433  reply.addFloat64(loc.x);
434  reply.addFloat64(loc.y);
435  reply.addFloat64(loc.theta);
436  }
437  else
438  {
439  yError() << "getAbsoluteLocationOfCurrentTarget() failed";
440  reply.addVocab(VOCAB_ERR);
441  }
442  }
443  else if (request == VOCAB_NAV_GET_REL_TARGET)
444  {
445  Map2DLocation loc;
446  bool ret;
448  if (ret)
449  {
450  reply.addVocab(VOCAB_OK);
451  reply.addFloat64(loc.x);
452  reply.addFloat64(loc.y);
453  reply.addFloat64(loc.theta);
454  }
455  else
456  {
457  yError() << "getRelativeLocationOfCurrentTarget() failed";
458  reply.addVocab(VOCAB_ERR);
459  }
460  }
461  else
462  {
463  yError() << "Invalid vocab received";
464  reply.addVocab(VOCAB_ERR);
465  }
466 
467  return true;
468 }
469 
471 {
472  yarp::os::Bottle command;
473  yarp::os::Bottle reply;
474  bool ok = command.read(connection);
475  if (!ok) return false;
476  reply.clear();
477 
478  //^^^^^^^^^^^^^^^^^ STRING SECTION
479  if (command.get(0).isString())
480  {
481  parse_respond_string(command, reply);
482  }
483  //^^^^^^^^^^^^^^^^^ VOCAB SECTION
484  else if (command.get(0).isVocab())
485  {
486  parse_respond_vocab(command, reply);
487  }
488  else
489  {
490  yError() << "Invalid command type";
491  reply.addVocab(VOCAB_ERR);
492  }
493 
494  yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
495  if (returnToSender != nullptr)
496  {
497  reply.write(*returnToSender);
498  }
499 
500  return true;
501 }
502 
504 {
506 
507  double m_stats_time_curr = yarp::os::Time::now();
508  if (m_stats_time_curr - m_stats_time_last > 5.0)
509  {
510  if (!ok)
511  {
512  yError("navigation2DServer, unable to get Navigation Status!\n");
513  }
514  else
515  {
516  yInfo() << "navigation2DServer running, ALL ok. Navigation status:" << getStatusAsString(m_navigation_status);
517  }
519  }
520 }
521 
522 std::string navigation2DServer::getStatusAsString(NavigationStatusEnum status)
523 {
524  if (status == navigation_status_idle) return std::string("navigation_status_idle");
525  else if (status == navigation_status_moving) return std::string("navigation_status_moving");
526  else if (status == navigation_status_waiting_obstacle) return std::string("navigation_status_waiting_obstacle");
527  else if (status == navigation_status_goal_reached) return std::string("navigation_status_goal_reached");
528  else if (status == navigation_status_aborted) return std::string("navigation_status_aborted");
529  else if (status == navigation_status_failing) return std::string("navigation_status_failing");
530  else if (status == navigation_status_paused) return std::string("navigation_status_paused");
531  else if (status == navigation_status_preparing_before_move) return std::string("navigation_status_preparing_before_move");
532  else if (status == navigation_status_thinking) return std::string("navigation_status_thinking");
533  else if (status == navigation_status_error) return std::string("navigation_status_error");
534  return std::string("navigation_status_error");
535 }
LogStream.h
navigation2DServer::detachAll
virtual bool detachAll() override
Detach the object (you must have first called attach).
Definition: navigation2DServer.cpp:76
navigation2DServer::pNav
yarp::dev::PolyDriver pNav
Definition: navigation2DServer.h:53
INavigation2D.h
yarp::os::Bottle
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:72
yarp::os::Value::asVocab
virtual std::int32_t asVocab() const
Get vocabulary identifier as an integer.
Definition: Value.cpp:231
MapGrid2D.h
navigation2DServer::m_rpcPort
yarp::os::Port m_rpcPort
Definition: navigation2DServer.h:46
yarp::os::Property::put
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:998
yarp::os::Bottle::clear
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:124
yarp::dev::Nav2D::navigation_status_thinking
@ navigation_status_thinking
Definition: INavigation2D.h:41
Network.h
navigation2DServer::open
virtual bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
Definition: navigation2DServer.cpp:87
yarp::os::Searchable
A base class for nested structures that can be searched.
Definition: Searchable.h:68
yarp::os::Bottle::size
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:254
yarp::dev::Nav2D::INavigation2DTargetActions::applyVelocityCommand
virtual bool applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout=0.1)=0
Apply a velocity command.
VOCAB_NAV_GET_ABS_TARGET
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_ABS_TARGET
Definition: ILocalization2D.h:119
yarp::dev::Map2DLocationData::y
double y
Definition: Map2DLocationData.h:32
VOCAB_NAV_SUSPEND
constexpr yarp::conf::vocab32_t VOCAB_NAV_SUSPEND
Definition: INavigation2D.h:301
Port.h
yarp::os::Searchable::toString
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
yarp::dev::Nav2D::INavigation2DControlActions::getAllNavigationWaypoints
virtual bool getAllNavigationWaypoints(yarp::dev::Nav2D::Map2DPath &waypoints)=0
Returns the list of waypoints generated by the navigation algorithm.
yarp::os::Property::fromString
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1046
yarp::dev::PolyDriverList::size
int size() const
Definition: PolyDriverList.cpp:39
yarp::dev::PolyDriver::isValid
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:194
yarp::dev::Nav2D::INavigation2DControlActions::resumeNavigation
virtual bool resumeNavigation()=0
Resume a previously suspended navigation task.
navigation2DServer::m_navigation_status
yarp::dev::Nav2D::NavigationStatusEnum m_navigation_status
Definition: navigation2DServer.h:50
VOCAB_NAV_GET_CURRENT_WAYPOINT
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_CURRENT_WAYPOINT
Definition: INavigation2D.h:304
yarp::dev::DeviceDriver::view
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:77
yarp::dev::Nav2D::NavigationStatusEnum
NavigationStatusEnum
Definition: INavigation2D.h:31
yarp::os::Port::open
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: Port.cpp:81
ret
bool ret
Definition: ImplementAxisInfo.cpp:72
yarp::dev::PolyDriverList
Definition: PolyDriverList.h:21
ControlBoardInterfaces.h
navigation2DServer::m_stats_time_last
double m_stats_time_last
Definition: navigation2DServer.h:58
yarp::os::Bottle::addFloat64
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition: Bottle.cpp:161
yarp::dev::PolyDriver::open
bool open(const std::string &txt)
Construct and configure a device by its common name.
Definition: PolyDriver.cpp:138
yError
#define yError(...)
Definition: Log.h:242
yarp::dev::Map2DLocationData::theta
double theta
Definition: Map2DLocationData.h:33
navigation2DServer::read
virtual bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: navigation2DServer.cpp:470
yarp::dev::Nav2D::Map2DPath::begin
iterator begin() noexcept
Returns an iterator to the begin of the Path.
Definition: Map2DPath.cpp:106
VOCAB_NAV_VELOCITY_CMD
constexpr yarp::conf::vocab32_t VOCAB_NAV_VELOCITY_CMD
Definition: ILocalization2D.h:114
yarp::os::Time::now
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:124
yarp::dev::Nav2D::INavigation2DTargetActions::getAbsoluteLocationOfCurrentTarget
virtual bool getAbsoluteLocationOfCurrentTarget(yarp::dev::Nav2D::Map2DLocation &loc)=0
Gets the last navigation target in the world reference frame.
yarp::dev
An interface for the device drivers.
Definition: audioBufferSizeData.cpp:17
navigation2DServer::run
virtual void run() override
Loop function.
Definition: navigation2DServer.cpp:503
yarp::dev::PolyDriverList::push
void push(PolyDriver *p, const char *k)
Definition: PolyDriverList.cpp:44
navigation2DServer::m_rpcPortName
std::string m_rpcPortName
Definition: navigation2DServer.h:47
VOCAB_NAV_GOTOABS
constexpr yarp::conf::vocab32_t VOCAB_NAV_GOTOABS
Definition: ILocalization2D.h:112
yarp::dev::Nav2D::INavigation2DControlActions::stopNavigation
virtual bool stopNavigation()=0
Terminates the current navigation task.
yarp::os::Value::isString
virtual bool isString() const
Checks if value is a string.
Definition: Value.cpp:159
yarp::dev::Nav2D::navigation_status_paused
@ navigation_status_paused
Definition: INavigation2D.h:40
VOCAB_NAV_GET_NAVIGATION_WAYPOINTS
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_NAVIGATION_WAYPOINTS
Definition: INavigation2D.h:303
VOCAB_NAV_GET_NAV_MAP
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_NAV_MAP
Definition: INavigation2D.h:305
yarp::dev::Nav2D::INavigation2DTargetActions::gotoTargetByRelativeLocation
virtual bool gotoTargetByRelativeLocation(double x, double y)=0
Ask the robot to reach a position defined in the robot reference frame.
yarp::os::Bottle::get
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:249
VOCAB_NAV_GET_NAVIGATION_STATUS
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_NAVIGATION_STATUS
Definition: ILocalization2D.h:128
yarp::os::Bottle::addList
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:185
yarp::os::Bottle::write
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition: Bottle.cpp:233
yarp::dev::Nav2D::MapGrid2D
Definition: MapGrid2D.h:30
yarp::os::ConnectionWriter
An interface for writing to a network connection.
Definition: ConnectionWriter.h:39
yarp::dev::Nav2D::INavigation2DControlActions::getNavigationStatus
virtual bool getNavigationStatus(NavigationStatusEnum &status)=0
Gets the current status of the navigation task.
yarp::dev::Nav2D::INavigation2DControlActions::suspendNavigation
virtual bool suspendNavigation(const double time_s=std::numeric_limits< double >::infinity())=0
Ask to the robot to suspend the current navigation task for a defined amount of time.
yarp::os::Value::asString
virtual std::string asString() const
Get string value.
Definition: Value.cpp:237
VOCAB_NAV_GET_REL_TARGET
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_REL_TARGET
Definition: ILocalization2D.h:120
yarp::dev::PolyDriver
Definition: PolyDriver.h:25
yarp::os::Searchable::check
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
navigation2DServer::m_period
double m_period
Definition: navigation2DServer.h:57
yarp::os::Searchable::find
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
yarp::os::Bottle::addInt32
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:143
yarp::os::Vocab::encode
NetInt32 encode(const std::string &str)
Convert a string into a vocabulary identifier.
Definition: Vocab.cpp:14
yarp::dev::Nav2D::navigation_status_moving
@ navigation_status_moving
Definition: INavigation2D.h:35
navigation2DServer::initialize_YARP
bool initialize_YARP(yarp::os::Searchable &config)
Definition: navigation2DServer.cpp:146
yarp::dev::Nav2D::navigation_status_failing
@ navigation_status_failing
Definition: INavigation2D.h:39
RFModule.h
yarp::dev::Nav2D::navigation_status_goal_reached
@ navigation_status_goal_reached
Definition: INavigation2D.h:37
yarp::os::ConnectionReader::getWriter
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
yarp::dev::Nav2D::navigation_status_idle
@ navigation_status_idle
Definition: INavigation2D.h:33
yarp::dev::Nav2D::Map2DPath
Definition: Map2DPath.h:26
yarp::dev::Nav2D
Definition: ILocalization2D.h:21
yarp::os::Bottle::addString
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:173
yarp::os::Port::setReader
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition: Port.cpp:504
yarp::os::PeriodicThread
Definition: PeriodicThread.h:24
yarp::os::Bottle::addVocab
void addVocab(int x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:167
VOCAB_ERR
constexpr yarp::conf::vocab32_t VOCAB_ERR
Definition: GenericVocabs.h:20
yarp::dev::Nav2D::navigation_status_aborted
@ navigation_status_aborted
Definition: INavigation2D.h:38
yarp::dev::Nav2D::Map2DLocation
Definition: Map2DLocation.h:29
yarp::dev::Nav2D::INavigation2DTargetActions::gotoTargetByAbsoluteLocation
virtual bool gotoTargetByAbsoluteLocation(yarp::dev::Nav2D::Map2DLocation loc)=0
Ask the robot to reach a position defined in the world reference frame.
yarp::os::ConnectionReader
An interface for reading from a network connection.
Definition: ConnectionReader.h:39
navigation2DServer::iNav_ctrl
yarp::dev::Nav2D::INavigation2DControlActions * iNav_ctrl
Definition: navigation2DServer.h:54
yarp::dev::Nav2D::navigation_status_waiting_obstacle
@ navigation_status_waiting_obstacle
Definition: INavigation2D.h:36
VOCAB_NAV_RESUME
constexpr yarp::conf::vocab32_t VOCAB_NAV_RESUME
Definition: INavigation2D.h:302
yarp::os::Value::asInt32
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:207
yarp::dev::Nav2D::NavigationMapTypeEnum
NavigationMapTypeEnum
Definition: INavigation2D.h:45
yarp::os
Definition: AbstractCarrier.h:17
VOCAB_NAV_STOP
constexpr yarp::conf::vocab32_t VOCAB_NAV_STOP
Definition: INavigation2D.h:300
DEFAULT_THREAD_PERIOD
#define DEFAULT_THREAD_PERIOD
Definition: AnalogWrapper.h:46
yTrace
#define yTrace(...)
Definition: Log.h:203
yarp::dev::Nav2D::INavigation2DTargetActions::getRelativeLocationOfCurrentTarget
virtual bool getRelativeLocationOfCurrentTarget(double &x, double &y, double &theta)=0
Gets the last navigation target in the robot reference frame.
yarp::dev::Nav2D::Map2DPath::end
iterator end() noexcept
Returns an iterator to the end of the Path.
Definition: Map2DPath.cpp:112
navigation2DServer::iNav_target
yarp::dev::Nav2D::INavigation2DTargetActions * iNav_target
Definition: navigation2DServer.h:55
navigation2DServer::m_streamingPortName
std::string m_streamingPortName
Definition: navigation2DServer.h:48
yInfo
#define yInfo(...)
Definition: Log.h:230
navigation2DServer::navigation2DServer
navigation2DServer()
Default module constructor.
Definition: navigation2DServer.cpp:42
VOCAB_NAV_RECOMPUTE_PATH
constexpr yarp::conf::vocab32_t VOCAB_NAV_RECOMPUTE_PATH
Definition: ILocalization2D.h:115
yarp::os::Bottle::read
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition: Bottle.cpp:243
yarp::dev::Nav2D::INavigation2DControlActions::getCurrentNavigationWaypoint
virtual bool getCurrentNavigationWaypoint(yarp::dev::Nav2D::Map2DLocation &curr_waypoint)=0
Returns the current waypoint pursued by the navigation algorithm.
yarp::dev::Nav2D::navigation_status_error
@ navigation_status_error
Definition: INavigation2D.h:42
yDebug
#define yDebug(...)
Definition: Log.h:217
Time.h
yarp::dev::Map2DLocationData::x
double x
Definition: Map2DLocationData.h:31
VOCAB_NAV_GOTOREL
constexpr yarp::conf::vocab32_t VOCAB_NAV_GOTOREL
Definition: ILocalization2D.h:113
yarp::dev::Nav2D::INavigation2DControlActions::recomputeCurrentNavigationPath
virtual bool recomputeCurrentNavigationPath()=0
Forces the navigation system to recompute the path from the current robot position to the current goa...
yarp::dev::Nav2D::INavigation2DControlActions::getCurrentNavigationMap
virtual bool getCurrentNavigationMap(yarp::dev::Nav2D::NavigationMapTypeEnum map_type, yarp::dev::Nav2D::MapGrid2D &map)=0
Returns the current navigation map processed by the navigation algorithm.
navigation2DServer::close
virtual bool close() override
Close the DeviceDriver.
Definition: navigation2DServer.cpp:157
yarp::dev::Map2DLocationData::map_id
std::string map_id
Definition: Map2DLocationData.h:30
yarp::os::Value::asFloat64
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:225
VOCAB_OK
constexpr yarp::conf::vocab32_t VOCAB_OK
Definition: GenericVocabs.h:18
yarp::dev::Nav2D::navigation_status_preparing_before_move
@ navigation_status_preparing_before_move
Definition: INavigation2D.h:34
VOCAB_INAVIGATION
constexpr yarp::conf::vocab32_t VOCAB_INAVIGATION
Definition: ILocalization2D.h:110
navigation2DServer::attachAll
virtual bool attachAll(const yarp::dev::PolyDriverList &l) override
Attach to a list of objects.
Definition: navigation2DServer.cpp:49
yarp::os::Value::isVocab
virtual bool isVocab() const
Checks if value is a vocabulary identifier.
Definition: Value.cpp:177
yarp::os::Property
A class for storing options and configuration information.
Definition: Property.h:34
navigation2DServer.h