YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Odometry2D_nws_ros.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2023 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
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 m_node = new yarp::os::Node(m_nodeName);
115 if (m_node == nullptr) {
116 yCError(ODOMETRY2D_NWS_ROS) << " opening " << m_nodeName << " Node, check your yarp-ROS network configuration\n";
117 return false;
118 }
119 if (!rosPublisherPort_odometry.topic(m_topicName)) {
120 yCError(ODOMETRY2D_NWS_ROS) << " opening " << m_topicName << " Topic, check your yarp-ROS network configuration\n";
121 return false;
122 }
123
124 if (m_enable_publish_tf)
125 {
126 if (!rosPublisherPort_tf.topic("/tf")) {
127 yCError(ODOMETRY2D_NWS_ROS) << " opening " << "/tf" << " Topic, check your yarp-ROS network configuration\n";
128 return false;
129 }
130 }
131 return true;
132}
133
137
139{
140 if (m_odometry2D_interface!=nullptr)
141 {
142 yarp::dev::OdometryData odometryData;
143 double synchronized_timestamp = 0;
144 m_odometry2D_interface->getOdometry(odometryData, &synchronized_timestamp);
145
146 if (std::isnan(synchronized_timestamp) == false)
147 {
148 m_lastStateStamp.update(synchronized_timestamp);
149 }
150 else
151 {
152 m_lastStateStamp.update(yarp::os::Time::now());
153 }
154
155 if (1)
156 {
157 yarp::rosmsg::nav_msgs::Odometry& rosData = rosPublisherPort_odometry.prepare();
158 rosData.header.seq = m_lastStateStamp.getCount();
159 rosData.header.stamp = m_lastStateStamp.getTime();
160 rosData.header.frame_id = m_odomFrame;
161 rosData.child_frame_id = m_baseFrame;
162
163 rosData.pose.pose.position.x = odometryData.odom_x;
164 rosData.pose.pose.position.y = odometryData.odom_y;
165 rosData.pose.pose.position.z = 0.0;
167 double halfYaw = odometryData.odom_theta * DEG2RAD * 0.5;
168 double cosYaw = cos(halfYaw);
169 double sinYaw = sin(halfYaw);
170 odom_quat.x = 0;
171 odom_quat.y = 0;
172 odom_quat.z = sinYaw;
173 odom_quat.w = cosYaw;
174 rosData.pose.pose.orientation = odom_quat;
175 rosData.twist.twist.linear.x = odometryData.base_vel_x;
176 rosData.twist.twist.linear.y = odometryData.base_vel_y;
177 rosData.twist.twist.linear.z = 0;
178 rosData.twist.twist.angular.x = 0;
179 rosData.twist.twist.angular.y = 0;
180 rosData.twist.twist.angular.z = odometryData.base_vel_theta * DEG2RAD;
181 rosPublisherPort_odometry.write();
182 }
183
184 if (m_enable_publish_tf)
185 {
186 yarp::rosmsg::tf2_msgs::TFMessage& rosData = rosPublisherPort_tf.prepare();
188 transform.header.frame_id = m_odomFrame;
189 transform.child_frame_id = m_baseFrame;
190 transform.header.seq = m_lastStateStamp.getCount();
191 transform.header.stamp = m_lastStateStamp.getTime();
192 double halfYaw = odometryData.odom_theta * DEG2RAD * 0.5;
193 double cosYaw = cos(halfYaw);
194 double sinYaw = sin(halfYaw);
195 transform.transform.rotation.x = 0;
196 transform.transform.rotation.y = 0;
197 transform.transform.rotation.z = sinYaw;
198 transform.transform.rotation.w = cosYaw;
199 transform.transform.translation.x = odometryData.odom_x;
200 transform.transform.translation.y = odometryData.odom_y;
201 transform.transform.translation.z = 0;
202 if (rosData.transforms.size() == 0)
203 {
204 rosData.transforms.push_back(transform);
205 }
206 else if (rosData.transforms.size() == 1)
207 {
208 rosData.transforms[0] = transform;
209 }
210 else
211 {
212 yCWarning(ODOMETRY2D_NWS_ROS) << "Size of /tf topic should be 1, instead it is:" << rosData.transforms.size();
213 }
214 rosPublisherPort_tf.write();
215 }
216
217 } else{
218 yCError(ODOMETRY2D_NWS_ROS) << "the interface is not valid";
219 }
220}
221
223{
224 yCTrace(ODOMETRY2D_NWS_ROS, "Close");
225 if (PeriodicThread::isRunning())
226 {
227 PeriodicThread::stop();
228 }
229
230 detach();
231
232 if (m_node)
233 {
234 rosPublisherPort_odometry.close();
235 if (m_enable_publish_tf)
236 {
237 rosPublisherPort_tf.close();
238 }
239 delete m_node;
240 m_node = nullptr;
241 }
242
243 yCDebug(ODOMETRY2D_NWS_ROS) << "Execution terminated";
244 return true;
245}
#define DEFAULT_THREAD_PERIOD
const yarp::os::LogComponent & ODOMETRY2D_NWS_ROS()
#define DEFAULT_THREAD_PERIOD
#define DEG2RAD
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.
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
double base_vel_theta
angular velocity of the robot [deg/s] expressed in the robot reference frame
double base_vel_y
velocity of the robot [m/s] expressed in the robot reference frame
double odom_x
position of the robot [m], expressed in the world reference frame
double odom_y
position of the robot [m], expressed in the world reference frame
double odom_theta
orientation the robot [deg], expressed in the world reference frame
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
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.
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
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,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
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