1 |
|
|
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH |
2 |
|
|
#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH |
3 |
|
|
#include <dynamic-graph/command.h> |
4 |
|
|
#include <dynamic-graph/entity.h> |
5 |
|
|
#include <dynamic-graph/signal-ptr.h> |
6 |
|
|
#include <dynamic-graph/signal-time-dependent.h> |
7 |
|
|
#include <ros/ros.h> |
8 |
|
|
|
9 |
|
|
#include <boost/shared_ptr.hpp> |
10 |
|
|
#include <map> |
11 |
|
|
#include <sot/core/matrix-geometry.hh> |
12 |
|
|
|
13 |
|
|
#include "converter.hh" |
14 |
|
|
#include "sot_to_ros.hh" |
15 |
|
|
|
16 |
|
|
namespace dynamicgraph { |
17 |
|
|
class RosSubscribe; |
18 |
|
|
|
19 |
|
|
namespace command { |
20 |
|
|
namespace rosSubscribe { |
21 |
|
|
using ::dynamicgraph::command::Command; |
22 |
|
|
using ::dynamicgraph::command::Value; |
23 |
|
|
|
24 |
|
|
#define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \ |
25 |
|
|
class CMD : public Command { \ |
26 |
|
|
public: \ |
27 |
|
|
CMD(RosSubscribe& entity, const std::string& docstring); \ |
28 |
|
|
virtual Value doExecute(); \ |
29 |
|
|
} |
30 |
|
|
|
31 |
|
|
ROS_SUBSCRIBE_MAKE_COMMAND(Add); |
32 |
|
|
|
33 |
|
|
#undef ROS_SUBSCRIBE_MAKE_COMMAND |
34 |
|
|
|
35 |
|
|
} // namespace rosSubscribe |
36 |
|
|
} // end of namespace command. |
37 |
|
|
|
38 |
|
|
namespace internal { |
39 |
|
|
template <typename T> |
40 |
|
|
struct Add; |
41 |
|
|
} // namespace internal |
42 |
|
|
|
43 |
|
|
/// \brief Publish ROS information in the dynamic-graph. |
44 |
|
|
class RosSubscribe : public dynamicgraph::Entity { |
45 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
46 |
|
|
typedef boost::posix_time::ptime ptime; |
47 |
|
|
|
48 |
|
|
public: |
49 |
|
|
typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >, |
50 |
|
|
boost::shared_ptr<ros::Subscriber> > |
51 |
|
|
bindedSignal_t; |
52 |
|
|
|
53 |
|
|
RosSubscribe(const std::string& n); |
54 |
|
|
virtual ~RosSubscribe(); |
55 |
|
|
|
56 |
|
|
virtual std::string getDocString() const; |
57 |
|
|
void display(std::ostream& os) const; |
58 |
|
|
|
59 |
|
|
void add(const std::string& signal, const std::string& topic); |
60 |
|
|
void rm(const std::string& signal); |
61 |
|
|
std::vector<std::string> list(); |
62 |
|
|
void clear(); |
63 |
|
|
|
64 |
|
|
template <typename T> |
65 |
|
|
void add(const std::string& signal, const std::string& topic); |
66 |
|
|
|
67 |
|
|
std::map<std::string, bindedSignal_t>& bindedSignal() { |
68 |
|
|
return bindedSignal_; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
ros::NodeHandle& nh() { return nh_; } |
72 |
|
|
|
73 |
|
|
template <typename R, typename S> |
74 |
|
|
void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, |
75 |
|
|
const R& data); |
76 |
|
|
|
77 |
|
|
template <typename R> |
78 |
|
|
void callbackTimestamp( |
79 |
|
|
boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, |
80 |
|
|
const R& data); |
81 |
|
|
|
82 |
|
|
template <typename T> |
83 |
|
|
friend class internal::Add; |
84 |
|
|
|
85 |
|
|
private: |
86 |
|
|
static const std::string docstring_; |
87 |
|
|
ros::NodeHandle& nh_; |
88 |
|
|
std::map<std::string, bindedSignal_t> bindedSignal_; |
89 |
|
|
}; |
90 |
|
|
} // end of namespace dynamicgraph. |
91 |
|
|
|
92 |
|
|
#include "ros_subscribe.hxx" |
93 |
|
|
#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH |