YARP
Yet Another Robot Platform
Localization2D_nws_ros.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 using namespace std;
35 
36 #define DEFAULT_THREAD_PERIOD 0.01
37 
38 namespace {
39 YARP_LOG_COMPONENT(LOCALIZATION2D_NWS_ROS, "yarp.device.localization2D_nws_ros")
40 }
41 
42 //------------------------------------------------------------------------------------------------------------------------------
43 
45  m_period(DEFAULT_THREAD_PERIOD)
46 {
48 }
49 
51 {
52  if (driver->isValid())
53  {
54  driver->view(iLoc);
55  }
56 
57  if (nullptr == iLoc)
58  {
59  yCError(LOCALIZATION2D_NWS_ROS, "Subdevice passed to attach method is invalid");
60  return false;
61  }
62 
63  //initialize m_current_position and m_current_status, if available
64  bool ret = true;
66  Map2DLocation loc;
67  ret &= iLoc->getLocalizationStatus(status);
68  ret &= iLoc->getCurrentPosition(loc);
69  if (ret)
70  {
71  m_current_status = status;
72  m_current_position = loc;
73  }
74  else
75  {
76  yCWarning(LOCALIZATION2D_NWS_ROS) << "Localization data not yet available during server initialization";
77  }
78 
79  PeriodicThread::setPeriod(m_period);
80  return PeriodicThread::start();
81 }
82 
84 {
85  if (PeriodicThread::isRunning())
86  {
87  PeriodicThread::stop();
88  }
89  iLoc = nullptr;
90  return true;
91 }
92 
94 {
95  Property params;
96  params.fromString(config.toString().c_str());
97  yCDebug(LOCALIZATION2D_NWS_ROS) << "Configuration: \n" << config.toString().c_str();
98 
99  if (!config.check("period"))
100  {
101  yCInfo(LOCALIZATION2D_NWS_ROS) << "Missing 'period' parameter. Using default value: " << DEFAULT_THREAD_PERIOD;
103  }
104  else
105  {
106  m_period = config.find("period").asFloat64();
107  yCInfo(LOCALIZATION2D_NWS_ROS) << "Period requested: " << m_period;
108  }
109 
110  if (!config.check("publish_odometry"))
111  {
112  m_enable_publish_odometry_topic = config.find("publish_odometry").asBool();
113  yCInfo(LOCALIZATION2D_NWS_ROS) << "publish_odometry=" << m_enable_publish_odometry_topic;
114  }
115  if (!config.check("publish_tf"))
116  {
117  m_enable_publish_odometry_tf = config.find("publish_tf").asBool();
118  yCInfo(LOCALIZATION2D_NWS_ROS) << "publish_tf=" << m_enable_publish_odometry_tf;
119  }
120 
121  if (!config.check("yarp_base_name"))
122  {
123  yCError(LOCALIZATION2D_NWS_ROS) << "Missing yarp_base_name parameter";
124  return false;
125  }
126  m_local_name = config.find("yarp_base_name").asString();
127  if (m_local_name.c_str()[0] != '/') {
128  yCError(LOCALIZATION2D_NWS_ROS) << "Missing '/' in yarp_base_name parameter";
129  return false;
130  }
131 
132  m_rpcPortName = m_local_name + "/rpc";
133 
134  if (config.check("subdevice"))
135  {
136  Property p;
137  p.fromString(config.toString(), false);
138  p.put("device", config.find("subdevice").asString());
139 
140  if (!pLoc.open(p) || !pLoc.isValid())
141  {
142  yCError(LOCALIZATION2D_NWS_ROS) << "Failed to open subdevice.. check params";
143  return false;
144  }
145  if (!attach(&pLoc))
146  {
147  yCError(LOCALIZATION2D_NWS_ROS) << "Failed to open subdevice.. check params";
148  return false;
149  }
150  }
151  else
152  {
153  yCInfo(LOCALIZATION2D_NWS_ROS) << "Waiting for device to attach";
154  }
156 
157  if (!initialize_YARP(config))
158  {
159  yCError(LOCALIZATION2D_NWS_ROS) << "Error initializing YARP ports";
160  return false;
161  }
162 
163  if (!initialize_ROS(config))
164  {
165  yCError(LOCALIZATION2D_NWS_ROS) << "Error initializing ROS system";
166  return false;
167  }
168 
169  return true;
170 }
171 
173 {
174  if (params.check("parent_frame_id"))
175  {
176  m_parent_frame_id = params.find("parent_frame_id").asString();
177  }
178 
179  if (params.check("child_frame_id"))
180  {
181  m_child_frame_id = params.find("child_frame_id").asString();
182  }
183 
184  if (params.check("topic_name"))
185  {
186  m_odom_topic_name = params.find("topic_name").asString();
187  }
188 
189  if (params.check("node_name"))
190  {
191  m_node_name = params.find("node_name").asString();
192  }
194 
195  if (m_node == nullptr)
196  {
197  bool b= false;
199  if (m_node == nullptr)
200  {
201  yCError(LOCALIZATION2D_NWS_ROS) << "Opening " << m_node_name << " Node, check your yarp-ROS network configuration";
202  return false;
203  }
204 
206  if (!b)
207  {
208  yCError(LOCALIZATION2D_NWS_ROS) << "Unable to publish data on" << m_odom_topic_name << "topic";
209  return false;
210  }
211  b = m_tf_publisher.topic("/tf");
212  if (!b)
213  {
214  yCError(LOCALIZATION2D_NWS_ROS) << "Unable to publish data on /tf topic";
215  return false;
216  }
217  yCInfo(LOCALIZATION2D_NWS_ROS) << "ROS initialized";
218  }
219  return true;
220 }
221 
223 {
224  if (!m_rpcPort.open(m_rpcPortName.c_str()))
225  {
226  yCError(LOCALIZATION2D_NWS_ROS, "Failed to open port %s", m_rpcPortName.c_str());
227  return false;
228  }
229  m_rpcPort.setReader(*this);
230 
231  return true;
232 }
233 
235 {
236  yCTrace(LOCALIZATION2D_NWS_ROS, "Close");
237  if (PeriodicThread::isRunning())
238  {
239  PeriodicThread::stop();
240  }
241 
242  detach();
243 
245  m_rpcPort.close();
246 
247  if (m_node)
248  {
251  delete m_node;
252  m_node = nullptr;
253  }
254 
255  yCDebug(LOCALIZATION2D_NWS_ROS) << "Execution terminated";
256  return true;
257 }
258 
260 {
261  yarp::os::Bottle command;
262  yarp::os::Bottle reply;
263  bool ok = command.read(connection);
264  if (!ok) {
265  return false;
266  }
267 
268  reply.clear();
269 
270  if (command.get(0).isString() && command.get(0).asString() == "help")
271  {
272  reply.addVocab32("many");
273  reply.addString("No commands currently available:");
274  }
275  else
276  {
277  yCError(LOCALIZATION2D_NWS_ROS) << "Invalid command. Try `help`";
278  reply.addVocab32(VOCAB_ERR);
279  }
280 
281  yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
282  if (returnToSender != nullptr)
283  {
284  reply.write(*returnToSender);
285  }
286 
287  return true;
288 }
289 
291 {
292  double m_stats_time_curr = yarp::os::Time::now();
293  if (m_stats_time_curr - m_stats_time_last > 5.0)
294  {
295  yCInfo(LOCALIZATION2D_NWS_ROS) << "Running";
297  }
298 
300  if (ret == false)
301  {
302  yCError(LOCALIZATION2D_NWS_ROS) << "getLocalizationStatus() failed";
303  }
304 
306  {
308  if (ret2 == false)
309  {
310  yCError(LOCALIZATION2D_NWS_ROS) << "getCurrentPosition() failed";
311  }
312  else
313  {
315  }
317  if (ret3 == false)
318  {
319  //yCError(LOCALIZATION2D_NWS_ROS) << "getEstimatedOdometry() failed";
320  }
321  else
322  {
324  }
325  }
326  else
327  {
328  yCWarning(LOCALIZATION2D_NWS_ROS, "The system is not properly localized!");
329  }
330 
332  publish_odometry_on_ROS_topic();
333  }
335  publish_odometry_on_TF_topic();
336  }
337 }
338 
339 void Localization2D_nws_ros::publish_odometry_on_TF_topic()
340 {
343  transform.child_frame_id = m_child_frame_id;
344  transform.header.frame_id = m_parent_frame_id;
345  transform.header.seq = m_odom_stamp.getCount();
346  transform.header.stamp = m_odom_stamp.getTime();
347  double halfYaw = m_current_odometry.odom_theta / 180.0 * M_PI * 0.5;
348  double cosYaw = cos(halfYaw);
349  double sinYaw = sin(halfYaw);
350  transform.transform.rotation.x = 0;
351  transform.transform.rotation.y = 0;
352  transform.transform.rotation.z = sinYaw;
353  transform.transform.rotation.w = cosYaw;
356  transform.transform.translation.z = 0;
357  if (rosData.transforms.size() == 0)
358  {
359  rosData.transforms.push_back(transform);
360  }
361  else
362  {
363  rosData.transforms[0] = transform;
364  }
365 
367 }
368 
369 void Localization2D_nws_ros::publish_odometry_on_ROS_topic()
370 {
372  {
374  odom.clear();
376  odom.header.seq = m_odom_stamp.getCount();
377  odom.header.stamp = m_odom_stamp.getTime();
379 
382  odom.pose.pose.position.z = 0;
383  yarp::sig::Vector vecrpy(3);
384  vecrpy[0] = 0;
385  vecrpy[1] = 0;
386  vecrpy[2] = m_current_odometry.odom_theta;
387  yarp::sig::Matrix matrix = yarp::math::rpy2dcm(vecrpy);
389  odom.pose.pose.orientation.x = q.x();
390  odom.pose.pose.orientation.y = q.y();
391  odom.pose.pose.orientation.z = q.z();
392  odom.pose.pose.orientation.w = q.w();
393  //odom.pose.covariance = 0;
394 
397  odom.twist.twist.linear.z = 0;
398  odom.twist.twist.angular.x = 0;
399  odom.twist.twist.angular.y = 0;
401  //odom.twist.covariance = 0;
402 
404  }
405 }
define control board standard interfaces
constexpr yarp::conf::vocab32_t VOCAB_ERR
Definition: GenericVocabs.h:17
bool ret
#define DEFAULT_THREAD_PERIOD
contains the definition of a Vector type
bool close() override
Close the DeviceDriver.
bool initialize_YARP(yarp::os::Searchable &config)
yarp::dev::OdometryData m_current_odometry
yarp::os::Publisher< yarp::rosmsg::nav_msgs::Odometry > m_odometry_publisher
yarp::dev::Nav2D::ILocalization2D * iLoc
yarp::dev::Nav2D::Map2DLocation m_current_position
yarp::dev::PolyDriver pLoc
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
yarp::dev::Nav2D::LocalizationStatusEnum m_current_status
yarp::os::Publisher< yarp::rosmsg::tf2_msgs::TFMessage > m_tf_publisher
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
void run() override
Loop function.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool initialize_ROS(yarp::os::Searchable &config)
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:74
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.
double base_vel_x
velocity of the robot [m/s] expressed in the robot reference frame
Definition: OdometryData.h:42
double base_vel_theta
angular velocity of the robot [deg/s] expressed in the robot reference frame
Definition: OdometryData.h:50
double base_vel_y
velocity of the robot [m/s] expressed in the robot reference frame
Definition: OdometryData.h:46
double odom_x
position of the robot [m], expressed in the world reference frame
Definition: OdometryData.h:30
double odom_y
position of the robot [m], expressed in the world reference frame
Definition: OdometryData.h:34
double odom_theta
orientation the robot [deg], expressed in the world reference frame
Definition: OdometryData.h:38
A container for a device driver.
Definition: PolyDriver.h:24
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:153
double x() const
Definition: Quaternion.cpp:72
double z() const
Definition: Quaternion.cpp:82
double w() const
Definition: Quaternion.cpp:67
double y() const
Definition: Quaternion.cpp:77
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:74
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:164
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition: Bottle.cpp:240
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
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 addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:170
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:24
An abstraction for a periodic thread.
int getOutputCount() override
Determine how many output connections this port has.
Definition: Port.cpp:558
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition: Port.cpp:502
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition: Port.cpp:374
void close() override
Stop port activity.
Definition: Port.cpp:354
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
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:85
bool topic(const std::string &name)
Set topic to publish to.
Definition: Publisher.h:64
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::Publisher::write.
Definition: Publisher.h:124
void write(bool forceStrict=false)
Write the current object being returned by Publisher::prepare.
Definition: Publisher.h:149
Port & asPort() override
Get the concrete Port being used for communication.
Definition: Publisher.h:170
A base class for nested structures that can be searched.
Definition: Searchable.h:66
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.
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 bool asBool() const
Get boolean value.
Definition: Value.cpp:186
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:43
#define M_PI
#define yCInfo(component,...)
Definition: LogComponent.h:132
#define yCError(component,...)
Definition: LogComponent.h:154
#define yCTrace(component,...)
Definition: LogComponent.h:85
#define yCWarning(component,...)
Definition: LogComponent.h:143
#define yCDebug(component,...)
Definition: LogComponent.h:109
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:77
An interface for the device drivers.
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.