Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/ros_queued_subscribe.hh	Lines:	0	28	0.0 %
Date:	2022-09-08 07:44:08	Branches:	0	14	0.0 %


//
// Copyright (c) 2017-2018 CNRS
// Authors: Joseph Mirabel
//
//

#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <ros/ros.h>

#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <map>
#include <sot/core/matrix-geometry.hh>

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

namespace dynamicgraph {
class RosQueuedSubscribe;

namespace command {
namespace rosQueuedSubscribe {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;

#define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD)                     \
  class CMD : public Command {                                     \
   public:                                                         \
    CMD(RosQueuedSubscribe& entity, const std::string& docstring); \
    virtual Value doExecute();                                     \
  }

ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);

#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND

}  // end of namespace rosQueuedSubscribe.
}  // end of namespace command.

class RosQueuedSubscribe;

namespace internal {
template <typename T>
struct Add;

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

  BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
  virtual ~BindedSignalBase() {}

  virtual void clear() = 0;
  virtual std::size_t size() const = 0;

  Subscriber_t subscriber;
  RosQueuedSubscribe* entity;
};

template <typename T, int BufferSize>
struct BindedSignal : BindedSignalBase {
  typedef dynamicgraph::Signal<T, int> Signal_t;
  typedef boost::shared_ptr<Signal_t> SignalPtr_t;
  typedef std::vector<T> buffer_t;
  typedef typename buffer_t::size_type size_type;

  BindedSignal(RosQueuedSubscribe* e)
      : BindedSignalBase(e),
        frontIdx(0),
        backIdx(0),
        buffer(BufferSize),
        init(false) {}
  ~BindedSignal() {
    signal.reset();
    clear();
  }

  /// See comments in reader and writer for details about synchronisation.
  void clear() {
    // synchronize with method writer
    wmutex.lock();
    if (!empty()) {
      if (backIdx == 0)
        last = buffer[BufferSize - 1];
      else
        last = buffer[backIdx - 1];
    }
    // synchronize with method reader
    rmutex.lock();
    frontIdx = backIdx = 0;
    rmutex.unlock();
    wmutex.unlock();
  }

  bool empty() const { return frontIdx == backIdx; }

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

  size_type size() const {
    if (frontIdx <= backIdx)
      return backIdx - frontIdx;
    else
      return backIdx + BufferSize - frontIdx;
  }

  SignalPtr_t signal;
  /// 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;
  boost::mutex wmutex, rmutex;
  T last;
  bool init;

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

/// \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::vector<std::string> list();
  std::vector<std::string> listTopics();
  void clear();
  void clearQueue(const std::string& signal);
  void readQueue(int beginReadingAt);
  std::size_t queueSize(const std::string& signal) const;

  template <typename T>
  void add(const std::string& type, const std::string& signal,
           const std::string& topic);

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

  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_;
  std::map<std::string, std::string> topics_;

  int readQueue_;
  // Signal<bool, int> readQueue_;

  template <typename T>
  friend class internal::BindedSignal;
};
}  // end of namespace dynamicgraph.

#include "ros_queued_subscribe.hxx"
#endif  //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017-2018 CNRS
3			// Authors: Joseph Mirabel
4			//
5			//
6
7			#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
8			#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
9			#include <dynamic-graph/command.h>
10			#include <dynamic-graph/entity.h>
11			#include <dynamic-graph/signal-ptr.h>
12			#include <dynamic-graph/signal-time-dependent.h>
13			#include <ros/ros.h>
14
15			#include <boost/shared_ptr.hpp>
16			#include <boost/thread/mutex.hpp>
17			#include <map>
18			#include <sot/core/matrix-geometry.hh>
19
20			#include "converter.hh"
21			#include "sot_to_ros.hh"
22
23			namespace dynamicgraph {
24			class RosQueuedSubscribe;
25
26			namespace command {
27			namespace rosQueuedSubscribe {
28			using ::dynamicgraph::command::Command;
29			using ::dynamicgraph::command::Value;
30
31			#define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD) \
32			class CMD : public Command { \
33			public: \
34			CMD(RosQueuedSubscribe& entity, const std::string& docstring); \
35			virtual Value doExecute(); \
36			}
37
38			ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
39
40			#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
41
42			} // end of namespace rosQueuedSubscribe.
43			} // end of namespace command.
44
45			class RosQueuedSubscribe;
46
47			namespace internal {
48			template <typename T>
49			struct Add;
50
51			struct BindedSignalBase {
52			typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
53
54			BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
55			virtual ~BindedSignalBase() {}
56
57			virtual void clear() = 0;
58			virtual std::size_t size() const = 0;
59
60			Subscriber_t subscriber;
61			RosQueuedSubscribe* entity;
62			};
63
64			template <typename T, int BufferSize>
65			struct BindedSignal : BindedSignalBase {
66			typedef dynamicgraph::Signal<T, int> Signal_t;
67			typedef boost::shared_ptr<Signal_t> SignalPtr_t;
68			typedef std::vector<T> buffer_t;
69			typedef typename buffer_t::size_type size_type;
70
71			BindedSignal(RosQueuedSubscribe* e)
72			: BindedSignalBase(e),
73			frontIdx(0),
74			backIdx(0),
75			buffer(BufferSize),
76			init(false) {}
77			~BindedSignal() {
78			signal.reset();
79			clear();
80			}
81
82			/// See comments in reader and writer for details about synchronisation.
83			void clear() {
84			// synchronize with method writer
85			wmutex.lock();
86			if (!empty()) {
87			if (backIdx == 0)
88			last = buffer[BufferSize - 1];
89			else
90			last = buffer[backIdx - 1];
91			}
92			// synchronize with method reader
93			rmutex.lock();
94			frontIdx = backIdx = 0;
95			rmutex.unlock();
96			wmutex.unlock();
97			}
98
99			bool empty() const { return frontIdx == backIdx; }
100
101			bool full() const { return ((backIdx + 1) % BufferSize) == frontIdx; }
102
103			size_type size() const {
104			if (frontIdx <= backIdx)
105			return backIdx - frontIdx;
106			else
107			return backIdx + BufferSize - frontIdx;
108			}
109
110			SignalPtr_t signal;
111			/// Index of the next value to be read.
112			size_type frontIdx;
113			/// Index of the slot where to write next value (does not contain valid data).
114			size_type backIdx;
115			buffer_t buffer;
116			boost::mutex wmutex, rmutex;
117			T last;
118			bool init;
119
120			template <typename R>
121			void writer(const R& data);
122			T& reader(T& val, int time);
123			};
124			} // namespace internal
125
126			/// \brief Publish ROS information in the dynamic-graph.
127			class RosQueuedSubscribe : public dynamicgraph::Entity {
128			DYNAMIC_GRAPH_ENTITY_DECL();
129			typedef boost::posix_time::ptime ptime;
130
131			public:
132			typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;
133
134			RosQueuedSubscribe(const std::string& n);
135			virtual ~RosQueuedSubscribe();
136
137			virtual std::string getDocString() const;
138			void display(std::ostream& os) const;
139
140			void rm(const std::string& signal);
141			std::vector<std::string> list();
142			std::vector<std::string> listTopics();
143			void clear();
144			void clearQueue(const std::string& signal);
145			void readQueue(int beginReadingAt);
146			std::size_t queueSize(const std::string& signal) const;
147
148			template <typename T>
149			void add(const std::string& type, const std::string& signal,
150			const std::string& topic);
151
152			std::map<std::string, bindedSignal_t>& bindedSignal() {
153			return bindedSignal_;
154			}
155			std::map<std::string, std::string>& topics() { return topics_; }
156
157			ros::NodeHandle& nh() { return nh_; }
158
159			template <typename R, typename S>
160			void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
161			const R& data);
162
163			template <typename R>
164			void callbackTimestamp(
165			boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
166			const R& data);
167
168			template <typename T>
169			friend class internal::Add;
170
171			private:
172			static const std::string docstring_;
173			ros::NodeHandle& nh_;
174			std::map<std::string, bindedSignal_t> bindedSignal_;
175			std::map<std::string, std::string> topics_;
176
177			int readQueue_;
178			// Signal<bool, int> readQueue_;
179
180			template <typename T>
181			friend class internal::BindedSignal;
182			};
183			} // end of namespace dynamicgraph.
184
185			#include "ros_queued_subscribe.hxx"
186			#endif //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH