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 
35 #define DEFAULT_THREAD_PERIOD 0.01
36 
37 namespace {
38 YARP_LOG_COMPONENT(LOCALIZATION2D_NWS_ROS, "yarp.device.localization2D_nws_ros")
39 }
40 
41 //------------------------------------------------------------------------------------------------------------------------------
42 
44  m_period(DEFAULT_THREAD_PERIOD)
45 {
47 }
48 
50 {
51  if (driver->isValid())
52  {
53  driver->view(iLoc);
54  }
55 
56  if (nullptr == iLoc)
57  {
58  yCError(LOCALIZATION2D_NWS_ROS, "Subdevice passed to attach method is invalid");
59  return false;
60  }
61 
62  //initialize m_current_position and m_current_status, if available
63  bool ret = true;
65  Map2DLocation loc;
66  ret &= iLoc->getLocalizationStatus(status);
67  ret &= iLoc->getCurrentPosition(loc);
68  if (ret)
69  {
70  m_current_status = status;
71  m_current_position = loc;
72  }
73  else
74  {
75  yCWarning(LOCALIZATION2D_NWS_ROS) << "Localization data not yet available during server initialization";
76  }
77 
78  PeriodicThread::setPeriod(m_period);
79  return PeriodicThread::start();
80 }
81 
83 {
84  if (PeriodicThread::isRunning())
85  {
86  PeriodicThread::stop();
87  }
88  iLoc = nullptr;
89  return true;
90 }
91 
93 {
94  Property params;
95  params.fromString(config.toString().c_str());
96  yCDebug(LOCALIZATION2D_NWS_ROS) << "Configuration: \n" << config.toString().c_str();
97 
98  if (!config.check("period"))
99  {
100  yCInfo(LOCALIZATION2D_NWS_ROS) << "Missing 'period' parameter. Using default value: " << DEFAULT_THREAD_PERIOD;
102  }
103  else
104  {
105  m_period = config.find("period").asFloat64();
106  yCInfo(LOCALIZATION2D_NWS_ROS) << "Period requested: " << m_period;
107  }
108 
109  if (!config.check("publish_odometry"))
110  {
111  m_enable_publish_odometry_topic = config.find("publish_odometry").asBool();
112  yCInfo(LOCALIZATION2D_NWS_ROS) << "publish_odometry=" << m_enable_publish_odometry_topic;
113  }
114  if (!config.check("publish_tf"))
115  {
116  m_enable_publish_odometry_tf = config.find("publish_tf").asBool();
117  yCInfo(LOCALIZATION2D_NWS_ROS) << "publish_tf=" << m_enable_publish_odometry_tf;
118  }
119 
120  if (!config.check("yarp_base_name"))
121  {
122  yCError(LOCALIZATION2D_NWS_ROS) << "Missing yarp_base_name parameter";
123  return false;
124  }
125  m_local_name = config.find("yarp_base_name").asString();
126  if (m_local_name.c_str()[0] != '/') {
127  yCError(LOCALIZATION2D_NWS_ROS) << "Missing '/' in yarp_base_name parameter";
128  return false;
129  }
130 
131  m_rpcPortName = m_local_name + "/rpc";
132 
133  if (config.check("subdevice"))
134  {
135  Property p;
136  p.fromString(config.toString(), false);
137  p.put("device", config.find("subdevice").asString());
138 
139  if (!pLoc.open(p) || !pLoc.isValid())
140  {
141  yCError(LOCALIZATION2D_NWS_ROS) << "Failed to open subdevice.. check params";
142  return false;
143  }
144  if (!attach(&pLoc))
145  {
146  yCError(LOCALIZATION2D_NWS_ROS) << "Failed to open subdevice.. check params";
147  return false;
148  }
149  }
150  else
151  {
152  yCInfo(LOCALIZATION2D_NWS_ROS) << "Waiting for device to attach";
153  }
155 
156  if (!initialize_YARP(config))
157  {
158  yCError(LOCALIZATION2D_NWS_ROS) << "Error initializing YARP ports";
159  return false;
160  }
161 
162  if (!initialize_ROS(config))
163  {
164  yCError(LOCALIZATION2D_NWS_ROS) << "Error initializing ROS system";
165  return false;
166  }
167 
168  return true;
169 }
170 
172 {
173  if (params.check("parent_frame_id"))
174  {
175  m_parent_frame_id = params.find("parent_frame_id").asString();
176  }
177 
178  if (params.check("child_frame_id"))
179  {
180  m_child_frame_id = params.find("child_frame_id").asString();
181  }
182 
183  if (params.check("topic_name"))
184  {
185  m_odom_topic_name = params.find("topic_name").asString();
186  }
187 
188  if (params.check("node_name"))
189  {
190  m_node_name = params.find("node_name").asString();
191  }
193 
194  if (m_node == nullptr)
195  {
196  bool b= false;
198  if (m_node == nullptr)
199  {
200  yCError(LOCALIZATION2D_NWS_ROS) << "Opening " << m_node_name << " Node, check your yarp-ROS network configuration";
201  return false;
202  }
203 
205  if (!b)
206  {
207  yCError(LOCALIZATION2D_NWS_ROS) << "Unable to publish data on" << m_odom_topic_name << "topic";
208  return false;
209  }
210  b = m_tf_publisher.topic("/tf");
211  if (!b)
212  {
213  yCError(LOCALIZATION2D_NWS_ROS) << "Unable to publish data on /tf topic";
214  return false;
215  }
216  yCInfo(LOCALIZATION2D_NWS_ROS) << "ROS initialized";
217  }
218  return true;
219 }
220 
222 {
223  if (!m_rpcPort.open(m_rpcPortName.c_str()))
224  {
225  yCError(LOCALIZATION2D_NWS_ROS, "Failed to open port %s", m_rpcPortName.c_str());
226  return false;
227  }
228  m_rpcPort.setReader(*this);
229 
230  return true;
231 }
232 
234 {
235  yCTrace(LOCALIZATION2D_NWS_ROS, "Close");
236  if (PeriodicThread::isRunning())
237  {
238  PeriodicThread::stop();
239  }
240 
241  detach();
242 
244  m_rpcPort.close();
245 
246  if (m_node)
247  {
250  delete m_node;
251  m_node = nullptr;
252  }
253 
254  yCDebug(LOCALIZATION2D_NWS_ROS) << "Execution terminated";
255  return true;
256 }
257 
259 {
260  yarp::os::Bottle command;
261  yarp::os::Bottle reply;
262  bool ok = command.read(connection);
263  if (!ok) {
264  return false;
265  }
266 
267  reply.clear();
268 
269  if (command.get(0).isString() && command.get(0).asString() == "help")
270  {
271  reply.addVocab32("many");
272  reply.addString("No commands currently available:");
273  }
274  else
275  {
276  yCError(LOCALIZATION2D_NWS_ROS) << "Invalid command. Try `help`";
277  reply.addVocab32(VOCAB_ERR);
278  }
279 
280  yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
281  if (returnToSender != nullptr)
282  {
283  reply.write(*returnToSender);
284  }
285 
286  return true;
287 }
288 
290 {
291  double m_stats_time_curr = yarp::os::Time::now();
292  if (m_stats_time_curr - m_stats_time_last > 5.0)
293  {
294  yCInfo(LOCALIZATION2D_NWS_ROS) << "Running";
296  }
297 
299  if (ret == false)
300  {
301  yCError(LOCALIZATION2D_NWS_ROS) << "getLocalizationStatus() failed";
302  }
303 
305  {
307  if (ret2 == false)
308  {
309  yCError(LOCALIZATION2D_NWS_ROS) << "getCurrentPosition() failed";
310  }
311  else
312  {
314  }
316  if (ret3 == false)
317  {
318  //yCError(LOCALIZATION2D_NWS_ROS) << "getEstimatedOdometry() failed";
319  }
320  else
321  {
323  }
324  }
325  else
326  {
327  yCWarning(LOCALIZATION2D_NWS_ROS, "The system is not properly localized!");
328  }
329 
331  publish_odometry_on_ROS_topic();
332  }
334  publish_odometry_on_TF_topic();
335  }
336 }
337 
338 void Localization2D_nws_ros::publish_odometry_on_TF_topic()
339 {
342  transform.child_frame_id = m_child_frame_id;
343  transform.header.frame_id = m_parent_frame_id;
344  transform.header.seq = m_odom_stamp.getCount();
345  transform.header.stamp = m_odom_stamp.getTime();
346  double halfYaw = m_current_odometry.odom_theta / 180.0 * M_PI * 0.5;
347  double cosYaw = cos(halfYaw);
348  double sinYaw = sin(halfYaw);
349  transform.transform.rotation.x = 0;
350  transform.transform.rotation.y = 0;
351  transform.transform.rotation.z = sinYaw;
352  transform.transform.rotation.w = cosYaw;
355  transform.transform.translation.z = 0;
356  if (rosData.transforms.size() == 0)
357  {
358  rosData.transforms.push_back(transform);
359  }
360  else
361  {
362  rosData.transforms[0] = transform;
363  }
364 
366 }
367 
368 void Localization2D_nws_ros::publish_odometry_on_ROS_topic()
369 {
371  {
373  odom.clear();
375  odom.header.seq = m_odom_stamp.getCount();
376  odom.header.stamp = m_odom_stamp.getTime();
378 
381  odom.pose.pose.position.z = 0;
382  yarp::sig::Vector vecrpy(3);
383  vecrpy[0] = 0;
384  vecrpy[1] = 0;
385  vecrpy[2] = m_current_odometry.odom_theta;
386  yarp::sig::Matrix matrix = yarp::math::rpy2dcm(vecrpy);
388  odom.pose.pose.orientation.x = q.x();
389  odom.pose.pose.orientation.y = q.y();
390  odom.pose.pose.orientation.z = q.z();
391  odom.pose.pose.orientation.w = q.w();
392  //odom.pose.covariance = 0;
393 
396  odom.twist.twist.linear.z = 0;
397  odom.twist.twist.angular.x = 0;
398  odom.twist.twist.angular.y = 0;
400  //odom.twist.covariance = 0;
401 
403  }
404 }
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:88
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:41
double base_vel_theta
angular velocity of the robot [deg/s] expressed in the robot reference frame
Definition: OdometryData.h:49
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
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
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: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.
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:39
#define M_PI
#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.
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.