diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index fd5fc51c2..a5a7c9eaa 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include "permuter.hpp" diff --git a/test_tf2/test/test_buffer_client.cpp b/test_tf2/test/test_buffer_client.cpp index 35108193b..171d3b378 100644 --- a/test_tf2/test/test_buffer_client.cpp +++ b/test_tf2/test/test_buffer_client.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/test_tf2/test/test_buffer_server.cpp b/test_tf2/test/test_buffer_server.cpp index 10d77ab6b..24f519604 100644 --- a/test_tf2/test/test_buffer_server.cpp +++ b/test_tf2/test/test_buffer_server.cpp @@ -34,9 +34,9 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#include -#include -#include +#include +#include +#include #include #include diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 2b1a82d6d..f636c901d 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -44,9 +44,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include class Notification { diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index b28ff4b12..f43d87334 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -39,10 +39,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "permuter.hpp" diff --git a/test_tf2/test/test_tf2_bullet.cpp b/test_tf2/test/test_tf2_bullet.cpp index 70d0895b9..c8ff80a01 100644 --- a/test_tf2/test/test_tf2_bullet.cpp +++ b/test_tf2/test/test_tf2_bullet.cpp @@ -30,7 +30,7 @@ /** \author Wim Meeussen */ #include -#include +#include #include #include #include diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp index f0f3ef296..6cff68240 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp @@ -38,7 +38,7 @@ #include "LinearMath/btScalar.h" #include "LinearMath/btTransform.h" #include "geometry_msgs/msg/point_stamped.hpp" -#include "tf2_ros/buffer_interface.h" +#include "tf2_ros/buffer_interface.hpp" #if (BT_BULLET_VERSION <= 282) // Suppress compilation warning on older versions of Bullet. diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp index 8c5247ef9..c5723f8f2 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp @@ -39,8 +39,8 @@ #include "geometry_msgs/msg/twist.hpp" #include "tf2/convert.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/buffer_interface.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/buffer_interface.hpp" namespace tf2 { diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index d9ab12a60..54445563c 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -64,8 +64,8 @@ #include "tf2/convert.hpp" #include "tf2/transform_datatypes.hpp" #include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" TEST(TfEigen, ConvertVector3dStamped) { diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 784fa31fe..8f14fce93 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -57,7 +57,7 @@ #include "tf2/LinearMath/Quaternion.hpp" #include "tf2/LinearMath/Transform.hpp" #include "tf2/LinearMath/Vector3.hpp" -#include "tf2_ros/buffer_interface.h" +#include "tf2_ros/buffer_interface.hpp" namespace tf2 { diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 069965a0c..da209c4b5 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -43,8 +43,8 @@ #include "rclcpp/clock.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp index a8ec978be..f1d8f3c37 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp @@ -33,7 +33,7 @@ #define TF2_KDL_HPP #include -#include +#include #include #include #include diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index cad3cf5a2..be0dd4d34 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -42,7 +42,7 @@ #include #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include std::unique_ptr tf_buffer; diff --git a/tf2_ros/include/tf2_ros/async_buffer_interface.h b/tf2_ros/include/tf2_ros/async_buffer_interface.h index 4f79bd84a..9049a0828 100644 --- a/tf2_ros/include/tf2_ros/async_buffer_interface.h +++ b/tf2_ros/include/tf2_ros/async_buffer_interface.h @@ -30,94 +30,6 @@ #ifndef TF2_ROS__ASYNC_BUFFER_INTERFACE_H_ #define TF2_ROS__ASYNC_BUFFER_INTERFACE_H_ -#include -#include -#include -#include - -#include "tf2_ros/visibility_control.h" -#include "tf2/buffer_core.hpp" -#include "tf2/time.hpp" -#include "tf2/transform_datatypes.hpp" - -#include "geometry_msgs/msg/transform_stamped.hpp" - -namespace tf2_ros -{ - -class TransformStampedFuture : public std::shared_future -{ - using BaseType = std::shared_future; - -public: - /// Constructor - explicit TransformStampedFuture(BaseType && future) noexcept - : BaseType(std::move(future)) {} - - /// Copy constructor - TransformStampedFuture(const TransformStampedFuture & ts_future) noexcept - : BaseType(ts_future), - handle_(ts_future.handle_) {} - - /// Move constructor - TransformStampedFuture(TransformStampedFuture && ts_future) noexcept - : BaseType(std::move(ts_future)), - handle_(std::move(ts_future.handle_)) {} - - void setHandle(const tf2::TransformableRequestHandle handle) - { - handle_ = handle; - } - - tf2::TransformableRequestHandle getHandle() const - { - return handle_; - } - -private: - tf2::TransformableRequestHandle handle_ {}; -}; - -using TransformReadyCallback = std::function; - -/** - * \brief Abstract interface for asynchronous operations on a `tf2::BufferCoreInterface`. - * Implementations include tf2_ros::Buffer. - */ -class AsyncBufferInterface -{ -public: - TF2_ROS_PUBLIC - virtual - ~AsyncBufferInterface() = default; - - /** \brief Wait for a transform between two frames to become available. - * \param target_frame The frame into which to transform. - * \param source_frame The frame from which to tranform. - * \param time The time at which to transform. - * \param timeout Duration after which waiting will be stopped. - * \param callback The function to be called when the transform becomes available or a timeout - * occurs. In the case of timeout, an exception will be set on the future. - * \return A future to the requested transform. If a timeout occurs a `tf2::LookupException` - * will be set on the future. - */ - TF2_ROS_PUBLIC - virtual TransformStampedFuture - waitForTransform( - const std::string & target_frame, - const std::string & source_frame, - const tf2::TimePoint & time, - const tf2::Duration & timeout, - TransformReadyCallback callback) = 0; - - /** - * \brief Cancel the future to make sure the callback of requested transform is clean. - * \param ts_future The future to the requested transform. - */ - virtual void - cancel(const TransformStampedFuture & ts_future) = 0; -}; // class AsyncBufferInterface - -} // namespace tf2_ros +#include #endif // TF2_ROS__ASYNC_BUFFER_INTERFACE_H_ diff --git a/tf2_ros/include/tf2_ros/async_buffer_interface.hpp b/tf2_ros/include/tf2_ros/async_buffer_interface.hpp new file mode 100644 index 000000000..92dd989b3 --- /dev/null +++ b/tf2_ros/include/tf2_ros/async_buffer_interface.hpp @@ -0,0 +1,123 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TF2_ROS__ASYNC_BUFFER_INTERFACE_HPP_ +#define TF2_ROS__ASYNC_BUFFER_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "tf2_ros/visibility_control.hpp" +#include "tf2/buffer_core.hpp" +#include "tf2/time.hpp" +#include "tf2/transform_datatypes.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" + +namespace tf2_ros +{ + +class TransformStampedFuture : public std::shared_future +{ + using BaseType = std::shared_future; + +public: + /// Constructor + explicit TransformStampedFuture(BaseType && future) noexcept + : BaseType(std::move(future)) {} + + /// Copy constructor + TransformStampedFuture(const TransformStampedFuture & ts_future) noexcept + : BaseType(ts_future), + handle_(ts_future.handle_) {} + + /// Move constructor + TransformStampedFuture(TransformStampedFuture && ts_future) noexcept + : BaseType(std::move(ts_future)), + handle_(std::move(ts_future.handle_)) {} + + void setHandle(const tf2::TransformableRequestHandle handle) + { + handle_ = handle; + } + + tf2::TransformableRequestHandle getHandle() const + { + return handle_; + } + +private: + tf2::TransformableRequestHandle handle_ {}; +}; + +using TransformReadyCallback = std::function; + +/** + * \brief Abstract interface for asynchronous operations on a `tf2::BufferCoreInterface`. + * Implementations include tf2_ros::Buffer. + */ +class AsyncBufferInterface +{ +public: + TF2_ROS_PUBLIC + virtual + ~AsyncBufferInterface() = default; + + /** \brief Wait for a transform between two frames to become available. + * \param target_frame The frame into which to transform. + * \param source_frame The frame from which to tranform. + * \param time The time at which to transform. + * \param timeout Duration after which waiting will be stopped. + * \param callback The function to be called when the transform becomes available or a timeout + * occurs. In the case of timeout, an exception will be set on the future. + * \return A future to the requested transform. If a timeout occurs a `tf2::LookupException` + * will be set on the future. + */ + TF2_ROS_PUBLIC + virtual TransformStampedFuture + waitForTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration & timeout, + TransformReadyCallback callback) = 0; + + /** + * \brief Cancel the future to make sure the callback of requested transform is clean. + * \param ts_future The future to the requested transform. + */ + virtual void + cancel(const TransformStampedFuture & ts_future) = 0; +}; // class AsyncBufferInterface + +} // namespace tf2_ros + +#endif // TF2_ROS__ASYNC_BUFFER_INTERFACE_HPP_ diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 45a0cb3b9..ad85deff6 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -32,327 +32,6 @@ #ifndef TF2_ROS__BUFFER_H_ #define TF2_ROS__BUFFER_H_ -#include -#include -#include -#include -#include -#include - -#include "tf2_ros/async_buffer_interface.h" -#include "tf2_ros/buffer_interface.h" -#include "tf2_ros/create_timer_interface.h" -#include "tf2_ros/visibility_control.h" -#include "tf2/buffer_core.hpp" -#include "tf2/time.hpp" - -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_msgs/srv/frame_graph.hpp" -#include "rclcpp/node_interfaces/get_node_base_interface.hpp" -#include "rclcpp/node_interfaces/get_node_services_interface.hpp" -#include "rclcpp/node_interfaces/get_node_logging_interface.hpp" -#include "rclcpp/node_interfaces/node_logging_interface.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace tf2_ros -{ - -/** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type. - * - * Inherits tf2_ros::BufferInterface and tf2::BufferCore. - * Stores known frames and offers a ROS service, "tf_frames", which responds to client requests - * with a response containing a tf2_msgs::FrameGraph representing the relationship of known frames. - */ -class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2::BufferCore -{ -public: - using tf2::BufferCore::lookupTransform; - using tf2::BufferCore::canTransform; - using SharedPtr = std::shared_ptr; - - /** \brief Constructor for a Buffer object - * \param clock A clock to use for time and sleeping - * \param cache_time How long to keep a history of transforms - * \param node If passed advertise the view_frames service that exposes debugging information from the buffer - * \param qos If passed change the quality of service of the frames_server_ service - */ - template - Buffer( - rclcpp::Clock::SharedPtr clock, - tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), - NodeT && node = NodeT(), - const rclcpp::QoS & qos = rclcpp::ServicesQoS()) - : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) - { - if (nullptr == clock_) { - throw std::invalid_argument("clock must be a valid instance"); - } - - auto post_jump_cb = [this](const rcl_time_jump_t & jump_info) {onTimeJump(jump_info);}; - - rcl_jump_threshold_t jump_threshold; - // Disable forward jump callbacks - jump_threshold.min_forward.nanoseconds = 0; - // Anything backwards is a jump - jump_threshold.min_backward.nanoseconds = -1; - // Callback if the clock changes too - jump_threshold.on_clock_change = true; - - jump_handler_ = clock_->create_jump_callback(nullptr, post_jump_cb, jump_threshold); - - if (node) { - node_logging_interface_ = rclcpp::node_interfaces::get_node_logging_interface(node); - - frames_server_ = rclcpp::create_service( - rclcpp::node_interfaces::get_node_base_interface(node), - rclcpp::node_interfaces::get_node_services_interface(node), - "tf2_frames", std::bind( - &Buffer::getFrames, this, std::placeholders::_1, - std::placeholders::_2), qos, nullptr); - } - } - - /** \brief Get the transform between two frames by frame ID. - * \param target_frame The frame to which data should be transformed - * \param source_frame The frame where the data originated - * \param time The time at which the value of the transform is desired. (0 will get the latest) - * \param timeout How long to block before failing - * \return The transform between the frames - * - * Possible exceptions tf2::LookupException, tf2::ConnectivityException, - * tf2::ExtrapolationException, tf2::InvalidArgumentException - */ - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, const tf2::Duration timeout) const override; - - /** \brief Get the transform between two frames by frame ID. - * \sa lookupTransform(const std::string&, const std::string&, const tf2::TimePoint&, - * const tf2::Duration) - */ - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time, - const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0)) const - { - return lookupTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout)); - } - - /** \brief Get the transform between two frames by frame ID assuming fixed frame. - * \param target_frame The frame to which data should be transformed - * \param target_time The time to which the data should be transformed. (0 will get the latest) - * \param source_frame The frame where the data originated - * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) - * \param fixed_frame The frame in which to assume the transform is constant in time. - * \param timeout How long to block before failing - * \return The transform between the frames - * - * Possible exceptions tf2::LookupException, tf2::ConnectivityException, - * tf2::ExtrapolationException, tf2::InvalidArgumentException - */ - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const tf2::TimePoint & target_time, - const std::string & source_frame, const tf2::TimePoint & source_time, - const std::string & fixed_frame, const tf2::Duration timeout) const override; - - /** \brief Get the transform between two frames by frame ID assuming fixed frame. - * \sa lookupTransform(const std::string&, const tf2::TimePoint&, - * const std::string&, const tf2::TimePoint&, - * const std::string&, const tf2::Duration) - */ - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const rclcpp::Time & target_time, - const std::string & source_frame, const rclcpp::Time & source_time, - const std::string & fixed_frame, - const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0)) const - { - return lookupTransform( - target_frame, fromRclcpp(target_time), - source_frame, fromRclcpp(source_time), - fixed_frame, fromRclcpp(timeout)); - } - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param source_frame The frame from which to transform - * \param target_time The time at which to transform - * \param timeout How long to block before failing - * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - TF2_ROS_PUBLIC - bool - canTransform( - const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & target_time, const tf2::Duration timeout, - std::string * errstr = NULL) const override; - - /** \brief Test if a transform is possible - * \sa canTransform(const std::string&, const std::string&, - * const tf2::TimePoint&, const tf2::Duration, std::string*) - */ - TF2_ROS_PUBLIC - bool - canTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time, - const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), - std::string * errstr = NULL) const - { - return canTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout), errstr); - } - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param target_time The time into which to transform - * \param source_frame The frame from which to transform - * \param source_time The time from which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time - * \param timeout How long to block before failing - * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - TF2_ROS_PUBLIC - bool - canTransform( - const std::string & target_frame, const tf2::TimePoint & target_time, - const std::string & source_frame, const tf2::TimePoint & source_time, - const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr = NULL) const override; - - /** \brief Test if a transform is possible - * \sa - * canTransform(const std::string&, const tf2::TimePoint&, - * const std::string&, const tf2::TimePoint&, - * const std::string&, const tf2::Duration, std::string*) - */ - TF2_ROS_PUBLIC - bool - canTransform( - const std::string & target_frame, const rclcpp::Time & target_time, - const std::string & source_frame, const rclcpp::Time & source_time, - const std::string & fixed_frame, - const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), - std::string * errstr = NULL) const - { - return canTransform( - target_frame, fromRclcpp(target_time), - source_frame, fromRclcpp(source_time), - fixed_frame, fromRclcpp(timeout), - errstr); - } - - /** \brief Wait for a transform between two frames to become available. - * - * Before this method can be called, a tf2_ros::CreateTimerInterface must be registered - * by first calling setCreateTimerInterface. - * If no tf2_ros::CreateTimerInterface is set, then a tf2_ros::CreateTimerInterfaceException - * is thrown. - * - * \param target_frame The frame into which to transform. - * \param source_frame The frame from which to tranform. - * \param time The time at which to transform. - * \param timeout Duration after which waiting will be stopped. - * \param callback The function to be called when the transform becomes available or a timeout - * occurs. In the case of timeout, an exception will be set on the future. - * \return A future to the requested transform. If a timeout occurs a `tf2::LookupException` - * will be set on the future. - */ - TF2_ROS_PUBLIC - TransformStampedFuture - waitForTransform( - const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, const tf2::Duration & timeout, - TransformReadyCallback callback) override; - - /** \brief Wait for a transform between two frames to become available. - * \sa waitForTransform(const std::string &, const std::string &, const tf2::TimePoint &, - * const tf2::Duration &, TransformReadyCallback); - */ - TF2_ROS_PUBLIC - TransformStampedFuture - waitForTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time, - const rclcpp::Duration & timeout, TransformReadyCallback callback) - { - return waitForTransform( - target_frame, source_frame, - fromRclcpp(time), fromRclcpp(timeout), - callback); - } - - /** - * \brief Cancel the future to make sure the callback of requested transform is clean. - * \param ts_future The future to the requested transform. - */ - TF2_ROS_PUBLIC - void - cancel(const TransformStampedFuture & ts_future) override; - - TF2_ROS_PUBLIC - inline void - setCreateTimerInterface(CreateTimerInterface::SharedPtr create_timer_interface) - { - timer_interface_ = create_timer_interface; - } - -private: - void timerCallback( - const TimerHandle & timer_handle, - std::shared_ptr> promise, - TransformStampedFuture future, - TransformReadyCallback callback); - - TF2_ROS_PUBLIC - bool getFrames( - const tf2_msgs::srv::FrameGraph::Request::SharedPtr req, - tf2_msgs::srv::FrameGraph::Response::SharedPtr res); - - TF2_ROS_PUBLIC - void onTimeJump(const rcl_time_jump_t & jump); - - // conditionally error if dedicated_thread unset. - bool checkAndErrorDedicatedThreadPresent(std::string * errstr) const; - - /// Get the logger to use for calls to RCLCPP log macros. - rclcpp::Logger getLogger() const; - - // framegraph service - rclcpp::Service::SharedPtr frames_server_; - - /// \brief A clock to use for time and sleeping - rclcpp::Clock::SharedPtr clock_; - - /// \brief A node logging interface to access the buffer node's logger - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; - - /// \brief Interface for creating timers - CreateTimerInterface::SharedPtr timer_interface_; - - /// \brief A map from active timers to BufferCore request handles - std::unordered_map timer_to_request_map_; - - /// \brief A mutex on the timer_to_request_map_ data - std::mutex timer_to_request_map_mutex_; - - /// \brief Reference to a jump handler registered to the clock - rclcpp::JumpHandler::SharedPtr jump_handler_; -}; - -static const char threading_error[] = "Do not call canTransform or lookupTransform with a timeout " - "unless you are using another thread for populating data. Without a dedicated thread it will " - "always timeout. If you have a separate thread servicing tf messages, call " - "setUsingDedicatedThread(true) on your Buffer instance."; - -} // namespace tf2_ros +#include #endif // TF2_ROS__BUFFER_H_ diff --git a/tf2_ros/include/tf2_ros/buffer.hpp b/tf2_ros/include/tf2_ros/buffer.hpp new file mode 100644 index 000000000..e3c135347 --- /dev/null +++ b/tf2_ros/include/tf2_ros/buffer.hpp @@ -0,0 +1,358 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + +#ifndef TF2_ROS__BUFFER_HPP_ +#define TF2_ROS__BUFFER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "tf2_ros/async_buffer_interface.hpp" +#include "tf2_ros/buffer_interface.hpp" +#include "tf2_ros/create_timer_interface.hpp" +#include "tf2_ros/visibility_control.hpp" +#include "tf2/buffer_core.hpp" +#include "tf2/time.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_msgs/srv/frame_graph.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_services_interface.hpp" +#include "rclcpp/node_interfaces/get_node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace tf2_ros +{ + +/** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type. + * + * Inherits tf2_ros::BufferInterface and tf2::BufferCore. + * Stores known frames and offers a ROS service, "tf_frames", which responds to client requests + * with a response containing a tf2_msgs::FrameGraph representing the relationship of known frames. + */ +class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2::BufferCore +{ +public: + using tf2::BufferCore::lookupTransform; + using tf2::BufferCore::canTransform; + using SharedPtr = std::shared_ptr; + + /** \brief Constructor for a Buffer object + * \param clock A clock to use for time and sleeping + * \param cache_time How long to keep a history of transforms + * \param node If passed advertise the view_frames service that exposes debugging information from the buffer + * \param qos If passed change the quality of service of the frames_server_ service + */ + template + Buffer( + rclcpp::Clock::SharedPtr clock, + tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), + NodeT && node = NodeT(), + const rclcpp::QoS & qos = rclcpp::ServicesQoS()) + : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) + { + if (nullptr == clock_) { + throw std::invalid_argument("clock must be a valid instance"); + } + + auto post_jump_cb = [this](const rcl_time_jump_t & jump_info) {onTimeJump(jump_info);}; + + rcl_jump_threshold_t jump_threshold; + // Disable forward jump callbacks + jump_threshold.min_forward.nanoseconds = 0; + // Anything backwards is a jump + jump_threshold.min_backward.nanoseconds = -1; + // Callback if the clock changes too + jump_threshold.on_clock_change = true; + + jump_handler_ = clock_->create_jump_callback(nullptr, post_jump_cb, jump_threshold); + + if (node) { + node_logging_interface_ = rclcpp::node_interfaces::get_node_logging_interface(node); + + frames_server_ = rclcpp::create_service( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_services_interface(node), + "tf2_frames", std::bind( + &Buffer::getFrames, this, std::placeholders::_1, + std::placeholders::_2), qos, nullptr); + } + } + + /** \brief Get the transform between two frames by frame ID. + * \param target_frame The frame to which data should be transformed + * \param source_frame The frame where the data originated + * \param time The time at which the value of the transform is desired. (0 will get the latest) + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, const tf2::Duration timeout) const override; + + /** \brief Get the transform between two frames by frame ID. + * \sa lookupTransform(const std::string&, const std::string&, const tf2::TimePoint&, + * const tf2::Duration) + */ + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const rclcpp::Time & time, + const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0)) const + { + return lookupTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout)); + } + + /** \brief Get the transform between two frames by frame ID assuming fixed frame. + * \param target_frame The frame to which data should be transformed + * \param target_time The time to which the data should be transformed. (0 will get the latest) + * \param source_frame The frame where the data originated + * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) + * \param fixed_frame The frame in which to assume the transform is constant in time. + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const tf2::TimePoint & target_time, + const std::string & source_frame, const tf2::TimePoint & source_time, + const std::string & fixed_frame, const tf2::Duration timeout) const override; + + /** \brief Get the transform between two frames by frame ID assuming fixed frame. + * \sa lookupTransform(const std::string&, const tf2::TimePoint&, + * const std::string&, const tf2::TimePoint&, + * const std::string&, const tf2::Duration) + */ + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const rclcpp::Time & target_time, + const std::string & source_frame, const rclcpp::Time & source_time, + const std::string & fixed_frame, + const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0)) const + { + return lookupTransform( + target_frame, fromRclcpp(target_time), + source_frame, fromRclcpp(source_time), + fixed_frame, fromRclcpp(timeout)); + } + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param source_frame The frame from which to transform + * \param target_time The time at which to transform + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + TF2_ROS_PUBLIC + bool + canTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & target_time, const tf2::Duration timeout, + std::string * errstr = NULL) const override; + + /** \brief Test if a transform is possible + * \sa canTransform(const std::string&, const std::string&, + * const tf2::TimePoint&, const tf2::Duration, std::string*) + */ + TF2_ROS_PUBLIC + bool + canTransform( + const std::string & target_frame, const std::string & source_frame, + const rclcpp::Time & time, + const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), + std::string * errstr = NULL) const + { + return canTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout), errstr); + } + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param target_time The time into which to transform + * \param source_frame The frame from which to transform + * \param source_time The time from which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + TF2_ROS_PUBLIC + bool + canTransform( + const std::string & target_frame, const tf2::TimePoint & target_time, + const std::string & source_frame, const tf2::TimePoint & source_time, + const std::string & fixed_frame, const tf2::Duration timeout, + std::string * errstr = NULL) const override; + + /** \brief Test if a transform is possible + * \sa + * canTransform(const std::string&, const tf2::TimePoint&, + * const std::string&, const tf2::TimePoint&, + * const std::string&, const tf2::Duration, std::string*) + */ + TF2_ROS_PUBLIC + bool + canTransform( + const std::string & target_frame, const rclcpp::Time & target_time, + const std::string & source_frame, const rclcpp::Time & source_time, + const std::string & fixed_frame, + const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), + std::string * errstr = NULL) const + { + return canTransform( + target_frame, fromRclcpp(target_time), + source_frame, fromRclcpp(source_time), + fixed_frame, fromRclcpp(timeout), + errstr); + } + + /** \brief Wait for a transform between two frames to become available. + * + * Before this method can be called, a tf2_ros::CreateTimerInterface must be registered + * by first calling setCreateTimerInterface. + * If no tf2_ros::CreateTimerInterface is set, then a tf2_ros::CreateTimerInterfaceException + * is thrown. + * + * \param target_frame The frame into which to transform. + * \param source_frame The frame from which to tranform. + * \param time The time at which to transform. + * \param timeout Duration after which waiting will be stopped. + * \param callback The function to be called when the transform becomes available or a timeout + * occurs. In the case of timeout, an exception will be set on the future. + * \return A future to the requested transform. If a timeout occurs a `tf2::LookupException` + * will be set on the future. + */ + TF2_ROS_PUBLIC + TransformStampedFuture + waitForTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, const tf2::Duration & timeout, + TransformReadyCallback callback) override; + + /** \brief Wait for a transform between two frames to become available. + * \sa waitForTransform(const std::string &, const std::string &, const tf2::TimePoint &, + * const tf2::Duration &, TransformReadyCallback); + */ + TF2_ROS_PUBLIC + TransformStampedFuture + waitForTransform( + const std::string & target_frame, const std::string & source_frame, + const rclcpp::Time & time, + const rclcpp::Duration & timeout, TransformReadyCallback callback) + { + return waitForTransform( + target_frame, source_frame, + fromRclcpp(time), fromRclcpp(timeout), + callback); + } + + /** + * \brief Cancel the future to make sure the callback of requested transform is clean. + * \param ts_future The future to the requested transform. + */ + TF2_ROS_PUBLIC + void + cancel(const TransformStampedFuture & ts_future) override; + + TF2_ROS_PUBLIC + inline void + setCreateTimerInterface(CreateTimerInterface::SharedPtr create_timer_interface) + { + timer_interface_ = create_timer_interface; + } + +private: + void timerCallback( + const TimerHandle & timer_handle, + std::shared_ptr> promise, + TransformStampedFuture future, + TransformReadyCallback callback); + + TF2_ROS_PUBLIC + bool getFrames( + const tf2_msgs::srv::FrameGraph::Request::SharedPtr req, + tf2_msgs::srv::FrameGraph::Response::SharedPtr res); + + TF2_ROS_PUBLIC + void onTimeJump(const rcl_time_jump_t & jump); + + // conditionally error if dedicated_thread unset. + bool checkAndErrorDedicatedThreadPresent(std::string * errstr) const; + + /// Get the logger to use for calls to RCLCPP log macros. + rclcpp::Logger getLogger() const; + + // framegraph service + rclcpp::Service::SharedPtr frames_server_; + + /// \brief A clock to use for time and sleeping + rclcpp::Clock::SharedPtr clock_; + + /// \brief A node logging interface to access the buffer node's logger + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; + + /// \brief Interface for creating timers + CreateTimerInterface::SharedPtr timer_interface_; + + /// \brief A map from active timers to BufferCore request handles + std::unordered_map timer_to_request_map_; + + /// \brief A mutex on the timer_to_request_map_ data + std::mutex timer_to_request_map_mutex_; + + /// \brief Reference to a jump handler registered to the clock + rclcpp::JumpHandler::SharedPtr jump_handler_; +}; + +static const char threading_error[] = "Do not call canTransform or lookupTransform with a timeout " + "unless you are using another thread for populating data. Without a dedicated thread it will " + "always timeout. If you have a separate thread servicing tf messages, call " + "setUsingDedicatedThread(true) on your Buffer instance."; + +} // namespace tf2_ros + +#endif // TF2_ROS__BUFFER_HPP_ diff --git a/tf2_ros/include/tf2_ros/buffer_client.h b/tf2_ros/include/tf2_ros/buffer_client.h index 5265d4bc2..a09396fe1 100644 --- a/tf2_ros/include/tf2_ros/buffer_client.h +++ b/tf2_ros/include/tf2_ros/buffer_client.h @@ -38,226 +38,6 @@ #ifndef TF2_ROS__BUFFER_CLIENT_H_ #define TF2_ROS__BUFFER_CLIENT_H_ -#include -#include - -#include "tf2_ros/buffer_interface.h" -#include "tf2_ros/visibility_control.h" -#include "tf2/time.hpp" - -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_msgs/action/lookup_transform.hpp" - -namespace tf2_ros -{ -/** - * \ brief Base class for lookup transform action goal exceptions. - */ -class LookupTransformGoalException : public std::runtime_error -{ -public: - TF2_ROS_PUBLIC - explicit LookupTransformGoalException(const std::string & message) - : std::runtime_error(message) - { - } -}; - -class GoalRejectedException : public LookupTransformGoalException -{ -public: - TF2_ROS_PUBLIC - explicit GoalRejectedException(const std::string & message) - : LookupTransformGoalException(message) - { - } -}; - -class GoalAbortedException : public LookupTransformGoalException -{ -public: - TF2_ROS_PUBLIC - explicit GoalAbortedException(const std::string & message) - : LookupTransformGoalException(message) - { - } -}; - -class GoalCanceledException : public LookupTransformGoalException -{ -public: - TF2_ROS_PUBLIC - explicit GoalCanceledException(const std::string & message) - : LookupTransformGoalException(message) - { - } -}; - -class UnexpectedResultCodeException : public LookupTransformGoalException -{ -public: - TF2_ROS_PUBLIC - explicit UnexpectedResultCodeException(const std::string & message) - : LookupTransformGoalException(message) - { - } -}; - -/** \brief Action client-based implementation of the tf2_ros::BufferInterface abstract data type. - * - * BufferClient uses actions to coordinate waiting for available transforms. - * - * You can use this class with a tf2_ros::BufferServer and tf2_ros::TransformListener in a separate process. - */ -class BufferClient : public BufferInterface -{ -public: - using LookupTransformAction = tf2_msgs::action::LookupTransform; - - /** \brief BufferClient constructor - * \param node The node to add the buffer client to - * \param ns The namespace in which to look for a BufferServer - * \param check_frequency The frequency in Hz to check whether the BufferServer has completed a request - * \param timeout_padding The amount of time to allow passed the desired timeout on the client side for communication lag - */ - template - BufferClient( - NodePtr node, - const std::string ns, - const double & check_frequency = 10.0, - const tf2::Duration & timeout_padding = tf2::durationFromSec(2.0)) - : check_frequency_(check_frequency), - timeout_padding_(timeout_padding) - { - client_ = rclcpp_action::create_client(node, ns); - } - - virtual ~BufferClient() = default; - - /** \brief Get the transform between two frames by frame ID. - * - * If there is a communication failure, timeout, or transformation error, - * an exception is thrown. - * - * \param target_frame The frame to which data should be transformed - * \param source_frame The frame where the data originated - * \param time The time at which the value of the transform is desired. (0 will get the latest) - * \param timeout How long to block before failing - * \return The transform between the frames - * - * \throws tf2::TransformException One of the following - * - tf2::LookupException - * - tf2::ConnectivityException - * - tf2::ExtrapolationException - * - tf2::InvalidArgumentException - * \throws tf2_ros::LookupTransformGoalException One of the following - * - tf2_ros::GoalRejectedException - * - tf2_ros::GoalAbortedException - * - tf2_ros::GoalCanceledException - * - tf2_ros::UnexpectedResultCodeException - */ - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, - const std::string & source_frame, - const tf2::TimePoint & time, - const tf2::Duration timeout = tf2::durationFromSec(0.0)) const override; - - /** \brief Get the transform between two frames by frame ID assuming fixed frame. - * - * If there is a communication failure, timeout, or transformation error, - * an exception is thrown. - * - * \param target_frame The frame to which data should be transformed - * \param target_time The time to which the data should be transformed. (0 will get the latest) - * \param source_frame The frame where the data originated - * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) - * \param fixed_frame The frame in which to assume the transform is constant in time. - * \param timeout How long to block before failing - * \return The transform between the frames - * - * \throws tf2::TransformException One of the following - * - tf2::LookupException - * - tf2::ConnectivityException - * - tf2::ExtrapolationException - * - tf2::InvalidArgumentException - * \throws tf2_ros::LookupTransformGoalException One of the following - * - tf2_ros::GoalRejectedException - * - tf2_ros::GoalAbortedException - * - tf2_ros::GoalCanceledException - * - tf2_ros::UnexpectedResultCodeException - */ - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, - const tf2::TimePoint & target_time, - const std::string & source_frame, - const tf2::TimePoint & source_time, - const std::string & fixed_frame, - const tf2::Duration timeout = tf2::durationFromSec(0.0)) const override; - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param source_frame The frame from which to transform - * \param time The time at which to transform - * \param timeout How long to block before failing - * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - TF2_ROS_PUBLIC - bool - canTransform( - const std::string & target_frame, - const std::string & source_frame, - const tf2::TimePoint & time, - const tf2::Duration timeout = tf2::durationFromSec(0.0), - std::string * errstr = nullptr) const override; - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param target_time The time into which to transform - * \param source_frame The frame from which to transform - * \param source_time The time from which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time - * \param timeout How long to block before failing - * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - TF2_ROS_PUBLIC - bool - canTransform( - const std::string & target_frame, - const tf2::TimePoint & target_time, - const std::string & source_frame, - const tf2::TimePoint & source_time, - const std::string & fixed_frame, - const tf2::Duration timeout = tf2::durationFromSec(0.0), - std::string * errstr = nullptr) const override; - - /** \brief Block until the action server is ready to respond to requests. - * \param timeout Time to wait for the server. - * \return True if the server is ready, false otherwise. - */ - TF2_ROS_PUBLIC - bool waitForServer(const tf2::Duration & timeout = tf2::durationFromSec(0)) - { - return client_->wait_for_action_server(timeout); - } - -private: - geometry_msgs::msg::TransformStamped - processGoal(const LookupTransformAction::Goal & goal) const; - - geometry_msgs::msg::TransformStamped - processResult(const LookupTransformAction::Result::SharedPtr & result) const; - - rclcpp_action::Client::SharedPtr client_; - double check_frequency_; - tf2::Duration timeout_padding_; -}; -} // namespace tf2_ros +#include #endif // TF2_ROS__BUFFER_CLIENT_H_ diff --git a/tf2_ros/include/tf2_ros/buffer_client.hpp b/tf2_ros/include/tf2_ros/buffer_client.hpp new file mode 100644 index 000000000..5d2a2c723 --- /dev/null +++ b/tf2_ros/include/tf2_ros/buffer_client.hpp @@ -0,0 +1,263 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ + +#ifndef TF2_ROS__BUFFER_CLIENT_HPP_ +#define TF2_ROS__BUFFER_CLIENT_HPP_ + +#include +#include + +#include "tf2_ros/buffer_interface.hpp" +#include "tf2_ros/visibility_control.hpp" +#include "tf2/time.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_msgs/action/lookup_transform.hpp" + +namespace tf2_ros +{ +/** + * \ brief Base class for lookup transform action goal exceptions. + */ +class LookupTransformGoalException : public std::runtime_error +{ +public: + TF2_ROS_PUBLIC + explicit LookupTransformGoalException(const std::string & message) + : std::runtime_error(message) + { + } +}; + +class GoalRejectedException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit GoalRejectedException(const std::string & message) + : LookupTransformGoalException(message) + { + } +}; + +class GoalAbortedException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit GoalAbortedException(const std::string & message) + : LookupTransformGoalException(message) + { + } +}; + +class GoalCanceledException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit GoalCanceledException(const std::string & message) + : LookupTransformGoalException(message) + { + } +}; + +class UnexpectedResultCodeException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit UnexpectedResultCodeException(const std::string & message) + : LookupTransformGoalException(message) + { + } +}; + +/** \brief Action client-based implementation of the tf2_ros::BufferInterface abstract data type. + * + * BufferClient uses actions to coordinate waiting for available transforms. + * + * You can use this class with a tf2_ros::BufferServer and tf2_ros::TransformListener in a separate process. + */ +class BufferClient : public BufferInterface +{ +public: + using LookupTransformAction = tf2_msgs::action::LookupTransform; + + /** \brief BufferClient constructor + * \param node The node to add the buffer client to + * \param ns The namespace in which to look for a BufferServer + * \param check_frequency The frequency in Hz to check whether the BufferServer has completed a request + * \param timeout_padding The amount of time to allow passed the desired timeout on the client side for communication lag + */ + template + BufferClient( + NodePtr node, + const std::string ns, + const double & check_frequency = 10.0, + const tf2::Duration & timeout_padding = tf2::durationFromSec(2.0)) + : check_frequency_(check_frequency), + timeout_padding_(timeout_padding) + { + client_ = rclcpp_action::create_client(node, ns); + } + + virtual ~BufferClient() = default; + + /** \brief Get the transform between two frames by frame ID. + * + * If there is a communication failure, timeout, or transformation error, + * an exception is thrown. + * + * \param target_frame The frame to which data should be transformed + * \param source_frame The frame where the data originated + * \param time The time at which the value of the transform is desired. (0 will get the latest) + * \param timeout How long to block before failing + * \return The transform between the frames + * + * \throws tf2::TransformException One of the following + * - tf2::LookupException + * - tf2::ConnectivityException + * - tf2::ExtrapolationException + * - tf2::InvalidArgumentException + * \throws tf2_ros::LookupTransformGoalException One of the following + * - tf2_ros::GoalRejectedException + * - tf2_ros::GoalAbortedException + * - tf2_ros::GoalCanceledException + * - tf2_ros::UnexpectedResultCodeException + */ + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration timeout = tf2::durationFromSec(0.0)) const override; + + /** \brief Get the transform between two frames by frame ID assuming fixed frame. + * + * If there is a communication failure, timeout, or transformation error, + * an exception is thrown. + * + * \param target_frame The frame to which data should be transformed + * \param target_time The time to which the data should be transformed. (0 will get the latest) + * \param source_frame The frame where the data originated + * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) + * \param fixed_frame The frame in which to assume the transform is constant in time. + * \param timeout How long to block before failing + * \return The transform between the frames + * + * \throws tf2::TransformException One of the following + * - tf2::LookupException + * - tf2::ConnectivityException + * - tf2::ExtrapolationException + * - tf2::InvalidArgumentException + * \throws tf2_ros::LookupTransformGoalException One of the following + * - tf2_ros::GoalRejectedException + * - tf2_ros::GoalAbortedException + * - tf2_ros::GoalCanceledException + * - tf2_ros::UnexpectedResultCodeException + */ + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, + const tf2::TimePoint & target_time, + const std::string & source_frame, + const tf2::TimePoint & source_time, + const std::string & fixed_frame, + const tf2::Duration timeout = tf2::durationFromSec(0.0)) const override; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param source_frame The frame from which to transform + * \param time The time at which to transform + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + TF2_ROS_PUBLIC + bool + canTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration timeout = tf2::durationFromSec(0.0), + std::string * errstr = nullptr) const override; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param target_time The time into which to transform + * \param source_frame The frame from which to transform + * \param source_time The time from which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + TF2_ROS_PUBLIC + bool + canTransform( + const std::string & target_frame, + const tf2::TimePoint & target_time, + const std::string & source_frame, + const tf2::TimePoint & source_time, + const std::string & fixed_frame, + const tf2::Duration timeout = tf2::durationFromSec(0.0), + std::string * errstr = nullptr) const override; + + /** \brief Block until the action server is ready to respond to requests. + * \param timeout Time to wait for the server. + * \return True if the server is ready, false otherwise. + */ + TF2_ROS_PUBLIC + bool waitForServer(const tf2::Duration & timeout = tf2::durationFromSec(0)) + { + return client_->wait_for_action_server(timeout); + } + +private: + geometry_msgs::msg::TransformStamped + processGoal(const LookupTransformAction::Goal & goal) const; + + geometry_msgs::msg::TransformStamped + processResult(const LookupTransformAction::Result::SharedPtr & result) const; + + rclcpp_action::Client::SharedPtr client_; + double check_frequency_; + tf2::Duration timeout_padding_; +}; +} // namespace tf2_ros + +#endif // TF2_ROS__BUFFER_CLIENT_HPP_ diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index 34d2b5b0c..ff14ca8ea 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -32,330 +32,6 @@ #ifndef TF2_ROS__BUFFER_INTERFACE_H_ #define TF2_ROS__BUFFER_INTERFACE_H_ -#include -#include -#include -#include - -#include "tf2_ros/visibility_control.h" -#include "tf2/transform_datatypes.hpp" -#include "tf2/exceptions.hpp" -#include "tf2/convert.hpp" - -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace tf2_ros -{ - -inline builtin_interfaces::msg::Time toMsg(const tf2::TimePoint & t) -{ - std::chrono::nanoseconds ns = - std::chrono::duration_cast(t.time_since_epoch()); - std::chrono::seconds s = - std::chrono::duration_cast(t.time_since_epoch()); - builtin_interfaces::msg::Time time_msg; - time_msg.sec = static_cast(s.count()); - time_msg.nanosec = static_cast(ns.count() % 1000000000ull); - return time_msg; -} - -inline tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg) -{ - int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; - std::chrono::nanoseconds ns(d); - return tf2::TimePoint(std::chrono::duration_cast(ns)); -} - -inline builtin_interfaces::msg::Duration toMsg(const tf2::Duration & t) -{ - std::chrono::nanoseconds ns = - std::chrono::duration_cast(t); - std::chrono::seconds s = - std::chrono::duration_cast(t); - builtin_interfaces::msg::Duration duration_msg; - duration_msg.sec = static_cast(s.count()); - duration_msg.nanosec = static_cast(ns.count() % 1000000000ull); - return duration_msg; -} - -inline tf2::Duration fromMsg(const builtin_interfaces::msg::Duration & duration_msg) -{ - int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec; - std::chrono::nanoseconds ns(d); - return tf2::Duration(std::chrono::duration_cast(ns)); -} - -inline double timeToSec(const builtin_interfaces::msg::Time & time_msg) -{ - auto ns = std::chrono::duration(time_msg.nanosec); - auto s = std::chrono::duration(time_msg.sec); - return (s + std::chrono::duration_cast>(ns)).count(); -} - -inline tf2::TimePoint fromRclcpp(const rclcpp::Time & time) -{ - // tf2::TimePoint is a typedef to a system time point, but rclcpp::Time may be ROS time. - // Ignore that, and assume the clock used from rclcpp time points is consistent. - return tf2::TimePoint(std::chrono::nanoseconds(time.nanoseconds())); -} - -inline rclcpp::Time toRclcpp(const tf2::TimePoint & time) -{ - // tf2::TimePoint is a typedef to a system time point, but rclcpp::Time may be ROS time. - // Use whatever the default clock is. - return rclcpp::Time(std::chrono::nanoseconds(time.time_since_epoch()).count()); -} - -inline tf2::Duration fromRclcpp(const rclcpp::Duration & duration) -{ - return tf2::Duration(std::chrono::nanoseconds(duration.nanoseconds())); -} - -inline rclcpp::Duration toRclcpp(const tf2::Duration & duration) -{ - return rclcpp::Duration(std::chrono::duration_cast(duration)); -} - -/** \brief Abstract interface for wrapping tf2::BufferCoreInterface in a ROS-based API. - * Implementations include tf2_ros::Buffer and tf2_ros::BufferClient. - */ -class BufferInterface -{ -public: - /** \brief Get the transform between two frames by frame ID. - * \param target_frame The frame to which data should be transformed - * \param source_frame The frame where the data originated - * \param time The time at which the value of the transform is desired. (0 will get the latest) - * \param timeout How long to block before failing - * \return The transform between the frames - * - * Possible exceptions tf2::LookupException, tf2::ConnectivityException, - * tf2::ExtrapolationException, tf2::InvalidArgumentException - */ - TF2_ROS_PUBLIC - virtual geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, const tf2::Duration timeout) const = 0; - - /** \brief Get the transform between two frames by frame ID assuming fixed frame. - * \param target_frame The frame to which data should be transformed - * \param target_time The time to which the data should be transformed. (0 will get the latest) - * \param source_frame The frame where the data originated - * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) - * \param fixed_frame The frame in which to assume the transform is constant in time. - * \param timeout How long to block before failing - * \return The transform between the frames - * - * Possible exceptions tf2::LookupException, tf2::ConnectivityException, - * tf2::ExtrapolationException, tf2::InvalidArgumentException - */ - TF2_ROS_PUBLIC - virtual geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const tf2::TimePoint & target_time, - const std::string & source_frame, const tf2::TimePoint & source_time, - const std::string & fixed_frame, const tf2::Duration timeout) const = 0; - - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param source_frame The frame from which to transform - * \param time The time at which to transform - * \param timeout How long to block before failing - * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - TF2_ROS_PUBLIC - virtual bool - canTransform( - const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, const tf2::Duration timeout, - std::string * errstr = NULL) const = 0; - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param target_time The time into which to transform - * \param source_frame The frame from which to transform - * \param source_time The time from which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time - * \param timeout How long to block before failing - * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL - * \return True if the transform is possible, false otherwise - */ - TF2_ROS_PUBLIC - virtual bool - canTransform( - const std::string & target_frame, const tf2::TimePoint & target_time, - const std::string & source_frame, const tf2::TimePoint & source_time, - const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr = NULL) const = 0; - - /** \brief Transform an input into the target frame. - * This function is templated and can take as input any valid mathematical object that tf knows - * how to apply a transform to, by way of the templated math conversions interface. - * For example, the template type could be a Transform, Pose, Vector, or Quaternion message - * type (as defined in geometry_msgs). - * \tparam T The type of the object to transform. - * \param in The object to transform - * \param out The transformed output, preallocated by the caller. - * \param target_frame The string identifer for the frame to transform into. - * \param timeout How long to wait for the target frame. Default value is zero (no blocking). - */ - template - T & transform( - const T & in, T & out, - const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const - { - // do the transform - tf2::doTransform( - in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout)); - return out; - } - - /** \brief Transform an input into the target frame. - * This function is templated and can take as input any valid mathematical object that tf knows - * how to apply a transform to, by way of the templated math conversions interface. - * For example, the template type could be a Transform, Pose, Vector, or Quaternion message - * type (as defined in geometry_msgs). - * \tparam T The type of the object to transform. - * \param in The object to transform. - * \param target_frame The string identifer for the frame to transform into. - * \param timeout How long to wait for the target frame. Default value is zero (no blocking). - * \return The transformed output. - */ - template - T transform( - const T & in, - const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const - { - T out; - return this->transform(in, out, target_frame, timeout); - } - - /** \brief Transform an input into the target frame and convert to a specified output type. - * It is templated on two types: the type of the input object and the type of the - * transformed output. - * For example, the template types could be Transform, Pose, Vector, or Quaternion messages - * type (as defined in geometry_msgs). - * The function will calculate the transformation and then convert the result into the - * specified output type. - * Compilation will fail if a known conversion does not exist bewteen the two template - * parameters. - * \tparam A The type of the object to transform. - * \tparam B The type of the transformed output. - * \param in The object to transform - * \param out The transformed output, converted to the specified type. - * \param target_frame The string identifer for the frame to transform into. - * \param timeout How long to wait for the target frame. Default value is zero (no blocking). - * \return The transformed output, converted to the specified type. - */ - template - B & transform( - const A & in, B & out, - const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const - { - A copy = this->transform(in, target_frame, timeout); - tf2::convert(copy, out); - return out; - } - - /** \brief Transform an input into the target frame (advanced). - * This function is templated and can take as input any valid mathematical object that tf knows - * how to apply a transform to, by way of the templated math conversions interface. - * For example, the template type could be a Transform, Pose, Vector, or Quaternion message - * type (as defined in geometry_msgs). - * This function follows the advanced API, which allows transforming between different time - * points, and specifying a fixed frame that does not varying in time. - * \tparam T The type of the object to transform. - * \param in The object to transform - * \param out The transformed output, preallocated by the caller. - * \param target_frame The string identifer for the frame to transform into. - * \param target_time The time into which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time. - * \param timeout How long to wait for the target frame. Default value is zero (no blocking). - */ - template - T & transform( - const T & in, T & out, - const std::string & target_frame, const tf2::TimePoint & target_time, - const std::string & fixed_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const - { - // do the transform - tf2::doTransform( - in, out, lookupTransform( - target_frame, target_time, - tf2::getFrameId(in), tf2::getTimestamp(in), - fixed_frame, timeout)); - return out; - } - - /** \brief Transform an input into the target frame (advanced). - * This function is templated and can take as input any valid mathematical object that tf knows - * how to apply a transform to, by way of the templated math conversions interface. - * For example, the template type could be a Transform, Pose, Vector, or Quaternion message - * type (as defined in geometry_msgs). - * This function follows the advanced API, which allows transforming between different time - * points, and specifying a fixed frame that does not varying in time. - * \tparam T The type of the object to transform. - * \param in The object to transform - * \param target_frame The string identifer for the frame to transform into. - * \param target_time The time into which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time. - * \param timeout How long to wait for the target frame. Default value is zero (no blocking). - * \return The transformed output. - */ - template - T transform( - const T & in, - const std::string & target_frame, const tf2::TimePoint & target_time, - const std::string & fixed_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const - { - T out; - return this->transform(in, out, target_frame, target_time, fixed_frame, timeout); - } - - /** \brief Transform an input into the target frame and convert to a specified output type (advanced). - * It is templated on two types: the type of the input object and the type of the - * transformed output. - * For example, the template type could be a Transform, Pose, Vector, or Quaternion message - * type (as defined in geometry_msgs). - * The function will calculate the transformation and then convert the result into the - * specified output type. - * Compilation will fail if a known conversion does not exist bewteen the two template - * parameters. - * This function follows the advanced API, which allows transforming between different time - * points, and specifying a fixed frame that does not varying in time. - * \tparam A The type of the object to transform. - * \tparam B The type of the transformed output. - * \param in The object to transform - * \param out The transformed output, converted to the specified output type. - * \param target_frame The string identifer for the frame to transform into. - * \param target_time The time into which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time. - * \param timeout How long to wait for the target frame. Default value is zero (no blocking). - * \return The transformed output, converted to the specified output type. - */ - template - B & transform( - const A & in, B & out, - const std::string & target_frame, const tf2::TimePoint & target_time, - const std::string & fixed_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const - { - // do the transform - A copy = this->transform(in, target_frame, target_time, fixed_frame, timeout); - tf2::convert(copy, out); - return out; - } - - virtual ~BufferInterface() - { - } -}; - -} // namespace tf2_ros +#include #endif // TF2_ROS__BUFFER_INTERFACE_H_ diff --git a/tf2_ros/include/tf2_ros/buffer_interface.hpp b/tf2_ros/include/tf2_ros/buffer_interface.hpp new file mode 100644 index 000000000..0fbdb7957 --- /dev/null +++ b/tf2_ros/include/tf2_ros/buffer_interface.hpp @@ -0,0 +1,361 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + +#ifndef TF2_ROS__BUFFER_INTERFACE_HPP_ +#define TF2_ROS__BUFFER_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "tf2_ros/visibility_control.hpp" +#include "tf2/transform_datatypes.hpp" +#include "tf2/exceptions.hpp" +#include "tf2/convert.hpp" + +#include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace tf2_ros +{ + +inline builtin_interfaces::msg::Time toMsg(const tf2::TimePoint & t) +{ + std::chrono::nanoseconds ns = + std::chrono::duration_cast(t.time_since_epoch()); + std::chrono::seconds s = + std::chrono::duration_cast(t.time_since_epoch()); + builtin_interfaces::msg::Time time_msg; + time_msg.sec = static_cast(s.count()); + time_msg.nanosec = static_cast(ns.count() % 1000000000ull); + return time_msg; +} + +inline tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg) +{ + int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; + std::chrono::nanoseconds ns(d); + return tf2::TimePoint(std::chrono::duration_cast(ns)); +} + +inline builtin_interfaces::msg::Duration toMsg(const tf2::Duration & t) +{ + std::chrono::nanoseconds ns = + std::chrono::duration_cast(t); + std::chrono::seconds s = + std::chrono::duration_cast(t); + builtin_interfaces::msg::Duration duration_msg; + duration_msg.sec = static_cast(s.count()); + duration_msg.nanosec = static_cast(ns.count() % 1000000000ull); + return duration_msg; +} + +inline tf2::Duration fromMsg(const builtin_interfaces::msg::Duration & duration_msg) +{ + int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec; + std::chrono::nanoseconds ns(d); + return tf2::Duration(std::chrono::duration_cast(ns)); +} + +inline double timeToSec(const builtin_interfaces::msg::Time & time_msg) +{ + auto ns = std::chrono::duration(time_msg.nanosec); + auto s = std::chrono::duration(time_msg.sec); + return (s + std::chrono::duration_cast>(ns)).count(); +} + +inline tf2::TimePoint fromRclcpp(const rclcpp::Time & time) +{ + // tf2::TimePoint is a typedef to a system time point, but rclcpp::Time may be ROS time. + // Ignore that, and assume the clock used from rclcpp time points is consistent. + return tf2::TimePoint(std::chrono::nanoseconds(time.nanoseconds())); +} + +inline rclcpp::Time toRclcpp(const tf2::TimePoint & time) +{ + // tf2::TimePoint is a typedef to a system time point, but rclcpp::Time may be ROS time. + // Use whatever the default clock is. + return rclcpp::Time(std::chrono::nanoseconds(time.time_since_epoch()).count()); +} + +inline tf2::Duration fromRclcpp(const rclcpp::Duration & duration) +{ + return tf2::Duration(std::chrono::nanoseconds(duration.nanoseconds())); +} + +inline rclcpp::Duration toRclcpp(const tf2::Duration & duration) +{ + return rclcpp::Duration(std::chrono::duration_cast(duration)); +} + +/** \brief Abstract interface for wrapping tf2::BufferCoreInterface in a ROS-based API. + * Implementations include tf2_ros::Buffer and tf2_ros::BufferClient. + */ +class BufferInterface +{ +public: + /** \brief Get the transform between two frames by frame ID. + * \param target_frame The frame to which data should be transformed + * \param source_frame The frame where the data originated + * \param time The time at which the value of the transform is desired. (0 will get the latest) + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + TF2_ROS_PUBLIC + virtual geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, const tf2::Duration timeout) const = 0; + + /** \brief Get the transform between two frames by frame ID assuming fixed frame. + * \param target_frame The frame to which data should be transformed + * \param target_time The time to which the data should be transformed. (0 will get the latest) + * \param source_frame The frame where the data originated + * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) + * \param fixed_frame The frame in which to assume the transform is constant in time. + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + TF2_ROS_PUBLIC + virtual geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const tf2::TimePoint & target_time, + const std::string & source_frame, const tf2::TimePoint & source_time, + const std::string & fixed_frame, const tf2::Duration timeout) const = 0; + + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param source_frame The frame from which to transform + * \param time The time at which to transform + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + TF2_ROS_PUBLIC + virtual bool + canTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, const tf2::Duration timeout, + std::string * errstr = NULL) const = 0; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param target_time The time into which to transform + * \param source_frame The frame from which to transform + * \param source_time The time from which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + TF2_ROS_PUBLIC + virtual bool + canTransform( + const std::string & target_frame, const tf2::TimePoint & target_time, + const std::string & source_frame, const tf2::TimePoint & source_time, + const std::string & fixed_frame, const tf2::Duration timeout, + std::string * errstr = NULL) const = 0; + + /** \brief Transform an input into the target frame. + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * \tparam T The type of the object to transform. + * \param in The object to transform + * \param out The transformed output, preallocated by the caller. + * \param target_frame The string identifer for the frame to transform into. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + */ + template + T & transform( + const T & in, T & out, + const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const + { + // do the transform + tf2::doTransform( + in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout)); + return out; + } + + /** \brief Transform an input into the target frame. + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * \tparam T The type of the object to transform. + * \param in The object to transform. + * \param target_frame The string identifer for the frame to transform into. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output. + */ + template + T transform( + const T & in, + const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const + { + T out; + return this->transform(in, out, target_frame, timeout); + } + + /** \brief Transform an input into the target frame and convert to a specified output type. + * It is templated on two types: the type of the input object and the type of the + * transformed output. + * For example, the template types could be Transform, Pose, Vector, or Quaternion messages + * type (as defined in geometry_msgs). + * The function will calculate the transformation and then convert the result into the + * specified output type. + * Compilation will fail if a known conversion does not exist bewteen the two template + * parameters. + * \tparam A The type of the object to transform. + * \tparam B The type of the transformed output. + * \param in The object to transform + * \param out The transformed output, converted to the specified type. + * \param target_frame The string identifer for the frame to transform into. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output, converted to the specified type. + */ + template + B & transform( + const A & in, B & out, + const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const + { + A copy = this->transform(in, target_frame, timeout); + tf2::convert(copy, out); + return out; + } + + /** \brief Transform an input into the target frame (advanced). + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * This function follows the advanced API, which allows transforming between different time + * points, and specifying a fixed frame that does not varying in time. + * \tparam T The type of the object to transform. + * \param in The object to transform + * \param out The transformed output, preallocated by the caller. + * \param target_frame The string identifer for the frame to transform into. + * \param target_time The time into which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + */ + template + T & transform( + const T & in, T & out, + const std::string & target_frame, const tf2::TimePoint & target_time, + const std::string & fixed_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const + { + // do the transform + tf2::doTransform( + in, out, lookupTransform( + target_frame, target_time, + tf2::getFrameId(in), tf2::getTimestamp(in), + fixed_frame, timeout)); + return out; + } + + /** \brief Transform an input into the target frame (advanced). + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * This function follows the advanced API, which allows transforming between different time + * points, and specifying a fixed frame that does not varying in time. + * \tparam T The type of the object to transform. + * \param in The object to transform + * \param target_frame The string identifer for the frame to transform into. + * \param target_time The time into which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output. + */ + template + T transform( + const T & in, + const std::string & target_frame, const tf2::TimePoint & target_time, + const std::string & fixed_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const + { + T out; + return this->transform(in, out, target_frame, target_time, fixed_frame, timeout); + } + + /** \brief Transform an input into the target frame and convert to a specified output type (advanced). + * It is templated on two types: the type of the input object and the type of the + * transformed output. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * The function will calculate the transformation and then convert the result into the + * specified output type. + * Compilation will fail if a known conversion does not exist bewteen the two template + * parameters. + * This function follows the advanced API, which allows transforming between different time + * points, and specifying a fixed frame that does not varying in time. + * \tparam A The type of the object to transform. + * \tparam B The type of the transformed output. + * \param in The object to transform + * \param out The transformed output, converted to the specified output type. + * \param target_frame The string identifer for the frame to transform into. + * \param target_time The time into which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output, converted to the specified output type. + */ + template + B & transform( + const A & in, B & out, + const std::string & target_frame, const tf2::TimePoint & target_time, + const std::string & fixed_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const + { + // do the transform + A copy = this->transform(in, target_frame, target_time, fixed_frame, timeout); + tf2::convert(copy, out); + return out; + } + + virtual ~BufferInterface() + { + } +}; + +} // namespace tf2_ros + +#endif // TF2_ROS__BUFFER_INTERFACE_HPP_ diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 401f84300..54db2b9fd 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -38,96 +38,6 @@ #ifndef TF2_ROS__BUFFER_SERVER_H_ #define TF2_ROS__BUFFER_SERVER_H_ -#include -#include -#include -#include -#include - -#include "tf2/time.hpp" -#include "tf2/buffer_core_interface.hpp" -#include "tf2_ros/visibility_control.h" - -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "rclcpp/create_timer.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_msgs/action/lookup_transform.hpp" - -namespace tf2_ros -{ -/** \brief Action server for the action-based implementation of tf2::BufferCoreInterface. - * - * Use this class with a tf2_ros::TransformListener in the same process. - * You can use this class with a tf2_ros::BufferClient in a different process. - */ -class BufferServer -{ - using LookupTransformAction = tf2_msgs::action::LookupTransform; - using GoalHandle = std::shared_ptr>; - -public: - /** \brief Constructor - * \param buffer The Buffer that this BufferServer will wrap. - * \param node The node to add the buffer server to. - * \param ns The namespace in which to look for action clients. - * \param check_period How often to check for changes to known transforms (via a timer event). - */ - template - BufferServer( - const tf2::BufferCoreInterface & buffer, - NodePtr node, - const std::string & ns, - tf2::Duration check_period = tf2::durationFromSec(0.01)) - : buffer_(buffer), - logger_(node->get_logger()) - { - server_ = rclcpp_action::create_server( - node, - ns, - std::bind(&BufferServer::goalCB, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&BufferServer::cancelCB, this, std::placeholders::_1), - std::bind(&BufferServer::acceptedCB, this, std::placeholders::_1)); - - check_timer_ = rclcpp::create_timer( - node, node->get_clock(), check_period, std::bind(&BufferServer::checkTransforms, this)); - RCLCPP_DEBUG(logger_, "Buffer server started"); - } - -private: - struct GoalInfo - { - GoalHandle handle; - tf2::TimePoint end_time; - }; - - TF2_ROS_PUBLIC - rclcpp_action::GoalResponse goalCB( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - - TF2_ROS_PUBLIC - void acceptedCB(GoalHandle gh); - - TF2_ROS_PUBLIC - rclcpp_action::CancelResponse cancelCB(GoalHandle gh); - - TF2_ROS_PUBLIC - void checkTransforms(); - - TF2_ROS_PUBLIC - bool canTransform(GoalHandle gh); - - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh); - - const tf2::BufferCoreInterface & buffer_; - rclcpp::Logger logger_; - rclcpp_action::Server::SharedPtr server_; - std::list active_goals_; - std::mutex mutex_; - rclcpp::TimerBase::SharedPtr check_timer_; -}; - -} // namespace tf2_ros +#include #endif // TF2_ROS__BUFFER_SERVER_H_ diff --git a/tf2_ros/include/tf2_ros/buffer_server.hpp b/tf2_ros/include/tf2_ros/buffer_server.hpp new file mode 100644 index 000000000..cf1fd57c8 --- /dev/null +++ b/tf2_ros/include/tf2_ros/buffer_server.hpp @@ -0,0 +1,133 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ + +#ifndef TF2_ROS__BUFFER_SERVER_HPP_ +#define TF2_ROS__BUFFER_SERVER_HPP_ + +#include +#include +#include +#include +#include + +#include "tf2/time.hpp" +#include "tf2/buffer_core_interface.hpp" +#include "tf2_ros/visibility_control.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/create_timer.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_msgs/action/lookup_transform.hpp" + +namespace tf2_ros +{ +/** \brief Action server for the action-based implementation of tf2::BufferCoreInterface. + * + * Use this class with a tf2_ros::TransformListener in the same process. + * You can use this class with a tf2_ros::BufferClient in a different process. + */ +class BufferServer +{ + using LookupTransformAction = tf2_msgs::action::LookupTransform; + using GoalHandle = std::shared_ptr>; + +public: + /** \brief Constructor + * \param buffer The Buffer that this BufferServer will wrap. + * \param node The node to add the buffer server to. + * \param ns The namespace in which to look for action clients. + * \param check_period How often to check for changes to known transforms (via a timer event). + */ + template + BufferServer( + const tf2::BufferCoreInterface & buffer, + NodePtr node, + const std::string & ns, + tf2::Duration check_period = tf2::durationFromSec(0.01)) + : buffer_(buffer), + logger_(node->get_logger()) + { + server_ = rclcpp_action::create_server( + node, + ns, + std::bind(&BufferServer::goalCB, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&BufferServer::cancelCB, this, std::placeholders::_1), + std::bind(&BufferServer::acceptedCB, this, std::placeholders::_1)); + + check_timer_ = rclcpp::create_timer( + node, node->get_clock(), check_period, std::bind(&BufferServer::checkTransforms, this)); + RCLCPP_DEBUG(logger_, "Buffer server started"); + } + +private: + struct GoalInfo + { + GoalHandle handle; + tf2::TimePoint end_time; + }; + + TF2_ROS_PUBLIC + rclcpp_action::GoalResponse goalCB( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + TF2_ROS_PUBLIC + void acceptedCB(GoalHandle gh); + + TF2_ROS_PUBLIC + rclcpp_action::CancelResponse cancelCB(GoalHandle gh); + + TF2_ROS_PUBLIC + void checkTransforms(); + + TF2_ROS_PUBLIC + bool canTransform(GoalHandle gh); + + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh); + + const tf2::BufferCoreInterface & buffer_; + rclcpp::Logger logger_; + rclcpp_action::Server::SharedPtr server_; + std::list active_goals_; + std::mutex mutex_; + rclcpp::TimerBase::SharedPtr check_timer_; +}; + +} // namespace tf2_ros + +#endif // TF2_ROS__BUFFER_SERVER_HPP_ diff --git a/tf2_ros/include/tf2_ros/create_timer_interface.h b/tf2_ros/include/tf2_ros/create_timer_interface.h index 6588e0986..6caedd006 100644 --- a/tf2_ros/include/tf2_ros/create_timer_interface.h +++ b/tf2_ros/include/tf2_ros/create_timer_interface.h @@ -30,110 +30,6 @@ #ifndef TF2_ROS__CREATE_TIMER_INTERFACE_H_ #define TF2_ROS__CREATE_TIMER_INTERFACE_H_ -#include -#include -#include -#include - -#include "tf2/time.hpp" - -#include "tf2_ros/visibility_control.h" - -#include "rclcpp/rclcpp.hpp" - -namespace tf2_ros -{ - -using TimerHandle = uint64_t; -using TimerCallbackType = std::function; - -class CreateTimerInterfaceException : public std::runtime_error -{ -public: - TF2_ROS_PUBLIC - explicit CreateTimerInterfaceException(const std::string & errorDescription) - : std::runtime_error(errorDescription) - { - } -}; - -class InvalidTimerHandleException : public std::runtime_error -{ -public: - TF2_ROS_PUBLIC - explicit InvalidTimerHandleException(const std::string & description) - : std::runtime_error(description) - { - } -}; - -/** - * \brief Abstract interface for creating timers. - */ -class CreateTimerInterface -{ -public: - using SharedPtr = std::shared_ptr; - using ConstSharedPtr = std::shared_ptr; - using UniquePtr = std::unique_ptr; - - TF2_ROS_PUBLIC - virtual - ~CreateTimerInterface() = default; - - /** - * \brief Create a new timer. - * - * After creation, the timer will periodically execute the user-provided callback. - * - * \param clock The clock providing the current time - * \param period The interval at which the timer fires - * \param callback The callback function to execute every interval - */ - TF2_ROS_PUBLIC - virtual TimerHandle - createTimer( - rclcpp::Clock::SharedPtr clock, - const tf2::Duration & period, - TimerCallbackType callback) = 0; - - /** - * \brief Cancel a timer. - * - * The timer will stop executing user callbacks. - * - * \param timer_handle Handle to the timer to cancel - * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist - */ - TF2_ROS_PUBLIC - virtual void - cancel(const TimerHandle & timer_handle) = 0; - - /** - * \brief Reset the timer. - * - * The timer will reset and continue to execute user callbacks periodically. - * - * \param timer_handle Handle to the timer to reset - * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist - */ - TF2_ROS_PUBLIC - virtual void - reset(const TimerHandle & timer_handle) = 0; - - /** - * \brief Remove a timer. - * - * The timer will be canceled and removed from internal storage. - * - * \param timer_handle Handle to the timer to reset - * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist - */ - TF2_ROS_PUBLIC - virtual void - remove(const TimerHandle & timer_handle) = 0; -}; // class CreateTimerInterface - -} // namespace tf2_ros +#include #endif // TF2_ROS__CREATE_TIMER_INTERFACE_H_ diff --git a/tf2_ros/include/tf2_ros/create_timer_interface.hpp b/tf2_ros/include/tf2_ros/create_timer_interface.hpp new file mode 100644 index 000000000..b7d32c982 --- /dev/null +++ b/tf2_ros/include/tf2_ros/create_timer_interface.hpp @@ -0,0 +1,139 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TF2_ROS__CREATE_TIMER_INTERFACE_HPP_ +#define TF2_ROS__CREATE_TIMER_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "tf2/time.hpp" + +#include "tf2_ros/visibility_control.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace tf2_ros +{ + +using TimerHandle = uint64_t; +using TimerCallbackType = std::function; + +class CreateTimerInterfaceException : public std::runtime_error +{ +public: + TF2_ROS_PUBLIC + explicit CreateTimerInterfaceException(const std::string & errorDescription) + : std::runtime_error(errorDescription) + { + } +}; + +class InvalidTimerHandleException : public std::runtime_error +{ +public: + TF2_ROS_PUBLIC + explicit InvalidTimerHandleException(const std::string & description) + : std::runtime_error(description) + { + } +}; + +/** + * \brief Abstract interface for creating timers. + */ +class CreateTimerInterface +{ +public: + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; + using UniquePtr = std::unique_ptr; + + TF2_ROS_PUBLIC + virtual + ~CreateTimerInterface() = default; + + /** + * \brief Create a new timer. + * + * After creation, the timer will periodically execute the user-provided callback. + * + * \param clock The clock providing the current time + * \param period The interval at which the timer fires + * \param callback The callback function to execute every interval + */ + TF2_ROS_PUBLIC + virtual TimerHandle + createTimer( + rclcpp::Clock::SharedPtr clock, + const tf2::Duration & period, + TimerCallbackType callback) = 0; + + /** + * \brief Cancel a timer. + * + * The timer will stop executing user callbacks. + * + * \param timer_handle Handle to the timer to cancel + * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist + */ + TF2_ROS_PUBLIC + virtual void + cancel(const TimerHandle & timer_handle) = 0; + + /** + * \brief Reset the timer. + * + * The timer will reset and continue to execute user callbacks periodically. + * + * \param timer_handle Handle to the timer to reset + * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist + */ + TF2_ROS_PUBLIC + virtual void + reset(const TimerHandle & timer_handle) = 0; + + /** + * \brief Remove a timer. + * + * The timer will be canceled and removed from internal storage. + * + * \param timer_handle Handle to the timer to reset + * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist + */ + TF2_ROS_PUBLIC + virtual void + remove(const TimerHandle & timer_handle) = 0; +}; // class CreateTimerInterface + +} // namespace tf2_ros + +#endif // TF2_ROS__CREATE_TIMER_INTERFACE_HPP_ diff --git a/tf2_ros/include/tf2_ros/create_timer_ros.h b/tf2_ros/include/tf2_ros/create_timer_ros.h index a44803e43..b97e9af9e 100644 --- a/tf2_ros/include/tf2_ros/create_timer_ros.h +++ b/tf2_ros/include/tf2_ros/create_timer_ros.h @@ -30,104 +30,6 @@ #ifndef TF2_ROS__CREATE_TIMER_ROS_H_ #define TF2_ROS__CREATE_TIMER_ROS_H_ -#include -#include - -#include "tf2_ros/create_timer_interface.h" -#include "tf2_ros/visibility_control.h" -#include "tf2/time.hpp" - -#include "rclcpp/rclcpp.hpp" - -namespace tf2_ros -{ - -/** - * \brief Create and manage ROS timers. - * - * This class is thread safe. - */ -class CreateTimerROS : public CreateTimerInterface -{ -public: - TF2_ROS_PUBLIC - CreateTimerROS( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, - rclcpp::CallbackGroup::SharedPtr callback_group = nullptr); - - virtual ~CreateTimerROS() = default; - - /** - * \brief Create a new timer. - * - * After creation, the timer will periodically execute the user-provided callback. - * - * \param clock The clock providing the current time - * \param period The interval at which the timer fires - * \param callback The callback function to execute every interval - */ - TF2_ROS_PUBLIC - TimerHandle - createTimer( - rclcpp::Clock::SharedPtr clock, - const tf2::Duration & period, - TimerCallbackType callback) override; - - /** - * \brief Cancel a timer. - * - * The timer will stop executing user callbacks. - * - * \param timer_handle Handle to the timer to cancel - * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist - */ - TF2_ROS_PUBLIC - void - cancel(const TimerHandle & timer_handle) override; - - /** - * \brief Reset the timer. - * - * The timer will reset and continue to execute user callbacks periodically. - * - * \param timer_handle Handle to the timer to reset - * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist - */ - TF2_ROS_PUBLIC - void - reset(const TimerHandle & timer_handle) override; - - /** - * \brief Remove a timer. - * - * The timer will be canceled and removed from internal storage. - * - * \param timer_handle Handle to the timer to reset. - * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist - */ - TF2_ROS_PUBLIC - void - remove(const TimerHandle & timer_handle) override; - -private: - void - cancelNoLock(const TimerHandle & timer_handle); - - void - timerCallback( - const TimerHandle & timer_handle, - TimerCallbackType callback); - - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; - TimerHandle next_timer_handle_index_; - std::unordered_map timers_map_; - std::mutex timers_map_mutex_; - - rclcpp::CallbackGroup::SharedPtr callback_group_; -}; // class CreateTimerROS - -} // namespace tf2_ros +#include #endif // TF2_ROS__CREATE_TIMER_ROS_H_ diff --git a/tf2_ros/include/tf2_ros/create_timer_ros.hpp b/tf2_ros/include/tf2_ros/create_timer_ros.hpp new file mode 100644 index 000000000..5accc00eb --- /dev/null +++ b/tf2_ros/include/tf2_ros/create_timer_ros.hpp @@ -0,0 +1,133 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TF2_ROS__CREATE_TIMER_ROS_HPP_ +#define TF2_ROS__CREATE_TIMER_ROS_HPP_ + +#include +#include + +#include "tf2_ros/create_timer_interface.hpp" +#include "tf2_ros/visibility_control.hpp" +#include "tf2/time.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace tf2_ros +{ + +/** + * \brief Create and manage ROS timers. + * + * This class is thread safe. + */ +class CreateTimerROS : public CreateTimerInterface +{ +public: + TF2_ROS_PUBLIC + CreateTimerROS( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr); + + virtual ~CreateTimerROS() = default; + + /** + * \brief Create a new timer. + * + * After creation, the timer will periodically execute the user-provided callback. + * + * \param clock The clock providing the current time + * \param period The interval at which the timer fires + * \param callback The callback function to execute every interval + */ + TF2_ROS_PUBLIC + TimerHandle + createTimer( + rclcpp::Clock::SharedPtr clock, + const tf2::Duration & period, + TimerCallbackType callback) override; + + /** + * \brief Cancel a timer. + * + * The timer will stop executing user callbacks. + * + * \param timer_handle Handle to the timer to cancel + * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist + */ + TF2_ROS_PUBLIC + void + cancel(const TimerHandle & timer_handle) override; + + /** + * \brief Reset the timer. + * + * The timer will reset and continue to execute user callbacks periodically. + * + * \param timer_handle Handle to the timer to reset + * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist + */ + TF2_ROS_PUBLIC + void + reset(const TimerHandle & timer_handle) override; + + /** + * \brief Remove a timer. + * + * The timer will be canceled and removed from internal storage. + * + * \param timer_handle Handle to the timer to reset. + * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist + */ + TF2_ROS_PUBLIC + void + remove(const TimerHandle & timer_handle) override; + +private: + void + cancelNoLock(const TimerHandle & timer_handle); + + void + timerCallback( + const TimerHandle & timer_handle, + TimerCallbackType callback); + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + TimerHandle next_timer_handle_index_; + std::unordered_map timers_map_; + std::mutex timers_map_mutex_; + + rclcpp::CallbackGroup::SharedPtr callback_group_; +}; // class CreateTimerROS + +} // namespace tf2_ros + +#endif // TF2_ROS__CREATE_TIMER_ROS_HPP_ diff --git a/tf2_ros/include/tf2_ros/message_filter.h b/tf2_ros/include/tf2_ros/message_filter.h index e7709a892..13deab60f 100644 --- a/tf2_ros/include/tf2_ros/message_filter.h +++ b/tf2_ros/include/tf2_ros/message_filter.h @@ -32,757 +32,6 @@ #ifndef TF2_ROS__MESSAGE_FILTER_H_ #define TF2_ROS__MESSAGE_FILTER_H_ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "message_filters/connection.h" -#include "message_filters/message_traits.h" -#include "message_filters/simple_filter.h" -#include "tf2/buffer_core_interface.hpp" -#include "tf2/time.hpp" -#include "tf2_ros/async_buffer_interface.h" -#include "tf2_ros/buffer.h" - -#include "builtin_interfaces/msg/time.hpp" -#include "rclcpp/rclcpp.hpp" - -#define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ - RCUTILS_LOG_DEBUG_NAMED( \ - "tf2_ros_message_filter", \ - std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \ - getTargetFramesString().c_str(), __VA_ARGS__) - -#define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \ - RCUTILS_LOG_WARN_NAMED( \ - "tf2_ros_message_filter", \ - std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \ - getTargetFramesString().c_str(), __VA_ARGS__) - -namespace tf2_ros -{ - -namespace filter_failure_reasons -{ -enum FilterFailureReason -{ - // NOTE when adding new values, do not explicitly assign a number. See FilterFailureReasonCount - - /// The message buffer overflowed, and this message was pushed off the back of the queue, but the - // reason it was unable to be transformed is unknown. - Unknown, - /// The timestamp on the message is earlier than all the data in the transform cache - OutTheBack, - /// The frame_id on the message is empty - EmptyFrameID, - /// No transform found - NoTransformFound, - /// Queue size full - QueueFull, - /// Max enum value for iteration, keep it at the end of the enum - FilterFailureReasonCount, -}; - -} // namespace filter_failure_reasons - -static std::string get_filter_failure_reason_string( - filter_failure_reasons::FilterFailureReason reason) -{ - switch (reason) { - case filter_failure_reasons::OutTheBack: - return - "the timestamp on the message is earlier than all the data in the transform cache"; - case filter_failure_reasons::EmptyFrameID: - return "the frame id of the message is empty"; - case filter_failure_reasons::NoTransformFound: - return "did not find a valid transform, this usually happens at startup ..."; - case filter_failure_reasons::QueueFull: - return "discarding message because the queue is full"; - case filter_failure_reasons::Unknown: // fallthrough - default: - return "unknown"; - } -} - -typedef filter_failure_reasons::FilterFailureReason FilterFailureReason; - -class MessageFilterBase -{ -public: - typedef std::vector V_string; - - virtual ~MessageFilterBase() {} - virtual void clear() = 0; - virtual void setTargetFrame(const std::string & target_frame) = 0; - virtual void setTargetFrames(const V_string & target_frames) = 0; - virtual void setTolerance(const rclcpp::Duration & tolerance) = 0; -}; - -/** - * \brief Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available - * - * The callbacks used in this class are of the same form as those used by rclcpp's message callbacks. - * - * MessageFilter is templated on a message type. - * - * \section example_usage Example Usage - * - * If you want to hook a MessageFilter into a ROS topic: - \verbatim - message_filters::Subscriber sub(node_, "topic", 10); - tf::MessageFilter tf_filter(sub, tf_listener_, "/map", 10); - tf_filter.registerCallback(&MyClass::myCallback, this); - \endverbatim - */ -template -class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter -{ -public: - using MConstPtr = std::shared_ptr; - typedef message_filters::MessageEvent MEvent; - - /** - * \brief Constructor - * - * \param buffer The buffer this filter should use - * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. - * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). - * \param node The ros2 node to use for logging and clock operations - * \param buffer_timeout The timeout duration after requesting transforms from the buffer. - */ - template - MessageFilter( - BufferT & buffer, const std::string & target_frame, uint32_t queue_size, - const rclcpp::Node::SharedPtr & node, - std::chrono::duration buffer_timeout = - std::chrono::duration::max()) - : MessageFilter( - buffer, target_frame, queue_size, node->get_node_logging_interface(), - node->get_node_clock_interface(), buffer_timeout) - { - static_assert( - std::is_base_of::value, - "Buffer type must implement tf2::BufferCoreInterface"); - static_assert( - std::is_base_of::value, - "Buffer type must implement tf2_ros::AsyncBufferInterface"); - } - - /** - * \brief Constructor - * - * \param buffer The buffer this filter should use - * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. - * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). - * \param node_logging The logging interface to use for any log messages - * \param node_clock The clock interface to use to get the node clock - * \param buffer_timeout The timeout duration after requesting transforms from the buffer. - */ - template - MessageFilter( - BufferT & buffer, const std::string & target_frame, uint32_t queue_size, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, - std::chrono::duration buffer_timeout = - std::chrono::duration::max()) - : node_logging_(node_logging), - node_clock_(node_clock), - buffer_(buffer), - queue_size_(queue_size), - buffer_timeout_(buffer_timeout) - { - init(); - setTargetFrame(target_frame); - } - - /** - * \brief Constructor - * - * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. - * \param buffer The buffer this filter should use - * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. - * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). - * \param node The ros2 node to use for logging and clock operations - * \param buffer_timeout The timeout duration after requesting transforms from the buffer. - */ - template - MessageFilter( - F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, - const rclcpp::Node::SharedPtr & node, - std::chrono::duration buffer_timeout = - std::chrono::duration::max()) - : MessageFilter( - f, buffer, target_frame, queue_size, node->get_node_logging_interface(), - node->get_node_clock_interface(), buffer_timeout) - { - } - - /** - * \brief Constructor - * - * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. - * \param buffer The buffer this filter should use - * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. - * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). - * \param node_logging The logging interface to use for any log messages - * \param node_clock The clock interface to use to get the node clock - * \param buffer_timeout The timeout duration after requesting transforms from the buffer. - */ - template - MessageFilter( - F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, - std::chrono::duration buffer_timeout = - std::chrono::duration::max()) - : node_logging_(node_logging), - node_clock_(node_clock), - buffer_(buffer), - queue_size_(queue_size), - buffer_timeout_(buffer_timeout) - { - init(); - setTargetFrame(target_frame); - connectInput(f); - } - - /** - * \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first. - */ - template - void connectInput(F & f) - { - message_connection_.disconnect(); - message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); - } - - /** - * \brief Destructor - */ - ~MessageFilter() - { - message_connection_.disconnect(); - clear(); - - TF2_ROS_MESSAGEFILTER_DEBUG( - "Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, " - "Messages received: %llu, Total dropped: %llu", - static_cast(successful_transform_count_), - static_cast(failed_out_the_back_count_), - static_cast(transform_message_count_), - static_cast(incoming_message_count_), - static_cast(dropped_message_count_)); - } - - /** - * \brief Set the frame you need to be able to transform to before getting a message callback - */ - void setTargetFrame(const std::string & target_frame) - { - V_string frames; - frames.push_back(target_frame); - setTargetFrames(frames); - } - - /** - * \brief Set the frames you need to be able to transform to before getting a message callback - */ - void setTargetFrames(const V_string & target_frames) - { - std::unique_lock frames_lock(target_frames_mutex_); - - target_frames_.resize(target_frames.size()); - std::transform( - target_frames.begin(), target_frames.end(), - target_frames_.begin(), this->stripSlash); - expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); - - std::stringstream ss; - for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) { - ss << *it << " "; - } - target_frames_string_ = ss.str(); - } - - /** - * \brief Get the target frames as a string for debugging - */ - std::string getTargetFramesString() - { - std::unique_lock frames_lock(target_frames_mutex_); - return target_frames_string_; - } - - /** - * \brief Set the required tolerance for the notifier to return true - */ - void setTolerance(const rclcpp::Duration & tolerance) - { - std::unique_lock frames_lock(target_frames_mutex_); - time_tolerance_ = tolerance; - expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); - } - - /** - * \brief Clear any messages currently in the queue - */ - void clear() - { - { - std::unique_lock lock(ts_futures_mutex_); - for (auto & kv : ts_futures_) { - buffer_.cancel(kv.second); - } - ts_futures_.clear(); - } - - std::unique_lock unique_lock(messages_mutex_); - - TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared"); - - messages_.clear(); - - warned_about_empty_frame_id_ = false; - } - - void add(const MEvent & evt) - { - if (target_frames_.empty()) { - return; - } - - namespace mt = message_filters::message_traits; - const MConstPtr & message = evt.getMessage(); - std::string frame_id = stripSlash(mt::FrameId::value(*message)); - rclcpp::Time stamp = mt::TimeStamp::value(*message); - - if (frame_id.empty()) { - messageDropped(evt, filter_failure_reasons::EmptyFrameID); - return; - } - - std::vector> wait_params; - // iterate through the target frames and add requests for each of them - MessageInfo info; - info.handles.reserve(expected_success_count_); - { - V_string target_frames_copy; - // Copy target_frames_ to avoid deadlock from #79 - { - std::unique_lock frames_lock(target_frames_mutex_); - target_frames_copy = target_frames_; - } - - V_string::iterator it = target_frames_copy.begin(); - V_string::iterator end = target_frames_copy.end(); - for (; it != end; ++it) { - const std::string & target_frame = *it; - wait_params.emplace_back( - next_handle_index_, tf2_ros::fromRclcpp(stamp), target_frame); - info.handles.push_back(next_handle_index_++); - - if (time_tolerance_.nanoseconds()) { - wait_params.emplace_back( - next_handle_index_, - tf2_ros::fromRclcpp(stamp + time_tolerance_), - target_frame); - info.handles.push_back(next_handle_index_++); - } - } - } - - { - // Keep a lock on the messages - std::unique_lock unique_lock(messages_mutex_); - - // If this message is about to push us past our queue size, erase the oldest message - if (queue_size_ != 0 && messages_.size() + 1 > queue_size_) { - ++dropped_message_count_; - const MessageInfo & front = messages_.front(); - TF2_ROS_MESSAGEFILTER_DEBUG( - "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", - messages_.size(), - (mt::FrameId::value(*front.event.getMessage())).c_str(), - mt::TimeStamp::value(*front.event.getMessage()).seconds()); - - messageDropped(front.event, filter_failure_reasons::QueueFull); - - messages_.pop_front(); - } - - // Add the message to our list - info.event = evt; - messages_.push_back(info); - } - - TF2_ROS_MESSAGEFILTER_DEBUG( - "Added message in frame %s at time %.3f, count now %d", - frame_id.c_str(), stamp.seconds(), messages_.size()); - ++incoming_message_count_; - - for (const auto & param : wait_params) { - const auto & handle = std::get<0>(param); - const auto & stamp = std::get<1>(param); - const auto & target_frame = std::get<2>(param); - tf2_ros::TransformStampedFuture future = buffer_.waitForTransform( - target_frame, - frame_id, - stamp, - buffer_timeout_, - std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, handle)); - - // If handle of future is 0 or 0xffffffffffffffffULL, waitForTransform have already called - // the callback. - if (0 != future.getHandle() && 0xffffffffffffffffULL != future.getHandle()) { - std::unique_lock lock(ts_futures_mutex_); - ts_futures_.insert({handle, std::move(future)}); - } - } - } - - /** - * \brief Manually add a message into this filter. - * \note If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly - * multiple times - */ - void add(const MConstPtr & message) - { - auto t = node_clock_->get_clock()->now(); - add(MEvent(message, t)); - } - - /** - * \brief Register a callback to be called when a message is about to be dropped - * \param callback The callback to call - */ -#if 0 - message_filters::Connection registerFailureCallback(const FailureCallback & callback) - { - message_connection_failure.disconnect(); - message_connection_failure = this->registerCallback(callback, this); - } -#endif - - virtual void setQueueSize(uint32_t new_queue_size) - { - queue_size_ = new_queue_size; - } - - virtual uint32_t getQueueSize() - { - return queue_size_; - } - -private: - void init() - { - successful_transform_count_ = 0; - failed_out_the_back_count_ = 0; - transform_message_count_ = 0; - incoming_message_count_ = 0; - dropped_message_count_ = 0; - time_tolerance_ = rclcpp::Duration(0, 0); - warned_about_empty_frame_id_ = false; - expected_success_count_ = 1; - } - - void transformReadyCallback(const tf2_ros::TransformStampedFuture & future, const uint64_t handle) - { - namespace mt = message_filters::message_traits; - - MEvent saved_event; - bool event_found = false; - - { - std::unique_lock lock(ts_futures_mutex_); - auto iter = ts_futures_.find(handle); - if (iter != ts_futures_.end()) { - ts_futures_.erase(iter); - } - } - - { - // We will be accessing and mutating messages now, require unique lock - std::unique_lock lock(messages_mutex_); - - // find the message this request is associated with - typename L_MessageInfo::iterator msg_it = messages_.begin(); - typename L_MessageInfo::iterator msg_end = messages_.end(); - - for (; msg_it != msg_end; ++msg_it) { - MessageInfo & info = *msg_it; - auto handle_it = std::find(info.handles.begin(), info.handles.end(), handle); - if (handle_it != info.handles.end()) { - // found msg_it - ++info.success_count; - if (info.success_count >= expected_success_count_) { - saved_event = msg_it->event; - messages_.erase(msg_it); - event_found = true; - } - break; - } - } - } - - if (!event_found) { - return; - } - - bool can_transform = true; - const MConstPtr & message = saved_event.getMessage(); - std::string frame_id = stripSlash(mt::FrameId::value(*message)); - rclcpp::Time stamp = mt::TimeStamp::value(*message); - - bool transform_available = true; - FilterFailureReason error = filter_failure_reasons::Unknown; - try { - future.get(); - } catch (...) { - transform_available = false; - error = filter_failure_reasons::OutTheBack; - } - - if (transform_available) { - std::unique_lock frames_lock(target_frames_mutex_); - // make sure we can still perform all the necessary transforms - typename V_string::iterator it = target_frames_.begin(); - typename V_string::iterator end = target_frames_.end(); - for (; it != end; ++it) { - const std::string & target = *it; - if (!buffer_.canTransform(target, frame_id, tf2_ros::fromRclcpp(stamp), NULL)) { - can_transform = false; - break; - } - - if (time_tolerance_.nanoseconds()) { - if (!buffer_.canTransform( - target, frame_id, - tf2_ros::fromRclcpp(stamp + time_tolerance_), NULL)) - { - can_transform = false; - break; - } - } - } - } else { - can_transform = false; - } - - if (can_transform) { - TF2_ROS_MESSAGEFILTER_DEBUG( - "Message ready in frame %s at time %.3f, count now %d", - frame_id.c_str(), stamp.seconds(), messages_.size()); - - ++successful_transform_count_; - messageReady(saved_event); - } else { - ++dropped_message_count_; - - TF2_ROS_MESSAGEFILTER_DEBUG( - "Discarding message in frame %s at time %.3f, count now %d", - frame_id.c_str(), stamp.seconds(), messages_.size()); - messageDropped(saved_event, error); - } - } - - /** - * \brief Callback that happens when we receive a message on the message topic - */ - void incomingMessage(const message_filters::MessageEvent & evt) - { - add(evt); - } - - void checkFailures() - { - if (!next_failure_warning_.nanoseconds()) { - next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(15, 0); - } - - if (node_clock_->get_clock()->now() >= next_failure_warning_) { - if (incoming_message_count_ - messages_.size() == 0) { - return; - } - - double dropped_pct = static_cast(dropped_message_count_) / - static_cast(incoming_message_count_ - messages_.size()); - if (dropped_pct > 0.95) { - TF2_ROS_MESSAGEFILTER_WARN( - "Dropped %.2f%% of messages so far. Please turn the " - "[tf2_ros_message_filter.message_notifier] rosconsole logger to DEBUG for more " - "information.", - dropped_pct * 100); - next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(60, 0); - - if (static_cast(failed_out_the_back_count_) / - static_cast(dropped_message_count_) > 0.5) - { - TF2_ROS_MESSAGEFILTER_WARN( - " The majority of dropped messages were due to messages growing older than the TF " - "cache time. The last message's timestamp was: %f, and the last frame_id was: %s", - last_out_the_back_stamp_.seconds(), last_out_the_back_frame_.c_str()); - } - } - } - } - - // TODO(clalancette): reenable this once we have underlying support for callback queues -#if 0 - struct CBQueueCallback : public ros::CallbackInterface - { - CBQueueCallback( - MessageFilter * filter, const MEvent & event, bool success, FilterFailureReason reason) - : event_(event), - filter_(filter), - reason_(reason), - success_(success) - {} - - - virtual CallResult call() - { - if (success_) { - filter_->signalMessage(event_); - } else { - filter_->signalFailure(event_, reason_); - } - - return Success; - } - -private: - MEvent event_; - MessageFilter * filter_; - FilterFailureReason reason_; - bool success_; - }; -#endif - - void messageDropped(const MEvent & evt, FilterFailureReason reason) - { - // TODO(clalancette): reenable this once we have underlying support for callback queues -#if 0 - if (callback_queue_) { - ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason)); - callback_queue_->addCallback(cb, (uint64_t)this); - } else {} -#endif - { - signalFailure(evt, reason); - } - } - - void messageReady(const MEvent & evt) - { - // TODO(clalancette): reenable this once we have underlying support for callback queues -#if 0 - if (callback_queue_) { - ros::CallbackInterfacePtr cb(new CBQueueCallback( - this, evt, true, filter_failure_reasons::Unknown)); - callback_queue_->addCallback(cb, (uint64_t)this); - } else {} -#endif - { - this->signalMessage(evt); - } - } - - void signalFailure(const MEvent & evt, FilterFailureReason reason) - { - namespace mt = message_filters::message_traits; - const MConstPtr & message = evt.getMessage(); - std::string frame_id = stripSlash(mt::FrameId::value(*message)); - rclcpp::Time stamp = mt::TimeStamp::value(*message); - RCLCPP_INFO( - node_logging_->get_logger(), - "Message Filter dropping message: frame '%s' at time %.3f for reason '%s'", - frame_id.c_str(), stamp.seconds(), get_filter_failure_reason_string(reason).c_str()); - } - - static std::string stripSlash(const std::string & in) - { - if (!in.empty() && (in[0] == '/')) { - std::string out = in; - out.erase(0, 1); - return out; - } - - return in; - } - - ///< The node logging interface to use for any log messages - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - ///< The node clock interface to use to get the clock to use - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; - ///< The Transformer used to determine if transformation data is available - BufferT & buffer_; - ///< The frames we need to be able to transform to before a message is ready - V_string target_frames_; - std::string target_frames_string_; - ///< A mutex to protect access to the target_frames_ list and target_frames_string. - std::mutex target_frames_mutex_; - ///< The maximum number of messages we queue up - uint32_t queue_size_; - - uint64_t next_handle_index_ = 0; - struct MessageInfo - { - MessageInfo() - : success_count(0) {} - - MEvent event; - std::vector handles; - uint64_t success_count; - }; - typedef std::list L_MessageInfo; - L_MessageInfo messages_; - - ///< The mutex used for locking message list operations - std::mutex messages_mutex_; - uint64_t expected_success_count_; - - bool warned_about_empty_frame_id_; - - uint64_t successful_transform_count_; - uint64_t failed_out_the_back_count_; - uint64_t transform_message_count_; - uint64_t incoming_message_count_; - uint64_t dropped_message_count_; - - rclcpp::Time last_out_the_back_stamp_; - std::string last_out_the_back_frame_; - - rclcpp::Time next_failure_warning_; - - ///< Provide additional tolerance on time for messages which are stamped - // but can have associated duration - rclcpp::Duration time_tolerance_ = rclcpp::Duration(0, 0); - - message_filters::Connection message_connection_; - message_filters::Connection message_connection_failure; - - // Timeout duration when calling the buffer method 'waitForTransform' - tf2::Duration buffer_timeout_; - - ///< The mutex used for locking TransformStampedFuture map operations - std::mutex ts_futures_mutex_; - - ///< Store the TransformStampedFuture returned by 'waitForTransform', - // to clear the callback in the Buffer if MessageFiltered object is destroyed. - std::unordered_map ts_futures_; -}; -} // namespace tf2_ros +#include #endif // TF2_ROS__MESSAGE_FILTER_H_ diff --git a/tf2_ros/include/tf2_ros/message_filter.hpp b/tf2_ros/include/tf2_ros/message_filter.hpp new file mode 100644 index 000000000..99a2e584d --- /dev/null +++ b/tf2_ros/include/tf2_ros/message_filter.hpp @@ -0,0 +1,788 @@ +/* + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Josh Faust */ + +#ifndef TF2_ROS__MESSAGE_FILTER_HPP_ +#define TF2_ROS__MESSAGE_FILTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "message_filters/connection.hpp" +#include "message_filters/message_traits.hpp" +#include "message_filters/simple_filter.hpp" +#include "tf2/buffer_core_interface.hpp" +#include "tf2/time.hpp" +#include "tf2_ros/async_buffer_interface.hpp" +#include "tf2_ros/buffer.hpp" + +#include "builtin_interfaces/msg/time.hpp" +#include "rclcpp/rclcpp.hpp" + +#define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ + RCUTILS_LOG_DEBUG_NAMED( \ + "tf2_ros_message_filter", \ + std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \ + getTargetFramesString().c_str(), __VA_ARGS__) + +#define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \ + RCUTILS_LOG_WARN_NAMED( \ + "tf2_ros_message_filter", \ + std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \ + getTargetFramesString().c_str(), __VA_ARGS__) + +namespace tf2_ros +{ + +namespace filter_failure_reasons +{ +enum FilterFailureReason +{ + // NOTE when adding new values, do not explicitly assign a number. See FilterFailureReasonCount + + /// The message buffer overflowed, and this message was pushed off the back of the queue, but the + // reason it was unable to be transformed is unknown. + Unknown, + /// The timestamp on the message is earlier than all the data in the transform cache + OutTheBack, + /// The frame_id on the message is empty + EmptyFrameID, + /// No transform found + NoTransformFound, + /// Queue size full + QueueFull, + /// Max enum value for iteration, keep it at the end of the enum + FilterFailureReasonCount, +}; + +} // namespace filter_failure_reasons + +static std::string get_filter_failure_reason_string( + filter_failure_reasons::FilterFailureReason reason) +{ + switch (reason) { + case filter_failure_reasons::OutTheBack: + return + "the timestamp on the message is earlier than all the data in the transform cache"; + case filter_failure_reasons::EmptyFrameID: + return "the frame id of the message is empty"; + case filter_failure_reasons::NoTransformFound: + return "did not find a valid transform, this usually happens at startup ..."; + case filter_failure_reasons::QueueFull: + return "discarding message because the queue is full"; + case filter_failure_reasons::Unknown: // fallthrough + default: + return "unknown"; + } +} + +typedef filter_failure_reasons::FilterFailureReason FilterFailureReason; + +class MessageFilterBase +{ +public: + typedef std::vector V_string; + + virtual ~MessageFilterBase() {} + virtual void clear() = 0; + virtual void setTargetFrame(const std::string & target_frame) = 0; + virtual void setTargetFrames(const V_string & target_frames) = 0; + virtual void setTolerance(const rclcpp::Duration & tolerance) = 0; +}; + +/** + * \brief Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available + * + * The callbacks used in this class are of the same form as those used by rclcpp's message callbacks. + * + * MessageFilter is templated on a message type. + * + * \section example_usage Example Usage + * + * If you want to hook a MessageFilter into a ROS topic: + \verbatim + message_filters::Subscriber sub(node_, "topic", 10); + tf::MessageFilter tf_filter(sub, tf_listener_, "/map", 10); + tf_filter.registerCallback(&MyClass::myCallback, this); + \endverbatim + */ +template +class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter +{ +public: + using MConstPtr = std::shared_ptr; + typedef message_filters::MessageEvent MEvent; + + /** + * \brief Constructor + * + * \param buffer The buffer this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param node The ros2 node to use for logging and clock operations + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. + */ + template + MessageFilter( + BufferT & buffer, const std::string & target_frame, uint32_t queue_size, + const rclcpp::Node::SharedPtr & node, + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) + : MessageFilter( + buffer, target_frame, queue_size, node->get_node_logging_interface(), + node->get_node_clock_interface(), buffer_timeout) + { + static_assert( + std::is_base_of::value, + "Buffer type must implement tf2::BufferCoreInterface"); + static_assert( + std::is_base_of::value, + "Buffer type must implement tf2_ros::AsyncBufferInterface"); + } + + /** + * \brief Constructor + * + * \param buffer The buffer this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param node_logging The logging interface to use for any log messages + * \param node_clock The clock interface to use to get the node clock + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. + */ + template + MessageFilter( + BufferT & buffer, const std::string & target_frame, uint32_t queue_size, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) + : node_logging_(node_logging), + node_clock_(node_clock), + buffer_(buffer), + queue_size_(queue_size), + buffer_timeout_(buffer_timeout) + { + init(); + setTargetFrame(target_frame); + } + + /** + * \brief Constructor + * + * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. + * \param buffer The buffer this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param node The ros2 node to use for logging and clock operations + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. + */ + template + MessageFilter( + F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, + const rclcpp::Node::SharedPtr & node, + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) + : MessageFilter( + f, buffer, target_frame, queue_size, node->get_node_logging_interface(), + node->get_node_clock_interface(), buffer_timeout) + { + } + + /** + * \brief Constructor + * + * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. + * \param buffer The buffer this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param node_logging The logging interface to use for any log messages + * \param node_clock The clock interface to use to get the node clock + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. + */ + template + MessageFilter( + F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) + : node_logging_(node_logging), + node_clock_(node_clock), + buffer_(buffer), + queue_size_(queue_size), + buffer_timeout_(buffer_timeout) + { + init(); + setTargetFrame(target_frame); + connectInput(f); + } + + /** + * \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first. + */ + template + void connectInput(F & f) + { + message_connection_.disconnect(); + message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); + } + + /** + * \brief Destructor + */ + ~MessageFilter() + { + message_connection_.disconnect(); + clear(); + + TF2_ROS_MESSAGEFILTER_DEBUG( + "Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, " + "Messages received: %llu, Total dropped: %llu", + static_cast(successful_transform_count_), + static_cast(failed_out_the_back_count_), + static_cast(transform_message_count_), + static_cast(incoming_message_count_), + static_cast(dropped_message_count_)); + } + + /** + * \brief Set the frame you need to be able to transform to before getting a message callback + */ + void setTargetFrame(const std::string & target_frame) + { + V_string frames; + frames.push_back(target_frame); + setTargetFrames(frames); + } + + /** + * \brief Set the frames you need to be able to transform to before getting a message callback + */ + void setTargetFrames(const V_string & target_frames) + { + std::unique_lock frames_lock(target_frames_mutex_); + + target_frames_.resize(target_frames.size()); + std::transform( + target_frames.begin(), target_frames.end(), + target_frames_.begin(), this->stripSlash); + expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); + + std::stringstream ss; + for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) { + ss << *it << " "; + } + target_frames_string_ = ss.str(); + } + + /** + * \brief Get the target frames as a string for debugging + */ + std::string getTargetFramesString() + { + std::unique_lock frames_lock(target_frames_mutex_); + return target_frames_string_; + } + + /** + * \brief Set the required tolerance for the notifier to return true + */ + void setTolerance(const rclcpp::Duration & tolerance) + { + std::unique_lock frames_lock(target_frames_mutex_); + time_tolerance_ = tolerance; + expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); + } + + /** + * \brief Clear any messages currently in the queue + */ + void clear() + { + { + std::unique_lock lock(ts_futures_mutex_); + for (auto & kv : ts_futures_) { + buffer_.cancel(kv.second); + } + ts_futures_.clear(); + } + + std::unique_lock unique_lock(messages_mutex_); + + TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared"); + + messages_.clear(); + + warned_about_empty_frame_id_ = false; + } + + void add(const MEvent & evt) + { + if (target_frames_.empty()) { + return; + } + + namespace mt = message_filters::message_traits; + const MConstPtr & message = evt.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + rclcpp::Time stamp = mt::TimeStamp::value(*message); + + if (frame_id.empty()) { + messageDropped(evt, filter_failure_reasons::EmptyFrameID); + return; + } + + std::vector> wait_params; + // iterate through the target frames and add requests for each of them + MessageInfo info; + info.handles.reserve(expected_success_count_); + { + V_string target_frames_copy; + // Copy target_frames_ to avoid deadlock from #79 + { + std::unique_lock frames_lock(target_frames_mutex_); + target_frames_copy = target_frames_; + } + + V_string::iterator it = target_frames_copy.begin(); + V_string::iterator end = target_frames_copy.end(); + for (; it != end; ++it) { + const std::string & target_frame = *it; + wait_params.emplace_back( + next_handle_index_, tf2_ros::fromRclcpp(stamp), target_frame); + info.handles.push_back(next_handle_index_++); + + if (time_tolerance_.nanoseconds()) { + wait_params.emplace_back( + next_handle_index_, + tf2_ros::fromRclcpp(stamp + time_tolerance_), + target_frame); + info.handles.push_back(next_handle_index_++); + } + } + } + + { + // Keep a lock on the messages + std::unique_lock unique_lock(messages_mutex_); + + // If this message is about to push us past our queue size, erase the oldest message + if (queue_size_ != 0 && messages_.size() + 1 > queue_size_) { + ++dropped_message_count_; + const MessageInfo & front = messages_.front(); + TF2_ROS_MESSAGEFILTER_DEBUG( + "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", + messages_.size(), + (mt::FrameId::value(*front.event.getMessage())).c_str(), + mt::TimeStamp::value(*front.event.getMessage()).seconds()); + + messageDropped(front.event, filter_failure_reasons::QueueFull); + + messages_.pop_front(); + } + + // Add the message to our list + info.event = evt; + messages_.push_back(info); + } + + TF2_ROS_MESSAGEFILTER_DEBUG( + "Added message in frame %s at time %.3f, count now %d", + frame_id.c_str(), stamp.seconds(), messages_.size()); + ++incoming_message_count_; + + for (const auto & param : wait_params) { + const auto & handle = std::get<0>(param); + const auto & stamp = std::get<1>(param); + const auto & target_frame = std::get<2>(param); + tf2_ros::TransformStampedFuture future = buffer_.waitForTransform( + target_frame, + frame_id, + stamp, + buffer_timeout_, + std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, handle)); + + // If handle of future is 0 or 0xffffffffffffffffULL, waitForTransform have already called + // the callback. + if (0 != future.getHandle() && 0xffffffffffffffffULL != future.getHandle()) { + std::unique_lock lock(ts_futures_mutex_); + ts_futures_.insert({handle, std::move(future)}); + } + } + } + + /** + * \brief Manually add a message into this filter. + * \note If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly + * multiple times + */ + void add(const MConstPtr & message) + { + auto t = node_clock_->get_clock()->now(); + add(MEvent(message, t)); + } + + /** + * \brief Register a callback to be called when a message is about to be dropped + * \param callback The callback to call + */ +#if 0 + message_filters::Connection registerFailureCallback(const FailureCallback & callback) + { + message_connection_failure.disconnect(); + message_connection_failure = this->registerCallback(callback, this); + } +#endif + + virtual void setQueueSize(uint32_t new_queue_size) + { + queue_size_ = new_queue_size; + } + + virtual uint32_t getQueueSize() + { + return queue_size_; + } + +private: + void init() + { + successful_transform_count_ = 0; + failed_out_the_back_count_ = 0; + transform_message_count_ = 0; + incoming_message_count_ = 0; + dropped_message_count_ = 0; + time_tolerance_ = rclcpp::Duration(0, 0); + warned_about_empty_frame_id_ = false; + expected_success_count_ = 1; + } + + void transformReadyCallback(const tf2_ros::TransformStampedFuture & future, const uint64_t handle) + { + namespace mt = message_filters::message_traits; + + MEvent saved_event; + bool event_found = false; + + { + std::unique_lock lock(ts_futures_mutex_); + auto iter = ts_futures_.find(handle); + if (iter != ts_futures_.end()) { + ts_futures_.erase(iter); + } + } + + { + // We will be accessing and mutating messages now, require unique lock + std::unique_lock lock(messages_mutex_); + + // find the message this request is associated with + typename L_MessageInfo::iterator msg_it = messages_.begin(); + typename L_MessageInfo::iterator msg_end = messages_.end(); + + for (; msg_it != msg_end; ++msg_it) { + MessageInfo & info = *msg_it; + auto handle_it = std::find(info.handles.begin(), info.handles.end(), handle); + if (handle_it != info.handles.end()) { + // found msg_it + ++info.success_count; + if (info.success_count >= expected_success_count_) { + saved_event = msg_it->event; + messages_.erase(msg_it); + event_found = true; + } + break; + } + } + } + + if (!event_found) { + return; + } + + bool can_transform = true; + const MConstPtr & message = saved_event.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + rclcpp::Time stamp = mt::TimeStamp::value(*message); + + bool transform_available = true; + FilterFailureReason error = filter_failure_reasons::Unknown; + try { + future.get(); + } catch (...) { + transform_available = false; + error = filter_failure_reasons::OutTheBack; + } + + if (transform_available) { + std::unique_lock frames_lock(target_frames_mutex_); + // make sure we can still perform all the necessary transforms + typename V_string::iterator it = target_frames_.begin(); + typename V_string::iterator end = target_frames_.end(); + for (; it != end; ++it) { + const std::string & target = *it; + if (!buffer_.canTransform(target, frame_id, tf2_ros::fromRclcpp(stamp), NULL)) { + can_transform = false; + break; + } + + if (time_tolerance_.nanoseconds()) { + if (!buffer_.canTransform( + target, frame_id, + tf2_ros::fromRclcpp(stamp + time_tolerance_), NULL)) + { + can_transform = false; + break; + } + } + } + } else { + can_transform = false; + } + + if (can_transform) { + TF2_ROS_MESSAGEFILTER_DEBUG( + "Message ready in frame %s at time %.3f, count now %d", + frame_id.c_str(), stamp.seconds(), messages_.size()); + + ++successful_transform_count_; + messageReady(saved_event); + } else { + ++dropped_message_count_; + + TF2_ROS_MESSAGEFILTER_DEBUG( + "Discarding message in frame %s at time %.3f, count now %d", + frame_id.c_str(), stamp.seconds(), messages_.size()); + messageDropped(saved_event, error); + } + } + + /** + * \brief Callback that happens when we receive a message on the message topic + */ + void incomingMessage(const message_filters::MessageEvent & evt) + { + add(evt); + } + + void checkFailures() + { + if (!next_failure_warning_.nanoseconds()) { + next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(15, 0); + } + + if (node_clock_->get_clock()->now() >= next_failure_warning_) { + if (incoming_message_count_ - messages_.size() == 0) { + return; + } + + double dropped_pct = static_cast(dropped_message_count_) / + static_cast(incoming_message_count_ - messages_.size()); + if (dropped_pct > 0.95) { + TF2_ROS_MESSAGEFILTER_WARN( + "Dropped %.2f%% of messages so far. Please turn the " + "[tf2_ros_message_filter.message_notifier] rosconsole logger to DEBUG for more " + "information.", + dropped_pct * 100); + next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(60, 0); + + if (static_cast(failed_out_the_back_count_) / + static_cast(dropped_message_count_) > 0.5) + { + TF2_ROS_MESSAGEFILTER_WARN( + " The majority of dropped messages were due to messages growing older than the TF " + "cache time. The last message's timestamp was: %f, and the last frame_id was: %s", + last_out_the_back_stamp_.seconds(), last_out_the_back_frame_.c_str()); + } + } + } + } + + // TODO(clalancette): reenable this once we have underlying support for callback queues +#if 0 + struct CBQueueCallback : public ros::CallbackInterface + { + CBQueueCallback( + MessageFilter * filter, const MEvent & event, bool success, FilterFailureReason reason) + : event_(event), + filter_(filter), + reason_(reason), + success_(success) + {} + + + virtual CallResult call() + { + if (success_) { + filter_->signalMessage(event_); + } else { + filter_->signalFailure(event_, reason_); + } + + return Success; + } + +private: + MEvent event_; + MessageFilter * filter_; + FilterFailureReason reason_; + bool success_; + }; +#endif + + void messageDropped(const MEvent & evt, FilterFailureReason reason) + { + // TODO(clalancette): reenable this once we have underlying support for callback queues +#if 0 + if (callback_queue_) { + ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason)); + callback_queue_->addCallback(cb, (uint64_t)this); + } else {} +#endif + { + signalFailure(evt, reason); + } + } + + void messageReady(const MEvent & evt) + { + // TODO(clalancette): reenable this once we have underlying support for callback queues +#if 0 + if (callback_queue_) { + ros::CallbackInterfacePtr cb(new CBQueueCallback( + this, evt, true, filter_failure_reasons::Unknown)); + callback_queue_->addCallback(cb, (uint64_t)this); + } else {} +#endif + { + this->signalMessage(evt); + } + } + + void signalFailure(const MEvent & evt, FilterFailureReason reason) + { + namespace mt = message_filters::message_traits; + const MConstPtr & message = evt.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + rclcpp::Time stamp = mt::TimeStamp::value(*message); + RCLCPP_INFO( + node_logging_->get_logger(), + "Message Filter dropping message: frame '%s' at time %.3f for reason '%s'", + frame_id.c_str(), stamp.seconds(), get_filter_failure_reason_string(reason).c_str()); + } + + static std::string stripSlash(const std::string & in) + { + if (!in.empty() && (in[0] == '/')) { + std::string out = in; + out.erase(0, 1); + return out; + } + + return in; + } + + ///< The node logging interface to use for any log messages + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + ///< The node clock interface to use to get the clock to use + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + ///< The Transformer used to determine if transformation data is available + BufferT & buffer_; + ///< The frames we need to be able to transform to before a message is ready + V_string target_frames_; + std::string target_frames_string_; + ///< A mutex to protect access to the target_frames_ list and target_frames_string. + std::mutex target_frames_mutex_; + ///< The maximum number of messages we queue up + uint32_t queue_size_; + + uint64_t next_handle_index_ = 0; + struct MessageInfo + { + MessageInfo() + : success_count(0) {} + + MEvent event; + std::vector handles; + uint64_t success_count; + }; + typedef std::list L_MessageInfo; + L_MessageInfo messages_; + + ///< The mutex used for locking message list operations + std::mutex messages_mutex_; + uint64_t expected_success_count_; + + bool warned_about_empty_frame_id_; + + uint64_t successful_transform_count_; + uint64_t failed_out_the_back_count_; + uint64_t transform_message_count_; + uint64_t incoming_message_count_; + uint64_t dropped_message_count_; + + rclcpp::Time last_out_the_back_stamp_; + std::string last_out_the_back_frame_; + + rclcpp::Time next_failure_warning_; + + ///< Provide additional tolerance on time for messages which are stamped + // but can have associated duration + rclcpp::Duration time_tolerance_ = rclcpp::Duration(0, 0); + + message_filters::Connection message_connection_; + message_filters::Connection message_connection_failure; + + // Timeout duration when calling the buffer method 'waitForTransform' + tf2::Duration buffer_timeout_; + + ///< The mutex used for locking TransformStampedFuture map operations + std::mutex ts_futures_mutex_; + + ///< Store the TransformStampedFuture returned by 'waitForTransform', + // to clear the callback in the Buffer if MessageFiltered object is destroyed. + std::unordered_map ts_futures_; +}; +} // namespace tf2_ros + +#endif // TF2_ROS__MESSAGE_FILTER_HPP_ diff --git a/tf2_ros/include/tf2_ros/qos.hpp b/tf2_ros/include/tf2_ros/qos.hpp index 777d1a9ca..75fa6a8eb 100644 --- a/tf2_ros/include/tf2_ros/qos.hpp +++ b/tf2_ros/include/tf2_ros/qos.hpp @@ -31,7 +31,7 @@ #define TF2_ROS__QOS_HPP_ #include -#include +#include namespace tf2_ros { diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h index a0d4f15b7..a0cb4c420 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h @@ -33,83 +33,6 @@ #ifndef TF2_ROS__STATIC_TRANSFORM_BROADCASTER_H_ #define TF2_ROS__STATIC_TRANSFORM_BROADCASTER_H_ -#include -#include - -#include "tf2_ros/visibility_control.h" - -#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" -#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -#include "tf2_ros/qos.hpp" - -namespace tf2_ros -{ - -/** \brief This class provides an easy way to publish coordinate frame transform information. - * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the - * necessary data needed for each message. */ - -class StaticTransformBroadcaster -{ -public: - /** \brief Node constructor */ - template> - StaticTransformBroadcaster( - NodeT && node, - const rclcpp::QoS & qos = StaticBroadcasterQoS(), - const rclcpp::PublisherOptionsWithAllocator & options = [] () { - rclcpp::PublisherOptionsWithAllocator options; - options.qos_overriding_options = rclcpp::QosOverridingOptions{ - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability}; - return options; - } ()) - : StaticTransformBroadcaster( - rclcpp::node_interfaces::get_node_parameters_interface(node), - rclcpp::node_interfaces::get_node_topics_interface(node), - qos, - options) - {} - - /** \brief Node interfaces constructor */ - template> - StaticTransformBroadcaster( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::QoS & qos = StaticBroadcasterQoS(), - const rclcpp::PublisherOptionsWithAllocator & options = [] () { - rclcpp::PublisherOptionsWithAllocator options; - options.qos_overriding_options = rclcpp::QosOverridingOptions{ - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability}; - return options; - } ()) - { - publisher_ = rclcpp::create_publisher( - node_parameters, node_topics, "/tf_static", qos, options); - } - - /** \brief Send a TransformStamped message - * The stamped data structure includes frame_id, and time, and parent_id already. */ - TF2_ROS_PUBLIC - void sendTransform(const geometry_msgs::msg::TransformStamped & transform); - - /** \brief Send a vector of TransformStamped messages - * The stamped data structure includes frame_id, and time, and parent_id already. */ - TF2_ROS_PUBLIC - void sendTransform(const std::vector & transforms); - -private: - /// Internal reference to ros::Node - rclcpp::Publisher::SharedPtr publisher_; - tf2_msgs::msg::TFMessage net_message_; -}; - -} // namespace tf2_ros +#include #endif // TF2_ROS__STATIC_TRANSFORM_BROADCASTER_H_ diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.hpp b/tf2_ros/include/tf2_ros/static_transform_broadcaster.hpp new file mode 100644 index 000000000..b92a75662 --- /dev/null +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.hpp @@ -0,0 +1,115 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +/** \author Tully Foote */ + +#ifndef TF2_ROS__STATIC_TRANSFORM_BROADCASTER_HPP_ +#define TF2_ROS__STATIC_TRANSFORM_BROADCASTER_HPP_ + +#include +#include + +#include "tf2_ros/visibility_control.hpp" + +#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/qos.hpp" + +namespace tf2_ros +{ + +/** \brief This class provides an easy way to publish coordinate frame transform information. + * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the + * necessary data needed for each message. */ + +class StaticTransformBroadcaster +{ +public: + /** \brief Node constructor */ + template> + StaticTransformBroadcaster( + NodeT && node, + const rclcpp::QoS & qos = StaticBroadcasterQoS(), + const rclcpp::PublisherOptionsWithAllocator & options = [] () { + rclcpp::PublisherOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; + } ()) + : StaticTransformBroadcaster( + rclcpp::node_interfaces::get_node_parameters_interface(node), + rclcpp::node_interfaces::get_node_topics_interface(node), + qos, + options) + {} + + /** \brief Node interfaces constructor */ + template> + StaticTransformBroadcaster( + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + const rclcpp::QoS & qos = StaticBroadcasterQoS(), + const rclcpp::PublisherOptionsWithAllocator & options = [] () { + rclcpp::PublisherOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; + } ()) + { + publisher_ = rclcpp::create_publisher( + node_parameters, node_topics, "/tf_static", qos, options); + } + + /** \brief Send a TransformStamped message + * The stamped data structure includes frame_id, and time, and parent_id already. */ + TF2_ROS_PUBLIC + void sendTransform(const geometry_msgs::msg::TransformStamped & transform); + + /** \brief Send a vector of TransformStamped messages + * The stamped data structure includes frame_id, and time, and parent_id already. */ + TF2_ROS_PUBLIC + void sendTransform(const std::vector & transforms); + +private: + /// Internal reference to ros::Node + rclcpp::Publisher::SharedPtr publisher_; + tf2_msgs::msg::TFMessage net_message_; +}; + +} // namespace tf2_ros + +#endif // TF2_ROS__STATIC_TRANSFORM_BROADCASTER_HPP_ diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster_node.hpp b/tf2_ros/include/tf2_ros/static_transform_broadcaster_node.hpp index 070d7de6b..6df0705bc 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster_node.hpp +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster_node.hpp @@ -32,8 +32,8 @@ #include -#include "tf2_ros/static_transform_broadcaster.h" -#include "tf2_ros/static_transform_broadcaster_visibility_control.h" +#include "tf2_ros/static_transform_broadcaster.hpp" +#include "tf2_ros/static_transform_broadcaster_visibility_control.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster_visibility_control.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster_visibility_control.h index e7ca23f77..9fd70061d 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster_visibility_control.h +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster_visibility_control.h @@ -30,35 +30,6 @@ #ifndef TF2_ROS__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_H_ #define TF2_ROS__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_H_ -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define STATIC_TRANSFORM_BROADCASTER_EXPORT __attribute__ ((dllexport)) - #define STATIC_TRANSFORM_BROADCASTER_IMPORT __attribute__ ((dllimport)) - #else - #define STATIC_TRANSFORM_BROADCASTER_EXPORT __declspec(dllexport) - #define STATIC_TRANSFORM_BROADCASTER_IMPORT __declspec(dllimport) - #endif - #ifdef STATIC_TRANSFORM_BROADCASTER_BUILDING_DLL - #define STATIC_TRANSFORM_BROADCASTER_PUBLIC STATIC_TRANSFORM_BROADCASTER_EXPORT - #else - #define STATIC_TRANSFORM_BROADCASTER_PUBLIC STATIC_TRANSFORM_BROADCASTER_IMPORT - #endif - #define STATIC_TRANSFORM_BROADCASTER_PUBLIC_TYPE STATIC_TRANSFORM_BROADCASTER_PUBLIC - #define STATIC_TRANSFORM_BROADCASTER_LOCAL -#else - #define STATIC_TRANSFORM_BROADCASTER_EXPORT __attribute__ ((visibility("default"))) - #define STATIC_TRANSFORM_BROADCASTER_IMPORT - #if __GNUC__ >= 4 - #define STATIC_TRANSFORM_BROADCASTER_PUBLIC __attribute__ ((visibility("default"))) - #define STATIC_TRANSFORM_BROADCASTER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define STATIC_TRANSFORM_BROADCASTER_PUBLIC - #define STATIC_TRANSFORM_BROADCASTER_LOCAL - #endif - #define STATIC_TRANSFORM_BROADCASTER_PUBLIC_TYPE -#endif +#include #endif // TF2_ROS__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_H_ diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster_visibility_control.hpp b/tf2_ros/include/tf2_ros/static_transform_broadcaster_visibility_control.hpp new file mode 100644 index 000000000..af3792ea7 --- /dev/null +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster_visibility_control.hpp @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2019 Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TF2_ROS__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_HPP_ +#define TF2_ROS__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define STATIC_TRANSFORM_BROADCASTER_EXPORT __attribute__ ((dllexport)) + #define STATIC_TRANSFORM_BROADCASTER_IMPORT __attribute__ ((dllimport)) + #else + #define STATIC_TRANSFORM_BROADCASTER_EXPORT __declspec(dllexport) + #define STATIC_TRANSFORM_BROADCASTER_IMPORT __declspec(dllimport) + #endif + #ifdef STATIC_TRANSFORM_BROADCASTER_BUILDING_DLL + #define STATIC_TRANSFORM_BROADCASTER_PUBLIC STATIC_TRANSFORM_BROADCASTER_EXPORT + #else + #define STATIC_TRANSFORM_BROADCASTER_PUBLIC STATIC_TRANSFORM_BROADCASTER_IMPORT + #endif + #define STATIC_TRANSFORM_BROADCASTER_PUBLIC_TYPE STATIC_TRANSFORM_BROADCASTER_PUBLIC + #define STATIC_TRANSFORM_BROADCASTER_LOCAL +#else + #define STATIC_TRANSFORM_BROADCASTER_EXPORT __attribute__ ((visibility("default"))) + #define STATIC_TRANSFORM_BROADCASTER_IMPORT + #if __GNUC__ >= 4 + #define STATIC_TRANSFORM_BROADCASTER_PUBLIC __attribute__ ((visibility("default"))) + #define STATIC_TRANSFORM_BROADCASTER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define STATIC_TRANSFORM_BROADCASTER_PUBLIC + #define STATIC_TRANSFORM_BROADCASTER_LOCAL + #endif + #define STATIC_TRANSFORM_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // TF2_ROS__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_HPP_ diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.h b/tf2_ros/include/tf2_ros/transform_broadcaster.h index e2cab966f..ab998fc5d 100644 --- a/tf2_ros/include/tf2_ros/transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/transform_broadcaster.h @@ -33,92 +33,6 @@ #ifndef TF2_ROS__TRANSFORM_BROADCASTER_H_ #define TF2_ROS__TRANSFORM_BROADCASTER_H_ -#include -#include - -#include "tf2_ros/visibility_control.h" - -#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" -#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -#include "tf2_ros/qos.hpp" - -namespace tf2_ros -{ - -/** \brief This class provides an easy way to publish coordinate frame transform information. - * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the - * necessary data needed for each message. */ - -class TransformBroadcaster -{ -public: - /** \brief Node constructor */ - template> - TransformBroadcaster( - NodeT && node, - const rclcpp::QoS & qos = DynamicBroadcasterQoS(), - const rclcpp::PublisherOptionsWithAllocator & options = [] () { - rclcpp::PublisherOptionsWithAllocator options; - options.qos_overriding_options = rclcpp::QosOverridingOptions{ - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability}; - return options; - } ()) - : TransformBroadcaster( - rclcpp::node_interfaces::get_node_parameters_interface(node), - rclcpp::node_interfaces::get_node_topics_interface(node), - qos, - options) - {} - - /** \brief Node interfaces constructor */ - template> - TransformBroadcaster( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::QoS & qos = DynamicBroadcasterQoS(), - const rclcpp::PublisherOptionsWithAllocator & options = [] () { - rclcpp::PublisherOptionsWithAllocator options; - options.qos_overriding_options = rclcpp::QosOverridingOptions{ - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability}; - return options; - } ()) - { - publisher_ = rclcpp::create_publisher( - node_parameters, node_topics, "/tf", qos, options); - } - - /** \brief Send a TransformStamped message - * - * The transform ʰTₐ added is from `child_frame_id`, `a` to `header.frame_id`, - * `h`. That is, position in `child_frame_id` ᵃp can be transformed to - * position in `header.frame_id` ʰp such that ʰp = ʰTₐ ᵃp . - * - */ - TF2_ROS_PUBLIC - void sendTransform(const geometry_msgs::msg::TransformStamped & transform); - - /** \brief Send a vector of TransformStamped messages - * - * The transforms ʰTₐ added are from `child_frame_id`, `a` to `header.frame_id`, - * `h`. That is, position in `child_frame_id` ᵃp can be transformed to - * position in `header.frame_id` ʰp such that ʰp = ʰTₐ ᵃp . - */ - TF2_ROS_PUBLIC - void sendTransform(const std::vector & transforms); - -private: - rclcpp::Publisher::SharedPtr publisher_; -}; - -} // namespace tf2_ros +#include #endif // TF2_ROS__TRANSFORM_BROADCASTER_H_ diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.hpp b/tf2_ros/include/tf2_ros/transform_broadcaster.hpp new file mode 100644 index 000000000..e4b2e6610 --- /dev/null +++ b/tf2_ros/include/tf2_ros/transform_broadcaster.hpp @@ -0,0 +1,124 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +/** \author Tully Foote */ + +#ifndef TF2_ROS__TRANSFORM_BROADCASTER_HPP_ +#define TF2_ROS__TRANSFORM_BROADCASTER_HPP_ + +#include +#include + +#include "tf2_ros/visibility_control.hpp" + +#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/qos.hpp" + +namespace tf2_ros +{ + +/** \brief This class provides an easy way to publish coordinate frame transform information. + * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the + * necessary data needed for each message. */ + +class TransformBroadcaster +{ +public: + /** \brief Node constructor */ + template> + TransformBroadcaster( + NodeT && node, + const rclcpp::QoS & qos = DynamicBroadcasterQoS(), + const rclcpp::PublisherOptionsWithAllocator & options = [] () { + rclcpp::PublisherOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; + } ()) + : TransformBroadcaster( + rclcpp::node_interfaces::get_node_parameters_interface(node), + rclcpp::node_interfaces::get_node_topics_interface(node), + qos, + options) + {} + + /** \brief Node interfaces constructor */ + template> + TransformBroadcaster( + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + const rclcpp::QoS & qos = DynamicBroadcasterQoS(), + const rclcpp::PublisherOptionsWithAllocator & options = [] () { + rclcpp::PublisherOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; + } ()) + { + publisher_ = rclcpp::create_publisher( + node_parameters, node_topics, "/tf", qos, options); + } + + /** \brief Send a TransformStamped message + * + * The transform ʰTₐ added is from `child_frame_id`, `a` to `header.frame_id`, + * `h`. That is, position in `child_frame_id` ᵃp can be transformed to + * position in `header.frame_id` ʰp such that ʰp = ʰTₐ ᵃp . + * + */ + TF2_ROS_PUBLIC + void sendTransform(const geometry_msgs::msg::TransformStamped & transform); + + /** \brief Send a vector of TransformStamped messages + * + * The transforms ʰTₐ added are from `child_frame_id`, `a` to `header.frame_id`, + * `h`. That is, position in `child_frame_id` ᵃp can be transformed to + * position in `header.frame_id` ʰp such that ʰp = ʰTₐ ᵃp . + */ + TF2_ROS_PUBLIC + void sendTransform(const std::vector & transforms); + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; + +} // namespace tf2_ros + +#endif // TF2_ROS__TRANSFORM_BROADCASTER_HPP_ diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index f7120ec55..a2658fb48 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -32,203 +32,6 @@ #ifndef TF2_ROS__TRANSFORM_LISTENER_H_ #define TF2_ROS__TRANSFORM_LISTENER_H_ -#include -#include -#include -#include - -#include "tf2/buffer_core.hpp" -#include "tf2/time.hpp" -#include "tf2_ros/visibility_control.h" - -#include "tf2_msgs/msg/tf_message.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "tf2_ros/qos.hpp" - -namespace tf2_ros -{ - -namespace detail -{ -template> -rclcpp::SubscriptionOptionsWithAllocator -get_default_transform_listener_sub_options() -{ - rclcpp::SubscriptionOptionsWithAllocator options; - options.qos_overriding_options = rclcpp::QosOverridingOptions{ - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability}; - return options; -} - -template> -rclcpp::SubscriptionOptionsWithAllocator -get_default_transform_listener_static_sub_options() -{ - rclcpp::SubscriptionOptionsWithAllocator options; - options.qos_overriding_options = rclcpp::QosOverridingOptions{ - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability}; - return options; -} -} // namespace detail - -/** \brief This class provides an easy way to request and receive coordinate frame transform information. - */ -class TransformListener -{ -public: - /** \brief Simplified constructor for transform listener. - * - * This constructor will create a new ROS 2 node under the hood. - * If you already have access to a ROS 2 node and you want to associate the 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); - - /** \brief Node constructor */ - template> - TransformListener( - tf2::BufferCore & buffer, - NodeT && node, - bool spin_thread = true, - const rclcpp::QoS & qos = DynamicListenerQoS(), - const rclcpp::QoS & static_qos = StaticListenerQoS(), - const rclcpp::SubscriptionOptionsWithAllocator & options = - detail::get_default_transform_listener_sub_options(), - const rclcpp::SubscriptionOptionsWithAllocator & static_options = - detail::get_default_transform_listener_static_sub_options()) - : TransformListener( - buffer, - node->get_node_base_interface(), - node->get_node_logging_interface(), - node->get_node_parameters_interface(), - node->get_node_topics_interface(), - spin_thread, - qos, - static_qos, - options, - static_options) - {} - - /** \brief Node interface constructor */ - template> - TransformListener( - 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 & qos = DynamicListenerQoS(), - const rclcpp::QoS & static_qos = StaticListenerQoS(), - const rclcpp::SubscriptionOptionsWithAllocator & options = - detail::get_default_transform_listener_sub_options(), - const rclcpp::SubscriptionOptionsWithAllocator & static_options = - detail::get_default_transform_listener_static_sub_options()) - : buffer_(buffer) - { - init( - node_base, - node_logging, - node_parameters, - node_topics, - spin_thread, - qos, - static_qos, - options, - static_options); - } - - TF2_ROS_PUBLIC - virtual ~TransformListener(); - - /// Callback function for ros message subscription - TF2_ROS_PUBLIC - virtual void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static); - -private: - template> - void init( - 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, - const rclcpp::QoS & qos, - const rclcpp::QoS & static_qos, - const rclcpp::SubscriptionOptionsWithAllocator & options, - const rclcpp::SubscriptionOptionsWithAllocator & static_options) - { - spin_thread_ = spin_thread; - node_base_interface_ = node_base; - node_logging_interface_ = node_logging; - - using callback_t = std::function; - callback_t cb = std::bind( - &TransformListener::subscription_callback, this, std::placeholders::_1, false); - callback_t static_cb = std::bind( - &TransformListener::subscription_callback, this, std::placeholders::_1, true); - - if (spin_thread_) { - // 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 tf_options = options; - rclcpp::SubscriptionOptionsWithAllocator tf_static_options = static_options; - tf_options.callback_group = callback_group_; - tf_static_options.callback_group = callback_group_; - - message_subscription_tf_ = rclcpp::create_subscription( - node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options); - message_subscription_tf_static_ = rclcpp::create_subscription( - node_parameters, - node_topics, - "/tf_static", - static_qos, - std::move(static_cb), - tf_static_options); - - // Create executor with dedicated thread to spin. - executor_ = std::make_shared(); - executor_->add_callback_group(callback_group_, node_base_interface_); - dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); - // Tell the buffer we have a dedicated thread to enable timeouts - buffer_.setUsingDedicatedThread(true); - } else { - message_subscription_tf_ = rclcpp::create_subscription( - node_parameters, node_topics, "/tf", qos, std::move(cb), options); - message_subscription_tf_static_ = rclcpp::create_subscription( - node_parameters, - node_topics, - "/tf_static", - static_qos, - std::move(static_cb), - static_options); - } - } - - bool spin_thread_{false}; - std::unique_ptr dedicated_listener_thread_ {nullptr}; - rclcpp::Executor::SharedPtr executor_ {nullptr}; - - rclcpp::Node::SharedPtr optional_default_node_ {nullptr}; - rclcpp::Subscription::SharedPtr - message_subscription_tf_ {nullptr}; - rclcpp::Subscription::SharedPtr - message_subscription_tf_static_ {nullptr}; - tf2::BufferCore & buffer_; - tf2::TimePoint last_update_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ {nullptr}; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_ {nullptr}; - rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; -}; -} // namespace tf2_ros +#include #endif // TF2_ROS__TRANSFORM_LISTENER_H_ diff --git a/tf2_ros/include/tf2_ros/transform_listener.hpp b/tf2_ros/include/tf2_ros/transform_listener.hpp new file mode 100644 index 000000000..5d88f7f26 --- /dev/null +++ b/tf2_ros/include/tf2_ros/transform_listener.hpp @@ -0,0 +1,234 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Tully Foote */ + +#ifndef TF2_ROS__TRANSFORM_LISTENER_HPP_ +#define TF2_ROS__TRANSFORM_LISTENER_HPP_ + +#include +#include +#include +#include + +#include "tf2/buffer_core.hpp" +#include "tf2/time.hpp" +#include "tf2_ros/visibility_control.hpp" + +#include "tf2_msgs/msg/tf_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "tf2_ros/qos.hpp" + +namespace tf2_ros +{ + +namespace detail +{ +template> +rclcpp::SubscriptionOptionsWithAllocator +get_default_transform_listener_sub_options() +{ + rclcpp::SubscriptionOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; +} + +template> +rclcpp::SubscriptionOptionsWithAllocator +get_default_transform_listener_static_sub_options() +{ + rclcpp::SubscriptionOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; +} +} // namespace detail + +/** \brief This class provides an easy way to request and receive coordinate frame transform information. + */ +class TransformListener +{ +public: + /** \brief Simplified constructor for transform listener. + * + * This constructor will create a new ROS 2 node under the hood. + * If you already have access to a ROS 2 node and you want to associate the 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); + + /** \brief Node constructor */ + template> + TransformListener( + tf2::BufferCore & buffer, + NodeT && node, + bool spin_thread = true, + const rclcpp::QoS & qos = DynamicListenerQoS(), + const rclcpp::QoS & static_qos = StaticListenerQoS(), + const rclcpp::SubscriptionOptionsWithAllocator & options = + detail::get_default_transform_listener_sub_options(), + const rclcpp::SubscriptionOptionsWithAllocator & static_options = + detail::get_default_transform_listener_static_sub_options()) + : TransformListener( + buffer, + node->get_node_base_interface(), + node->get_node_logging_interface(), + node->get_node_parameters_interface(), + node->get_node_topics_interface(), + spin_thread, + qos, + static_qos, + options, + static_options) + {} + + /** \brief Node interface constructor */ + template> + TransformListener( + 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 & qos = DynamicListenerQoS(), + const rclcpp::QoS & static_qos = StaticListenerQoS(), + const rclcpp::SubscriptionOptionsWithAllocator & options = + detail::get_default_transform_listener_sub_options(), + const rclcpp::SubscriptionOptionsWithAllocator & static_options = + detail::get_default_transform_listener_static_sub_options()) + : buffer_(buffer) + { + init( + node_base, + node_logging, + node_parameters, + node_topics, + spin_thread, + qos, + static_qos, + options, + static_options); + } + + TF2_ROS_PUBLIC + virtual ~TransformListener(); + + /// Callback function for ros message subscription + TF2_ROS_PUBLIC + virtual void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static); + +private: + template> + void init( + 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, + const rclcpp::QoS & qos, + const rclcpp::QoS & static_qos, + const rclcpp::SubscriptionOptionsWithAllocator & options, + const rclcpp::SubscriptionOptionsWithAllocator & static_options) + { + spin_thread_ = spin_thread; + node_base_interface_ = node_base; + node_logging_interface_ = node_logging; + + using callback_t = std::function; + callback_t cb = std::bind( + &TransformListener::subscription_callback, this, std::placeholders::_1, false); + callback_t static_cb = std::bind( + &TransformListener::subscription_callback, this, std::placeholders::_1, true); + + if (spin_thread_) { + // 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 tf_options = options; + rclcpp::SubscriptionOptionsWithAllocator tf_static_options = static_options; + tf_options.callback_group = callback_group_; + tf_static_options.callback_group = callback_group_; + + message_subscription_tf_ = rclcpp::create_subscription( + node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options); + message_subscription_tf_static_ = rclcpp::create_subscription( + node_parameters, + node_topics, + "/tf_static", + static_qos, + std::move(static_cb), + tf_static_options); + + // Create executor with dedicated thread to spin. + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node_base_interface_); + dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); + // Tell the buffer we have a dedicated thread to enable timeouts + buffer_.setUsingDedicatedThread(true); + } else { + message_subscription_tf_ = rclcpp::create_subscription( + node_parameters, node_topics, "/tf", qos, std::move(cb), options); + message_subscription_tf_static_ = rclcpp::create_subscription( + node_parameters, + node_topics, + "/tf_static", + static_qos, + std::move(static_cb), + static_options); + } + } + + bool spin_thread_{false}; + std::unique_ptr dedicated_listener_thread_ {nullptr}; + rclcpp::Executor::SharedPtr executor_ {nullptr}; + + rclcpp::Node::SharedPtr optional_default_node_ {nullptr}; + rclcpp::Subscription::SharedPtr + message_subscription_tf_ {nullptr}; + rclcpp::Subscription::SharedPtr + message_subscription_tf_static_ {nullptr}; + tf2::BufferCore & buffer_; + tf2::TimePoint last_update_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ {nullptr}; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_ {nullptr}; + rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; +}; +} // namespace tf2_ros + +#endif // TF2_ROS__TRANSFORM_LISTENER_HPP_ diff --git a/tf2_ros/include/tf2_ros/visibility_control.h b/tf2_ros/include/tf2_ros/visibility_control.h index 61573d203..6230a674d 100644 --- a/tf2_ros/include/tf2_ros/visibility_control.h +++ b/tf2_ros/include/tf2_ros/visibility_control.h @@ -30,35 +30,6 @@ #ifndef TF2_ROS__VISIBILITY_CONTROL_H_ #define TF2_ROS__VISIBILITY_CONTROL_H_ -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define TF2_ROS_EXPORT __attribute__ ((dllexport)) - #define TF2_ROS_IMPORT __attribute__ ((dllimport)) - #else - #define TF2_ROS_EXPORT __declspec(dllexport) - #define TF2_ROS_IMPORT __declspec(dllimport) - #endif - #ifdef TF2_ROS_BUILDING_DLL - #define TF2_ROS_PUBLIC TF2_ROS_EXPORT - #else - #define TF2_ROS_PUBLIC TF2_ROS_IMPORT - #endif - #define TF2_ROS_PUBLIC_TYPE TF2_ROS_PUBLIC - #define TF2_ROS_LOCAL -#else - #define TF2_ROS_EXPORT __attribute__ ((visibility("default"))) - #define TF2_ROS_IMPORT - #if __GNUC__ >= 4 - #define TF2_ROS_PUBLIC __attribute__ ((visibility("default"))) - #define TF2_ROS_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define TF2_ROS_PUBLIC - #define TF2_ROS_LOCAL - #endif - #define TF2_ROS_PUBLIC_TYPE -#endif +#include #endif // TF2_ROS__VISIBILITY_CONTROL_H_ diff --git a/tf2_ros/include/tf2_ros/visibility_control.hpp b/tf2_ros/include/tf2_ros/visibility_control.hpp new file mode 100644 index 000000000..b88c4fc46 --- /dev/null +++ b/tf2_ros/include/tf2_ros/visibility_control.hpp @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2017 Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TF2_ROS__VISIBILITY_CONTROL_HPP_ +#define TF2_ROS__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define TF2_ROS_EXPORT __attribute__ ((dllexport)) + #define TF2_ROS_IMPORT __attribute__ ((dllimport)) + #else + #define TF2_ROS_EXPORT __declspec(dllexport) + #define TF2_ROS_IMPORT __declspec(dllimport) + #endif + #ifdef TF2_ROS_BUILDING_DLL + #define TF2_ROS_PUBLIC TF2_ROS_EXPORT + #else + #define TF2_ROS_PUBLIC TF2_ROS_IMPORT + #endif + #define TF2_ROS_PUBLIC_TYPE TF2_ROS_PUBLIC + #define TF2_ROS_LOCAL +#else + #define TF2_ROS_EXPORT __attribute__ ((visibility("default"))) + #define TF2_ROS_IMPORT + #if __GNUC__ >= 4 + #define TF2_ROS_PUBLIC __attribute__ ((visibility("default"))) + #define TF2_ROS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define TF2_ROS_PUBLIC + #define TF2_ROS_LOCAL + #endif + #define TF2_ROS_PUBLIC_TYPE +#endif + +#endif // TF2_ROS__VISIBILITY_CONTROL_HPP_ diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 10ee57cc2..a5da4c57c 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -30,7 +30,7 @@ /** \author Wim Meeussen */ -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include #include diff --git a/tf2_ros/src/buffer_client.cpp b/tf2_ros/src/buffer_client.cpp index 32702ab32..e09f0e2e6 100644 --- a/tf2_ros/src/buffer_client.cpp +++ b/tf2_ros/src/buffer_client.cpp @@ -35,7 +35,7 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#include "tf2_ros/buffer_client.h" +#include "tf2_ros/buffer_client.hpp" #include #include diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index aa2b6da1c..229962c51 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -35,8 +35,8 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#include // Only needed for toMsg() and fromMsg() -#include +#include // Only needed for toMsg() and fromMsg() +#include #include #include diff --git a/tf2_ros/src/buffer_server_main.cpp b/tf2_ros/src/buffer_server_main.cpp index 7b931b08e..cdc4cd94d 100644 --- a/tf2_ros/src/buffer_server_main.cpp +++ b/tf2_ros/src/buffer_server_main.cpp @@ -37,9 +37,9 @@ #include -#include "tf2_ros/buffer.h" -#include "tf2_ros/buffer_server.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/buffer_server.hpp" +#include "tf2_ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_ros/src/create_timer_ros.cpp b/tf2_ros/src/create_timer_ros.cpp index 2bc1e1dee..2f75379aa 100644 --- a/tf2_ros/src/create_timer_ros.cpp +++ b/tf2_ros/src/create_timer_ros.cpp @@ -27,7 +27,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include #include diff --git a/tf2_ros/src/static_transform_broadcaster.cpp b/tf2_ros/src/static_transform_broadcaster.cpp index 6ea8add20..35fe3f2de 100644 --- a/tf2_ros/src/static_transform_broadcaster.cpp +++ b/tf2_ros/src/static_transform_broadcaster.cpp @@ -30,7 +30,7 @@ /** \author Tully Foote */ -#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.hpp" #include diff --git a/tf2_ros/src/tf2_echo.cpp b/tf2_ros/src/tf2_echo.cpp index 44bd777d7..f8256c3a3 100644 --- a/tf2_ros/src/tf2_echo.cpp +++ b/tf2_ros/src/tf2_echo.cpp @@ -43,8 +43,8 @@ #include #include -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_ros/src/tf2_monitor.cpp b/tf2_ros/src/tf2_monitor.cpp index 936527fac..4e35da1aa 100644 --- a/tf2_ros/src/tf2_monitor.cpp +++ b/tf2_ros/src/tf2_monitor.cpp @@ -37,8 +37,8 @@ #include #include -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "tf2_ros/qos.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_ros/src/transform_broadcaster.cpp b/tf2_ros/src/transform_broadcaster.cpp index ee37b558b..f4a2566ac 100644 --- a/tf2_ros/src/transform_broadcaster.cpp +++ b/tf2_ros/src/transform_broadcaster.cpp @@ -30,7 +30,7 @@ /** \author Tully Foote */ -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 5e31b6a8d..52467ffb2 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -35,7 +35,7 @@ #include #include -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" namespace tf2_ros { diff --git a/tf2_ros/test/listener_unittest.cpp b/tf2_ros/test/listener_unittest.cpp index cb4539ee6..c41baa409 100644 --- a/tf2_ros/test/listener_unittest.cpp +++ b/tf2_ros/test/listener_unittest.cpp @@ -34,8 +34,8 @@ #include "gtest/gtest.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" #include "builtin_interfaces/msg/time.hpp" diff --git a/tf2_ros/test/message_filter_test.cpp b/tf2_ros/test/message_filter_test.cpp index d6fd9c18c..aee82a270 100644 --- a/tf2_ros/test/message_filter_test.cpp +++ b/tf2_ros/test/message_filter_test.cpp @@ -37,11 +37,11 @@ #include "message_filters/simple_filter.h" #include "message_filters/message_traits.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/message_filter.h" -#include "tf2_ros/static_transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/message_filter.hpp" +#include "tf2_ros/static_transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point_stamped.hpp" diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index cec7f1b07..877f91c7a 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -37,10 +37,10 @@ #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/create_timer_interface.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/create_timer_interface.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" class MockCreateTimer final : public tf2_ros::CreateTimerInterface { diff --git a/tf2_ros/test/test_buffer_client.cpp b/tf2_ros/test/test_buffer_client.cpp index 75d997b67..d810e45aa 100644 --- a/tf2_ros/test/test_buffer_client.cpp +++ b/tf2_ros/test/test_buffer_client.cpp @@ -39,8 +39,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/buffer_client.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/buffer_client.hpp" static const char ACTION_NAME[] = "test_tf2_buffer_action"; diff --git a/tf2_ros/test/test_buffer_server.cpp b/tf2_ros/test/test_buffer_server.cpp index cd1a65861..13c4b00f9 100644 --- a/tf2_ros/test/test_buffer_server.cpp +++ b/tf2_ros/test/test_buffer_server.cpp @@ -38,8 +38,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/buffer_server.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/buffer_server.hpp" static const char ACTION_NAME[] = "test_tf2_buffer_action"; diff --git a/tf2_ros/test/test_static_transform_broadcaster.cpp b/tf2_ros/test/test_static_transform_broadcaster.cpp index 5419681c0..2acd958a4 100644 --- a/tf2_ros/test/test_static_transform_broadcaster.cpp +++ b/tf2_ros/test/test_static_transform_broadcaster.cpp @@ -28,10 +28,11 @@ */ #include -#include #include +#include + #include "node_wrapper.hpp" class CustomNode : public rclcpp::Node diff --git a/tf2_ros/test/test_transform_broadcaster.cpp b/tf2_ros/test/test_transform_broadcaster.cpp index c3c98840a..31715d976 100644 --- a/tf2_ros/test/test_transform_broadcaster.cpp +++ b/tf2_ros/test/test_transform_broadcaster.cpp @@ -28,10 +28,11 @@ */ #include -#include #include +#include + #include "node_wrapper.hpp" class CustomNode : public rclcpp::Node diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp index 46c633e28..66b34bba2 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -28,11 +28,15 @@ */ #include -#include -#include #include +#include +#include +#include +#include + + #include "node_wrapper.hpp" class CustomNode : public rclcpp::Node diff --git a/tf2_ros/test/time_reset_test.cpp b/tf2_ros/test/time_reset_test.cpp index ad18be4d0..ea8981070 100644 --- a/tf2_ros/test/time_reset_test.cpp +++ b/tf2_ros/test/time_reset_test.cpp @@ -32,9 +32,9 @@ #include "gtest/gtest.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" #include "builtin_interfaces/msg/time.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_ros/test/velocity_test.cpp b/tf2_ros/test/velocity_test.cpp index 1d18980bc..7f87b2f86 100644 --- a/tf2_ros/test/velocity_test.cpp +++ b/tf2_ros/test/velocity_test.cpp @@ -32,10 +32,10 @@ #include #include -#include "tf2_ros/buffer.h" -#include "tf2_ros/create_timer_interface.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/create_timer_interface.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" // #include "tf/LinearMath/Vector3.h" diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp index f518e964e..b0d48f983 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp @@ -47,7 +47,7 @@ #pragma GCC diagnostic pop #endif -#include "tf2_ros/buffer_interface.h" +#include "tf2_ros/buffer_interface.hpp" #include "tf2/convert.hpp" #include "tf2/time.hpp" diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index dd3985fdb..cde898a08 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -30,8 +30,8 @@ #include "gtest/gtest.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "builtin_interfaces/msg/time.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"