Skip to content

Commit

Permalink
Adding static transform listener (#673)
Browse files Browse the repository at this point in the history 
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
@ayrton04 @ahcorde
ayrton04 and ahcorde authored Aug 28, 2024
1 parent 3e6e9d3 commit d851022
Show file tree
Hide file tree
Showing 3 changed files with 198 additions and 15 deletions.
113 changes: 100 additions & 13 deletions tf2_ros/include/tf2_ros/transform_listener.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,10 @@ class TransformListener
* to it, then it's recommended to use one of the other constructors.
*/
TF2_ROS_PUBLIC
explicit TransformListener(tf2::BufferCore & buffer, bool spin_thread = true);
explicit TransformListener(
tf2::BufferCore & buffer,
bool spin_thread = true,
bool static_only = false);

/** \brief Node constructor */
template<class NodeT, class AllocatorT = std::allocator<void>>
Expand All @@ -102,7 +105,8 @@ class TransformListener
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
detail::get_default_transform_listener_sub_options<AllocatorT>(),
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
detail::get_default_transform_listener_static_sub_options<AllocatorT>())
detail::get_default_transform_listener_static_sub_options<AllocatorT>(),
bool static_only = false)
: TransformListener(
buffer,
node->get_node_base_interface(),
Expand All @@ -113,7 +117,8 @@ class TransformListener
qos,
static_qos,
options,
static_options)
static_options,
static_only)
{}

/** \brief Node interface constructor */
Expand All @@ -130,7 +135,8 @@ class TransformListener
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
detail::get_default_transform_listener_sub_options<AllocatorT>(),
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
detail::get_default_transform_listener_static_sub_options<AllocatorT>())
detail::get_default_transform_listener_static_sub_options<AllocatorT>(),
bool static_only = false)
: buffer_(buffer)
{
init(
Expand All @@ -142,7 +148,8 @@ class TransformListener
qos,
static_qos,
options,
static_options);
static_options,
static_only);
}

TF2_ROS_PUBLIC
Expand All @@ -163,7 +170,8 @@ class TransformListener
const rclcpp::QoS & qos,
const rclcpp::QoS & static_qos,
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options)
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options,
bool static_only = false)
{
spin_thread_ = spin_thread;
node_base_interface_ = node_base;
Expand All @@ -179,14 +187,19 @@ class TransformListener
// Create new callback group for message_subscription of tf and tf_static
callback_group_ = node_base_interface_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive, false);
// Duplicate to modify option of subscription
rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> tf_options = options;

if (!static_only) {
// Duplicate to modify subscription options
rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> tf_options = options;
tf_options.callback_group = callback_group_;

message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options);
}

rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> tf_static_options = static_options;
tf_options.callback_group = callback_group_;
tf_static_options.callback_group = callback_group_;

message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options);
message_subscription_tf_static_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
node_parameters,
node_topics,
Expand All @@ -202,8 +215,10 @@ class TransformListener
// Tell the buffer we have a dedicated thread to enable timeouts
buffer_.setUsingDedicatedThread(true);
} else {
message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
node_parameters, node_topics, "/tf", qos, std::move(cb), options);
if (!static_only) {
message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
node_parameters, node_topics, "/tf", qos, std::move(cb), options);
}
message_subscription_tf_static_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
node_parameters,
node_topics,
Expand All @@ -229,6 +244,78 @@ class TransformListener
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_ {nullptr};
rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
};

/** \brief Convenience class for subscribing to static-only transforms
*
* While users can achieve the same thing with a normal TransformListener, the number of parameters that must be
* provided is unwieldy.
*/
class StaticTransformListener : public TransformListener
{
public:
/** \brief Simplified constructor for a static transform listener
* \see the simplified TransformListener documentation
*/
TF2_ROS_PUBLIC
explicit StaticTransformListener(tf2::BufferCore & buffer, bool spin_thread = true)
: TransformListener(buffer, spin_thread, true)
{
}

/** \brief Node constructor */
template<class NodeT, class AllocatorT = std::allocator<void>>
StaticTransformListener(
tf2::BufferCore & buffer,
NodeT && node,
bool spin_thread = true,
const rclcpp::QoS & static_qos = StaticListenerQoS(),
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
detail::get_default_transform_listener_static_sub_options<AllocatorT>())
: TransformListener(
buffer,
node,
spin_thread,
rclcpp::QoS(1),
static_qos,
rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>(),
static_options,
true)
{
}

/** \brief Node interface constructor */
template<class AllocatorT = std::allocator<void>>
StaticTransformListener(
tf2::BufferCore & buffer,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
bool spin_thread = true,
const rclcpp::QoS & static_qos = StaticListenerQoS(),
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
detail::get_default_transform_listener_static_sub_options<AllocatorT>())
: TransformListener(
buffer,
node_base,
node_logging,
node_parameters,
node_topics,
spin_thread,
rclcpp::QoS(1),
static_qos,
rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>(),
static_options,
true)
{
}

TF2_ROS_PUBLIC
virtual ~StaticTransformListener()
{
}
};

} // namespace tf2_ros

#endif // TF2_ROS__TRANSFORM_LISTENER_H_
5 changes: 3 additions & 2 deletions tf2_ros/src/transform_listener.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
namespace tf2_ros
{

TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread)
TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread, bool static_only)
: buffer_(buffer)
{
rclcpp::NodeOptions options;
Expand All @@ -64,7 +64,8 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread)
optional_default_node_->get_node_topics_interface(),
spin_thread, DynamicListenerQoS(), StaticListenerQoS(),
detail::get_default_transform_listener_sub_options(),
detail::get_default_transform_listener_static_sub_options());
detail::get_default_transform_listener_static_sub_options(),
static_only);
}

TransformListener::~TransformListener()
Expand Down
95 changes: 95 additions & 0 deletions tf2_ros/test/test_transform_listener.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,10 @@
#include <gtest/gtest.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <chrono>
#include <memory>

#include "node_wrapper.hpp"
Expand All @@ -49,6 +52,14 @@ class CustomNode : public rclcpp::Node
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(buffer, shared_from_this(), false);
}

void init_static_tf_listener()
{
rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf_listener_ =
std::make_shared<tf2_ros::StaticTransformListener>(buffer, shared_from_this(), false);
}

private:
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
Expand All @@ -67,6 +78,14 @@ class CustomComposableNode : public rclcpp::Node
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(buffer, shared_from_this(), false);
}

void init_static_tf_listener()
{
rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf_listener_ =
std::make_shared<tf2_ros::StaticTransformListener>(buffer, shared_from_this(), false);
}

private:
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
Expand Down Expand Up @@ -104,6 +123,82 @@ TEST(tf2_test_transform_listener, transform_listener_with_intraprocess)
custom_node->init_tf_listener();
}

TEST(tf2_test_static_transform_listener, static_transform_listener_rclcpp_node)
{
auto node = rclcpp::Node::make_shared("tf2_ros_static_transform_listener");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
}

TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp_node)
{
auto node = std::make_shared<NodeWrapper>("tf2_ros_static_transform_listener");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::StaticTransformListener tfl(buffer, node, false);
}

TEST(tf2_test_static_transform_listener, static_transform_listener_as_member)
{
auto custom_node = std::make_shared<CustomNode>();
custom_node->init_static_tf_listener();
}

TEST(tf2_test_static_transform_listener, static_transform_listener_with_intraprocess)
{
rclcpp::executors::SingleThreadedExecutor exec;
rclcpp::NodeOptions options;
options = options.use_intra_process_comms(true);
auto custom_node = std::make_shared<CustomComposableNode>(options);
custom_node->init_static_tf_listener();
}

TEST(tf2_test_listeners, static_vs_dynamic)
{
auto node = rclcpp::Node::make_shared("tf2_ros_static_transform_listener");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer dynamic_buffer(clock);
tf2_ros::Buffer static_buffer(clock);
tf2_ros::TransformListener tfl(dynamic_buffer, node, true);
tf2_ros::StaticTransformListener stfl(static_buffer, node, true);
tf2_ros::TransformBroadcaster broadcaster(node);
tf2_ros::StaticTransformBroadcaster static_broadcaster(node);

geometry_msgs::msg::TransformStamped static_trans;
static_trans.header.stamp = clock->now();
static_trans.header.frame_id = "parent_static";
static_trans.child_frame_id = "child_static";
static_trans.transform.rotation.w = 1.0;
static_broadcaster.sendTransform(static_trans);

geometry_msgs::msg::TransformStamped dynamic_trans;
dynamic_trans.header.frame_id = "parent_dynamic";
dynamic_trans.child_frame_id = "child_dynamic";
dynamic_trans.transform.rotation.w = 1.0;

for (int i = 0; i < 10; ++i) {
dynamic_trans.header.stamp = clock->now();
broadcaster.sendTransform(dynamic_trans);

rclcpp::spin_some(node);
rclcpp::sleep_for(std::chrono::milliseconds(10));
}

// Dynamic buffer should have both dynamic and static transforms available
EXPECT_NO_THROW(
dynamic_buffer.lookupTransform("parent_dynamic", "child_dynamic", tf2::TimePointZero));
EXPECT_NO_THROW(dynamic_buffer.lookupTransform("parent_static", "child_static", clock->now()));

// Static buffer should have only static transforms available
EXPECT_THROW(
static_buffer.lookupTransform("parent_dynamic", "child_dynamic", tf2::TimePointZero),
tf2::LookupException);
EXPECT_NO_THROW(static_buffer.lookupTransform("parent_static", "child_static", clock->now()));
}

int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
Expand Down

0 comments on commit d851022

Please sign in to comment.