6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
18using namespace std::chrono_literals;
41 if (
nullptr == m_iLoc)
78 if (m_current_status == LocalizationStatusEnum::localization_status_localized_ok)
104 if (m_publisher_odom->get_subscription_count() > 0)
106 publish_odometry_on_ROS_topic();
108 publish_odometry_on_TF_topic();
129 if (m_node ==
nullptr) {
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);
158void Localization2D_nws_ros2::publish_odometry_on_TF_topic()
160 tf2_msgs::msg::TFMessage
rosData;
162 geometry_msgs::msg::TransformStamped
tsData;
165 tsData.header.stamp = m_node->get_clock()->now();
169 tsData.transform.rotation.x = 0;
170 tsData.transform.rotation.y = 0;
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;
177 if (
rosData.transforms.size() == 0)
186 m_publisher_tf->publish(
rosData);
189void Localization2D_nws_ros2::publish_odometry_on_ROS_topic()
191 nav_msgs::msg::Odometry
rosData;
193 rosData.header.frame_id = m_fixed_frame;
194 rosData.header.stamp = m_node->get_clock()->now();
195 rosData.child_frame_id = m_robot_frame;
199 rosData.pose.pose.position.z = 0;
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();
214 rosData.twist.twist.linear.z = 0;
215 rosData.twist.twist.angular.x = 0;
216 rosData.twist.twist.angular.y = 0;
220 m_publisher_odom->publish(
rosData);
const yarp::os::LogComponent & LOCALIZATION2D_NWS_ROS2()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::string m_ROS_child_frame_id
std::string m_ROS_parent_frame_id
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).
Localization2D_nws_ros2()
bool close() override
Close the DeviceDriver.
void run() override
Loop function.
static rclcpp::Node::SharedPtr createNode(std::string name)
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.
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.
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.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
#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...
double now()
Return the current time in seconds, relative to an arbitrary starting point.
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.