ros_subscribe.hh 5.29 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
# define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
# include <iostream>
# include <map>

# include <boost/shared_ptr.hpp>
# include <boost/thread/mutex.hpp>

# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
# include <dynamic-graph/signal-ptr.h>
# include <dynamic-graph/command.h>
# include <sot/core/matrix-geometry.hh>

# include <ros/ros.h>

# include "converter.hh"
# include "sot_to_ros.hh"

namespace dynamicgraph
{
  class RosQueuedSubscribe;

  namespace command
  {
26
    namespace rosQueuedSubscribe
27 28 29 30
    {
      using ::dynamicgraph::command::Command;
      using ::dynamicgraph::command::Value;

31
# define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD)			\
32 33 34 35 36 37 38 39
      class CMD : public Command			\
      {							\
      public:						\
	CMD (RosQueuedSubscribe& entity,				\
	     const std::string& docstring);		\
	virtual Value doExecute ();			\
      }

40 41 42 43 44 45 46
      ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
      ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Clear);
      ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(List);
      ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Rm);
      ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ClearQueue);
      ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(QueueSize);
      ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ReadQueue);
47

48
#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
49

50
    } // end of namespace rosQueuedSubscribe.
51 52
  } // end of namespace command.

53
  class RosQueuedSubscribe;
54 55 56 57 58 59 60 61 62

  namespace internal
  {
    template <typename T>
    struct Add;

    struct BindedSignalBase {
      typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;

63
      BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
64 65 66
      virtual ~BindedSignalBase() {}

      virtual void clear () = 0;
Joseph Mirabel's avatar
Joseph Mirabel committed
67
      virtual std::size_t size () const = 0;
68 69

      Subscriber_t subscriber;
70
      RosQueuedSubscribe* entity;
71 72
    };

73
    template <typename T, int BufferSize>
74 75 76
    struct BindedSignal : BindedSignalBase {
      typedef dynamicgraph::Signal<T, int> Signal_t;
      typedef boost::shared_ptr<Signal_t> SignalPtr_t;
77
      typedef std::vector<T> buffer_t;
Joseph Mirabel's avatar
Joseph Mirabel committed
78
      typedef typename buffer_t::size_type size_type;
79 80 81 82 83 84 85 86

      BindedSignal(RosQueuedSubscribe* e)
        : BindedSignalBase (e)
        , frontIdx(0)
        , backIdx(0)
        , buffer (BufferSize)
        , init(false)
      {}
87 88 89 90 91 92 93
      ~BindedSignal()
      {
        std::cout << signal->getName() << ": Delete" << std::endl;
        signal.reset();
        clear();
      }

94
      /// See comments in reader and writer for details about synchronisation.
95 96
      void clear ()
      {
97 98
        // synchronize with method writer
        wmutex.lock();
99 100 101 102 103 104
        if (!empty()) {
          if (backIdx == 0)
            last = buffer[BufferSize-1];
          else
            last = buffer[backIdx-1];
        }
105 106
        // synchronize with method reader
        rmutex.lock();
107
        frontIdx = backIdx = 0;
108 109
        rmutex.unlock();
        wmutex.unlock();
110 111 112 113 114 115 116 117 118 119
      }

      bool empty () const
      {
        return frontIdx == backIdx;
      }
      
      bool full () const
      {
        return ((backIdx + 1) % BufferSize) == frontIdx;
120 121
      }

122
      size_type size () const
Joseph Mirabel's avatar
Joseph Mirabel committed
123
      {
124 125 126 127
        if (frontIdx < backIdx)
          return frontIdx + BufferSize - backIdx;
        else
          return frontIdx - backIdx;
Joseph Mirabel's avatar
Joseph Mirabel committed
128 129
      }

130
      SignalPtr_t signal;
131 132 133 134 135
      /// Index of the next value to be read.
      size_type frontIdx;
      /// Index of the slot where to write next value (does not contain valid data).
      size_type backIdx;
      buffer_t buffer;
136
      boost::mutex wmutex, rmutex;
137
      T last;
138
      bool init;
139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163

      template <typename R> void writer (const R& data);
      T& reader (T& val, int time);
    };
  } // end of internal namespace.


  /// \brief Publish ROS information in the dynamic-graph.
  class RosQueuedSubscribe : public dynamicgraph::Entity
  {
    DYNAMIC_GRAPH_ENTITY_DECL();
    typedef boost::posix_time::ptime ptime;
  public:
    typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;

    RosQueuedSubscribe (const std::string& n);
    virtual ~RosQueuedSubscribe ();

    virtual std::string getDocString () const;
    void display (std::ostream& os) const;

    void rm (const std::string& signal);
    std::string list ();
    void clear ();
    void clearQueue (const std::string& signal);
164
    void readQueue (int beginReadingAt);
Joseph Mirabel's avatar
Joseph Mirabel committed
165
    std::size_t queueSize (const std::string& signal) const;
166 167

    template <typename T>
Joseph Mirabel's avatar
Joseph Mirabel committed
168
    void add (const std::string& type, const std::string& signal, const std::string& topic);
169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196

    std::map<std::string, bindedSignal_t>&
    bindedSignal ()
    {
      return bindedSignal_;
    }

    ros::NodeHandle& nh ()
    {
      return nh_;
    }

    template <typename R, typename S>
    void callback
    (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
     const R& data);

    template <typename R>
    void callbackTimestamp
    (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
     const R& data);

    template <typename T>
    friend class internal::Add;
  private:
    static const std::string docstring_;
    ros::NodeHandle& nh_;
    std::map<std::string, bindedSignal_t> bindedSignal_;
197

198
    int readQueue_;
199 200 201 202
    // Signal<bool, int> readQueue_;

    template <typename T>
    friend class internal::BindedSignal;
203 204 205 206 207
  };
} // end of namespace dynamicgraph.

# include "ros_subscribe.hxx"
#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH