YARP
Yet Another Robot Platform
Odometry2D_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
10#include <yarp/os/LogStream.h>
11#include <yarp/os/Stamp.h>
12
13#include <cmath>
14
15YARP_LOG_COMPONENT(ODOMETRY2D_NWS_ROS, "yarp.devices.Odometry2D_nws_ros")
16
18{
19}
20
22{
23 m_odometry2D_interface = nullptr;
24}
25
26
28{
29
30 if (driver->isValid())
31 {
32 driver->view(m_odometry2D_interface);
33 } else {
34 yCError(ODOMETRY2D_NWS_ROS) << "not valid driver";
35 }
36
37 if (m_odometry2D_interface == nullptr)
38 {
39 yCError(ODOMETRY2D_NWS_ROS, "Subdevice passed to attach method is invalid");
40 return false;
41 }
42
43 PeriodicThread::setPeriod(m_period);
44 return PeriodicThread::start();
45}
46
47
49{
50 if (PeriodicThread::isRunning())
51 {
52 PeriodicThread::stop();
53 }
54 m_odometry2D_interface = nullptr;
55 return true;
56}
57
59{
60 return true;
61}
62
64{
65 yarp::os::Property params;
66 params.fromString(config.toString());
67
68 if (config.check("publish_tf_topic")) {
69 m_enable_publish_tf = true;
70 }
71 if (config.check("skip_tf_topic")) {
72 m_enable_publish_tf = false;
73 }
74
75 if (!config.check("period")) {
76 yCWarning(ODOMETRY2D_NWS_ROS) << "missing 'period' parameter, using default value of" << DEFAULT_THREAD_PERIOD;
77 } else {
78 m_period = config.find("period").asFloat64();
79 }
80
81 if (!config.check("node_name")) {
82 yCError(ODOMETRY2D_NWS_ROS) << "missing node_name parameter";
83 return false;
84 }
85 m_nodeName = config.find("node_name").asString();
86 if (m_nodeName[0] != '/') {
87 yCError(ODOMETRY2D_NWS_ROS) << "missing initial / in node_name parameter";
88 return false;
89 }
90
91 if (!config.check("topic_name")) {
92 yCError(ODOMETRY2D_NWS_ROS) << "missing topic_name parameter";
93 return false;
94 }
95 m_topicName = config.find("topic_name").asString();
96 if (m_topicName[0] != '/') {
97 yCError(ODOMETRY2D_NWS_ROS) << "missing initial / in topic_name parameter";
98 return false;
99 }
100
101 if (!config.check("odom_frame")) {
102 yCError(ODOMETRY2D_NWS_ROS) << "missing odom_frame parameter";
103 return false;
104 }
105 m_odomFrame = config.find("odom_frame").asString();
106
107
108 if (!config.check("base_frame")) {
109 yCError(ODOMETRY2D_NWS_ROS) << "missing base_frame parameter";
110 return false;
111 }
112 m_baseFrame = config.find("base_frame").asString();
113
114 if (config.check("subdevice")) {
116 p.fromString(config.toString(), false);
117 p.put("device", config.find("subdevice").asString());
118
119 if (!m_driver.open(p) || !m_driver.isValid()) {
120 yCError(ODOMETRY2D_NWS_ROS) << "failed to open subdevice.. check params";
121 return false;
122 }
123
124 if (!attach(&m_driver)) {
125 yCError(ODOMETRY2D_NWS_ROS) << "failed to open subdevice.. check params";
126 return false;
127 }
128 }
129 m_node = new yarp::os::Node(m_nodeName);
130 if (m_node == nullptr) {
131 yCError(ODOMETRY2D_NWS_ROS) << " opening " << m_nodeName << " Node, check your yarp-ROS network configuration\n";
132 return false;
133 }
134 if (!rosPublisherPort_odometry.topic(m_topicName)) {
135 yCError(ODOMETRY2D_NWS_ROS) << " opening " << m_topicName << " Topic, check your yarp-ROS network configuration\n";
136 return false;
137 }
138
139 if (m_enable_publish_tf)
140 {
141 if (!rosPublisherPort_tf.topic("/tf")) {
142 yCError(ODOMETRY2D_NWS_ROS) << " opening " << "/tf" << " Topic, check your yarp-ROS network configuration\n";
143 return false;
144 }
145 }
146 return true;
147}
148
150{
151}
152
154{
155 if (m_odometry2D_interface!=nullptr)
156 {
157 yarp::dev::OdometryData odometryData;
158 double synchronized_timestamp = 0;
159 m_odometry2D_interface->getOdometry(odometryData, &synchronized_timestamp);
160
161 if (std::isnan(synchronized_timestamp) == false)
162 {
163 m_lastStateStamp.update(synchronized_timestamp);
164 }
165 else
166 {
167 m_lastStateStamp.update(yarp::os::Time::now());
168 }
169
170 if (1)
171 {
172 yarp::rosmsg::nav_msgs::Odometry& rosData = rosPublisherPort_odometry.prepare();
173 rosData.header.seq = m_lastStateStamp.getCount();
174 rosData.header.stamp = m_lastStateStamp.getTime();
175 rosData.header.frame_id = m_odomFrame;
176 rosData.child_frame_id = m_baseFrame;
177
178 rosData.pose.pose.position.x = odometryData.odom_x;
179 rosData.pose.pose.position.y = odometryData.odom_y;
180 rosData.pose.pose.position.z = 0.0;
182 double halfYaw = odometryData.odom_theta * DEG2RAD * 0.5;
183 double cosYaw = cos(halfYaw);
184 double sinYaw = sin(halfYaw);
185 odom_quat.x = 0;
186 odom_quat.y = 0;
187 odom_quat.z = sinYaw;
188 odom_quat.w = cosYaw;
189 rosData.pose.pose.orientation = odom_quat;
190 rosData.twist.twist.linear.x = odometryData.base_vel_x;
191 rosData.twist.twist.linear.y = odometryData.base_vel_y;
192 rosData.twist.twist.linear.z = 0;
193 rosData.twist.twist.angular.x = 0;
194 rosData.twist.twist.angular.y = 0;
195 rosData.twist.twist.angular.z = odometryData.base_vel_theta * DEG2RAD;
196 rosPublisherPort_odometry.write();
197 }
198
199 if (m_enable_publish_tf)
200 {
201 yarp::rosmsg::tf2_msgs::TFMessage& rosData = rosPublisherPort_tf.prepare();
203 transform.header.frame_id = m_odomFrame;
204 transform.child_frame_id = m_baseFrame;
205 transform.header.seq = m_lastStateStamp.getCount();
206 transform.header.stamp = m_lastStateStamp.getTime();
207 double halfYaw = odometryData.odom_theta * DEG2RAD * 0.5;
208 double cosYaw = cos(halfYaw);
209 double sinYaw = sin(halfYaw);
210 transform.transform.rotation.x = 0;
211 transform.transform.rotation.y = 0;
212 transform.transform.rotation.z = sinYaw;
213 transform.transform.rotation.w = cosYaw;
214 transform.transform.translation.x = odometryData.odom_x;
215 transform.transform.translation.y = odometryData.odom_y;
216 transform.transform.translation.z = 0;
217 if (rosData.transforms.size() == 0)
218 {
219 rosData.transforms.push_back(transform);
220 }
221 else if (rosData.transforms.size() == 1)
222 {
223 rosData.transforms[0] = transform;
224 }
225 else
226 {
227 yCWarning(ODOMETRY2D_NWS_ROS) << "Size of /tf topic should be 1, instead it is:" << rosData.transforms.size();
228 }
229 rosPublisherPort_tf.write();
230 }
231
232 } else{
233 yCError(ODOMETRY2D_NWS_ROS) << "the interface is not valid";
234 }
235}
236
238{
239 yCTrace(ODOMETRY2D_NWS_ROS, "Close");
240 if (PeriodicThread::isRunning())
241 {
242 PeriodicThread::stop();
243 }
244
245 detach();
246
247 if (m_node)
248 {
249 rosPublisherPort_odometry.close();
250 if (m_enable_publish_tf)
251 {
252 rosPublisherPort_tf.close();
253 }
254 delete m_node;
255 m_node = nullptr;
256 }
257
258 yCDebug(ODOMETRY2D_NWS_ROS) << "Execution terminated";
259 return true;
260}
#define DEG2RAD
const yarp::os::LogComponent & ODOMETRY2D_NWS_ROS()
constexpr double DEFAULT_THREAD_PERIOD
Odometry2D_nws_ros: A ros nws to get odometry and publish it on a ros topic. The attached device must...
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
bool threadInit() override
Initialization method.
void run() override
Loop function.
void threadRelease() override
Release method.
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
bool close() override
Close the DeviceDriver.
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
virtual bool getOdometry(yarp::dev::OdometryData &odom, double *timestamp=nullptr)=0
Gets the odometry of the robot, including its velocity expressed in the world and in the local refere...
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
The Node class.
Definition: Node.h:23
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
void write(bool forceStrict=false)
Write the current object being returned by Publisher::prepare.
Definition: Publisher.h:148
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::Publisher::write.
Definition: Publisher.h:123
A base class for nested structures that can be searched.
Definition: Searchable.h:56
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 Value & find(const std::string &key) const =0
Gets a value 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 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
#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
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
The main, catch-all namespace for YARP.
Definition: dirs.h:16