interruptible_buffer.h

Go to the documentation of this file.

#pragma once
 
#include <memory>
#include <mutex>
#include <string>
 
#include <geometry_msgs/TransformStamped.h>
#include <ros/duration.h>
#include <ros/time.h>
#include <tf2/buffer_core.h>
#include <tf2_ros/buffer.h>
 
#include <cras_cpp_common/time_utils/interruptible_sleep_interface.h>
#include <cras_cpp_common/thread_utils/semaphore.hpp>
 
namespace cras
{
 
class InterruptibleTFBuffer : protected ::tf2_ros::Buffer, public ::cras::InterruptibleSleepInterface
{
public:
  explicit InterruptibleTFBuffer(const ::ros::Duration& cacheTime = {::tf2::BufferCore::DEFAULT_CACHE_TIME, 0});
 
  explicit InterruptibleTFBuffer(const ::std::shared_ptr<::tf2::BufferCore>& parentBuffer);
 
  ~InterruptibleTFBuffer() override;
 
  bool ok() const override;
 
  virtual void requestStop();
 
  bool canTransform(const ::std::string& target_frame, const ::std::string& source_frame,
    const ::ros::Time& time, ::ros::Duration timeout, ::std::string* errstr) const override;
 
  bool canTransform(const ::std::string& target_frame, const ::std::string& source_frame,
    const ::ros::Time& time, ::ros::Duration timeout) const;
 
  bool canTransform(const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& source_frame, const ::ros::Time& source_time,
    const ::std::string& fixed_frame, ::ros::Duration timeout,
    ::std::string* errstr) const override;
 
  bool canTransform(const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& source_frame, const ::ros::Time& source_time,
    const ::std::string& fixed_frame, ::ros::Duration timeout) const;
 
  ::geometry_msgs::TransformStamped lookupTransform(
    const ::std::string& target_frame, const ::std::string& source_frame,
    const ::ros::Time& time, const ::ros::Duration timeout) const override;  // NOLINT
 
  ::geometry_msgs::TransformStamped lookupTransform(
    const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& source_frame, const ::ros::Time& source_time,
    const ::std::string& fixed_frame, const ::ros::Duration timeout) const override;  // NOLINT
 
  bool setCanTransformPollingScale(double scale);
 
  bool setMinPollingDuration(const ::ros::Duration& duration);
 
  ::tf2::BufferCore& getRawBuffer();
 
  const ::tf2::BufferCore& getRawBuffer() const;
 
  // The transform() functions internally use the overridden lookupTransform(), so it is okay to just call the parent
  // implementation.
 
  template <class T>
  T& transform(const T& in, T& out, const ::std::string& target_frame, ::ros::Duration timeout = {0, 0}) const
  {
    return ::tf2_ros::Buffer::transform(in, out, target_frame, timeout);
  }
 
  template <class T>
  T transform(const T& in, const ::std::string& target_frame, ::ros::Duration timeout = {0, 0}) const
  {
    T out;
    return ::tf2_ros::Buffer::transform(in, target_frame, timeout);
  }
 
  template <class A, class B>
  B& transform(const A& in, B& out, const ::std::string& target_frame, ::ros::Duration timeout = {0, 0}) const
  {
    return ::tf2_ros::Buffer::transform(in, out, target_frame, timeout);
  }
 
  template <class T>
  T& transform(const T& in, T& out, const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& fixed_frame, ::ros::Duration timeout = {0, 0}) const
  {
    return ::tf2_ros::Buffer::transform(in, out, target_frame, target_time, fixed_frame, timeout);
  }
 
 
  template <class T>
  T transform(const T& in, const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& fixed_frame, ::ros::Duration timeout = {0, 0}) const
  {
    return ::tf2_ros::Buffer::transform(in, target_frame, target_time, fixed_frame, timeout);
  }
 
  template <class A, class B>
  B& transform(const A& in, B& out, const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& fixed_frame, ::ros::Duration timeout = {0, 0}) const
  {
    return ::tf2_ros::Buffer::transform(in, out, target_frame, target_time, fixed_frame, timeout);  // NOLINT
  }
 
  // Methods copied from BufferCore, they do not rely on time.
 
  ::ros::Duration getCacheLength();
 
  void clear();
 
  bool setTransform(const ::geometry_msgs::TransformStamped& transform, const ::std::string& authority,
    bool is_static = false);
 
  ::geometry_msgs::TransformStamped lookupTransform(const ::std::string& target_frame,
    const ::std::string& source_frame, const ::ros::Time& time) const;
 
  ::geometry_msgs::TransformStamped lookupTransform(const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& source_frame, const ::ros::Time& source_time, const ::std::string& fixed_frame) const;
 
  bool canTransform(const ::std::string& target_frame, const ::std::string& source_frame, const ::ros::Time& time,
    ::std::string* error_msg = nullptr) const;
 
  bool canTransform(const ::std::string& target_frame, const ::ros::Time& target_time,
    const ::std::string& source_frame, const ::ros::Time& source_time, const ::std::string& fixed_frame,
    ::std::string* error_msg = nullptr) const;
 
  ::std::string allFramesAsYAML(double current_time) const;
 
  ::std::string allFramesAsYAML() const;
 
  ::std::string allFramesAsString() const;
 
protected:
  const ::std::shared_ptr<::tf2::BufferCore> parentBuffer {nullptr};
 
  double canTransformPollingScale {0.01};
 
  ::ros::Duration minPollingDuration {0, 1000000};
 
private:
  bool isOk {true};
 
  const ::std::shared_ptr<::cras::InterruptibleTFBuffer> interruptibleParentBuffer {nullptr};
 
  mutable ::cras::ReverseSemaphore callbackSemaphore;
};
 
}