YARP
Yet Another Robot Platform
Localization2DServer.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 #include <yarp/os/Network.h>
7 #include <yarp/os/RFModule.h>
8 #include <yarp/os/Time.h>
9 #include <yarp/os/Port.h>
10 #include <yarp/os/LogComponent.h>
11 #include <yarp/os/LogStream.h>
12 #include <yarp/dev/PolyDriver.h>
13 #include <yarp/os/Bottle.h>
14 #include <yarp/sig/Vector.h>
15 #include <yarp/dev/IMap2D.h>
18 #include <yarp/math/Math.h>
19 #include "Localization2DServer.h"
20 
21 #include <cmath>
22 
25 using namespace yarp::os;
26 using namespace yarp::dev;
27 using namespace yarp::dev::Nav2D;
28 
29 #define DEFAULT_THREAD_PERIOD 0.01
30 
31 #ifndef M_PI
32 #define M_PI 3.14159265358979323846
33 #endif
34 
35 namespace {
36 YARP_LOG_COMPONENT(LOCALIZATION2DSERVER, "yarp.device.localization2DServer")
37 }
38 
39 //------------------------------------------------------------------------------------------------------------------------------
40 
42 {
43  m_ros_node = nullptr;
47  iLoc = nullptr;
51 }
52 
54 {
55  if (device2attach.size() != 1)
56  {
57  yCError(LOCALIZATION2DSERVER, "Cannot attach more than one device");
58  return false;
59  }
60 
61  yarp::dev::PolyDriver * Idevice2attach = device2attach[0]->poly;
62 
63  if (Idevice2attach->isValid())
64  {
65  Idevice2attach->view(iLoc);
66  }
67 
68  if (nullptr == iLoc)
69  {
70  yCError(LOCALIZATION2DSERVER, "Subdevice passed to attach method is invalid");
71  return false;
72  }
73 
74  //initialize m_current_position and m_current_status, if available
75  bool ret = true;
77  Map2DLocation loc;
78  ret &= iLoc->getLocalizationStatus(status);
79  ret &= iLoc->getCurrentPosition(loc);
80  if (ret)
81  {
82  m_current_status = status;
83  m_current_position = loc;
84  }
85  else
86  {
87  yCWarning(LOCALIZATION2DSERVER) << "Localization data not yet available during server initialization";
88  }
89 
90  PeriodicThread::setPeriod(m_period);
91  return PeriodicThread::start();
92 }
93 
95 {
96  if (PeriodicThread::isRunning())
97  {
98  PeriodicThread::stop();
99  }
100  iLoc = nullptr;
101  return true;
102 }
103 
105 {
106  yCWarning(LOCALIZATION2DSERVER) << "The 'localization2DServer' device is deprecated in favour of 'localization2D_nws_yarp'.";
107  yCWarning(LOCALIZATION2DSERVER) << "The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.";
108  yCWarning(LOCALIZATION2DSERVER) << "Please update your scripts.";
109 
110  Property params;
111  params.fromString(config.toString().c_str());
112  yCDebug(LOCALIZATION2DSERVER) << "Configuration: \n" << config.toString().c_str();
113 
114  if (config.check("GENERAL") == false)
115  {
116  yCWarning(LOCALIZATION2DSERVER) << "Missing GENERAL group, assuming default options";
117  }
118 
119  Bottle& general_group = config.findGroup("GENERAL");
120  if (!general_group.check("period"))
121  {
122  yCInfo(LOCALIZATION2DSERVER) << "Missing 'period' parameter. Using default value: " << DEFAULT_THREAD_PERIOD;
124  }
125  else
126  {
127  m_period = general_group.find("period").asFloat64();
128  yCInfo(LOCALIZATION2DSERVER) << "Period requested: " << m_period;
129  }
130 
131  if (!general_group.check("retrieve_position_periodically"))
132  {
133  yCInfo(LOCALIZATION2DSERVER) << "Missing 'retrieve_position_periodically' parameter. Using default value: true. Period:" << m_period ;
135  }
136  else
137  {
138  m_getdata_using_periodic_thread = general_group.find("retrieve_position_periodically").asBool();
140  { yCInfo(LOCALIZATION2DSERVER) << "retrieve_position_periodically requested, Period:" << m_period; }
141  else
142  { yCInfo(LOCALIZATION2DSERVER) << "retrieve_position_periodically NOT requested. Localization data obtained asynchronously."; }
143  }
144 
145 
146  m_local_name = "/localizationServer";
147  if (!general_group.check("name"))
148  {
149  yCInfo(LOCALIZATION2DSERVER) << "Missing 'name' parameter. Using default value: /localizationServer";
150  }
151  else
152  {
153  m_local_name = general_group.find("name").asString();
154  if (m_local_name.c_str()[0] != '/') { yCError(LOCALIZATION2DSERVER) << "Missing '/' in name parameter" ; return false; }
155  yCInfo(LOCALIZATION2DSERVER) << "Using local name:" << m_local_name;
156  }
157 
158  m_rpcPortName = m_local_name + "/rpc";
159  m_2DLocationPortName = m_local_name + "/streaming:o";
160  m_odometryPortName = m_local_name + "/odometry:o";
161 
162  if (config.check("subdevice"))
163  {
164  Property p;
165  PolyDriverList driverlist;
166  p.fromString(config.toString(), false);
167  p.put("device", config.find("subdevice").asString());
168 
169  if (!pLoc.open(p) || !pLoc.isValid())
170  {
171  yCError(LOCALIZATION2DSERVER) << "Failed to open subdevice.. check params";
172  return false;
173  }
174 
175  driverlist.push(&pLoc, "1");
176  if (!attachAll(driverlist))
177  {
178  yCError(LOCALIZATION2DSERVER) << "Failed to open subdevice.. check params";
179  return false;
180  }
181  }
182  else
183  {
184  yCInfo(LOCALIZATION2DSERVER) << "Waiting for device to attach";
185  }
187 
188  if (!initialize_YARP(config))
189  {
190  yCError(LOCALIZATION2DSERVER) << "Error initializing YARP ports";
191  return false;
192  }
193 
194  if (!initialize_ROS(config))
195  {
196  yCError(LOCALIZATION2DSERVER) << "Error initializing ROS system";
197  return false;
198  }
199 
200  return true;
201 }
202 
204 {
205  if (params.check("ROS"))
206  {
207  Bottle& ros_group = params.findGroup("ROS");
208  if (ros_group.check("publish_tf"))
209  {
211  }
212  if (ros_group.check("publish_odom"))
213  {
215  }
216 
217  if (!ros_group.check("parent_frame_id"))
218  {
219  yCError(LOCALIZATION2DSERVER) << "Missing 'parent_frame_id' parameter";
220  //return false;
221  }
222  else
223  {
224  m_parent_frame_id = ros_group.find("parent_frame_id").asString();
225  }
226  if (!ros_group.check("child_frame_id"))
227  {
228  yCError(LOCALIZATION2DSERVER) << "Missing 'child_frame_id' parameter";
229  //return false;
230  }
231  else
232  {
233  m_child_frame_id = ros_group.find("child_frame_id").asString();
234  }
235 
236  }
237  else
238  {
239  yCInfo(LOCALIZATION2DSERVER) << "ROS initialization not requested";
240  return true;
241  }
242 
243  if (m_ros_node == nullptr)
244  {
245  bool b= false;
246  m_ros_node = new yarp::os::Node(m_local_name+"_ROSnode");
247  if (m_ros_node == nullptr)
248  {
249  yCError(LOCALIZATION2DSERVER) << "Opening " << m_local_name << " Node, check your yarp-ROS network configuration";
250  }
251 
252  std::string ros_odom_topic = m_local_name + std::string("/odom");
253  b = m_odometry_publisher.topic(ros_odom_topic);
254  if (!b)
255  {
256  yCError(LOCALIZATION2DSERVER) << "Unable to publish data on" << ros_odom_topic << "topic";
257  }
258  b = m_tf_publisher.topic("/tf");
259  if (!b)
260  {
261  yCError(LOCALIZATION2DSERVER) << "Unable to publish data on /tf topic";
262  }
263  yCInfo(LOCALIZATION2DSERVER) << "ROS initialized";
264  }
265  return true;
266 }
267 
269 {
271  {
272  yCError(LOCALIZATION2DSERVER, "Failed to open port %s", m_2DLocationPortName.c_str());
273  return false;
274  }
275 
276  if (!m_odometryPort.open(m_odometryPortName.c_str()))
277  {
278  yCError(LOCALIZATION2DSERVER, "Failed to open port %s", m_odometryPortName.c_str());
279  return false;
280  }
281 
282  if (!m_rpcPort.open(m_rpcPortName.c_str()))
283  {
284  yCError(LOCALIZATION2DSERVER, "Failed to open port %s", m_rpcPortName.c_str());
285  return false;
286  }
287  m_rpcPort.setReader(*this);
288 
289  return true;
290 }
291 
293 {
294  yCTrace(LOCALIZATION2DSERVER, "Close");
295  if (PeriodicThread::isRunning())
296  {
297  PeriodicThread::stop();
298  }
299 
300  detachAll();
301 
307  m_rpcPort.close();
308 
309  if (m_ros_node)
310  {
313  delete m_ros_node;
314  m_ros_node = nullptr;
315  }
316 
317  yCDebug(LOCALIZATION2DSERVER) << "Execution terminated";
318  return true;
319 }
320 
322 {
323  yarp::os::Bottle command;
324  yarp::os::Bottle reply;
325  bool ok = command.read(connection);
326  if (!ok) {
327  return false;
328  }
329 
330  reply.clear();
331 
332  if (command.get(0).isVocab32())
333  {
334  if (command.get(0).asVocab32() == VOCAB_INAVIGATION && command.get(1).isVocab32())
335  {
336  int request = command.get(1).asVocab32();
337  if (request == VOCAB_NAV_GET_CURRENT_POS)
338  {
339  bool b = true;
341  {
342  //m_current_position is obtained by run()
343  }
344  else
345  {
346  //m_current_position is obtained by getCurrentPosition()
348  }
349  if (b)
350  {
351  reply.addVocab32(VOCAB_OK);
356  }
357  else
358  {
359  reply.addVocab32(VOCAB_ERR);
360  }
361  }
362  else if (request == VOCAB_NAV_GET_ESTIMATED_ODOM)
363  {
364  bool b = true;
366  {
367  //m_current_position is obtained by run()
368  }
369  else
370  {
371  //m_current_position is obtained by getCurrentPosition()
373  }
374  if (b)
375  {
376  reply.addVocab32(VOCAB_OK);
386  }
387  else
388  {
389  reply.addVocab32(VOCAB_ERR);
390  }
391  }
392  else if (request == VOCAB_NAV_SET_INITIAL_POS)
393  {
394  Map2DLocation init_loc;
395  init_loc.map_id = command.get(2).asString();
396  init_loc.x = command.get(3).asFloat64();
397  init_loc.y = command.get(4).asFloat64();
398  init_loc.theta = command.get(5).asFloat64();
399  iLoc->setInitialPose(init_loc);
400  reply.addVocab32(VOCAB_OK);
401  }
402  else if (request == VOCAB_NAV_GET_CURRENT_POSCOV)
403  {
404  Map2DLocation init_loc;
405  yarp::sig::Matrix cov(3, 3);
406  iLoc->getCurrentPosition(init_loc, cov);
407  reply.addVocab32(VOCAB_OK);
412  yarp::os::Bottle& mc = reply.addList();
413  for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { mc.addFloat64(cov[i][j]); } }
414  }
415  else if (request == VOCAB_NAV_SET_INITIAL_POSCOV)
416  {
417  Map2DLocation init_loc;
418  yarp::sig::Matrix cov(3,3);
419  init_loc.map_id = command.get(2).asString();
420  init_loc.x = command.get(3).asFloat64();
421  init_loc.y = command.get(4).asFloat64();
422  init_loc.theta = command.get(5).asFloat64();
423  Bottle* mc = command.get(6).asList();
424  if (mc!=nullptr && mc->size() == 9)
425  {
426  for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { cov[i][j] = mc->get(i * 3 + j).asFloat64(); } }
427  bool ret = iLoc->setInitialPose(init_loc, cov);
428  if (ret) { reply.addVocab32(VOCAB_OK); }
429  else { reply.addVocab32(VOCAB_ERR); }
430  }
431  else
432  {
433  reply.addVocab32(VOCAB_ERR);
434  }
435  }
436  else if (request == VOCAB_NAV_LOCALIZATION_START)
437  {
439  reply.addVocab32(VOCAB_OK);
440  }
441  else if (request == VOCAB_NAV_LOCALIZATION_STOP)
442  {
444  reply.addVocab32(VOCAB_OK);
445  }
446  else if (request == VOCAB_NAV_GET_LOCALIZER_STATUS)
447  {
449  {
450  //m_current_status is obtained by run()
451  reply.addVocab32(VOCAB_OK);
453  }
454  else
455  {
456  //m_current_status is obtained by getLocalizationStatus()
458  reply.addVocab32(VOCAB_OK);
460  }
461  }
462  else if (request == VOCAB_NAV_GET_LOCALIZER_POSES)
463  {
464  std::vector<Map2DLocation> poses;
465  iLoc->getEstimatedPoses(poses);
466  reply.addVocab32(VOCAB_OK);
467  reply.addInt32(poses.size());
468  for (size_t i=0; i<poses.size(); i++)
469  {
470  Bottle& b = reply.addList();
471  b.addString(poses[i].map_id);
472  b.addFloat64(poses[i].x);
473  b.addFloat64(poses[i].y);
474  b.addFloat64(poses[i].theta);
475  }
476  }
477  else
478  {
479  reply.addVocab32(VOCAB_ERR);
480  }
481  }
482  else
483  {
484  yCError(LOCALIZATION2DSERVER) << "Invalid vocab received";
485  reply.addVocab32(VOCAB_ERR);
486  }
487  }
488  else if (command.get(0).isString() && command.get(0).asString() == "help")
489  {
490  reply.addVocab32("many");
491  reply.addString("Available commands are:");
492  reply.addString("getLoc");
493  reply.addString("initLoc <map_name> <x> <y> <angle in degrees>");
494  }
495  else if (command.get(0).isString() && command.get(0).asString() == "getLoc")
496  {
497  Map2DLocation curr_loc;
498  iLoc->getCurrentPosition(curr_loc);
499  std::string s = std::string("Current Location is: ") + curr_loc.toString();
500  reply.addString(s);
501  }
502  else if (command.get(0).isString() && command.get(0).asString() == "initLoc")
503  {
504  Map2DLocation init_loc;
505  init_loc.map_id = command.get(1).asString();
506  init_loc.x = command.get(2).asFloat64();
507  init_loc.y = command.get(3).asFloat64();
508  init_loc.theta = command.get(4).asFloat64();
509  iLoc->setInitialPose(init_loc);
510  std::string s = std::string("Localization initialized to: ") + init_loc.toString();
511  reply.addString(s);
512  }
513  else
514  {
515  yCError(LOCALIZATION2DSERVER) << "Invalid command type";
516  reply.addVocab32(VOCAB_ERR);
517  }
518 
519  yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
520  if (returnToSender != nullptr)
521  {
522  reply.write(*returnToSender);
523  }
524 
525  return true;
526 }
527 
529 {
530  double m_stats_time_curr = yarp::os::Time::now();
531  if (m_stats_time_curr - m_stats_time_last > 5.0)
532  {
533  yCInfo(LOCALIZATION2DSERVER) << "Running";
535  }
536 
538  {
540  if (ret == false)
541  {
542  yCError(LOCALIZATION2DSERVER) << "getLocalizationStatus() failed";
543  }
544 
546  {
547  //update the stamp
548 
549 
551  if (ret2 == false)
552  {
553  yCError(LOCALIZATION2DSERVER) << "getCurrentPosition() failed";
554  }
555  else
556  {
558  }
560  if (ret3 == false)
561  {
562  //yCError(LOCALIZATION2DSERVER) << "getEstimatedOdometry() failed";
563  }
564  else
565  {
567  }
568  }
569  else
570  {
571  yCWarning(LOCALIZATION2DSERVER, "The system is not properly localized!");
572  }
573  }
574 
575  if (1) {
576  publish_odometry_on_yarp_port();
577  }
578  if (1) {
579  publish_2DLocation_on_yarp_port();
580  }
582  publish_odometry_on_ROS_topic();
583  }
585  publish_odometry_on_TF_topic();
586  }
587 }
588 
589 void Localization2DServer::publish_odometry_on_yarp_port()
590 {
591  if (m_odometryPort.getOutputCount() > 0)
592  {
594  odom = m_current_odometry;
595 
596  //send data to port
599  }
600 }
601 
602 void Localization2DServer::publish_2DLocation_on_yarp_port()
603 {
605  {
608  {
609  loc = m_current_position;
610  }
611  else
612  {
613  Map2DLocation temp_loc;
614  temp_loc.x = std::nan("");
615  temp_loc.y = std::nan("");
616  temp_loc.theta = std::nan("");
617  loc = temp_loc;
618  }
619 
620  //send data to port
623  }
624 }
625 
626 void Localization2DServer::publish_odometry_on_TF_topic()
627 {
630  transform.child_frame_id = m_child_frame_id;
631  transform.header.frame_id = m_parent_frame_id;
632  transform.header.seq = m_odom_stamp.getCount();
633  transform.header.stamp = m_odom_stamp.getTime();
634  double halfYaw = m_current_odometry.odom_theta / 180.0 * M_PI * 0.5;
635  double cosYaw = cos(halfYaw);
636  double sinYaw = sin(halfYaw);
637  transform.transform.rotation.x = 0;
638  transform.transform.rotation.y = 0;
639  transform.transform.rotation.z = sinYaw;
640  transform.transform.rotation.w = cosYaw;
643  transform.transform.translation.z = 0;
644  if (rosData.transforms.size() == 0)
645  {
646  rosData.transforms.push_back(transform);
647  }
648  else
649  {
650  rosData.transforms[0] = transform;
651  }
652 
654 }
655 
656 void Localization2DServer::publish_odometry_on_ROS_topic()
657 {
659  {
661  odom.clear();
663  odom.header.seq = m_odom_stamp.getCount();
664  odom.header.stamp = m_odom_stamp.getTime();
666 
669  odom.pose.pose.position.z = 0;
670  yarp::sig::Vector vecrpy(3);
671  vecrpy[0] = 0;
672  vecrpy[1] = 0;
673  vecrpy[2] = m_current_odometry.odom_theta;
674  yarp::sig::Matrix matrix = yarp::math::rpy2dcm(vecrpy);
676  odom.pose.pose.orientation.x = q.x();
677  odom.pose.pose.orientation.y = q.y();
678  odom.pose.pose.orientation.z = q.z();
679  odom.pose.pose.orientation.w = q.w();
680  //odom.pose.covariance = 0;
681 
684  odom.twist.twist.linear.z = 0;
685  odom.twist.twist.angular.x = 0;
686  odom.twist.twist.angular.y = 0;
688  //odom.twist.covariance = 0;
689 
691  }
692 }
define control board standard interfaces
constexpr yarp::conf::vocab32_t VOCAB_OK
Definition: GenericVocabs.h:15
constexpr yarp::conf::vocab32_t VOCAB_ERR
Definition: GenericVocabs.h:17
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_LOCALIZER_POSES
constexpr yarp::conf::vocab32_t VOCAB_INAVIGATION
constexpr yarp::conf::vocab32_t VOCAB_NAV_LOCALIZATION_STOP
constexpr yarp::conf::vocab32_t VOCAB_NAV_SET_INITIAL_POSCOV
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_CURRENT_POS
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_LOCALIZER_STATUS
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_ESTIMATED_ODOM
constexpr yarp::conf::vocab32_t VOCAB_NAV_SET_INITIAL_POS
constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_CURRENT_POSCOV
constexpr yarp::conf::vocab32_t VOCAB_NAV_LOCALIZATION_START
bool ret
#define DEFAULT_THREAD_PERIOD
#define M_PI
contains the definition of a Vector type
bool initialize_ROS(yarp::os::Searchable &config)
yarp::dev::Nav2D::Map2DLocation m_current_position
yarp::dev::Nav2D::ILocalization2D * iLoc
yarp::os::BufferedPort< yarp::dev::Nav2D::Map2DLocation > m_2DLocationPort
virtual bool attachAll(const yarp::dev::PolyDriverList &l) override
Attach to a list of objects.
yarp::dev::PolyDriver pLoc
virtual bool close() override
Close the DeviceDriver.
yarp::dev::Nav2D::LocalizationStatusEnum m_current_status
virtual bool detachAll() override
Detach the object (you must have first called attach).
yarp::os::BufferedPort< yarp::dev::OdometryData > m_odometryPort
yarp::os::Publisher< yarp::rosmsg::nav_msgs::Odometry > m_odometry_publisher
yarp::os::Node * m_ros_node
virtual bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
yarp::os::Stamp m_loc_stamp
virtual void run() override
Loop function.
yarp::os::Publisher< yarp::rosmsg::tf2_msgs::TFMessage > m_tf_publisher
yarp::os::Stamp m_odom_stamp
yarp::dev::OdometryData m_current_odometry
virtual bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool initialize_YARP(yarp::os::Searchable &config)
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
std::string map_id
name of the map
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 getEstimatedPoses(std::vector< yarp::dev::Nav2D::Map2DLocation > &poses)=0
Gets a set of pose estimates computed by the localization algorithm.
virtual bool startLocalizationService()=0
Starts the localization service.
virtual bool stopLocalizationService()=0
Stops the localization service.
virtual bool setInitialPose(const yarp::dev::Nav2D::Map2DLocation &loc)=0
Sets the initial pose for the localization algorithm which estimates the current position of the robo...
virtual bool getLocalizationStatus(LocalizationStatusEnum &status)=0
Gets the current status of the localization task.
double base_vel_x
velocity of the robot [m/s] expressed in the robot reference frame
Definition: OdometryData.h:41
double odom_vel_y
velocity of the robot [m/s] expressed in the world reference frame
Definition: OdometryData.h:57
double base_vel_theta
angular velocity of the robot [deg/s] expressed in the robot reference frame
Definition: OdometryData.h:49
double odom_vel_theta
angular velocity of the robot [deg/s] expressed in the world reference frame
Definition: OdometryData.h:61
double base_vel_y
velocity of the robot [m/s] expressed in the robot reference frame
Definition: OdometryData.h:45
double odom_x
position of the robot [m], expressed in the world reference frame
Definition: OdometryData.h:29
double odom_y
position of the robot [m], expressed in the world reference frame
Definition: OdometryData.h:33
double odom_theta
orientation the robot [deg], expressed in the world reference frame
Definition: OdometryData.h:37
double odom_vel_x
velocity of the robot [m/s] expressed in the world reference frame
Definition: OdometryData.h:53
void push(PolyDriver *p, const char *k)
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
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
Definition: Quaternion.cpp:162
double x() const
Definition: Quaternion.cpp:81
double z() const
Definition: Quaternion.cpp:91
double w() const
Definition: Quaternion.cpp:76
double y() const
Definition: Quaternion.cpp:86
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:164
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:182
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:251
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition: Bottle.cpp:240
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:158
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Bottle.cpp:277
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:121
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition: Bottle.cpp:230
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
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.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
The Node class.
Definition: Node.h:23
An abstraction for a periodic thread.
int getOutputCount() override
Determine how many output connections this port has.
Definition: Port.cpp:567
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
void close() override
Stop port activity.
Definition: Publisher.h:84
bool topic(const std::string &name)
Set topic to publish to.
Definition: Publisher.h:63
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::Publisher::write.
Definition: Publisher.h:123
void write(bool forceStrict=false)
Write the current object being returned by Publisher::prepare.
Definition: Publisher.h:148
Port & asPort() override
Get the concrete Port being used for communication.
Definition: Publisher.h:169
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
double getTime() const
Get the time stamp.
Definition: Stamp.cpp:34
int getCount() const
Get the sequence number.
Definition: Stamp.cpp:29
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 bool asBool() const
Get boolean value.
Definition: Value.cpp:186
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:240
virtual bool isVocab32() const
Checks if value is a vocabulary identifier.
Definition: Value.cpp:174
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
yarp::conf::float64_t y
Definition: Point.h:33
yarp::conf::float64_t x
Definition: Point.h:32
yarp::conf::float64_t z
Definition: Point.h:34
yarp::rosmsg::geometry_msgs::Point position
Definition: Pose.h:33
yarp::rosmsg::geometry_msgs::Quaternion orientation
Definition: Pose.h:34
yarp::rosmsg::geometry_msgs::Transform transform
yarp::rosmsg::geometry_msgs::Quaternion rotation
Definition: Transform.h:35
yarp::rosmsg::geometry_msgs::Vector3 translation
Definition: Transform.h:34
yarp::rosmsg::geometry_msgs::Vector3 angular
Definition: Twist.h:33
yarp::rosmsg::geometry_msgs::Vector3 linear
Definition: Twist.h:32
yarp::conf::float64_t y
Definition: Vector3.h:38
yarp::conf::float64_t z
Definition: Vector3.h:39
yarp::conf::float64_t x
Definition: Vector3.h:37
yarp::rosmsg::geometry_msgs::PoseWithCovariance pose
Definition: Odometry.h:40
yarp::rosmsg::std_msgs::Header header
Definition: Odometry.h:38
yarp::rosmsg::geometry_msgs::TwistWithCovariance twist
Definition: Odometry.h:41
yarp::rosmsg::TickTime stamp
Definition: Header.h:45
std::vector< yarp::rosmsg::geometry_msgs::TransformStamped > transforms
Definition: TFMessage.h:30
A class for a Matrix.
Definition: Matrix.h:39
#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
@ localization_status_not_yet_localized
For streams capable of holding different kinds of content, check what they actually have.
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition: math.cpp:847
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.
std::string toString() const
Returns text representation of the location.
Definition: Map2DLocation.h:72