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;
};

}