diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index e6c6ca2c1..1eac9e3ef 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -29,9 +29,11 @@ /** \author Tully Foote */ -#ifndef TF2_ROS_TRANSFORMLISTENER_H -#define TF2_ROS_TRANSFORMLISTENER_H +#ifndef TF2_ROS__TRANSFORM_LISTENER_H_ +#define TF2_ROS__TRANSFORM_LISTENER_H_ +#include +#include #include #include "tf2_msgs/msg/tf_message.hpp" #include "rclcpp/rclcpp.hpp" @@ -39,55 +41,80 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/visibility_control.h" -namespace tf2_ros{ + +namespace tf2_ros +{ /** \brief This class provides an easy way to request and receive coordinate frame transform information. */ class TransformListener { - public: /**@brief Constructor for transform listener */ TF2_ROS_PUBLIC - TransformListener(tf2::BufferCore& buffer, bool spin_thread = true); - + TransformListener(tf2::BufferCore & buffer, bool spin_thread = true); + + TF2_ROS_PUBLIC + TransformListener(tf2::BufferCore & buffer, rclcpp::Node::SharedPtr nh, bool spin_thread = true); + TF2_ROS_PUBLIC - TransformListener(tf2::BufferCore& buffer, rclcpp::Node::SharedPtr nh, bool spin_thread = true); + TransformListener( + tf2::BufferCore & buffer, + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics, + bool spin_thread = true); TF2_ROS_PUBLIC ~TransformListener(); private: - /// Initialize this transform listener, subscribing, advertising services, etc. void init(); void initThread(); + template< + typename MessageT, + typename CallbackT, + typename Alloc = std::allocator, + typename SubscriptionT = rclcpp::Subscription< + typename rclcpp::subscription_traits::has_message_type::type, Alloc>> + std::shared_ptr + create_subscription( + const std::string & topic_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, + bool ignore_local_publications = false, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr + msg_mem_strat = nullptr, + std::shared_ptr allocator = nullptr); + /// Callback function for ros message subscriptoin void subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); void static_subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); void subscription_callback_impl(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static); // ros::CallbackQueue tf_message_callback_queue_; - std::thread* dedicated_listener_thread_; - rclcpp::Node::SharedPtr node_; + std::thread * dedicated_listener_thread_; + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::Subscription::SharedPtr message_subscription_tf_; rclcpp::Subscription::SharedPtr message_subscription_tf_static_; - tf2::BufferCore& buffer_; + tf2::BufferCore & buffer_; bool using_dedicated_thread_; tf2::TimePoint last_update_; - + void dedicatedListenerThread() { - while (using_dedicated_thread_) - { + while (using_dedicated_thread_) { break; - //TODO(tfoote) reenable callback queue processing - //tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); + // TODO(tfoote) reenable callback queue processing + // tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); } - }; - + } }; -} +} // namespace tf2_ros -#endif //TF_TRANSFORMLISTENER_H +#endif // TF2_ROS__TRANSFORM_LISTENER_H_ diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 517db362d..d426df314 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -29,61 +29,77 @@ /** \author Tully Foote */ +#include +#include +#include + #include "tf2_ros/transform_listener.h" using namespace tf2_ros; -//TODO(tfoote replace these terrible macros) +// TODO(tfoote replace these terrible macros) #define ROS_ERROR printf #define ROS_FATAL printf #define ROS_INFO printf #define ROS_WARN printf -TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread): - dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false) +TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) +: TransformListener(buffer, rclcpp::Node::make_shared("transform_listener_impl"), spin_thread) { - //TODO(tfoote)make this anonymous - node_ = rclcpp::Node::make_shared("transform_listener_impl"); - init(); - if (spin_thread) - initThread(); } -TransformListener::TransformListener(tf2::BufferCore& buffer, rclcpp::Node::SharedPtr nh, bool spin_thread) -: dedicated_listener_thread_(NULL) -, node_(nh) -, buffer_(buffer) -, using_dedicated_thread_(false) +TransformListener::TransformListener( + tf2::BufferCore & buffer, rclcpp::Node::SharedPtr nh, + bool spin_thread) +: TransformListener(buffer, nh->get_node_base_interface(), + nh->get_node_topics_interface(), spin_thread) +{ +} + +TransformListener::TransformListener( + tf2::BufferCore & buffer, + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics, + bool spin_thread) +: dedicated_listener_thread_(NULL), + node_base_(node_base), + node_topics_(node_topics), + buffer_(buffer), + using_dedicated_thread_(false) { init(); - if (spin_thread) + if (spin_thread) { initThread(); + } } - TransformListener::~TransformListener() { using_dedicated_thread_ = false; - if (dedicated_listener_thread_) - { + if (dedicated_listener_thread_) { dedicated_listener_thread_->join(); delete dedicated_listener_thread_; } } -void test_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg){ - return; +void test_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) +{ } void TransformListener::init() { rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; custom_qos_profile.depth = 100; - std::function standard_callback = std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1); - message_subscription_tf_ = node_->create_subscription("/tf", standard_callback, custom_qos_profile); - std::function static_callback = std::bind(&TransformListener::static_subscription_callback, this, std::placeholders::_1); - message_subscription_tf_static_ = node_->create_subscription("/tf_static", static_callback, custom_qos_profile); + std::function standard_callback = std::bind( + &TransformListener::subscription_callback, this, std::placeholders::_1); + message_subscription_tf_ = create_subscription("/tf", standard_callback, + custom_qos_profile); + std::function static_callback = std::bind( + &TransformListener::static_subscription_callback, this, std::placeholders::_1); + message_subscription_tf_static_ = create_subscription("/tf_static", + static_callback, + custom_qos_profile); } void TransformListener::initThread() @@ -93,15 +109,53 @@ void TransformListener::initThread() // rclcpp::spin, since there are more than one versions of it (overloaded). // see: http://stackoverflow.com/a/27389714/671658 // I (wjwwood) chose to use the lamda rather than the static cast solution. - auto run_func = [](rclcpp::Node::SharedPtr node) { - return rclcpp::spin(node); - }; - dedicated_listener_thread_ = new std::thread(run_func, node_); - //Tell the buffer we have a dedicated thread to enable timeouts + auto run_func = [](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) { + return rclcpp::spin(node_base); + }; + dedicated_listener_thread_ = new std::thread(run_func, node_base_); + // Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); } +template< + typename MessageT, + typename CallbackT, + typename Alloc, + typename SubscriptionT> +std::shared_ptr +TransformListener::create_subscription( + const std::string & topic_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group, + bool ignore_local_publications, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr + msg_mem_strat, + std::shared_ptr allocator) +{ + using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type::type; + + if (!allocator) { + allocator = std::make_shared(); + } + + if (!msg_mem_strat) { + using rclcpp::message_memory_strategy::MessageMemoryStrategy; + msg_mem_strat = MessageMemoryStrategy::create_default(); + } + return rclcpp::create_subscription( + node_topics_.get(), + topic_name, + std::forward(callback), + qos_profile, + nullptr, + false, + false, + msg_mem_strat, + allocator); +} void TransformListener::subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { @@ -112,23 +166,22 @@ void TransformListener::static_subscription_callback(const tf2_msgs::msg::TFMess subscription_callback_impl(msg, true); } -void TransformListener::subscription_callback_impl(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static) +void TransformListener::subscription_callback_impl( + const tf2_msgs::msg::TFMessage::SharedPtr msg, + bool is_static) { - const tf2_msgs::msg::TFMessage& msg_in = *msg; - //TODO(tfoote) find a way to get the authority - std::string authority = "Authority undetectable"; //msg_evt.getPublisherName(); // lookup the authority - for (unsigned int i = 0; i < msg_in.transforms.size(); i++) - { - try - { + const tf2_msgs::msg::TFMessage & msg_in = *msg; + // TODO(tfoote) find a way to get the authority + std::string authority = "Authority undetectable"; // msg_evt.getPublisherName(); // lookup the authority + for (unsigned int i = 0; i < msg_in.transforms.size(); i++) { + try { buffer_.setTransform(msg_in.transforms[i], authority, is_static); - } - - catch (tf2::TransformException& ex) - { - ///\todo Use error reporting + } catch (tf2::TransformException & ex) { + // /\todo Use error reporting std::string temp = ex.what(); - ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); + ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", + msg_in.transforms[i].child_frame_id.c_str(), + msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); } } -}; +}