Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion test_tf2/test/buffer_core_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@
#include <tf2/LinearMath/Quaternion.hpp>
#include <tf2/time.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer_interface.h>
#include <tf2_ros/buffer_interface.hpp>

#include "permuter.hpp"

Expand Down
2 changes: 1 addition & 1 deletion test_tf2/test/test_buffer_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
#include <tf2_bullet/tf2_bullet.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_kdl/tf2_kdl.hpp>
#include <tf2_ros/buffer_client.h>
#include <tf2_ros/buffer_client.hpp>

#include <chrono>
#include <functional>
Expand Down
6 changes: 3 additions & 3 deletions test_tf2/test/test_buffer_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,9 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <tf2_ros/buffer.h>
#include <tf2_ros/buffer_server.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.hpp>
#include <tf2_ros/buffer_server.hpp>
#include <tf2_ros/transform_listener.hpp>
#include <rclcpp/rclcpp.hpp>
#include <memory>

Expand Down
6 changes: 3 additions & 3 deletions test_tf2/test/test_message_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,9 +44,9 @@
#include <tf2/LinearMath/Quaternion.hpp>
#include <tf2/LinearMath/Vector3.hpp>
#include <tf2/time.hpp>
#include <tf2_ros/buffer_interface.h>
#include <tf2_ros/create_timer_ros.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/buffer_interface.hpp>
#include <tf2_ros/create_timer_ros.hpp>
#include <tf2_ros/message_filter.hpp>

class Notification
{
Expand Down
8 changes: 4 additions & 4 deletions test_tf2/test/test_static_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,10 @@
#include <tf2/buffer_core.hpp>
#include <tf2/exceptions.hpp>
#include <tf2/time.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/buffer_interface.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.hpp>
#include <tf2_ros/buffer_interface.hpp>
#include <tf2_ros/static_transform_broadcaster.hpp>
#include <tf2_ros/transform_listener.hpp>

#include "permuter.hpp"

Expand Down
2 changes: 1 addition & 1 deletion test_tf2/test/test_tf2_bullet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
/** \author Wim Meeussen */

#include <tf2_bullet/tf2_bullet.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/buffer.hpp>
#include <rclcpp/rclcpp.hpp>
#include <gtest/gtest.h>
#include <tf2/convert.hpp>
Expand Down
2 changes: 1 addition & 1 deletion tf2_bullet/include/tf2_bullet/tf2_bullet.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
#include "LinearMath/btScalar.h"
#include "LinearMath/btTransform.h"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "tf2_ros/buffer_interface.h"
#include "tf2_ros/buffer_interface.hpp"

#if (BT_BULLET_VERSION <= 282)
// Suppress compilation warning on older versions of Bullet.
Expand Down
4 changes: 2 additions & 2 deletions tf2_eigen/include/tf2_eigen/tf2_eigen.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,8 @@
#include "geometry_msgs/msg/twist.hpp"

#include "tf2/convert.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/buffer_interface.h"
#include "tf2_ros/buffer.hpp"
#include "tf2_ros/buffer_interface.hpp"

namespace tf2
{
Expand Down
4 changes: 2 additions & 2 deletions tf2_eigen/test/tf2_eigen-test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,8 @@
#include "tf2/convert.hpp"
#include "tf2/transform_datatypes.hpp"
#include "tf2_eigen/tf2_eigen.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.hpp"
#include "tf2_ros/transform_listener.hpp"

TEST(TfEigen, ConvertVector3dStamped)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2/LinearMath/Transform.hpp"
#include "tf2/LinearMath/Vector3.hpp"
#include "tf2_ros/buffer_interface.h"
#include "tf2_ros/buffer_interface.hpp"

namespace tf2
{
Expand Down
4 changes: 2 additions & 2 deletions tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,8 @@

#include "rclcpp/clock.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.hpp"
#include "tf2_ros/transform_listener.hpp"

#include <geometry_msgs/msg/velocity_stamped.hpp>

Expand Down
2 changes: 1 addition & 1 deletion tf2_kdl/include/tf2_kdl/tf2_kdl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#define TF2_KDL_HPP

#include <tf2/convert.hpp>
#include <tf2_ros/buffer_interface.h>
#include <tf2_ros/buffer_interface.hpp>
#include <kdl/frames.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
Expand Down
2 changes: 1 addition & 1 deletion tf2_kdl/test/test_tf2_kdl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
#include <kdl/frames_io.hpp>
#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include "tf2_ros/buffer.h"
#include "tf2_ros/buffer.hpp"
#include <tf2/convert.hpp>

std::unique_ptr<tf2_ros::Buffer> tf_buffer;
Expand Down
90 changes: 1 addition & 89 deletions tf2_ros/include/tf2_ros/async_buffer_interface.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,94 +30,6 @@
#ifndef TF2_ROS__ASYNC_BUFFER_INTERFACE_H_
#define TF2_ROS__ASYNC_BUFFER_INTERFACE_H_

#include <functional>
#include <future>
#include <string>
#include <utility>

#include "tf2_ros/visibility_control.h"
#include "tf2/buffer_core.hpp"
#include "tf2/time.hpp"
#include "tf2/transform_datatypes.hpp"

#include "geometry_msgs/msg/transform_stamped.hpp"

namespace tf2_ros
{

class TransformStampedFuture : public std::shared_future<geometry_msgs::msg::TransformStamped>
{
using BaseType = std::shared_future<geometry_msgs::msg::TransformStamped>;

public:
/// Constructor
explicit TransformStampedFuture(BaseType && future) noexcept
: BaseType(std::move(future)) {}

/// Copy constructor
TransformStampedFuture(const TransformStampedFuture & ts_future) noexcept
: BaseType(ts_future),
handle_(ts_future.handle_) {}

/// Move constructor
TransformStampedFuture(TransformStampedFuture && ts_future) noexcept
: BaseType(std::move(ts_future)),
handle_(std::move(ts_future.handle_)) {}

void setHandle(const tf2::TransformableRequestHandle handle)
{
handle_ = handle;
}

tf2::TransformableRequestHandle getHandle() const
{
return handle_;
}

private:
tf2::TransformableRequestHandle handle_ {};
};

using TransformReadyCallback = std::function<void (const TransformStampedFuture &)>;

/**
* \brief Abstract interface for asynchronous operations on a `tf2::BufferCoreInterface`.
* Implementations include tf2_ros::Buffer.
*/
class AsyncBufferInterface
{
public:
TF2_ROS_PUBLIC
virtual
~AsyncBufferInterface() = default;

/** \brief Wait for a transform between two frames to become available.
* \param target_frame The frame into which to transform.
* \param source_frame The frame from which to tranform.
* \param time The time at which to transform.
* \param timeout Duration after which waiting will be stopped.
* \param callback The function to be called when the transform becomes available or a timeout
* occurs. In the case of timeout, an exception will be set on the future.
* \return A future to the requested transform. If a timeout occurs a `tf2::LookupException`
* will be set on the future.
*/
TF2_ROS_PUBLIC
virtual TransformStampedFuture
waitForTransform(
const std::string & target_frame,
const std::string & source_frame,
const tf2::TimePoint & time,
const tf2::Duration & timeout,
TransformReadyCallback callback) = 0;

/**
* \brief Cancel the future to make sure the callback of requested transform is clean.
* \param ts_future The future to the requested transform.
*/
virtual void
cancel(const TransformStampedFuture & ts_future) = 0;
}; // class AsyncBufferInterface

} // namespace tf2_ros
#include <tf2_ros/async_buffer_interface.hpp>

#endif // TF2_ROS__ASYNC_BUFFER_INTERFACE_H_
123 changes: 123 additions & 0 deletions tf2_ros/include/tf2_ros/async_buffer_interface.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,123 @@
/*
* Copyright (c) 2019, Open Source Robotics Foundation, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef TF2_ROS__ASYNC_BUFFER_INTERFACE_HPP_
#define TF2_ROS__ASYNC_BUFFER_INTERFACE_HPP_

#include <functional>
#include <future>
#include <string>
#include <utility>

#include "tf2_ros/visibility_control.hpp"
#include "tf2/buffer_core.hpp"
#include "tf2/time.hpp"
#include "tf2/transform_datatypes.hpp"

#include "geometry_msgs/msg/transform_stamped.hpp"

namespace tf2_ros
{

class TransformStampedFuture : public std::shared_future<geometry_msgs::msg::TransformStamped>
{
using BaseType = std::shared_future<geometry_msgs::msg::TransformStamped>;

public:
/// Constructor
explicit TransformStampedFuture(BaseType && future) noexcept
: BaseType(std::move(future)) {}

/// Copy constructor
TransformStampedFuture(const TransformStampedFuture & ts_future) noexcept
: BaseType(ts_future),
handle_(ts_future.handle_) {}

/// Move constructor
TransformStampedFuture(TransformStampedFuture && ts_future) noexcept
: BaseType(std::move(ts_future)),
handle_(std::move(ts_future.handle_)) {}

void setHandle(const tf2::TransformableRequestHandle handle)
{
handle_ = handle;
}

tf2::TransformableRequestHandle getHandle() const
{
return handle_;
}

private:
tf2::TransformableRequestHandle handle_ {};
};

using TransformReadyCallback = std::function<void (const TransformStampedFuture &)>;

/**
* \brief Abstract interface for asynchronous operations on a `tf2::BufferCoreInterface`.
* Implementations include tf2_ros::Buffer.
*/
class AsyncBufferInterface
{
public:
TF2_ROS_PUBLIC
virtual
~AsyncBufferInterface() = default;

/** \brief Wait for a transform between two frames to become available.
* \param target_frame The frame into which to transform.
* \param source_frame The frame from which to tranform.
* \param time The time at which to transform.
* \param timeout Duration after which waiting will be stopped.
* \param callback The function to be called when the transform becomes available or a timeout
* occurs. In the case of timeout, an exception will be set on the future.
* \return A future to the requested transform. If a timeout occurs a `tf2::LookupException`
* will be set on the future.
*/
TF2_ROS_PUBLIC
virtual TransformStampedFuture
waitForTransform(
const std::string & target_frame,
const std::string & source_frame,
const tf2::TimePoint & time,
const tf2::Duration & timeout,
TransformReadyCallback callback) = 0;

/**
* \brief Cancel the future to make sure the callback of requested transform is clean.
* \param ts_future The future to the requested transform.
*/
virtual void
cancel(const TransformStampedFuture & ts_future) = 0;
}; // class AsyncBufferInterface

} // namespace tf2_ros

#endif // TF2_ROS__ASYNC_BUFFER_INTERFACE_HPP_
Loading