From 86dad644abdc248907995762eadec42ea3ad24e1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 14 Sep 2020 14:48:35 +0000 Subject: [PATCH] Style fixup in tf2_ros. Signed-off-by: Chris Lalancette --- tf2_ros/CMakeLists.txt | 11 + .../include/tf2_ros/async_buffer_interface.h | 29 +- tf2_ros/include/tf2_ros/buffer.h | 493 +++++++++--------- tf2_ros/include/tf2_ros/buffer_client.h | 414 +++++++-------- tf2_ros/include/tf2_ros/buffer_interface.h | 249 +++++---- tf2_ros/include/tf2_ros/buffer_server.h | 163 +++--- .../include/tf2_ros/create_timer_interface.h | 26 +- tf2_ros/include/tf2_ros/create_timer_ros.h | 24 +- tf2_ros/include/tf2_ros/message_filter.h | 194 ++++--- tf2_ros/include/tf2_ros/qos.hpp | 15 +- .../tf2_ros/static_transform_broadcaster.h | 32 +- .../static_transform_broadcaster_node.hpp | 13 +- ...transform_broadcaster_visibility_control.h | 6 +- .../include/tf2_ros/transform_broadcaster.h | 43 +- tf2_ros/include/tf2_ros/transform_listener.h | 31 +- tf2_ros/package.xml | 2 + tf2_ros/src/buffer.cpp | 172 +++--- tf2_ros/src/buffer_client.cpp | 302 ++++++----- tf2_ros/src/buffer_server.cpp | 337 ++++++------ tf2_ros/src/buffer_server_main.cpp | 6 +- tf2_ros/src/create_timer_ros.cpp | 10 +- tf2_ros/src/static_transform_broadcaster.cpp | 38 +- .../src/static_transform_broadcaster_node.cpp | 7 +- .../static_transform_broadcaster_program.cpp | 11 +- tf2_ros/src/tf2_echo.cpp | 117 ++--- tf2_ros/src/tf2_monitor.cpp | 300 +++++------ tf2_ros/src/transform_broadcaster.cpp | 26 +- tf2_ros/src/transform_listener.cpp | 22 +- tf2_ros/test/listener_unittest.cpp | 18 +- tf2_ros/test/message_filter_test.cpp | 14 +- tf2_ros/test/node_wrapper.hpp | 3 + tf2_ros/test/test_buffer.cpp | 34 +- tf2_ros/test/test_buffer_client.cpp | 27 +- tf2_ros/test/test_buffer_server.cpp | 23 +- .../test_static_transform_broadcaster.cpp | 2 + tf2_ros/test/test_transform_broadcaster.cpp | 2 + tf2_ros/test/test_transform_listener.cpp | 3 + tf2_ros/test/time_reset_test.cpp | 21 +- 38 files changed, 1703 insertions(+), 1537 deletions(-) diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index a9b33135f..c4a2b50d9 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -141,7 +141,18 @@ install(DIRECTORY include/${PROJECT_NAME}/ # Tests if(BUILD_TESTING) + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + + ament_cppcheck(LANGUAGE "c++") + ament_cpplint() + ament_lint_cmake() + ament_uncrustify(LANGUAGE "c++") + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_buffer test/test_buffer.cpp) ament_target_dependencies(test_buffer ${dependencies} diff --git a/tf2_ros/include/tf2_ros/async_buffer_interface.h b/tf2_ros/include/tf2_ros/async_buffer_interface.h index 19ef7f632..20ce56d18 100644 --- a/tf2_ros/include/tf2_ros/async_buffer_interface.h +++ b/tf2_ros/include/tf2_ros/async_buffer_interface.h @@ -27,22 +27,24 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_ROS__ASYNC_BUFFER_INTERFACE_H -#define TF2_ROS__ASYNC_BUFFER_INTERFACE_H +#ifndef TF2_ROS__ASYNC_BUFFER_INTERFACE_H_ +#define TF2_ROS__ASYNC_BUFFER_INTERFACE_H_ -#include -#include +#include +#include +#include #include -#include -#include +#include +#include +#include namespace tf2_ros { using TransformStampedFuture = std::shared_future; -using TransformReadyCallback = std::function; +using TransformReadyCallback = std::function; /** * \brief Abstract interface for asynchronous operations on a `tf2::BufferCoreInterface`. @@ -55,8 +57,7 @@ class AsyncBufferInterface virtual ~AsyncBufferInterface() = default; - /** - * \brief Wait for a transform between two frames to become available. + /** \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. @@ -69,13 +70,13 @@ class AsyncBufferInterface TF2_ROS_PUBLIC virtual TransformStampedFuture waitForTransform( - const std::string& target_frame, - const std::string& source_frame, - const tf2::TimePoint& time, - const tf2::Duration& timeout, + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration & timeout, TransformReadyCallback callback) = 0; }; // class AsyncBufferInterface } // namespace tf2_ros -#endif // TF2_ROS__ASYNC_BUFFER_INTERFACE_H +#endif // TF2_ROS__ASYNC_BUFFER_INTERFACE_H_ diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 090bfd390..76bb8a916 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -29,256 +29,275 @@ /** \author Wim Meeussen */ -#ifndef TF2_ROS_BUFFER_H -#define TF2_ROS_BUFFER_H - -#include -#include -#include +#ifndef TF2_ROS__BUFFER_H_ +#define TF2_ROS__BUFFER_H_ #include #include #include #include #include +#include + +#include #include #include -//TODO(tfoote) review removal #include +#include +#include +#include +#include +#include namespace tf2_ros { - /** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type. +/** \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; + + /** \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 + */ + TF2_ROS_PUBLIC Buffer( + rclcpp::Clock::SharedPtr clock, + tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), + rclcpp::Node::SharedPtr node = rclcpp::Node::SharedPtr()); + + /** \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(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 * - * 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. + * 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) */ - class Buffer: public BufferInterface, public AsyncBufferInterface, public tf2::BufferCore + 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(0)) const { - public: - using tf2::BufferCore::lookupTransform; - using tf2::BufferCore::canTransform; - - /** - * @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 - * @return - */ - TF2_ROS_PUBLIC Buffer( - rclcpp::Clock::SharedPtr clock, - tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), - rclcpp::Node::SharedPtr node = rclcpp::Node::SharedPtr()); - - /** \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(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(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(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(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); - } - - 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); - - bool getFrames(const tf2_msgs::srv::FrameGraph::Request::SharedPtr req, tf2_msgs::srv::FrameGraph::Response::SharedPtr res) ; - - void onTimeJump(const rcl_time_jump_t & jump); - - // conditionally error if dedicated_thread unset. - bool checkAndErrorDedicatedThreadPresent(std::string* errstr) const; - - // framegraph service - rclcpp::Service::SharedPtr frames_server_; - - /// \brief A clock to use for time and sleeping - rclcpp::Clock::SharedPtr clock_; - - /// \brief A node to advertise the view_frames service - rclcpp::Node::SharedPtr node_; - - /// \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_; - }; // class - -static const std::string 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 seperate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance."; - - -} // namespace - -#endif // TF2_ROS_BUFFER_H + 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(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(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); + } + + 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); + + bool getFrames( + const tf2_msgs::srv::FrameGraph::Request::SharedPtr req, + tf2_msgs::srv::FrameGraph::Response::SharedPtr res); + + void onTimeJump(const rcl_time_jump_t & jump); + + // conditionally error if dedicated_thread unset. + bool checkAndErrorDedicatedThreadPresent(std::string * errstr) const; + + // framegraph service + rclcpp::Service::SharedPtr frames_server_; + + /// \brief A clock to use for time and sleeping + rclcpp::Clock::SharedPtr clock_; + + /// \brief A node to advertise the view_frames service + rclcpp::Node::SharedPtr node_; + + /// \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_H_ diff --git a/tf2_ros/include/tf2_ros/buffer_client.h b/tf2_ros/include/tf2_ros/buffer_client.h index d217e5b62..ceb21a9eb 100644 --- a/tf2_ros/include/tf2_ros/buffer_client.h +++ b/tf2_ros/include/tf2_ros/buffer_client.h @@ -34,222 +34,230 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef TF2_ROS_BUFFER_CLIENT_H_ -#define TF2_ROS_BUFFER_CLIENT_H_ -#include -#include +#ifndef TF2_ROS__BUFFER_CLIENT_H_ +#define TF2_ROS__BUFFER_CLIENT_H_ #include #include +#include + +#include +#include +#include + +#include +#include namespace tf2_ros { - /** - * \ brief Base class for lookup transform action goal exceptions. - */ - class LookupTransformGoalException : public std::runtime_error +/** + * \ 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) { - public: - TF2_ROS_PUBLIC - explicit LookupTransformGoalException(const std::string & message) - : std::runtime_error(message) - { - } - }; - - class GoalRejectedException : public LookupTransformGoalException + } +}; + +class GoalRejectedException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit GoalRejectedException(const std::string & message) + : LookupTransformGoalException(message) { - public: - TF2_ROS_PUBLIC - explicit GoalRejectedException(const std::string & message) - : LookupTransformGoalException(message) - { - } - }; - - class GoalAbortedException : public LookupTransformGoalException + } +}; + +class GoalAbortedException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit GoalAbortedException(const std::string & message) + : LookupTransformGoalException(message) { - public: - TF2_ROS_PUBLIC - explicit GoalAbortedException(const std::string & message) - : LookupTransformGoalException(message) - { - } - }; - - class GoalCanceledException : public LookupTransformGoalException + } +}; + +class GoalCanceledException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit GoalCanceledException(const std::string & message) + : LookupTransformGoalException(message) { - public: - TF2_ROS_PUBLIC - explicit GoalCanceledException(const std::string & message) - : LookupTransformGoalException(message) - { - } - }; - - class UnexpectedResultCodeException : public LookupTransformGoalException + } +}; + +class UnexpectedResultCodeException : public LookupTransformGoalException +{ +public: + TF2_ROS_PUBLIC + explicit UnexpectedResultCodeException(const std::string & message) + : LookupTransformGoalException(message) { - 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. + } +}; + +/** \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 * - * BufferClient uses actions to coordinate waiting for available transforms. + * \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. * - * You can use this class with a tf2_ros::BufferServer and tf2_ros::TransformListener in a separate process. + * \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 */ - class BufferClient : public BufferInterface + 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)) { - 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_; - }; -} -#endif + 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_H_ diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index 73af82195..80a038886 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -29,90 +29,97 @@ /** \author Wim Meeussen */ -#ifndef TF2_ROS_BUFFER_INTERFACE_H -#define TF2_ROS_BUFFER_INTERFACE_H +#ifndef TF2_ROS__BUFFER_INTERFACE_H_ +#define TF2_ROS__BUFFER_INTERFACE_H_ -#include #include #include #include -#include -#include #include +#include +#include +#include +#include + +#include +#include +#include +#include + namespace tf2_ros { - using TransformStampedFuture = std::shared_future; - using TransformReadyCallback = std::function; +using TransformStampedFuture = std::shared_future; +using TransformReadyCallback = std::function; - 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 = (int32_t)s.count(); - time_msg.nanosec = (uint32_t)(ns.count() % 1000000000ull); - return time_msg; - } +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 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 = (int32_t)s.count(); - duration_msg.nanosec = (uint32_t)(ns.count() % 1000000000ull); - return duration_msg; - } +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 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 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 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 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 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)); - } +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. @@ -132,15 +139,16 @@ class BufferInterface */ 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; + 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 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 * @@ -148,10 +156,11 @@ class BufferInterface * 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; + 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 @@ -160,12 +169,14 @@ class BufferInterface * \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 + * \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; + 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 @@ -175,13 +186,15 @@ class BufferInterface * \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 + * \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; + 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 @@ -194,12 +207,14 @@ class BufferInterface * \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 + 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)); + tf2::doTransform( + in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout)); return out; } @@ -214,12 +229,13 @@ class BufferInterface * \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 + template + T transform( + const T & in, + const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const { T out; - return transform(in, out, target_frame, timeout); + return this->transform(in, out, target_frame, timeout); } /** \brief Transform an input into the target frame and convert to a specified output type. @@ -239,11 +255,12 @@ class BufferInterface * \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 + template + B & transform( + const A & in, B & out, + const std::string & target_frame, tf2::Duration timeout = tf2::durationFromSec(0.0)) const { - A copy = transform(in, target_frame, timeout); + A copy = this->transform(in, target_frame, timeout); tf2::convert(copy, out); return out; } @@ -263,15 +280,18 @@ class BufferInterface * \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 + 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)); + tf2::doTransform( + in, out, lookupTransform( + target_frame, target_time, + tf2::getFrameId(in), tf2::getTimestamp(in), + fixed_frame, timeout)); return out; } @@ -290,13 +310,14 @@ class BufferInterface * \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 + 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 transform(in, out, target_frame, target_time, fixed_frame, timeout); + 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). @@ -320,13 +341,14 @@ class BufferInterface * \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 + 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 = transform(in, target_frame, target_time, fixed_frame, timeout); + A copy = this->transform(in, target_frame, target_time, fixed_frame, timeout); tf2::convert(copy, out); return out; } @@ -334,9 +356,8 @@ class BufferInterface virtual ~BufferInterface() { } - }; // class - +}; -} // namespace +} // namespace tf2_ros -#endif // TF2_ROS_BUFFER_INTERFACE_H +#endif // TF2_ROS__BUFFER_INTERFACE_H_ diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 83d536986..756116053 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -34,10 +34,13 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef TF2_ROS_BUFFER_SERVER_H_ -#define TF2_ROS_BUFFER_SERVER_H_ -#include -#include + +#ifndef TF2_ROS__BUFFER_SERVER_H_ +#define TF2_ROS__BUFFER_SERVER_H_ + +#include +#include +#include #include #include @@ -45,84 +48,86 @@ #include #include -#include -#include +#include +#include +#include +#include +#include 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. +/** \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). */ - class BufferServer + 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()) { - private: - using LookupTransformAction = tf2_msgs::action::LookupTransform; - using GoalHandle = std::shared_ptr>; - - struct GoalInfo - { - GoalHandle handle; - tf2::TimePoint end_time; - }; - - 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()) - { - using namespace std::placeholders; - - server_ = rclcpp_action::create_server( - node, - ns, - std::bind(&BufferServer::goalCB, this, _1, _2), - std::bind(&BufferServer::cancelCB, this, _1), - std::bind(&BufferServer::acceptedCB, this, _1)); - - check_timer_ = rclcpp::create_timer( - node, node->get_clock(), check_period, std::bind(&BufferServer::checkTransforms, this)); - RCLCPP_DEBUG(logger_, "Buffer server started"); - } - - private: - 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_; + 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; }; -} -#endif + + 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_H_ diff --git a/tf2_ros/include/tf2_ros/create_timer_interface.h b/tf2_ros/include/tf2_ros/create_timer_interface.h index 2642e2243..4592706a3 100644 --- a/tf2_ros/include/tf2_ros/create_timer_interface.h +++ b/tf2_ros/include/tf2_ros/create_timer_interface.h @@ -27,38 +27,42 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_ROS__CREATE_TIMER_INTERFACE_H -#define TF2_ROS__CREATE_TIMER_INTERFACE_H +#ifndef TF2_ROS__CREATE_TIMER_INTERFACE_H_ +#define TF2_ROS__CREATE_TIMER_INTERFACE_H_ -#include +#include + +#include #include -#include +#include +#include +#include +#include -#include namespace tf2_ros { using TimerHandle = uint64_t; -using TimerCallbackType = std::function; +using TimerCallbackType = std::function; -class CreateTimerInterfaceException: public std::runtime_error +class CreateTimerInterfaceException : public std::runtime_error { public: TF2_ROS_PUBLIC - CreateTimerInterfaceException(const std::string errorDescription) + explicit CreateTimerInterfaceException(const std::string & errorDescription) : std::runtime_error(errorDescription) { } }; -class InvalidTimerHandleException: public std::runtime_error +class InvalidTimerHandleException : public std::runtime_error { public: TF2_ROS_PUBLIC - InvalidTimerHandleException(const std::string description) + explicit InvalidTimerHandleException(const std::string & description) : std::runtime_error(description) { } @@ -133,4 +137,4 @@ class CreateTimerInterface } // namespace tf2_ros -#endif // TF2_ROS__CREATE_TIMER_INTERFACE_H +#endif // TF2_ROS__CREATE_TIMER_INTERFACE_H_ diff --git a/tf2_ros/include/tf2_ros/create_timer_ros.h b/tf2_ros/include/tf2_ros/create_timer_ros.h index 94d4b7693..b79cd6648 100644 --- a/tf2_ros/include/tf2_ros/create_timer_ros.h +++ b/tf2_ros/include/tf2_ros/create_timer_ros.h @@ -27,16 +27,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef TF2_ROS__CREATE_TIMER_ROS_H -#define TF2_ROS__CREATE_TIMER_ROS_H +#ifndef TF2_ROS__CREATE_TIMER_ROS_H_ +#define TF2_ROS__CREATE_TIMER_ROS_H_ -#include -#include +#include +#include +#include #include -#include -#include +#include +#include namespace tf2_ros { @@ -66,7 +67,7 @@ class CreateTimerROS : public CreateTimerInterface * \param callback The callback function to execute every interval */ TF2_ROS_PUBLIC - virtual TimerHandle + TimerHandle createTimer( rclcpp::Clock::SharedPtr clock, const tf2::Duration & period, @@ -81,7 +82,7 @@ class CreateTimerROS : public CreateTimerInterface * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist */ TF2_ROS_PUBLIC - virtual void + void cancel(const TimerHandle & timer_handle) override; /** @@ -93,7 +94,7 @@ class CreateTimerROS : public CreateTimerInterface * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist */ TF2_ROS_PUBLIC - virtual void + void reset(const TimerHandle & timer_handle) override; /** @@ -105,11 +106,10 @@ class CreateTimerROS : public CreateTimerInterface * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist */ TF2_ROS_PUBLIC - virtual void + void remove(const TimerHandle & timer_handle) override; private: - void cancelNoLock(const TimerHandle & timer_handle); @@ -127,4 +127,4 @@ class CreateTimerROS : public CreateTimerInterface } // namespace tf2_ros -#endif // TF2_ROS__CREATE_TIMER_INTERFACE_H +#endif // TF2_ROS__CREATE_TIMER_ROS_H_ diff --git a/tf2_ros/include/tf2_ros/message_filter.h b/tf2_ros/include/tf2_ros/message_filter.h index a1ef3d1b4..8d68bb2fe 100644 --- a/tf2_ros/include/tf2_ros/message_filter.h +++ b/tf2_ros/include/tf2_ros/message_filter.h @@ -29,31 +29,42 @@ /** \author Josh Faust */ -#ifndef TF2_ROS_MESSAGE_FILTER_H -#define TF2_ROS_MESSAGE_FILTER_H - -#include -#include -#include -#include -#include +#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 +#include +#include +#include +#include +#include +#include +#include + #define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ - RCUTILS_LOG_DEBUG_NAMED("tf2_ros_message_filter", \ + 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", \ + 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__) @@ -66,9 +77,11 @@ 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. + /// 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 more than the cache length earlier than the newest data in the transform cache + /// The timestamp on the message is more than the cache length earlier than the newest data in + // the transform cache OutTheBack, /// The frame_id on the message is empty EmptyFrameID, @@ -76,9 +89,11 @@ enum FilterFailureReason FilterFailureReasonCount, }; -} +} // namespace filter_failure_reasons -static std::string get_filter_failure_reason_string(filter_failure_reasons::FilterFailureReason reason) { +static std::string get_filter_failure_reason_string( + filter_failure_reasons::FilterFailureReason reason) +{ switch (reason) { case filter_failure_reasons::Unknown: return "Unknown"; @@ -127,13 +142,6 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi public: using MConstPtr = std::shared_ptr; typedef message_filters::MessageEvent MEvent; - // typedef std::function FailureCallback; - - // If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it - // Actually, we need to check that the message has a header, or that it - // has the FrameId and Stamp traits. However I don't know how to do that - // so simply commenting out for now. - // ROS_STATIC_ASSERT(ros::message_traits::HasHeader::value); /** * \brief Constructor @@ -148,14 +156,17 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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()) + 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"); + 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"); } /** @@ -173,7 +184,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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()) + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) : node_logging_(node_logging), node_clock_(node_clock), buffer_(buffer), @@ -198,7 +210,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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()) + 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) { @@ -220,7 +233,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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()) + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) : node_logging_(node_logging), node_clock_(node_clock), buffer_(buffer), @@ -251,12 +265,13 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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_)); + "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_)); } /** @@ -277,7 +292,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi std::unique_lock frames_lock(target_frames_mutex_); target_frames_.resize(target_frames.size()); - std::transform(target_frames.begin(), target_frames.end(), + std::transform( + target_frames.begin(), target_frames.end(), target_frames_.begin(), this->stripSlash); expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); @@ -354,11 +370,15 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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::timeFromSec(stamp.seconds()), target_frame); + wait_params.emplace_back( + next_handle_index_, tf2::timeFromSec(stamp.seconds()), target_frame); info.handles.push_back(next_handle_index_++); if (time_tolerance_.nanoseconds()) { - wait_params.emplace_back(next_handle_index_, tf2::timeFromSec((stamp + time_tolerance_).seconds()), target_frame); + wait_params.emplace_back( + next_handle_index_, + tf2::timeFromSec((stamp + time_tolerance_).seconds()), + target_frame); info.handles.push_back(next_handle_index_++); } } @@ -370,7 +390,6 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi // If this message is about to push us past our queue size, erase the oldest message if (queue_size_ != 0 && message_count_ + 1 > queue_size_) { - ++dropped_message_count_; const MessageInfo & front = messages_.front(); TF2_ROS_MESSAGEFILTER_DEBUG( @@ -391,20 +410,21 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi ++message_count_; } - TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", + TF2_ROS_MESSAGEFILTER_DEBUG( + "Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.seconds(), message_count_); ++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); + 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); buffer_.waitForTransform( - target_frame, - frame_id, - stamp, - buffer_timeout_, - std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, handle)); + target_frame, + frame_id, + stamp, + buffer_timeout_, + std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, handle)); } } @@ -455,7 +475,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi expected_success_count_ = 1; } - void transformReadyCallback(const tf2_ros::TransformStampedFuture& future, const uint64_t handle) + void transformReadyCallback(const tf2_ros::TransformStampedFuture & future, const uint64_t handle) { namespace mt = message_filters::message_traits; @@ -481,7 +501,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi messages_.erase(msg_it); --message_count_; event_found = true; - } + } break; } } @@ -499,8 +519,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi bool transform_available = true; try { future.get(); - } catch (...) - { + } catch (...) { transform_available = false; } @@ -517,8 +536,9 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } if (time_tolerance_.nanoseconds()) { - if (!buffer_.canTransform(target, frame_id, - tf2::timeFromSec((stamp + time_tolerance_).seconds()), NULL)) + if (!buffer_.canTransform( + target, frame_id, + tf2::timeFromSec((stamp + time_tolerance_).seconds()), NULL)) { can_transform = false; break; @@ -530,7 +550,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } if (can_transform) { - TF2_ROS_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", + TF2_ROS_MESSAGEFILTER_DEBUG( + "Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.seconds(), message_count_ - 1); ++successful_transform_count_; @@ -538,7 +559,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } else { ++dropped_message_count_; - TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", + TF2_ROS_MESSAGEFILTER_DEBUG( + "Discarding message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.seconds(), message_count_ - 1); messageDropped(saved_event, filter_failure_reasons::Unknown); } @@ -567,13 +589,18 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi static_cast(incoming_message_count_ - message_count_); if (dropped_pct > 0.95) { TF2_ROS_MESSAGEFILTER_WARN( - "Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct * 100, - "tf2_ros_message_filter"); + "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) { + 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", + " 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()); } } @@ -584,45 +611,42 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi #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) + CBQueueCallback( + MessageFilter * filter, const MEvent & event, bool success, FilterFailureReason reason) + : event_(event), + filter_(filter), + reason_(reason), + success_(success) {} virtual CallResult call() { - if (success_) - { + if (success_) { filter_->signalMessage(event_); - } - else - { + } else { filter_->signalFailure(event_, reason_); } return Success; } - private: +private: MEvent event_; - MessageFilter* filter_; + MessageFilter * filter_; FilterFailureReason reason_; bool success_; }; #endif - void messageDropped(const MEvent& evt, FilterFailureReason reason) + 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 + } else {} #endif { signalFailure(evt, reason); @@ -634,10 +658,10 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi // 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)); + ros::CallbackInterfacePtr cb(new CBQueueCallback( + this, evt, true, filter_failure_reasons::Unknown)); callback_queue_->addCallback(cb, (uint64_t)this); - } - else + } else {} #endif { this->signalMessage(evt); @@ -713,7 +737,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi rclcpp::Time next_failure_warning_; - ///< Provide additional tolerance on time for messages which are stamped but can have associated duration + ///< 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_; @@ -722,7 +747,6 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi // Timeout duration when calling the buffer method 'waitForTransform' tf2::Duration buffer_timeout_; }; +} // namespace tf2_ros -} // namespace tf2 - -#endif +#endif // TF2_ROS__MESSAGE_FILTER_H_ diff --git a/tf2_ros/include/tf2_ros/qos.hpp b/tf2_ros/include/tf2_ros/qos.hpp index 53cf720e7..777d1a9ca 100644 --- a/tf2_ros/include/tf2_ros/qos.hpp +++ b/tf2_ros/include/tf2_ros/qos.hpp @@ -36,22 +36,25 @@ namespace tf2_ros { -class TF2_ROS_PUBLIC DynamicListenerQoS: public rclcpp::QoS +class TF2_ROS_PUBLIC DynamicListenerQoS : public rclcpp::QoS { public: - explicit DynamicListenerQoS(size_t depth = 100) : rclcpp::QoS(depth) {} + explicit DynamicListenerQoS(size_t depth = 100) + : rclcpp::QoS(depth) {} }; class TF2_ROS_PUBLIC DynamicBroadcasterQoS : public rclcpp::QoS { public: - explicit DynamicBroadcasterQoS(size_t depth = 100) : rclcpp::QoS(depth) {} + explicit DynamicBroadcasterQoS(size_t depth = 100) + : rclcpp::QoS(depth) {} }; class TF2_ROS_PUBLIC StaticListenerQoS : public rclcpp::QoS { public: - explicit StaticListenerQoS(size_t depth = 100) : rclcpp::QoS(depth) + explicit StaticListenerQoS(size_t depth = 100) + : rclcpp::QoS(depth) { transient_local(); } @@ -60,10 +63,12 @@ class TF2_ROS_PUBLIC StaticListenerQoS : public rclcpp::QoS class TF2_ROS_PUBLIC StaticBroadcasterQoS : public rclcpp::QoS { public: - explicit StaticBroadcasterQoS(size_t depth = 1) : rclcpp::QoS(depth) + explicit StaticBroadcasterQoS(size_t depth = 1) + : rclcpp::QoS(depth) { transient_local(); } }; } // namespace tf2_ros + #endif // TF2_ROS__QOS_HPP_ diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h index e10e5081d..b0f1e1965 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h @@ -1,10 +1,10 @@ /* * 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 @@ -13,7 +13,7 @@ * * 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 @@ -30,16 +30,18 @@ /** \author Tully Foote */ -#ifndef TF2_ROS_STATICTRANSFORMBROADCASTER_H -#define TF2_ROS_STATICTRANSFORMBROADCASTER_H +#ifndef TF2_ROS__STATIC_TRANSFORM_BROADCASTER_H_ +#define TF2_ROS__STATIC_TRANSFORM_BROADCASTER_H_ +#include +#include +#include +#include +#include -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -#include "tf2_ros/qos.hpp" -#include "tf2_ros/visibility_control.h" +#include +#include namespace tf2_ros { @@ -48,7 +50,8 @@ namespace tf2_ros * 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{ +class StaticTransformBroadcaster +{ public: /** \brief Node interface constructor */ template> @@ -56,7 +59,7 @@ class StaticTransformBroadcaster{ NodeT && node, const rclcpp::QoS & qos = StaticBroadcasterQoS(), const rclcpp::PublisherOptionsWithAllocator & options = - rclcpp::PublisherOptionsWithAllocator()) + rclcpp::PublisherOptionsWithAllocator()) { publisher_ = rclcpp::create_publisher( node, "/tf_static", qos, options); @@ -76,9 +79,8 @@ class StaticTransformBroadcaster{ /// Internal reference to ros::Node rclcpp::Publisher::SharedPtr publisher_; tf2_msgs::msg::TFMessage net_message_; - }; -} +} // namespace tf2_ros -#endif //TF_STATICTRANSFORMBROADCASTER_H +#endif // TF2_ROS__STATIC_TRANSFORM_BROADCASTER_H_ 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 2cbfae4ad..de382a6aa 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster_node.hpp +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster_node.hpp @@ -30,11 +30,12 @@ #ifndef TF2_ROS__STATIC_TRANSFORM_BROADCASTER_NODE_HPP_ #define TF2_ROS__STATIC_TRANSFORM_BROADCASTER_NODE_HPP_ -#include +#include +#include + +#include -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/static_transform_broadcaster.h" -#include "tf2_ros/static_transform_broadcaster_visibility_control.h" +#include namespace tf2_ros { @@ -50,5 +51,7 @@ class StaticTransformBroadcasterNode final : public rclcpp::Node private: std::unique_ptr broadcaster_; }; -} + +} // namespace tf2_ros + #endif // TF2_ROS__STATIC_TRANSFORM_BROADCASTER_NODE_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 44c9087b1..e7ca23f77 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 @@ -27,8 +27,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef STATIC_TRANSFORM_BROADCASTER__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_H_ -#define STATIC_TRANSFORM_BROADCASTER__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_H_ +#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 @@ -61,4 +61,4 @@ #define STATIC_TRANSFORM_BROADCASTER_PUBLIC_TYPE #endif -#endif // STATIC_TRANSFORM_BROADCASTER__VISIBILITY_CONTROL_H_ +#endif // TF2_ROS__STATIC_TRANSFORM_BROADCASTER_VISIBILITY_CONTROL_H_ diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.h b/tf2_ros/include/tf2_ros/transform_broadcaster.h index 14596945f..01abadc42 100644 --- a/tf2_ros/include/tf2_ros/transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/transform_broadcaster.h @@ -1,10 +1,10 @@ /* * 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 @@ -13,7 +13,7 @@ * * 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 @@ -30,14 +30,19 @@ /** \author Tully Foote */ -#ifndef TF2_ROS_TRANSFORMBROADCASTER_H -#define TF2_ROS_TRANSFORMBROADCASTER_H +#ifndef TF2_ROS__TRANSFORM_BROADCASTER_H_ +#define TF2_ROS__TRANSFORM_BROADCASTER_H_ + +#include + +#include +#include +#include +#include + +#include +#include -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -#include "tf2_ros/qos.hpp" -#include "tf2_ros/visibility_control.h" namespace tf2_ros { @@ -46,7 +51,8 @@ namespace tf2_ros * 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{ +class TransformBroadcaster +{ public: /** \brief Node interface constructor */ template> @@ -54,20 +60,12 @@ class TransformBroadcaster{ NodeT && node, const rclcpp::QoS & qos = DynamicBroadcasterQoS(), const rclcpp::PublisherOptionsWithAllocator & options = - rclcpp::PublisherOptionsWithAllocator()) + rclcpp::PublisherOptionsWithAllocator()) { publisher_ = rclcpp::create_publisher( node, "/tf", qos, options); } - /** \brief Send a StampedTransform - * The stamped data structure includes frame_id, and time, and parent_id already. */ - // void sendTransform(const StampedTransform & transform); - - /** \brief Send a vector of StampedTransforms - * The stamped data structure includes frame_id, and time, and parent_id already. */ - //void sendTransform(const std::vector & transforms); - /** \brief Send a TransformStamped message * The stamped data structure includes frame_id, and time, and parent_id already. */ TF2_ROS_PUBLIC @@ -80,9 +78,8 @@ class TransformBroadcaster{ private: rclcpp::Publisher::SharedPtr publisher_; - }; -} +} // namespace tf2_ros -#endif //TF_TRANSFORMBROADCASTER_H +#endif // TF2_ROS__TRANSFORM_BROADCASTER_H_ diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index a4695eac8..aab6b9857 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -32,15 +32,20 @@ #ifndef TF2_ROS__TRANSFORM_LISTENER_H_ #define TF2_ROS__TRANSFORM_LISTENER_H_ +#include +#include +#include + +#include +#include + +#include + +#include #include -#include #include -#include "tf2_msgs/msg/tf_message.hpp" -#include "rclcpp/rclcpp.hpp" +#include -#include "tf2_ros/buffer.h" -#include "tf2_ros/qos.hpp" -#include "tf2_ros/visibility_control.h" namespace tf2_ros { @@ -62,7 +67,7 @@ class TransformListener const rclcpp::QoS & qos = DynamicListenerQoS(), const rclcpp::QoS & static_qos = StaticListenerQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = - rclcpp::SubscriptionOptionsWithAllocator()) + rclcpp::SubscriptionOptionsWithAllocator()) : buffer_(buffer) { init(node, spin_thread, qos, static_qos, options); @@ -79,11 +84,13 @@ class TransformListener const rclcpp::QoS & qos, const rclcpp::QoS & static_qos, const rclcpp::SubscriptionOptionsWithAllocator & options = - rclcpp::SubscriptionOptionsWithAllocator()) + rclcpp::SubscriptionOptionsWithAllocator()) { - 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); + 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); message_subscription_tf_ = rclcpp::create_subscription( node, @@ -112,7 +119,7 @@ class TransformListener // ros::CallbackQueue tf_message_callback_queue_; using thread_ptr = - std::unique_ptr>; + std::unique_ptr>; thread_ptr dedicated_listener_thread_; rclcpp::Node::SharedPtr optional_default_node_ = nullptr; diff --git a/tf2_ros/package.xml b/tf2_ros/package.xml index 2fe2e9662..a15c957d9 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -24,6 +24,8 @@ tf2_msgs ament_cmake_gtest + ament_lint_auto + ament_lint_common ament_cmake diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 30f1f3c77..af4db9014 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -34,11 +34,13 @@ #include #include +#include #include #include +#include #include -//TODO(tfoote replace these terrible macros) +// TODO(tfoote): replace these terrible macros #define ROS_ERROR printf #define ROS_FATAL printf #define ROS_INFO printf @@ -48,15 +50,16 @@ namespace tf2_ros { -Buffer::Buffer(rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time, rclcpp::Node::SharedPtr node) : - BufferCore(cache_time), clock_(clock), node_(node), timer_interface_(nullptr) +Buffer::Buffer( + rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time, + rclcpp::Node::SharedPtr node) +: BufferCore(cache_time), clock_(clock), node_(node), timer_interface_(nullptr) { - if (nullptr == clock_) - { + 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); }; + 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 @@ -70,7 +73,9 @@ Buffer::Buffer(rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time, rclcpp: if (node_) { frames_server_ = node_->create_service( - "tf2_frames", std::bind(&Buffer::getFrames, this, std::placeholders::_1, std::placeholders::_2)); + "tf2_frames", std::bind( + &Buffer::getFrames, this, std::placeholders::_1, + std::placeholders::_2)); } } @@ -88,9 +93,10 @@ to_rclcpp(const tf2::Duration & duration) return rclcpp::Duration(std::chrono::nanoseconds(duration).count()); } -geometry_msgs::msg::TransformStamped -Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame, - const tf2::TimePoint& lookup_time, const tf2::Duration timeout) const +geometry_msgs::msg::TransformStamped +Buffer::lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & lookup_time, const tf2::Duration timeout) const { canTransform(target_frame, source_frame, lookup_time, timeout); return lookupTransform(target_frame, source_frame, lookup_time); @@ -99,99 +105,101 @@ Buffer::lookupTransform(const std::string& target_frame, const std::string& sour void Buffer::onTimeJump(const struct rcl_time_jump_t & time_jump) { if (RCL_ROS_TIME_ACTIVATED == time_jump.clock_change || - RCL_ROS_TIME_DEACTIVATED == time_jump.clock_change) + RCL_ROS_TIME_DEACTIVATED == time_jump.clock_change) { ROS_WARN("Detected time source change. Clearing TF buffer."); clear(); - } - else if (time_jump.delta.nanoseconds < 0) - { + } else if (time_jump.delta.nanoseconds < 0) { ROS_WARN("Detected jump back in time. Clearing TF buffer."); clear(); } } -geometry_msgs::msg::TransformStamped -Buffer::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 +geometry_msgs::msg::TransformStamped +Buffer::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 { canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame); } -void conditionally_append_timeout_info(std::string * errstr, const rclcpp::Time& start_time, - const rclcpp::Time & current_time, - const rclcpp::Duration& timeout) +void conditionally_append_timeout_info( + std::string * errstr, const rclcpp::Time & start_time, + const rclcpp::Time & current_time, + const rclcpp::Duration & timeout) { - if (errstr) - { + if (errstr) { std::stringstream ss; - ss << ". canTransform returned after " - << tf2::durationToSec(from_rclcpp(current_time - start_time)) - <<" timeout was " << tf2::durationToSec(from_rclcpp(timeout)) << "."; + ss << ". canTransform returned after " << + tf2::durationToSec(from_rclcpp(current_time - start_time)) << + " timeout was " << tf2::durationToSec(from_rclcpp(timeout)) << "."; (*errstr) += ss.str(); } } bool -Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, - const tf2::TimePoint& time, const tf2::Duration timeout, std::string* errstr) const +Buffer::canTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, const tf2::Duration timeout, std::string * errstr) const { - if (!checkAndErrorDedicatedThreadPresent(errstr)) + if (!checkAndErrorDedicatedThreadPresent(errstr)) { return false; + } rclcpp::Duration rclcpp_timeout(to_rclcpp(timeout)); // poll for transform if timeout is set rclcpp::Time start_time = clock_->now(); while (clock_->now() < start_time + rclcpp_timeout && - !canTransform(target_frame, source_frame, time) && - (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && //don't wait when we detect a bag loop - (rclcpp::ok()// || !ros::isInitialized() //TODO(tfoote) restore - )) // Make sure we haven't been stopped (won't work for pytf) - { - // TODO(sloretz) sleep using clock_->sleep_for when implemented - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } + !canTransform(target_frame, source_frame, time) && + (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && // don't wait bag loop detected + (rclcpp::ok())) // Make sure we haven't been stopped (won't work for pytf) + { + // TODO(sloretz) sleep using clock_->sleep_for when implemented + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } bool retval = canTransform(target_frame, source_frame, time, errstr); rclcpp::Time current_time = clock_->now(); conditionally_append_timeout_info(errstr, start_time, current_time, rclcpp_timeout); return retval; } - bool -Buffer::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) const +Buffer::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) const { - if (!checkAndErrorDedicatedThreadPresent(errstr)) + if (!checkAndErrorDedicatedThreadPresent(errstr)) { return false; + } rclcpp::Duration rclcpp_timeout(to_rclcpp(timeout)); // poll for transform if timeout is set rclcpp::Time start_time = clock_->now(); while (clock_->now() < start_time + rclcpp_timeout && - !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) && - (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && //don't wait when we detect a bag loop - (rclcpp::ok() //|| !ros::isInitialized() //TODO(tfoote) restore - ) - ) // Make sure we haven't been stopped (won't work for pytf) - { - // TODO(sloretz) sleep using clock_->sleep_for when implemented - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - bool retval = canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr); + !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) && + (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && // don't wait bag loop detected + (rclcpp::ok())) // Make sure we haven't been stopped (won't work for pytf) + { + // TODO(sloretz) sleep using clock_->sleep_for when implemented + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + bool retval = canTransform( + target_frame, target_time, + source_frame, source_time, fixed_frame, errstr); rclcpp::Time current_time = clock_->now(); conditionally_append_timeout_info(errstr, start_time, current_time, rclcpp_timeout); - return retval; + return retval; } TransformStampedFuture -Buffer::waitForTransform(const std::string& target_frame, const std::string& source_frame, const tf2::TimePoint& time, - const tf2::Duration& timeout, TransformReadyCallback callback) +Buffer::waitForTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout, TransformReadyCallback callback) { if (nullptr == timer_interface_) { throw CreateTimerInterfaceException("timer interface not set before call to waitForTransform"); @@ -201,8 +209,8 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou TransformStampedFuture future(promise->get_future()); auto cb = [this, promise, callback, future]( - tf2::TransformableRequestHandle request_handle, const std::string& target_frame, - const std::string& source_frame, tf2::TimePoint time, tf2::TransformableResult result) + tf2::TransformableRequestHandle request_handle, const std::string & target_frame, + const std::string & source_frame, tf2::TimePoint time, tf2::TransformableResult result) { (void) request_handle; @@ -226,11 +234,14 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou } if (result == tf2::TransformAvailable) { - geometry_msgs::msg::TransformStamped msg_stamped = this->lookupTransform(target_frame, source_frame, time); + geometry_msgs::msg::TransformStamped msg_stamped = this->lookupTransform( + target_frame, source_frame, time); promise->set_value(msg_stamped); } else { - promise->set_exception(std::make_exception_ptr(tf2::LookupException( - "Failed to transform from " + source_frame + " to " + target_frame))); + promise->set_exception( + std::make_exception_ptr( + tf2::LookupException( + "Failed to transform from " + source_frame + " to " + target_frame))); } callback(future); }; @@ -238,12 +249,15 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou auto handle = addTransformableRequest(cb, target_frame, source_frame, time); if (0 == handle) { // Immediately transformable - geometry_msgs::msg::TransformStamped msg_stamped = lookupTransform(target_frame, source_frame, time); + geometry_msgs::msg::TransformStamped msg_stamped = lookupTransform( + target_frame, source_frame, time); promise->set_value(msg_stamped); callback(future); } else if (0xffffffffffffffffULL == handle) { // Never transformable - promise->set_exception(std::make_exception_ptr(tf2::LookupException( + promise->set_exception( + std::make_exception_ptr( + tf2::LookupException( "Failed to transform from " + source_frame + " to " + target_frame))); callback(future); } else { @@ -260,10 +274,11 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou } void -Buffer::timerCallback(const TimerHandle & timer_handle, - std::shared_ptr> promise, - TransformStampedFuture future, - TransformReadyCallback callback) +Buffer::timerCallback( + const TimerHandle & timer_handle, + std::shared_ptr> promise, + TransformStampedFuture future, + TransformReadyCallback callback) { bool timer_is_valid = false; tf2::TransformableRequestHandle request_handle = 0u; @@ -281,34 +296,33 @@ Buffer::timerCallback(const TimerHandle & timer_handle, if (timer_is_valid) { cancelTransformableRequest(request_handle); promise->set_exception( - std::make_exception_ptr(tf2::TimeoutException(std::string("Timed out waiting for transform")))); + std::make_exception_ptr( + tf2::TimeoutException(std::string("Timed out waiting for transform")))); callback(future); } } -bool Buffer::getFrames(const tf2_msgs::srv::FrameGraph::Request::SharedPtr req, tf2_msgs::srv::FrameGraph::Response::SharedPtr res) +bool Buffer::getFrames( + const tf2_msgs::srv::FrameGraph::Request::SharedPtr req, + tf2_msgs::srv::FrameGraph::Response::SharedPtr res) { (void)req; res->frame_yaml = allFramesAsYAML(); return true; } - - -bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const +bool Buffer::checkAndErrorDedicatedThreadPresent(std::string * error_str) const { - if (isUsingDedicatedThread()) + if (isUsingDedicatedThread()) { return true; - - + } - if (error_str) + if (error_str) { *error_str = tf2_ros::threading_error; + } - ROS_ERROR("%s", tf2_ros::threading_error.c_str()); + ROS_ERROR("%s", tf2_ros::threading_error); return false; } - - -} +} // namespace tf2_ros diff --git a/tf2_ros/src/buffer_client.cpp b/tf2_ros/src/buffer_client.cpp index 67c199342..2abc9b643 100644 --- a/tf2_ros/src/buffer_client.cpp +++ b/tf2_ros/src/buffer_client.cpp @@ -34,175 +34,193 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ + #include +#include + +#include +#include +#include + namespace tf2_ros { - geometry_msgs::msg::TransformStamped BufferClient::lookupTransform( - const std::string& target_frame, - const std::string& source_frame, - const tf2::TimePoint& time, - const tf2::Duration timeout) const - { - // populate the goal message - LookupTransformAction::Goal goal; - goal.target_frame = target_frame; - goal.source_frame = source_frame; - goal.source_time = tf2_ros::toMsg(time); - goal.timeout = tf2_ros::toMsg(timeout); - goal.advanced = false; - - return processGoal(goal); +geometry_msgs::msg::TransformStamped BufferClient::lookupTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration timeout) const +{ + // populate the goal message + LookupTransformAction::Goal goal; + goal.target_frame = target_frame; + goal.source_frame = source_frame; + goal.source_time = tf2_ros::toMsg(time); + goal.timeout = tf2_ros::toMsg(timeout); + goal.advanced = false; + + return processGoal(goal); +} + +geometry_msgs::msg::TransformStamped BufferClient::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 +{ + // populate the goal message + LookupTransformAction::Goal goal; + goal.target_frame = target_frame; + goal.source_frame = source_frame; + goal.source_time = tf2_ros::toMsg(source_time); + goal.timeout = tf2_ros::toMsg(timeout); + goal.target_time = tf2_ros::toMsg(target_time); + goal.fixed_frame = fixed_frame; + goal.advanced = true; + + return processGoal(goal); +} + +geometry_msgs::msg::TransformStamped BufferClient::processGoal( + const LookupTransformAction::Goal & goal) const +{ + if (!client_->wait_for_action_server(tf2_ros::fromMsg(goal.timeout))) { + throw tf2::ConnectivityException("Failed find available action server"); } - geometry_msgs::msg::TransformStamped BufferClient::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 - { - // populate the goal message - LookupTransformAction::Goal goal; - goal.target_frame = target_frame; - goal.source_frame = source_frame; - goal.source_time = tf2_ros::toMsg(source_time); - goal.timeout = tf2_ros::toMsg(timeout); - goal.target_time = tf2_ros::toMsg(target_time); - goal.fixed_frame = fixed_frame; - goal.advanced = true; - - return processGoal(goal); + auto goal_handle_future = client_->async_send_goal(goal); + + const std::chrono::milliseconds period(static_cast((1.0 / check_frequency_) * 1000)); + bool ready = false; + bool timed_out = false; + tf2::TimePoint start_time = tf2::get_now(); + while (rclcpp::ok() && !ready && !timed_out) { + ready = (std::future_status::ready == goal_handle_future.wait_for(period)); + timed_out = tf2::get_now() > start_time + tf2_ros::fromMsg(goal.timeout) + timeout_padding_; } - geometry_msgs::msg::TransformStamped BufferClient::processGoal( - const LookupTransformAction::Goal& goal) const - { - if (!client_->wait_for_action_server(tf2_ros::fromMsg(goal.timeout))) { - throw tf2::ConnectivityException("Failed find available action server"); - } + if (timed_out) { + throw tf2::TimeoutException( + "Did not receive the goal response for the goal sent to " + "the action server. Something is likely wrong with the server."); + } - auto goal_handle_future = client_->async_send_goal(goal); + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) { + throw GoalRejectedException("Goal rejected by action server"); + } - const std::chrono::milliseconds period(static_cast((1.0 / check_frequency_) * 1000)); - bool ready = false; - bool timed_out = false; - tf2::TimePoint start_time = tf2::get_now(); - while (rclcpp::ok() && !ready && !timed_out) { - ready = (std::future_status::ready == goal_handle_future.wait_for(period)); - timed_out = tf2::get_now() > start_time + tf2_ros::fromMsg(goal.timeout) + timeout_padding_; - } + auto result_future = client_->async_get_result(goal_handle); - if (timed_out) { - throw tf2::TimeoutException("Did not receive the goal response for the goal sent to " - "the action server. Something is likely wrong with the server."); - } + ready = false; + while (rclcpp::ok() && !ready && !timed_out) { + ready = (std::future_status::ready == result_future.wait_for(period)); + timed_out = tf2::get_now() > start_time + tf2_ros::fromMsg(goal.timeout) + timeout_padding_; + } - auto goal_handle = goal_handle_future.get(); - if (!goal_handle) { - throw GoalRejectedException("Goal rejected by action server"); - } + if (timed_out) { + throw tf2::TimeoutException( + "Did not receive the result for the goal sent to " + "the action server. Something is likely wrong with the server."); + } - auto result_future = client_->async_get_result(goal_handle); + auto wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + throw GoalAbortedException("LookupTransform action was aborted"); + case rclcpp_action::ResultCode::CANCELED: + throw GoalCanceledException("LookupTransform action was canceled"); + default: + throw UnexpectedResultCodeException("Unexpected result code returned from server"); + } - ready = false; - while (rclcpp::ok() && !ready && !timed_out) { - ready = (std::future_status::ready == result_future.wait_for(period)); - timed_out = tf2::get_now() > start_time + tf2_ros::fromMsg(goal.timeout) + timeout_padding_; - } + // process the result for errors and return it + return processResult(wrapped_result.result); +} - if (timed_out) { - throw tf2::TimeoutException("Did not receive the result for the goal sent to " - "the action server. Something is likely wrong with the server."); +geometry_msgs::msg::TransformStamped BufferClient::processResult( + const LookupTransformAction::Result::SharedPtr & result) const +{ + // if there's no error, then we'll just return the transform + if (result->error.error != result->error.NO_ERROR) { + // otherwise, we'll have to throw the appropriate exception + if (result->error.error == result->error.LOOKUP_ERROR) { + throw tf2::LookupException(result->error.error_string); } - auto wrapped_result = result_future.get(); - - switch (wrapped_result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - throw GoalAbortedException("LookupTransform action was aborted"); - case rclcpp_action::ResultCode::CANCELED: - throw GoalCanceledException("LookupTransform action was canceled"); - default: - throw UnexpectedResultCodeException("Unexpected result code returned from server"); + if (result->error.error == result->error.CONNECTIVITY_ERROR) { + throw tf2::ConnectivityException(result->error.error_string); } - // process the result for errors and return it - return processResult(wrapped_result.result); - } - - geometry_msgs::msg::TransformStamped BufferClient::processResult( - const LookupTransformAction::Result::SharedPtr& result) const - { - // if there's no error, then we'll just return the transform - if (result->error.error != result->error.NO_ERROR) { - // otherwise, we'll have to throw the appropriate exception - if (result->error.error == result->error.LOOKUP_ERROR) - throw tf2::LookupException(result->error.error_string); - - if (result->error.error == result->error.CONNECTIVITY_ERROR) - throw tf2::ConnectivityException(result->error.error_string); - - if (result->error.error == result->error.EXTRAPOLATION_ERROR) - throw tf2::ExtrapolationException(result->error.error_string); - - if (result->error.error == result->error.INVALID_ARGUMENT_ERROR) - throw tf2::InvalidArgumentException(result->error.error_string); + if (result->error.error == result->error.EXTRAPOLATION_ERROR) { + throw tf2::ExtrapolationException(result->error.error_string); + } - if (result->error.error == result->error.TIMEOUT_ERROR) - throw tf2::TimeoutException(result->error.error_string); + if (result->error.error == result->error.INVALID_ARGUMENT_ERROR) { + throw tf2::InvalidArgumentException(result->error.error_string); + } - throw tf2::TransformException(result->error.error_string); + if (result->error.error == result->error.TIMEOUT_ERROR) { + throw tf2::TimeoutException(result->error.error_string); } - return result->transform; + throw tf2::TransformException(result->error.error_string); } - bool BufferClient::canTransform( - const std::string& target_frame, - const std::string& source_frame, - const tf2::TimePoint& time, - const tf2::Duration timeout, - std::string* errstr) const - { - try { - lookupTransform(target_frame, source_frame, time, timeout); - return true; - } catch (tf2::TransformException& ex) { - if (errstr) - *errstr = ex.what(); - return false; - } catch (LookupTransformGoalException& ex) { - if (errstr) - *errstr = ex.what(); - return false; - } + return result->transform; +} +bool BufferClient::canTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration timeout, + std::string * errstr) const +{ + try { + lookupTransform(target_frame, source_frame, time, timeout); + return true; + } catch (const tf2::TransformException & ex) { + if (errstr) { + *errstr = ex.what(); + } + return false; + } catch (const LookupTransformGoalException & ex) { + if (errstr) { + *errstr = ex.what(); + } + return false; } - - bool BufferClient::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) const - { - try { - lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); - return true; - } catch (tf2::TransformException& ex) { - if(errstr) - *errstr = ex.what(); - return false; - } catch (LookupTransformGoalException& ex) { - if (errstr) - *errstr = ex.what(); - return false; +} + +bool BufferClient::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) const +{ + try { + lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); + return true; + } catch (const tf2::TransformException & ex) { + if (errstr) { + *errstr = ex.what(); } + return false; + } catch (const LookupTransformGoalException & ex) { + if (errstr) { + *errstr = ex.what(); + } + return false; } +} + } // namespace tf2_ros diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 29a918211..a91de83a7 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -34,223 +34,202 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#include #include #include // Only needed for toMsg() and fromMsg() #include +#include +#include +#include + namespace tf2_ros { - void BufferServer::checkTransforms() - { - std::unique_lock lock(mutex_); - for(std::list::iterator it = active_goals_.begin(); it != active_goals_.end();) - { - GoalInfo& info = *it; - - //we want to lookup a transform if the time on the goal - //has expired, or a transform is available - if(canTransform(info.handle)) - { - auto result = std::make_shared(); - - //try to populate the result, catching exceptions if they occur - try - { - result->transform = lookupTransform(info.handle); - - RCLCPP_DEBUG( - logger_, - "Can transform for goal %s", - rclcpp_action::to_string(info.handle->get_goal_id()).c_str()); - - info.handle->succeed(result); - } - catch (tf2::ConnectivityException &ex) - { - result->error.error = result->error.CONNECTIVITY_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } - catch (tf2::LookupException &ex) - { - result->error.error = result->error.LOOKUP_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } - catch (tf2::ExtrapolationException &ex) - { - result->error.error = result->error.EXTRAPOLATION_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } - catch (tf2::InvalidArgumentException &ex) - { - result->error.error = result->error.INVALID_ARGUMENT_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } - catch (tf2::TimeoutException &ex) - { - result->error.error = result->error.TIMEOUT_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } - catch (tf2::TransformException &ex) - { - result->error.error = result->error.TRANSFORM_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } - - } else if (info.end_time < tf2::get_now()) { - // Timeout - auto result = std::make_shared(); - info.handle->abort(result); - } else { - ++it; - } +void BufferServer::checkTransforms() +{ + std::unique_lock lock(mutex_); + for (std::list::iterator it = active_goals_.begin(); it != active_goals_.end(); ) { + GoalInfo & info = *it; - // Remove goal if it has terminated - if (!info.handle->is_active()) { - it = active_goals_.erase(it); - } - } - } + // we want to lookup a transform if the time on the goal + // has expired, or a transform is available + if (canTransform(info.handle)) { + auto result = std::make_shared(); - rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh) - { - RCLCPP_DEBUG( - logger_, - "Cancel request for goal %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); + // try to populate the result, catching exceptions if they occur + try { + result->transform = lookupTransform(info.handle); - std::unique_lock lock(mutex_); - //we need to find the goal in the list and remove it... also setting it as canceled - //if its not in the list, we won't do anything since it will have already been set - //as completed - for(std::list::iterator it = active_goals_.begin(); it != active_goals_.end();) - { - GoalInfo& info = *it; - if(info.handle == gh) - { RCLCPP_DEBUG( logger_, - "Accept cancel request for goal %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); - it = active_goals_.erase(it); - auto result = std::make_shared(); - info.handle->canceled(result); - return rclcpp_action::CancelResponse::ACCEPT; - } - else - ++it; - } - - RCLCPP_DEBUG( - logger_, - "Reject cancel request for goal %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); - - return rclcpp_action::CancelResponse::REJECT; - } - - rclcpp_action::GoalResponse BufferServer::goalCB( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) - { - (void)uuid; - (void)goal; - // accept all goals we get - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - void BufferServer::acceptedCB(GoalHandle gh) - { - RCLCPP_DEBUG( - logger_, - "New goal accepted with ID %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); - //if the transform isn't immediately available, we'll push it onto our list to check - //along with the time that the goal will end - GoalInfo goal_info; - goal_info.handle = gh; - goal_info.end_time = tf2::get_now() + tf2_ros::fromMsg(gh->get_goal()->timeout); + "Can transform for goal %s", + rclcpp_action::to_string(info.handle->get_goal_id()).c_str()); - //we can do a quick check here to see if the transform is valid - //we'll also do this if the end time has been reached - if(canTransform(gh) || goal_info.end_time <= tf2::get_now()) - { - auto result = std::make_shared(); - try - { - result->transform = lookupTransform(gh); - } - catch (tf2::ConnectivityException &ex) - { + info.handle->succeed(result); + } catch (const tf2::ConnectivityException & ex) { result->error.error = result->error.CONNECTIVITY_ERROR; result->error.error_string = ex.what(); - } - catch (tf2::LookupException &ex) - { + info.handle->abort(result); + } catch (const tf2::LookupException & ex) { result->error.error = result->error.LOOKUP_ERROR; result->error.error_string = ex.what(); - } - catch (tf2::ExtrapolationException &ex) - { + info.handle->abort(result); + } catch (const tf2::ExtrapolationException & ex) { result->error.error = result->error.EXTRAPOLATION_ERROR; result->error.error_string = ex.what(); - } - catch (tf2::InvalidArgumentException &ex) - { + info.handle->abort(result); + } catch (const tf2::InvalidArgumentException & ex) { result->error.error = result->error.INVALID_ARGUMENT_ERROR; result->error.error_string = ex.what(); - } - catch (tf2::TimeoutException &ex) - { + info.handle->abort(result); + } catch (const tf2::TimeoutException & ex) { result->error.error = result->error.TIMEOUT_ERROR; result->error.error_string = ex.what(); - } - catch (tf2::TransformException &ex) - { + info.handle->abort(result); + } catch (const tf2::TransformException & ex) { result->error.error = result->error.TRANSFORM_ERROR; result->error.error_string = ex.what(); + info.handle->abort(result); } - RCLCPP_DEBUG(logger_, "Transform available immediately for new goal"); - gh->succeed(result); - return; + } else if (info.end_time < tf2::get_now()) { + // Timeout + auto result = std::make_shared(); + info.handle->abort(result); + } else { + ++it; + } + + // Remove goal if it has terminated + if (!info.handle->is_active()) { + it = active_goals_.erase(it); + } + } +} + +rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh) +{ + RCLCPP_DEBUG( + logger_, + "Cancel request for goal %s", + rclcpp_action::to_string(gh->get_goal_id()).c_str()); + + std::unique_lock lock(mutex_); + // we need to find the goal in the list and remove it... also setting it as canceled + // if its not in the list, we won't do anything since it will have already been set + // as completed + for (std::list::iterator it = active_goals_.begin(); it != active_goals_.end(); ) { + GoalInfo & info = *it; + if (info.handle == gh) { + RCLCPP_DEBUG( + logger_, + "Accept cancel request for goal %s", + rclcpp_action::to_string(gh->get_goal_id()).c_str()); + it = active_goals_.erase(it); + auto result = std::make_shared(); + info.handle->canceled(result); + return rclcpp_action::CancelResponse::ACCEPT; + } else { + ++it; + } + } + + RCLCPP_DEBUG( + logger_, + "Reject cancel request for goal %s", + rclcpp_action::to_string(gh->get_goal_id()).c_str()); + + return rclcpp_action::CancelResponse::REJECT; +} + +rclcpp_action::GoalResponse BufferServer::goalCB( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) +{ + (void)uuid; + (void)goal; + // accept all goals we get + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void BufferServer::acceptedCB(GoalHandle gh) +{ + RCLCPP_DEBUG( + logger_, + "New goal accepted with ID %s", + rclcpp_action::to_string(gh->get_goal_id()).c_str()); + // if the transform isn't immediately available, we'll push it onto our list to check + // along with the time that the goal will end + GoalInfo goal_info; + goal_info.handle = gh; + goal_info.end_time = tf2::get_now() + tf2_ros::fromMsg(gh->get_goal()->timeout); + + // we can do a quick check here to see if the transform is valid + // we'll also do this if the end time has been reached + if (canTransform(gh) || goal_info.end_time <= tf2::get_now()) { + auto result = std::make_shared(); + try { + result->transform = lookupTransform(gh); + } catch (const tf2::ConnectivityException & ex) { + result->error.error = result->error.CONNECTIVITY_ERROR; + result->error.error_string = ex.what(); + } catch (const tf2::LookupException & ex) { + result->error.error = result->error.LOOKUP_ERROR; + result->error.error_string = ex.what(); + } catch (const tf2::ExtrapolationException & ex) { + result->error.error = result->error.EXTRAPOLATION_ERROR; + result->error.error_string = ex.what(); + } catch (const tf2::InvalidArgumentException & ex) { + result->error.error = result->error.INVALID_ARGUMENT_ERROR; + result->error.error_string = ex.what(); + } catch (const tf2::TimeoutException & ex) { + result->error.error = result->error.TIMEOUT_ERROR; + result->error.error_string = ex.what(); + } catch (const tf2::TransformException & ex) { + result->error.error = result->error.TRANSFORM_ERROR; + result->error.error_string = ex.what(); } - std::unique_lock lock(mutex_); - active_goals_.push_back(goal_info); + RCLCPP_DEBUG(logger_, "Transform available immediately for new goal"); + gh->succeed(result); + return; } - bool BufferServer::canTransform(GoalHandle gh) - { - const auto goal = gh->get_goal(); + std::unique_lock lock(mutex_); + active_goals_.push_back(goal_info); +} - tf2::TimePoint source_time_point = tf2_ros::fromMsg(goal->source_time); +bool BufferServer::canTransform(GoalHandle gh) +{ + const auto goal = gh->get_goal(); - // check whether we need to used the advanced or simple api - if(!goal->advanced) - return buffer_.canTransform(goal->target_frame, goal->source_frame, source_time_point, nullptr); + tf2::TimePoint source_time_point = tf2_ros::fromMsg(goal->source_time); - tf2::TimePoint target_time_point = tf2_ros::fromMsg(goal->target_time); - return buffer_.canTransform(goal->target_frame, target_time_point, - goal->source_frame, source_time_point, goal->fixed_frame, nullptr); + // check whether we need to used the advanced or simple api + if (!goal->advanced) { + return buffer_.canTransform(goal->target_frame, goal->source_frame, source_time_point, nullptr); } - geometry_msgs::msg::TransformStamped BufferServer::lookupTransform(GoalHandle gh) - { - const auto goal = gh->get_goal(); + tf2::TimePoint target_time_point = tf2_ros::fromMsg(goal->target_time); + return buffer_.canTransform( + goal->target_frame, target_time_point, + goal->source_frame, source_time_point, goal->fixed_frame, nullptr); +} - //check whether we need to used the advanced or simple api - if(!goal->advanced) - return buffer_.lookupTransform(goal->target_frame, goal->source_frame, tf2_ros::fromMsg(goal->source_time)); +geometry_msgs::msg::TransformStamped BufferServer::lookupTransform(GoalHandle gh) +{ + const auto goal = gh->get_goal(); - return buffer_.lookupTransform(goal->target_frame, tf2_ros::fromMsg(goal->target_time), - goal->source_frame, tf2_ros::fromMsg(goal->source_time), goal->fixed_frame); + // check whether we need to used the advanced or simple api + if (!goal->advanced) { + return buffer_.lookupTransform( + goal->target_frame, goal->source_frame, + tf2_ros::fromMsg(goal->source_time)); } + + return buffer_.lookupTransform( + goal->target_frame, tf2_ros::fromMsg(goal->target_time), + goal->source_frame, tf2_ros::fromMsg(goal->source_time), goal->fixed_frame); +} + } // namespace tf2_ros diff --git a/tf2_ros/src/buffer_server_main.cpp b/tf2_ros/src/buffer_server_main.cpp index 4a092556a..7416a6c8c 100644 --- a/tf2_ros/src/buffer_server_main.cpp +++ b/tf2_ros/src/buffer_server_main.cpp @@ -34,12 +34,16 @@ * * Author: Wim Meeussen *********************************************************************/ + #include #include #include + #include -int main(int argc, char** argv) +#include + +int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared("tf_buffer"); diff --git a/tf2_ros/src/create_timer_ros.cpp b/tf2_ros/src/create_timer_ros.cpp index e76823121..7bf244865 100644 --- a/tf2_ros/src/create_timer_ros.cpp +++ b/tf2_ros/src/create_timer_ros.cpp @@ -26,14 +26,16 @@ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include + +#include +#include #include #include -#include - -#include +#include +#include +#include namespace tf2_ros { diff --git a/tf2_ros/src/static_transform_broadcaster.cpp b/tf2_ros/src/static_transform_broadcaster.cpp index 3c21aebc3..cbbb009db 100644 --- a/tf2_ros/src/static_transform_broadcaster.cpp +++ b/tf2_ros/src/static_transform_broadcaster.cpp @@ -1,10 +1,10 @@ /* * 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 @@ -13,7 +13,7 @@ * * 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 @@ -30,39 +30,45 @@ /** \author Tully Foote */ +#include -#include "rclcpp/rclcpp.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -#include "tf2_ros/static_transform_broadcaster.h" +#include +#include +#include -namespace tf2_ros { +#include -void StaticTransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf) +namespace tf2_ros +{ + +void StaticTransformBroadcaster::sendTransform( + const geometry_msgs::msg::TransformStamped & msgtf) { std::vector v1; v1.push_back(msgtf); sendTransform(v1); } -void StaticTransformBroadcaster::sendTransform(const std::vector & msgtf) +void StaticTransformBroadcaster::sendTransform( + const std::vector & msgtf) { - for (auto it_in = msgtf.begin(); it_in != msgtf.end(); ++it_in) - { + for (auto it_in = msgtf.begin(); it_in != msgtf.end(); ++it_in) { bool match_found = false; - for (auto it_msg = net_message_.transforms.begin(); it_msg != net_message_.transforms.end(); ++it_msg) + for (auto it_msg = net_message_.transforms.begin(); it_msg != net_message_.transforms.end(); + ++it_msg) { - if (it_in->child_frame_id == it_msg->child_frame_id) - { + if (it_in->child_frame_id == it_msg->child_frame_id) { *it_msg = *it_in; match_found = true; break; } } - if (! match_found) + if (!match_found) { net_message_.transforms.push_back(*it_in); + } } publisher_->publish(net_message_); } -} +} // namespace tf2_ros diff --git a/tf2_ros/src/static_transform_broadcaster_node.cpp b/tf2_ros/src/static_transform_broadcaster_node.cpp index 33bbd0377..37f2cf767 100644 --- a/tf2_ros/src/static_transform_broadcaster_node.cpp +++ b/tf2_ros/src/static_transform_broadcaster_node.cpp @@ -39,7 +39,7 @@ namespace { std::string get_unique_node_name() { - const static std::string chars = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"; + static const std::string chars = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"; static std::random_device rd; static std::minstd_rand g{rd()}; @@ -58,7 +58,7 @@ std::string get_unique_node_name() return s; } -} +} // namespace namespace tf2_ros { @@ -85,7 +85,8 @@ StaticTransformBroadcasterNode::StaticTransformBroadcasterNode(const rclcpp::Nod // check frame_id != child_frame_id if (tf_msg.header.frame_id == tf_msg.child_frame_id) { - RCLCPP_ERROR(this->get_logger(), + RCLCPP_ERROR( + this->get_logger(), "cannot publish static transform from '%s' to '%s', exiting", tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str()); throw std::runtime_error("child_frame_id cannot equal frame_id"); diff --git a/tf2_ros/src/static_transform_broadcaster_program.cpp b/tf2_ros/src/static_transform_broadcaster_program.cpp index 8a1cc46f8..691ef0c44 100644 --- a/tf2_ros/src/static_transform_broadcaster_program.cpp +++ b/tf2_ros/src/static_transform_broadcaster_program.cpp @@ -27,18 +27,19 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include + +#include +#include + #include #include #include #include -#include "rclcpp/rclcpp.hpp" -#include -#include "tf2_ros/static_transform_broadcaster_node.hpp" - int main(int argc, char ** argv) { - //Initialize ROS + // Initialize ROS std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); rclcpp::NodeOptions options; std::shared_ptr node; diff --git a/tf2_ros/src/tf2_echo.cpp b/tf2_ros/src/tf2_echo.cpp index a186b2b4b..fc9c42dc0 100644 --- a/tf2_ros/src/tf2_echo.cpp +++ b/tf2_ros/src/tf2_echo.cpp @@ -28,45 +28,43 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include + +#include + #include #include #include - -#include "tf2_ros/transform_listener.h" -#include "rclcpp/rclcpp.hpp" +#include #define _USE_MATH_DEFINES + class echoListener { public: tf2_ros::Buffer buffer_; std::shared_ptr tfl_; - //constructor with name - echoListener(rclcpp::Clock::SharedPtr clock) : - buffer_(clock) + explicit echoListener(rclcpp::Clock::SharedPtr clock) + : buffer_(clock) { tfl_ = std::make_shared(buffer_); - }; + } ~echoListener() { - - }; - -private: - + } }; int main(int argc, char ** argv) { - //Initialize ROS + // Initialize ROS rclcpp::init(argc, argv); // Allow 2 or 3 command line arguments - if (argc < 3 || argc > 4) - { + if (argc < 3 || argc > 4) { printf("Usage: tf2_echo source_frame target_frame [echo_rate]\n\n"); printf("This will echo the transform from the coordinate frame of the source_frame\n"); printf("to the coordinate frame of the target_frame. \n"); @@ -74,21 +72,18 @@ int main(int argc, char ** argv) printf("Default echo rate is 1 if echo_rate is not given.\n"); return -1; } - //TODO(tfoote) restore anonymous?? + // TODO(tfoote): restore anonymous?? // ros::init_options::AnonymousName); rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("tf2_echo"); double rate_hz; - if (argc == 4) - { + if (argc == 4) { // read rate from command line rate_hz = atof(argv[3]); - } - else - { + } else { rate_hz = 1.0; - //TODO(tfoote) restore parameter option + // TODO(tfoote): restore parameter option // // read rate parameter // ros::NodeHandle p_nh("~"); // p_nh.param("rate", rate_hz, 1.0); @@ -96,60 +91,58 @@ int main(int argc, char ** argv) rclcpp::Rate rate(rate_hz); rclcpp::Clock::SharedPtr clock = nh->get_clock(); - //Instantiate a local listener + // Instantiate a local listener echoListener echoListener(clock); - std::string source_frameid = std::string(argv[1]); std::string target_frameid = std::string(argv[2]); // Wait for up to one second for the first transforms to become avaiable. std::string warning_msg; while (rclcpp::ok() && !echoListener.buffer_.canTransform( - source_frameid, target_frameid, tf2::TimePoint(), &warning_msg)) + source_frameid, target_frameid, tf2::TimePoint(), &warning_msg)) { - RCLCPP_INFO_THROTTLE(nh->get_logger(), *clock, 1000, "Waiting for transform %s -> %s: %s", + RCLCPP_INFO_THROTTLE( + nh->get_logger(), *clock, 1000, "Waiting for transform %s -> %s: %s", source_frameid.c_str(), target_frameid.c_str(), warning_msg.c_str()); rate.sleep(); } - //Nothing needs to be done except wait for a quit - //The callbacks withing the listener class - //will take care of everything - while(rclcpp::ok()) - { - try - { - geometry_msgs::msg::TransformStamped echo_transform; - echo_transform = echoListener.buffer_.lookupTransform(source_frameid, target_frameid, tf2::TimePoint()); - std::cout.precision(3); - std::cout.setf(std::ios::fixed,std::ios::floatfield); - std::cout << "At time " << echo_transform.header.stamp.sec << "." << echo_transform.header.stamp.nanosec << std::endl; - //double yaw, pitch, roll; - //echo_transform.getBasis().getRPY(roll, pitch, yaw); - //tf::Quaternion q = echo_transform.getRotation(); - //tf::Vector3 v = echo_transform.getOrigin(); - auto translation = echo_transform.transform.translation; - auto rotation = echo_transform.transform.rotation; - std::cout << "- Translation: [" << translation.x << ", " << translation.y << ", " << translation.z << "]" << std::endl; - std::cout << "- Rotation: in Quaternion [" << rotation.x << ", " << rotation.y << ", " - << rotation.z << ", " << rotation.w << "]" << std::endl; - //TODO(tfoote) restory rpy - // << " in RPY (radian) [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl - // << " in RPY (degree) [" << roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl; - - //print transform - } - catch(tf2::TransformException& ex) - { - std::cout << "Failure at "<< clock->now().seconds() << std::endl; - std::cout << "Exception thrown:" << ex.what()<< std::endl; - std::cout << "The current list of frames is:" <now().seconds() << std::endl; + std::cout << "Exception thrown:" << ex.what() << std::endl; + std::cout << "The current list of frames is:" << std::endl; + std::cout << echoListener.buffer_.allFramesAsString() << std::endl; } + rate.sleep(); + } return 0; } diff --git a/tf2_ros/src/tf2_monitor.cpp b/tf2_ros/src/tf2_monitor.cpp index e85778d25..585ea8693 100644 --- a/tf2_ros/src/tf2_monitor.cpp +++ b/tf2_ros/src/tf2_monitor.cpp @@ -1,8 +1,6 @@ - - /* -* Copyright (c) 2008, Willow Garage, Inc. -* Copyright (c) 2015, Open Source Robotics Foundation, Inc. + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2015, Open Source Robotics Foundation, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,132 +30,126 @@ /** \author Wim Meeussen */ +#include +#include + +#include +#include +#include -#include "tf2_ros/qos.hpp" -#include "tf2_ros/transform_listener.h" +#include +#include +#include #include #include -#include "tf2_msgs/msg/tf_message.hpp" -#include "rclcpp/rclcpp.hpp" +#include class TFMonitor { public: std::string framea_, frameb_; bool using_specific_chain_; - + rclcpp::Node::SharedPtr node_; rclcpp::Subscription::SharedPtr subscriber_tf_, subscriber_tf_message_; std::vector chain_; std::map frame_authority_map; - std::map > delay_map; - std::map > authority_map; - std::map > authority_frequency_map; - + std::map> delay_map; + std::map> authority_map; + std::map> authority_frequency_map; + rclcpp::Clock::SharedPtr clock_; tf2_ros::Buffer buffer_; std::shared_ptr tf_; tf2_msgs::msg::TFMessage message_; std::mutex map_mutex_; - + void callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { - const tf2_msgs::msg::TFMessage& message = *(msg); - //TODO(tfoote) recover authority info - std::string authority = ""; //msg_evt.getPublisherName(); // lookup the authority + const tf2_msgs::msg::TFMessage & message = *(msg); + // TODO(tfoote): recover authority info + std::string authority = ""; double average_offset = 0; - std::unique_lock my_lock(map_mutex_); - for (unsigned int i = 0; i < message.transforms.size(); i++) - { + std::unique_lock my_lock(map_mutex_); + for (size_t i = 0; i < message.transforms.size(); i++) { frame_authority_map[message.transforms[i].child_frame_id] = authority; - double offset = clock_->now().seconds() - tf2_ros::timeToSec(message.transforms[i].header.stamp); - average_offset += offset; - - std::map >::iterator it = delay_map.find(message.transforms[i].child_frame_id); - if (it == delay_map.end()) - { - delay_map[message.transforms[i].child_frame_id] = std::vector(1,offset); - } - else - { + double offset = clock_->now().seconds() - tf2_ros::timeToSec( + message.transforms[i].header.stamp); + average_offset += offset; + + std::map>::iterator it = delay_map.find( + message.transforms[i].child_frame_id); + if (it == delay_map.end()) { + delay_map[message.transforms[i].child_frame_id] = std::vector(1, offset); + } else { it->second.push_back(offset); - if (it->second.size() > 1000) + if (it->second.size() > 1000) { it->second.erase(it->second.begin()); + } } - - } - + } + average_offset /= std::max((size_t) 1, message.transforms.size()); - //create the authority log - std::map >::iterator it2 = authority_map.find(authority); - if (it2 == authority_map.end()) - { - authority_map[authority] = std::vector(1,average_offset); - } - else - { + // create the authority log + std::map>::iterator it2 = authority_map.find(authority); + if (it2 == authority_map.end()) { + authority_map[authority] = std::vector(1, average_offset); + } else { it2->second.push_back(average_offset); - if (it2->second.size() > 1000) + if (it2->second.size() > 1000) { it2->second.erase(it2->second.begin()); + } } - - //create the authority frequency log - std::map >::iterator it3 = authority_frequency_map.find(authority); - if (it3 == authority_frequency_map.end()) - { + + // create the authority frequency log + std::map>::iterator it3 = authority_frequency_map.find( + authority); + if (it3 == authority_frequency_map.end()) { authority_frequency_map[authority] = std::vector(1, clock_->now().seconds()); - } - else - { + } else { it3->second.push_back(clock_->now().seconds()); - if (it3->second.size() > 1000) + if (it3->second.size() > 1000) { it3->second.erase(it3->second.begin()); + } } - - }; + } TFMonitor( rclcpp::Node::SharedPtr node, bool using_specific_chain, - std::string framea = "", std::string frameb = "") - : framea_(framea), - frameb_(frameb), - using_specific_chain_(using_specific_chain), - node_(node), - clock_(node->get_clock()), - buffer_(clock_, tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), node) + std::string framea = "", std::string frameb = "") + : framea_(framea), + frameb_(frameb), + using_specific_chain_(using_specific_chain), + node_(node), + clock_(node->get_clock()), + buffer_(clock_, tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), node) { tf_ = std::make_shared(buffer_); - if (using_specific_chain_) - { + if (using_specific_chain_) { std::string warning_msg; while (rclcpp::ok() && !buffer_.canTransform( - framea_, frameb_, tf2::TimePoint(), &warning_msg)) + framea_, frameb_, tf2::TimePoint(), &warning_msg)) { - RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 1000, + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *clock_, 1000, "Waiting for transform %s -> %s: %s", framea_.c_str(), frameb_.c_str(), warning_msg.c_str()); std::this_thread::sleep_for(std::chrono::milliseconds(500)); } - try{ - buffer_._chainAsVector(frameb_, tf2::TimePointZero, framea_, tf2::TimePointZero, frameb_, chain_); - } - catch(tf2::TransformException& ex){ + try { + buffer_._chainAsVector( + frameb_, tf2::TimePointZero, framea_, tf2::TimePointZero, frameb_, + chain_); + } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node->get_logger(), "Transform Exception %s", ex.what()); return; - } - - /* cout << "Chain currently is:" <create_subscription( @@ -166,29 +158,27 @@ class TFMonitor subscriber_tf_message_ = node_->create_subscription( "/tf_static", tf2_ros::StaticListenerQoS(), std::bind(&TFMonitor::callback, this, std::placeholders::_1)); - - // subscriber_tf_ = node_->create_subscriber.subscribe("tf", 100, boost::bind(&TFMonitor::callback, this, _1)); - // subscriber_tf_message_ = node_.subscribe("tf_message", 100, boost::bind(&TFMonitor::callback, this, _1)); } - std::string outputFrameInfo(const std::map >::iterator& it, const std::string& frame_authority) + std::string outputFrameInfo( + const std::map>::iterator & it, + const std::string & frame_authority) { std::stringstream ss; double average_delay = 0; double max_delay = 0; - for (unsigned int i = 0; i < it->second.size(); i++) - { + for (size_t i = 0; i < it->second.size(); i++) { average_delay += it->second[i]; max_delay = std::max(max_delay, it->second[i]); } average_delay /= it->second.size(); - ss << "Frame: " << it->first << ", published by "<< frame_authority << ", Average Delay: " << average_delay << ", Max Delay: " << max_delay << std::endl; + ss << "Frame: " << it->first << ", published by " << frame_authority << ", Average Delay: " << + average_delay << ", Max Delay: " << max_delay << std::endl; return ss.str(); } - + void spin() - { - + { // create tf listener double max_diff = 0; double avg_diff = 0; @@ -202,100 +192,95 @@ class TFMonitor } - while (rclcpp::ok()){ - counter++; - // printf("looping %d\n", counter); - if (using_specific_chain_) - { - auto tmp = buffer_.lookupTransform(framea_, frameb_, tf2::TimePointZero); - double diff = clock_->now().seconds() - tf2_ros::timeToSec(tmp.header.stamp); - avg_diff = lowpass * diff + (1-lowpass)*avg_diff; - if (diff > max_diff) max_diff = diff; - } - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - if (counter > 20){ - counter = 0; - - if (using_specific_chain_) - { - std::cout < "; + while (rclcpp::ok()) { + counter++; + if (using_specific_chain_) { + auto tmp = buffer_.lookupTransform(framea_, frameb_, tf2::TimePointZero); + double diff = clock_->now().seconds() - tf2_ros::timeToSec(tmp.header.stamp); + avg_diff = lowpass * diff + (1 - lowpass) * avg_diff; + if (diff > max_diff) { + max_diff = diff; } - std::cout << std::endl; - std::cout << "Net delay " << " avg = " << avg_diff <<": max = " << max_diff << std::endl; } - else - std::cout < lock(map_mutex_); - std::cout < >::iterator it = delay_map.begin(); - for ( ; it != delay_map.end() ; ++it) - { - if (using_specific_chain_ ) - { - for ( unsigned int i = 0 ; i < chain_.size(); i++) - { - if (it->first != chain_[i]) - continue; - + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + if (counter > 20) { + counter = 0; + + if (using_specific_chain_) { + std::cout << std::endl << std::endl << std::endl << "RESULTS: for " << framea_ << + " to " << frameb_ << std::endl; + std::cout << "Chain is: "; + for (size_t i = 0; i < chain_.size(); i++) { + std::cout << chain_[i]; + if (i != chain_.size() - 1) { + std::cout << " -> "; + } + } + std::cout << std::endl; + std::cout << "Net delay " << " avg = " << avg_diff << ": max = " << max_diff << + std::endl; + } else { + std::cout << std::endl << std::endl << std::endl << "RESULTS: for all Frames" << + std::endl; + } + std::unique_lock lock(map_mutex_); + std::cout << std::endl << "Frames:" << std::endl; + std::map>::iterator it = delay_map.begin(); + for ( ; it != delay_map.end(); ++it) { + if (using_specific_chain_) { + for (size_t i = 0; i < chain_.size(); i++) { + if (it->first != chain_[i]) { + continue; + } + + std::cout << outputFrameInfo(it, frame_authority_map[it->first]); + } + } else { std::cout << outputFrameInfo(it, frame_authority_map[it->first]); } } - else - std::cout << outputFrameInfo(it, frame_authority_map[it->first]); - } - std::cerr < >::iterator it1 = authority_map.begin(); - std::map >::iterator it2 = authority_frequency_map.begin(); - for ( ; it1 != authority_map.end() ; ++it1, ++it2) - { - double average_delay = 0; - double max_delay = 0; - for (unsigned int i = 0; i < it1->second.size(); i++) - { - average_delay += it1->second[i]; - max_delay = std::max(max_delay, it1->second[i]); + std::cerr << std::endl << "All Broadcasters:" << std::endl; + std::map>::iterator it1 = authority_map.begin(); + std::map>::iterator it2 = authority_frequency_map.begin(); + for ( ; it1 != authority_map.end(); ++it1, ++it2) { + double average_delay = 0; + double max_delay = 0; + for (size_t i = 0; i < it1->second.size(); i++) { + average_delay += it1->second[i]; + max_delay = std::max(max_delay, it1->second[i]); + } + average_delay /= it1->second.size(); + double frequency_out = static_cast(it2->second.size()) / + std::max(0.00000001, (it2->second.back() - it2->second.front())); + std::cout << "Node: " << it1->first << " " << frequency_out << " Hz, Average Delay: " << + average_delay << " Max Delay: " << max_delay << std::endl; } - average_delay /= it1->second.size(); - double frequency_out = (double)(it2->second.size())/std::max(0.00000001, (it2->second.back() - it2->second.front())); - //cout << "output" <<&(*it2) <<" " << it2->second.back() <<" " << it2->second.front() <<" " << std::max((size_t)1, it2->second.size()) << " " << frequency_out << endl; - std::cout << "Node: " <first << " " << frequency_out <<" Hz, Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl; } - } } - - } }; int main(int argc, char ** argv) { - //Initialize ROS + // Initialize ROS std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); - - //TODO(tfoote) make anonymous - rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("tf2_monitor_main"); + // TODO(tfoote): make anonymous + rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("tf2_monitor_main"); std::string framea, frameb; bool using_specific_chain = true; - if (args.size() == 3){ + if (args.size() == 3) { framea = args[1]; frameb = args[2]; - } - else if (args.size() == 1) + } else if (args.size() == 1) { using_specific_chain = false; - else{ + } else { RCLCPP_INFO(nh->get_logger(), "TF_Monitor: usage: tf2_monitor framea frameb"); return -1; } - + // TODO(tfoote) restore simtime logic // //Make sure we don't start before recieving time when in simtime // int iterations = 0; @@ -314,13 +299,12 @@ int main(int argc, char ** argv) // see: http://stackoverflow.com/a/27389714/671658 // I (wjwwood) chose to use the lamda rather than the static cast solution. auto run_func = [](rclcpp::Node::SharedPtr node) { - return rclcpp::spin(node); - }; + return rclcpp::spin(node); + }; TFMonitor monitor(nh, using_specific_chain, framea, frameb); std::thread spinner(run_func, nh); monitor.spin(); spinner.join(); return 0; - } diff --git a/tf2_ros/src/transform_broadcaster.cpp b/tf2_ros/src/transform_broadcaster.cpp index 2720367da..e9a8c7ba2 100644 --- a/tf2_ros/src/transform_broadcaster.cpp +++ b/tf2_ros/src/transform_broadcaster.cpp @@ -1,10 +1,10 @@ /* * 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 @@ -13,7 +13,7 @@ * * 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 @@ -30,12 +30,16 @@ /** \author Tully Foote */ - -#include "rclcpp/rclcpp.hpp" -#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/transform_broadcaster.h" -namespace tf2_ros { +#include +#include +#include + +#include + +namespace tf2_ros +{ void TransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf) { @@ -44,14 +48,16 @@ void TransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStam sendTransform(v1); } -void TransformBroadcaster::sendTransform(const std::vector & msgtf) +void TransformBroadcaster::sendTransform( + const std::vector & msgtf) { tf2_msgs::msg::TFMessage message; - for (std::vector::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it) + for (std::vector::const_iterator it = msgtf.begin(); + it != msgtf.end(); ++it) { message.transforms.push_back(*it); } publisher_->publish(message); } -} +} // namespace tf2_ros diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 8ea3b7ec2..62676021a 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -30,21 +30,24 @@ /** \author Tully Foote */ #include +#include #include +#include #include #include "tf2_ros/transform_listener.h" #include "rclcpp/create_subscription.hpp" -using namespace tf2_ros; - // TODO(tfoote replace these terrible macros) #define ROS_ERROR printf #define ROS_FATAL printf #define ROS_INFO printf #define ROS_WARN printf +namespace tf2_ros +{ + TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) : buffer_(buffer) { @@ -92,20 +95,25 @@ void TransformListener::initThread( buffer_.setUsingDedicatedThread(true); } -void TransformListener::subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static) +void TransformListener::subscription_callback( + const tf2_msgs::msg::TFMessage::SharedPtr msg, + bool is_static) { const tf2_msgs::msg::TFMessage & msg_in = *msg; // TODO(tfoote) find a way to get the authority - std::string authority = "Authority undetectable"; // msg_evt.getPublisherName(); // lookup the authority - for (auto i = 0u; i < msg_in.transforms.size(); i++) { + std::string authority = "Authority undetectable"; + for (size_t i = 0u; i < msg_in.transforms.size(); i++) { try { buffer_.setTransform(msg_in.transforms[i], authority, is_static); - } catch (tf2::TransformException & ex) { + } catch (const tf2::TransformException & ex) { // /\todo Use error reporting std::string temp = ex.what(); - ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", + ROS_ERROR( + "Failure to set received transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); } } } + +} // namespace tf2_ros diff --git a/tf2_ros/test/listener_unittest.cpp b/tf2_ros/test/listener_unittest.cpp index 50eda945a..d3440fa39 100644 --- a/tf2_ros/test/listener_unittest.cpp +++ b/tf2_ros/test/listener_unittest.cpp @@ -27,14 +27,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include +#include +#include + +#include +#include + +#include #include #include #include -#include -#include -#include TEST(tf2_ros_test_listener, transform_listener) { @@ -66,7 +69,9 @@ TEST(tf2_ros_test_listener, transform_listener) EXPECT_TRUE(buffer.canTransform("a", "b", tf2::timeFromSec(0))); - geometry_msgs::msg::TransformStamped out_rootc = buffer.lookupTransform("a", "b", builtin_interfaces::msg::Time()); + geometry_msgs::msg::TransformStamped out_rootc = buffer.lookupTransform( + "a", "b", + builtin_interfaces::msg::Time()); EXPECT_EQ(1, out_rootc.transform.translation.x); EXPECT_EQ(2, out_rootc.transform.translation.y); @@ -81,7 +86,8 @@ TEST(tf2_ros_test_listener, transform_listener) node.reset(); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); diff --git a/tf2_ros/test/message_filter_test.cpp b/tf2_ros/test/message_filter_test.cpp index 4a19106ef..961049f78 100644 --- a/tf2_ros/test/message_filter_test.cpp +++ b/tf2_ros/test/message_filter_test.cpp @@ -27,9 +27,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include #include #include #include @@ -40,6 +37,14 @@ #include #include +#include +#include +#include + +#include +#include +#include + uint8_t filter_callback_fired = 0; void filter_callback(const geometry_msgs::msg::PointStamped & msg) { @@ -49,7 +54,6 @@ void filter_callback(const geometry_msgs::msg::PointStamped & msg) TEST(tf2_ros_message_filter, construction_and_destruction) { - auto node = rclcpp::Node::make_shared("test_message_filter_node"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); @@ -209,7 +213,7 @@ TEST(tf2_ros_message_filter, failure_reason_string_conversion) ); // Make sure all values have been given a string - for (size_t i=0; i < tf2_ros::filter_failure_reasons::FilterFailureReasonCount; i++) { + for (size_t i = 0; i < tf2_ros::filter_failure_reasons::FilterFailureReasonCount; i++) { EXPECT_NE( "Invalid Failure Reason", tf2_ros::get_filter_failure_reason_string( diff --git a/tf2_ros/test/node_wrapper.hpp b/tf2_ros/test/node_wrapper.hpp index 45613d718..a616f69f5 100644 --- a/tf2_ros/test/node_wrapper.hpp +++ b/tf2_ros/test/node_wrapper.hpp @@ -33,6 +33,9 @@ #include #include +#include +#include + class NodeWrapper { public: diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index 69bd67e17..ca0543c2a 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -1,10 +1,10 @@ /* * Copyright (c) 2018, 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 @@ -13,7 +13,7 @@ * * 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 @@ -27,16 +27,18 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include + #include +#include #include #include #include - -using namespace tf2; +#include +#include +#include +#include +#include class MockCreateTimer final : public tf2_ros::CreateTimerInterface { @@ -182,9 +184,15 @@ TEST(test_buffer, wait_for_transform_valid) EXPECT_DOUBLE_EQ(transform.transform.translation.x, transform_result.transform.translation.x); EXPECT_DOUBLE_EQ(transform.transform.translation.y, transform_result.transform.translation.y); EXPECT_DOUBLE_EQ(transform.transform.translation.z, transform_result.transform.translation.z); - EXPECT_DOUBLE_EQ(transform.transform.translation.x, transform_callback_result.transform.translation.x); - EXPECT_DOUBLE_EQ(transform.transform.translation.y, transform_callback_result.transform.translation.y); - EXPECT_DOUBLE_EQ(transform.transform.translation.z, transform_callback_result.transform.translation.z); + EXPECT_DOUBLE_EQ( + transform.transform.translation.x, + transform_callback_result.transform.translation.x); + EXPECT_DOUBLE_EQ( + transform.transform.translation.y, + transform_callback_result.transform.translation.y); + EXPECT_DOUBLE_EQ( + transform.transform.translation.z, + transform_callback_result.transform.translation.z); // Expect there to be exactly one timer EXPECT_EQ(mock_create_timer->timer_to_callback_map_.size(), 1u); @@ -287,8 +295,8 @@ TEST(test_buffer, wait_for_transform_race) EXPECT_FALSE(callback_timeout); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - diff --git a/tf2_ros/test/test_buffer_client.cpp b/tf2_ros/test/test_buffer_client.cpp index 24dfc785e..82c3e5537 100644 --- a/tf2_ros/test/test_buffer_client.cpp +++ b/tf2_ros/test/test_buffer_client.cpp @@ -27,10 +27,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include - #include #include @@ -40,7 +36,13 @@ #include #include -static const std::string ACTION_NAME = "test_tf2_buffer_action"; +#include +#include +#include +#include +#include + +static const char ACTION_NAME[] = "test_tf2_buffer_action"; class MockBufferServer : public rclcpp::Node { @@ -49,8 +51,8 @@ class MockBufferServer : public rclcpp::Node public: MockBufferServer() - : rclcpp::Node("mock_buffer_server"), - transform_available_(false) + : rclcpp::Node("mock_buffer_server"), + transform_available_(false) { action_server_ = rclcpp_action::create_server( get_node_base_interface(), @@ -59,8 +61,8 @@ class MockBufferServer : public rclcpp::Node get_node_waitables_interface(), ACTION_NAME, [](const rclcpp_action::GoalUUID &, std::shared_ptr) - {return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;}, - [](const std::shared_ptr){return rclcpp_action::CancelResponse::ACCEPT;}, + {return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;}, + [](const std::shared_ptr) {return rclcpp_action::CancelResponse::ACCEPT;}, std::bind(&MockBufferServer::handleAccepted, this, std::placeholders::_1)); } @@ -128,7 +130,9 @@ class TestBufferClient : public ::testing::Test ASSERT_TRUE(client_->waitForServer(std::chrono::seconds(10))); // This hack ensure the action server can see the client // TODO(jacobperron): Replace with discovery rclcpp API when it exists - while (mock_server_->count_subscribers("/" + ACTION_NAME + "/_action/feedback") < 1u) { + while (mock_server_->count_subscribers( + "/" + std::string(ACTION_NAME) + "/_action/feedback") < 1u) + { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } @@ -187,7 +191,8 @@ TEST_F(TestBufferClient, can_transform_unavailable) EXPECT_FALSE(client_->canTransform("test_target_frame", "test_source_frame", tf2::get_now())); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/tf2_ros/test/test_buffer_server.cpp b/tf2_ros/test/test_buffer_server.cpp index bdb85fdca..965aa33cb 100644 --- a/tf2_ros/test/test_buffer_server.cpp +++ b/tf2_ros/test/test_buffer_server.cpp @@ -27,8 +27,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include #include @@ -39,7 +37,12 @@ #include #include -static const std::string ACTION_NAME = "test_tf2_buffer_action"; +#include +#include +#include +#include + +static const char ACTION_NAME[] = "test_tf2_buffer_action"; class MockBufferClient : public rclcpp::Node { @@ -48,8 +51,8 @@ class MockBufferClient : public rclcpp::Node public: MockBufferClient() - : rclcpp::Node("mock_buffer_client"), - accepted_(false) + : rclcpp::Node("mock_buffer_client"), + accepted_(false) { action_client_ = rclcpp_action::create_client( get_node_base_interface(), @@ -91,9 +94,9 @@ class MockBufferClient : public rclcpp::Node }; send_goal_options.result_callback = [this, promise](const GoalHandle::WrappedResult & result) { - this->result_ = result; - promise->set_value(true); - }; + this->result_ = result; + promise->set_value(true); + }; action_client_->async_send_goal(goal, send_goal_options); return std::shared_future(promise->get_future()); } @@ -233,8 +236,8 @@ TEST_F(TestBufferServer, lookup_transform_delayed) EXPECT_EQ(mock_client_->result_.code, rclcpp_action::ResultCode::SUCCEEDED); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - diff --git a/tf2_ros/test/test_static_transform_broadcaster.cpp b/tf2_ros/test/test_static_transform_broadcaster.cpp index 76890d8be..b480622d1 100644 --- a/tf2_ros/test/test_static_transform_broadcaster.cpp +++ b/tf2_ros/test/test_static_transform_broadcaster.cpp @@ -30,6 +30,8 @@ #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 9a4a68979..0210b0c20 100644 --- a/tf2_ros/test/test_transform_broadcaster.cpp +++ b/tf2_ros/test/test_transform_broadcaster.cpp @@ -30,6 +30,8 @@ #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 e368921ef..14a166a1f 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -28,8 +28,11 @@ */ #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 503555d85..799545451 100644 --- a/tf2_ros/test/time_reset_test.cpp +++ b/tf2_ros/test/time_reset_test.cpp @@ -27,20 +27,23 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include +#include #include #include -#include + #include #include +#include -void spin_for_a_second(std::shared_ptr& node) +#include +#include + +void spin_for_a_second(std::shared_ptr & node) { rclcpp::Rate r(10); rclcpp::spin_some(node); - for (int i = 0; i < 10; ++i) - { + for (int i = 0; i < 10; ++i) { r.sleep(); rclcpp::spin_some(node); } @@ -48,7 +51,8 @@ void spin_for_a_second(std::shared_ptr& node) TEST(tf2_ros_time_reset_test, time_backwards) { - std::shared_ptr node_ = std::make_shared("transform_listener_backwards_reset"); + std::shared_ptr node_ = std::make_shared( + "transform_listener_backwards_reset"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); @@ -86,7 +90,7 @@ TEST(tf2_ros_time_reset_test, time_backwards) // verify it's been set ASSERT_TRUE(buffer.canTransform("foo", "bar", rclcpp::Time(101, 0))); - // TODO (ahcorde). review this + // TODO(ahcorde): review this // c.clock.sec = 90; // c.clock.nanosec = 0; // clock_pub->publish(c); @@ -110,7 +114,8 @@ TEST(tf2_ros_time_reset_test, time_backwards) // ASSERT_FALSE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS();