YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Localization2D_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#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
8#endif
9
11
13#include <yarp/os/LogStream.h>
14#include <Ros2Utils.h>
15
16#include <cmath>
17
18using namespace std::chrono_literals;
19using namespace yarp::os;
20using namespace yarp::dev;
21using namespace yarp::dev::Nav2D;
22
23
25
26
32
34{
35 if (poly->isValid())
36 {
37 poly->view(m_iLoc);
38 }
39
40 //attach the hardware device
41 if (nullptr == m_iLoc)
42 {
43 yCError(LOCALIZATION2D_NWS_ROS2, "Subdevice passed to attach method is invalid");
44 return false;
45 }
46
47 return true;
48}
49
51{
53 {
55 }
56 m_iLoc = nullptr;
57
58 return true;
59}
60
62{
64 if (m_stats_time_curr - m_stats_time_last > 5.0)
65 {
66 yCInfo(LOCALIZATION2D_NWS_ROS2) << "Running";
67 m_stats_time_last = yarp::os::Time::now();
68 }
69
70 if (m_iLoc!=nullptr)
71 {
72 bool ret = m_iLoc->getLocalizationStatus(m_current_status);
73 if (!ret)
74 {
75 yCError(LOCALIZATION2D_NWS_ROS2) << "getLocalizationStatus() failed";
76 }
77
78 if (m_current_status == LocalizationStatusEnum::localization_status_localized_ok)
79 {
80 bool ret2 = m_iLoc->getCurrentPosition(m_current_position);
81 if (!ret2)
82 {
83 yCError(LOCALIZATION2D_NWS_ROS2) << "getCurrentPosition() failed";
84 }
85 else
86 {
87 m_loc_stamp.update();
88 }
89 bool ret3 = m_iLoc->getEstimatedOdometry(m_current_odometry);
90 if (!ret3)
91 {
92 //yCError(LOCALIZATION2D_NWS_ROS2) << "getEstimatedOdometry() failed";
93 }
94 else
95 {
96 m_odom_stamp.update();
97 }
98 }
99 else
100 {
101 yCWarning(LOCALIZATION2D_NWS_ROS2, "The system is not properly localized!");
102 }
103
104 if (m_publisher_odom->get_subscription_count() > 0)
105 {
106 publish_odometry_on_ROS_topic();
107 }
108 publish_odometry_on_TF_topic();
109 }
110}
111
113{
114 if(m_node_name[0] == '/'){
115 yCError(LOCALIZATION2D_NWS_ROS2) << "node_name cannot begin with an initial /";
116 return false;
117 }
118
119 parseParams(config);
120
121 //create the topics
122 const std::string m_odom_topic ="/odom";
123 const std::string m_tf_topic ="/tf";
124 if(m_namespace.empty()) {
126 } else {
128 }
129 if (m_node == nullptr) {
130 yCError(LOCALIZATION2D_NWS_ROS2) << " opening " << m_node_name << " Node, check your yarp-ROS2 network configuration\n";
131 return false;
132 }
133
134 m_publisher_odom = m_node->create_publisher<nav_msgs::msg::Odometry>(m_odom_topic, 10);
135 m_publisher_tf = m_node->create_publisher<tf2_msgs::msg::TFMessage>(m_tf_topic, 10);
136 yCInfo(LOCALIZATION2D_NWS_ROS2, "Opened topics: %s, %s", m_odom_topic.c_str(), m_tf_topic.c_str());
137
138 yCInfo(LOCALIZATION2D_NWS_ROS2) << "Waiting for device to attach";
139
140 //start the publishig thread
142 start();
143 return true;
144}
145
147{
150 {
152 }
153
154 detach();
155 return true;
156}
157
158void Localization2D_nws_ros2::publish_odometry_on_TF_topic()
159{
160 tf2_msgs::msg::TFMessage rosData;
161
162 geometry_msgs::msg::TransformStamped tsData;
163 tsData.child_frame_id = m_ROS_child_frame_id;
164 tsData.header.frame_id = m_ROS_parent_frame_id;
165 tsData.header.stamp = m_node->get_clock()->now(); //@@@@@@@@@@@ CHECK HERE: simulation time?
166 double halfYaw = m_current_odometry.odom_theta / 180.0 * M_PI * 0.5;
167 double cosYaw = cos(halfYaw);
168 double sinYaw = sin(halfYaw);
169 tsData.transform.rotation.x = 0;
170 tsData.transform.rotation.y = 0;
171 tsData.transform.rotation.z = sinYaw;
172 tsData.transform.rotation.w = cosYaw;
173 tsData.transform.translation.x = m_current_odometry.odom_x;
174 tsData.transform.translation.y = m_current_odometry.odom_y;
175 tsData.transform.translation.z = 0;
176
177 if (rosData.transforms.size() == 0)
178 {
179 rosData.transforms.push_back(tsData);
180 }
181 else
182 {
183 rosData.transforms[0] = tsData;
184 }
185
186 m_publisher_tf->publish(rosData);
187}
188
189void Localization2D_nws_ros2::publish_odometry_on_ROS_topic()
190{
191 nav_msgs::msg::Odometry rosData;
192
193 rosData.header.frame_id = m_fixed_frame;
194 rosData.header.stamp = m_node->get_clock()->now(); //@@@@@@@@@@@ CHECK HERE: simulation time?
195 rosData.child_frame_id = m_robot_frame;
196
197 rosData.pose.pose.position.x = m_current_odometry.odom_x;
198 rosData.pose.pose.position.y = m_current_odometry.odom_y;
199 rosData.pose.pose.position.z = 0;
201 vecrpy[0] = 0;
202 vecrpy[1] = 0;
203 vecrpy[2] = m_current_odometry.odom_theta;
206 rosData.pose.pose.orientation.x = q.x();
207 rosData.pose.pose.orientation.y = q.y();
208 rosData.pose.pose.orientation.z = q.z();
209 rosData.pose.pose.orientation.w = q.w();
210 //rosData.pose.covariance = 0;
211
212 rosData.twist.twist.linear.x = m_current_odometry.base_vel_x;
213 rosData.twist.twist.linear.y = m_current_odometry.base_vel_y;
214 rosData.twist.twist.linear.z = 0;
215 rosData.twist.twist.angular.x = 0;
216 rosData.twist.twist.angular.y = 0;
217 rosData.twist.twist.angular.z = m_current_odometry.base_vel_theta;
218 //rosData.twist.covariance = 0;
219
220 m_publisher_odom->publish(rosData);
221}
const yarp::os::LogComponent & LOCALIZATION2D_NWS_ROS2()
#define M_PI
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool detach() override
Detach the object (you must have first called attach).
bool close() override
Close the DeviceDriver.
void run() override
Loop function.
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getEstimatedOdometry(yarp::dev::Nav2D::Odometry &odom)=0
Gets the estimated odometry the robot, including its velocity expressed in the world and in the local...
virtual yarp::dev::ReturnValue getLocalizationStatus(LocalizationStatusEnum &status)=0
Gets the current status of the localization task.
virtual yarp::dev::ReturnValue getCurrentPosition(yarp::dev::Nav2D::Map2DLocation &loc)=0
Gets the current position of the robot w.r.t world reference frame.
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.
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
A mini-server for performing network communication in the background.
@ TraceType
Definition Log.h:92
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
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
A class for a Matrix.
Definition Matrix.h:39
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
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.
Definition OSTestMsg.cpp:13
The main, catch-all namespace for YARP.
Definition dirs.h:16