YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
frameTransformGet_nwc_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
7#include <yarp/os/Log.h>
9#include <yarp/os/LogStream.h>
10
11using namespace std;
12using namespace yarp::dev;
13using namespace yarp::os;
14using namespace yarp::sig;
15using namespace yarp::math;
16using namespace std::placeholders;
17
18namespace {
19YARP_LOG_COMPONENT(FRAMETRANSFORGETNWCROS2, "yarp.device.frameTransformGet_nwc_ros2")
20}
21
23{
25 yCError(FRAMETRANSFORGETNWCROS2,"Error! YARP Network is not initialized");
26 return false;
27 }
28 parseParams(config);
29
31 if(m_ROS2_namespace.empty())
32 {
34 } else {
36 }
37 if(m_node == nullptr) {
38 yCError(FRAMETRANSFORGETNWCROS2) << " opening " << m_ROS2_ft_node << " Node, check your yarp-ROS2 network configuration\n";
39 return false;
40 }
41 m_subscriptionFtTimed = m_node->create_subscription<tf2_msgs::msg::TFMessage>(m_ROS2_ft_topic, 10,
43 this, _1));
44
45 rclcpp::QoS qos(10);
46 qos = qos.transient_local(); // This line and the previous are needed to reestablish the "latched" behaviour for the subscriptio
47 m_subscriptionFtStatic = m_node->create_subscription<tf2_msgs::msg::TFMessage>(m_ROS2_ft_topic_static, qos,
49 this, _1));
50
51 m_spinner = new Ros2Spinner(m_node);
53
55
56 return true;
57}
58
60{
61 yCInfo(FRAMETRANSFORGETNWCROS2, "closing...");
62 delete m_spinner;
64 return true;
65}
66
67yarp::dev::ReturnValue FrameTransformGet_nwc_ros2::getTransforms(std::vector<yarp::math::FrameTransform>& transforms) const
68{
69 std::lock_guard<std::mutex> lock(m_trf_mutex);
70 if(!m_ftContainer.checkAndRemoveExpired())
71 {
72 yCError(FRAMETRANSFORGETNWCROS2,"Unable to remove expired transforms.");
74 }
75 auto ret = m_ftContainer.getTransforms(transforms);
76 if(!ret)
77 {
78 yCError(FRAMETRANSFORGETNWCROS2,"Unable to get transforms. Error: %s",ret.toString().c_str());
79 }
80 return ret;
81}
82
83void FrameTransformGet_nwc_ros2::frameTransformTimedGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg)
84{
85 std::lock_guard<std::mutex> lock(m_trf_mutex);
87 updateBuffer(msg->transforms,false);
88}
89
90void FrameTransformGet_nwc_ros2::frameTransformStaticGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg)
91{
92 std::lock_guard<std::mutex> lock(m_trf_mutex);
94 updateBuffer(msg->transforms,true);
95}
96
97void FrameTransformGet_nwc_ros2::ros2TransformToYARP(const geometry_msgs::msg::TransformStamped& input, yarp::math::FrameTransform& output, bool isStatic)
98{
99 output.translation.tX = input.transform.translation.x;
100 output.translation.tY = input.transform.translation.y;
101 output.translation.tZ = input.transform.translation.z;
102
103 output.rotation.w() = input.transform.rotation.w;
104 output.rotation.x() = input.transform.rotation.x;
105 output.rotation.y() = input.transform.rotation.y;
106 output.rotation.z() = input.transform.rotation.z;
107
108 output.timestamp = yarpTimeFromRos2(input.header.stamp);
109 output.src_frame_id = input.header.frame_id;
110 output.dst_frame_id = input.child_frame_id;
111 output.isStatic = isStatic;
112}
113
114bool FrameTransformGet_nwc_ros2::updateBuffer(const std::vector<geometry_msgs::msg::TransformStamped>& transforms, bool areStatic)
115{
116 for (auto& it : transforms)
117 {
120 m_ftContainer.setTransform(tempFT);
121 }
122 return true;
123}
double yarpTimeFromRos2(builtin_interfaces::msg::Time ros2Time)
Definition Ros2Utils.cpp:57
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool updateBuffer(const std::vector< geometry_msgs::msg::TransformStamped > &transforms, bool areStatic)
void frameTransformTimedGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg)
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue getTransforms(std::vector< yarp::math::FrameTransform > &transforms) const override
Obtains all frame transforms saved in a storage.
void frameTransformStaticGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg)
void ros2TransformToYARP(const geometry_msgs::msg::TransformStamped &input, yarp::math::FrameTransform &output, bool isStatic)
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
yarp::dev::ReturnValue getTransforms(std::vector< yarp::math::FrameTransform > &transforms) const override
Obtains all frame transforms saved in a storage.
yarp::dev::ReturnValue setTransform(const yarp::math::FrameTransform &transform) override
Save a frame transform in a storage.
@ return_value_error_generic
Method was successfully executed.
struct yarp::math::FrameTransform::Translation_t translation
A mini-server for performing network communication in the background.
static bool checkNetwork()
Check if the YARP Network is up and running.
Definition Network.cpp:1495
A base class for nested structures that can be searched.
Definition Searchable.h:31
bool start()
Start the new thread running.
Definition Thread.cpp:93
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define YARP_LOG_COMPONENT(name,...)
STL namespace.
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13