YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Odometry2D_nws_ros2.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6// For M_PI
7#define _USE_MATH_DEFINES
10#include <yarp/os/LogStream.h>
11#include <yarp/os/Stamp.h>
12#include <cmath>
13#include <Ros2Utils.h>
15
16YARP_LOG_COMPONENT(ODOMETRY2D_NWS_ROS2, "yarp.devices.Odometry2D_nws_ros2")
17
21
23{
24 m_odometry2D_interface = nullptr;
25}
26
27
29{
30
31 if (driver->isValid())
32 {
33 driver->view(m_odometry2D_interface);
34 } else {
35 yCError(ODOMETRY2D_NWS_ROS2) << "not valid driver";
36 }
37
38 if (m_odometry2D_interface == nullptr)
39 {
40 yCError(ODOMETRY2D_NWS_ROS2, "Subdevice passed to attach method is invalid");
41 return false;
42 }
43
44 yCInfo(ODOMETRY2D_NWS_ROS2, "Attach complete");
45 PeriodicThread::setPeriod(m_period);
46 return PeriodicThread::start();
47}
48
49
51{
52 if (PeriodicThread::isRunning())
53 {
54 PeriodicThread::stop();
55 }
56 m_odometry2D_interface = nullptr;
57 return true;
58}
59
61{
62 return true;
63}
64
66{
67 parseParams(config);
68 if (m_node_name[0] == '/') {
69 yCError(ODOMETRY2D_NWS_ROS2) << "node_name parameter cannot begin with '/'";
70 return false;
71 }
72 if (m_topic_name[0] != '/') {
73 yCError(ODOMETRY2D_NWS_ROS2) << "missing initial / in topic_name parameter";
74 return false;
75 }
76
77 rclcpp::NodeOptions node_options;
78 node_options.allow_undeclared_parameters(true);
79 node_options.automatically_declare_parameters_from_overrides(true);
80 if(m_namespace.empty()) {
81 m_node = NodeCreator::createNode(m_node_name, node_options);
82 } else {
83 m_node = NodeCreator::createNode(m_node_name, m_namespace, node_options);
84 }
85 if (m_node == nullptr) {
86 yCError(ODOMETRY2D_NWS_ROS2) << " opening " << m_node_name << " Node, check your yarp-ROS2 network configuration\n";
87 return false;
88 }
89
90 rclcpp::Parameter simTime( "use_sim_time", rclcpp::ParameterValue( true ) );
91 m_node->set_parameter( simTime );
92 const std::string m_tf_topic ="/tf";
93 m_publisher_tf = m_node->create_publisher<tf2_msgs::msg::TFMessage>(m_tf_topic, 10);
94
95 m_ros2Publisher_odometry = m_node->create_publisher<nav_msgs::msg::Odometry>(m_topic_name, 10);
96
97 yCInfo(ODOMETRY2D_NWS_ROS2) << "Waiting for device to attach";
98 return true;
99}
100
104
106{
107
108 if (m_odometry2D_interface!=nullptr && m_ros2Publisher_odometry && m_publisher_tf) {
109 yarp::dev::OdometryData odometryData;
110 double synchronized_timestamp = 0;
111 m_odometry2D_interface->getOdometry(odometryData, &synchronized_timestamp);
112
113 if (std::isnan(synchronized_timestamp) == false)
114 {
115 m_timeStamp.update(synchronized_timestamp);
116 }
117 else
118 {
119 m_timeStamp.update(yarp::os::Time::now());
120 }
121
122 nav_msgs::msg::Odometry odometryMsg;
123 odometryMsg.header.frame_id = m_odom_frame;
124 odometryMsg.child_frame_id = m_base_frame;
125
126 odometryMsg.pose.pose.position.x = odometryData.odom_x;
127 odometryMsg.pose.pose.position.y = odometryData.odom_y;
128 odometryMsg.pose.pose.position.z = 0.0;
129 geometry_msgs::msg::Quaternion odom_quat;
130 double halfYaw = odometryData.odom_theta * DEG2RAD * 0.5;
131 double cosYaw = cos(halfYaw);
132 double sinYaw = sin(halfYaw);
133 odom_quat.x = 0;
134 odom_quat.y = 0;
135 odom_quat.z = sinYaw;
136 odom_quat.w = cosYaw;
137 odometryMsg.pose.pose.orientation = odom_quat;
138 odometryMsg.twist.twist.linear.x = odometryData.base_vel_x;
139 odometryMsg.twist.twist.linear.y = odometryData.base_vel_y;
140 odometryMsg.twist.twist.linear.z = 0;
141 odometryMsg.twist.twist.angular.x = 0;
142 odometryMsg.twist.twist.angular.y = 0;
143 odometryMsg.twist.twist.angular.z = odometryData.base_vel_theta * DEG2RAD;
144
145 // tf publisher
146 tf2_msgs::msg::TFMessage rosData;
147
148 geometry_msgs::msg::TransformStamped tsData;
149 tsData.child_frame_id = m_base_frame;
150 tsData.header.frame_id = m_odom_frame;
151
152 tsData.transform.rotation.x = 0;
153 tsData.transform.rotation.y = 0;
154 tsData.transform.rotation.z = sinYaw;
155 tsData.transform.rotation.w = cosYaw;
156 tsData.transform.translation.x = odometryData.odom_x;
157 tsData.transform.translation.y = odometryData.odom_y;
158 tsData.transform.translation.z = 0;
159
160 odometryMsg.header.stamp.sec = int(m_timeStamp.getTime());
161 odometryMsg.header.stamp.nanosec = int(1000000000UL * (m_timeStamp.getTime() - int(m_timeStamp.getTime())));
162
163 tsData.header.stamp.sec = int(m_timeStamp.getTime());
164 tsData.header.stamp.nanosec = int(1000000000UL * (m_timeStamp.getTime() - int(m_timeStamp.getTime())));
165
166 if (rosData.transforms.size() == 0)
167 {
168 rosData.transforms.push_back(tsData);
169 }
170 else if (rosData.transforms.size() == 1)
171 {
172 rosData.transforms[0] = tsData;
173 }
174 else
175 {
176 yCWarning(ODOMETRY2D_NWS_ROS2) << "Size of /tf topic should be 1, instead it is:" << rosData.transforms.size();
177 }
178
179 if(m_ros2Publisher_odometry->get_subscription_count() > 0)
180 m_ros2Publisher_odometry->publish(odometryMsg);
181
182 m_publisher_tf->publish(rosData);
183
184
185 } else{
186 yCError(ODOMETRY2D_NWS_ROS2) << "the interface is not valid";
187 }
188}
189
191{
193 if (PeriodicThread::isRunning())
194 {
195 PeriodicThread::stop();
196 }
197
198 detach();
199 return true;
200}
#define DEFAULT_THREAD_PERIOD
const yarp::os::LogComponent & ODOMETRY2D_NWS_ROS2()
#define DEG2RAD
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
Odometry2D_nws_ros2: A ros2 nws to get odometry and publish it on a ros2 topic. The attached device m...
void run() override
Loop function.
bool threadInit() override
Initialization method.
bool close() override
Close the DeviceDriver.
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
void threadRelease() override
Release method.
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getOdometry(yarp::dev::Nav2D::Odometry &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.
A base class for nested structures that can be searched.
Definition Searchable.h:31
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
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(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