1 |
|
|
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX |
2 |
|
|
#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX |
3 |
|
|
#include <dynamic-graph/linear-algebra.h> |
4 |
|
|
#include <dynamic-graph/signal-cast-helper.h> |
5 |
|
|
#include <dynamic-graph/signal-caster.h> |
6 |
|
|
#include <std_msgs/Float64.h> |
7 |
|
|
|
8 |
|
|
#include <boost/bind.hpp> |
9 |
|
|
#include <boost/date_time/posix_time/posix_time.hpp> |
10 |
|
|
#include <vector> |
11 |
|
|
|
12 |
|
|
#include "dynamic_graph_bridge_msgs/Matrix.h" |
13 |
|
|
#include "dynamic_graph_bridge_msgs/Vector.h" |
14 |
|
|
#include "ros_time.hh" |
15 |
|
|
|
16 |
|
|
namespace dg = dynamicgraph; |
17 |
|
|
|
18 |
|
|
namespace dynamicgraph { |
19 |
|
|
template <typename R, typename S> |
20 |
|
|
void RosSubscribe::callback( |
21 |
|
|
boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) { |
22 |
|
|
typedef S sot_t; |
23 |
|
|
sot_t value; |
24 |
|
|
converter(value, data); |
25 |
|
|
signal->setConstant(value); |
26 |
|
|
} |
27 |
|
|
|
28 |
|
|
template <typename R> |
29 |
|
|
void RosSubscribe::callbackTimestamp( |
30 |
|
|
boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, |
31 |
|
|
const R& data) { |
32 |
|
|
ptime time = rosTimeToPtime(data->header.stamp); |
33 |
|
|
signal->setConstant(time); |
34 |
|
|
} |
35 |
|
|
|
36 |
|
|
namespace internal { |
37 |
|
|
template <typename T> |
38 |
|
|
struct Add { |
39 |
|
|
void operator()(RosSubscribe& RosSubscribe, const std::string& signal, |
40 |
|
|
const std::string& topic) { |
41 |
|
|
typedef typename SotToRos<T>::sot_t sot_t; |
42 |
|
|
typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t; |
43 |
|
|
typedef typename SotToRos<T>::signalIn_t signal_t; |
44 |
|
|
|
45 |
|
|
// Initialize the bindedSignal object. |
46 |
|
|
RosSubscribe::bindedSignal_t bindedSignal; |
47 |
|
|
|
48 |
|
|
// Initialize the signal. |
49 |
|
|
boost::format signalName("RosSubscribe(%1%)::%2%"); |
50 |
|
|
signalName % RosSubscribe.getName() % signal; |
51 |
|
|
|
52 |
|
|
boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str())); |
53 |
|
|
SotToRos<T>::setDefault(*signal_); |
54 |
|
|
bindedSignal.first = signal_; |
55 |
|
|
RosSubscribe.signalRegistration(*bindedSignal.first); |
56 |
|
|
|
57 |
|
|
// Initialize the subscriber. |
58 |
|
|
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t; |
59 |
|
|
callback_t callback = |
60 |
|
|
boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, |
61 |
|
|
&RosSubscribe, signal_, _1); |
62 |
|
|
|
63 |
|
|
bindedSignal.second = boost::make_shared<ros::Subscriber>( |
64 |
|
|
RosSubscribe.nh().subscribe(topic, 1, callback)); |
65 |
|
|
|
66 |
|
|
RosSubscribe.bindedSignal()[signal] = bindedSignal; |
67 |
|
|
} |
68 |
|
|
}; |
69 |
|
|
|
70 |
|
|
template <typename T> |
71 |
|
|
struct Add<std::pair<T, dg::Vector> > { |
72 |
|
|
void operator()(RosSubscribe& RosSubscribe, const std::string& signal, |
73 |
|
|
const std::string& topic) { |
74 |
|
|
typedef std::pair<T, dg::Vector> type_t; |
75 |
|
|
|
76 |
|
|
typedef typename SotToRos<type_t>::sot_t sot_t; |
77 |
|
|
typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t; |
78 |
|
|
typedef typename SotToRos<type_t>::signalIn_t signal_t; |
79 |
|
|
|
80 |
|
|
// Initialize the bindedSignal object. |
81 |
|
|
RosSubscribe::bindedSignal_t bindedSignal; |
82 |
|
|
|
83 |
|
|
// Initialize the signal. |
84 |
|
|
boost::format signalName("RosSubscribe(%1%)::%2%"); |
85 |
|
|
signalName % RosSubscribe.getName() % signal; |
86 |
|
|
|
87 |
|
|
boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str())); |
88 |
|
|
SotToRos<T>::setDefault(*signal_); |
89 |
|
|
bindedSignal.first = signal_; |
90 |
|
|
RosSubscribe.signalRegistration(*bindedSignal.first); |
91 |
|
|
|
92 |
|
|
// Initialize the publisher. |
93 |
|
|
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t; |
94 |
|
|
callback_t callback = |
95 |
|
|
boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, |
96 |
|
|
&RosSubscribe, signal_, _1); |
97 |
|
|
|
98 |
|
|
bindedSignal.second = boost::make_shared<ros::Subscriber>( |
99 |
|
|
RosSubscribe.nh().subscribe(topic, 1, callback)); |
100 |
|
|
|
101 |
|
|
RosSubscribe.bindedSignal()[signal] = bindedSignal; |
102 |
|
|
|
103 |
|
|
// Timestamp. |
104 |
|
|
typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t; |
105 |
|
|
std::string signalTimestamp = |
106 |
|
|
(boost::format("%1%%2%") % signal % "Timestamp").str(); |
107 |
|
|
|
108 |
|
|
// Initialize the bindedSignal object. |
109 |
|
|
RosSubscribe::bindedSignal_t bindedSignalTimestamp; |
110 |
|
|
|
111 |
|
|
// Initialize the signal. |
112 |
|
|
boost::format signalNameTimestamp("RosSubscribe(%1%)::%2%"); |
113 |
|
|
signalNameTimestamp % RosSubscribe.name % signalTimestamp; |
114 |
|
|
|
115 |
|
|
boost::shared_ptr<signalTimestamp_t> signalTimestamp_( |
116 |
|
|
new signalTimestamp_t(0, signalNameTimestamp.str())); |
117 |
|
|
|
118 |
|
|
RosSubscribe::ptime zero(rosTimeToPtime(ros::Time(0, 0))); |
119 |
|
|
signalTimestamp_->setConstant(zero); |
120 |
|
|
bindedSignalTimestamp.first = signalTimestamp_; |
121 |
|
|
RosSubscribe.signalRegistration(*bindedSignalTimestamp.first); |
122 |
|
|
|
123 |
|
|
// Initialize the publisher. |
124 |
|
|
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t; |
125 |
|
|
callback_t callbackTimestamp = |
126 |
|
|
boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>, |
127 |
|
|
&RosSubscribe, signalTimestamp_, _1); |
128 |
|
|
|
129 |
|
|
bindedSignalTimestamp.second = boost::make_shared<ros::Subscriber>( |
130 |
|
|
RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp)); |
131 |
|
|
|
132 |
|
|
RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp; |
133 |
|
|
} |
134 |
|
|
}; |
135 |
|
|
} // end of namespace internal. |
136 |
|
|
|
137 |
|
|
template <typename T> |
138 |
|
|
void RosSubscribe::add(const std::string& signal, const std::string& topic) { |
139 |
|
|
internal::Add<T>()(*this, signal, topic); |
140 |
|
|
} |
141 |
|
|
} // end of namespace dynamicgraph. |
142 |
|
|
|
143 |
|
|
#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX |