tf2_server.h

Go to the documentation of this file.

#ifndef TF2_SERVER_TF2_SERVER_H
#define TF2_SERVER_TF2_SERVER_H

#include <map>
#include <memory>
#include <mutex>
#include <unordered_set>

#include <tf2_ros/buffer_server.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_server/RequestTransformStream.h>
#include <ros/ros.h>

namespace tf2_server
{

struct RequestComparatorByFrames
{
  bool operator()(const tf2_server::RequestTransformStreamRequest& r1,
                  const tf2_server::RequestTransformStreamRequest& r2) const;
  bool equals(const tf2_server::RequestTransformStreamRequest& r1,
              const tf2_server::RequestTransformStreamRequest& r2) const;
};

struct RequestComparator
{
  bool operator()(const tf2_server::RequestTransformStreamRequest& r1,
                  const tf2_server::RequestTransformStreamRequest& r2) const;
  bool equals(const tf2_server::RequestTransformStreamRequest& r1,
              const tf2_server::RequestTransformStreamRequest& r2) const;
};

class TF2Server
{

  protected: ros::NodeHandle& nh;
  protected: ros::NodeHandle& pnh;

  protected: std::unique_ptr<tf2_ros::Buffer> buffer;
  protected: std::unique_ptr<tf2_ros::BufferServer> server;
  protected: std::unique_ptr<tf2_ros::TransformListener> listener;
  protected: std::mutex mutex;
  protected: std::mutex subscriberMutex;
  protected: std::mutex streamsMutex;

  protected: ros::ServiceServer requestTransformStreamServer;

  protected: std::unordered_set<std::string> topicNames;
  protected: std::map<std::string, ros::Publisher> publishers;
  protected: std::map<std::string, ros::Publisher> staticPublishers;
  protected: std::map<std::string, tf2_msgs::TFMessage> lastStaticTransforms;

  protected: typedef std::pair<std::string, std::string> FrameSpec;
  protected: typedef std::vector<FrameSpec> FramesList;
  protected: std::map<RequestTransformStreamRequest, std::unique_ptr<FramesList>, RequestComparatorByFrames> frames;

  protected: typedef std::pair<std::string, std::string> TopicsSpec;
  protected: std::map<TopicsSpec, ros::Timer> timers;
  protected: std::map<TopicsSpec, size_t> subscriberNumbers;
  protected: std::map<TopicsSpec, RequestTransformStreamRequest> streams;

  protected: std::vector<RequestTransformStreamRequest> initialStreams;
  protected: ros::Duration initialStreamsWaitTime;
  protected: ros::Timer initialStreamsTimer;

  protected: ros::Time lastTransformsUpdateTime;
  protected: ros::Duration transformsUpdatePeriod;

  protected: bool started = false;

  public: explicit TF2Server(ros::NodeHandle& nh, ros::NodeHandle& pnh);

  public: virtual void start();

  protected: void registerInitialStreams();

  protected: virtual bool onRequestTransformStream(RequestTransformStreamRequest& req, RequestTransformStreamResponse& resp);

  protected: virtual void streamTransform(const ros::TimerEvent& event, const RequestTransformStreamRequest& request, const TopicsSpec& topics);

  protected: virtual std::unique_ptr<FramesList> getFramesList(const RequestTransformStreamRequest& req) const;

  protected: virtual TopicsSpec getTopicsNames(const RequestTransformStreamRequest& request);

  protected: virtual void updateFramesLists();

  protected: virtual void onSubscriberConnected(const TopicsSpec& topics);
  protected: virtual void onSubscriberDisconnected(const TopicsSpec& topics);

};

}

#endif //TF2_SERVER_TF2_SERVER_H