1 |
|
|
#include "ros_subscribe.hh" |
2 |
|
|
|
3 |
|
|
#include <dynamic-graph/factory.h> |
4 |
|
|
#include <ros/ros.h> |
5 |
|
|
#include <std_msgs/Float64.h> |
6 |
|
|
#include <std_msgs/UInt32.h> |
7 |
|
|
|
8 |
|
|
#include <boost/assign.hpp> |
9 |
|
|
#include <boost/bind.hpp> |
10 |
|
|
#include <boost/format.hpp> |
11 |
|
|
#include <boost/function.hpp> |
12 |
|
|
#include <boost/make_shared.hpp> |
13 |
|
|
|
14 |
|
|
#include "dynamic_graph_bridge/ros_init.hh" |
15 |
|
|
|
16 |
|
|
namespace dynamicgraph { |
17 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe"); |
18 |
|
|
|
19 |
|
|
namespace command { |
20 |
|
|
namespace rosSubscribe { |
21 |
|
|
Add::Add(RosSubscribe& entity, const std::string& docstring) |
22 |
|
|
: Command( |
23 |
|
|
entity, |
24 |
|
|
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), |
25 |
|
|
docstring) {} |
26 |
|
|
|
27 |
|
|
Value Add::doExecute() { |
28 |
|
|
RosSubscribe& entity = static_cast<RosSubscribe&>(owner()); |
29 |
|
|
std::vector<Value> values = getParameterValues(); |
30 |
|
|
|
31 |
|
|
const std::string& type = values[0].value(); |
32 |
|
|
const std::string& signal = values[1].value(); |
33 |
|
|
const std::string& topic = values[2].value(); |
34 |
|
|
|
35 |
|
|
if (type == "double") |
36 |
|
|
entity.add<double>(signal, topic); |
37 |
|
|
else if (type == "unsigned") |
38 |
|
|
entity.add<unsigned int>(signal, topic); |
39 |
|
|
else if (type == "matrix") |
40 |
|
|
entity.add<dg::Matrix>(signal, topic); |
41 |
|
|
else if (type == "vector") |
42 |
|
|
entity.add<dg::Vector>(signal, topic); |
43 |
|
|
else if (type == "vector3") |
44 |
|
|
entity.add<specific::Vector3>(signal, topic); |
45 |
|
|
else if (type == "vector3Stamped") |
46 |
|
|
entity.add<std::pair<specific::Vector3, dg::Vector> >(signal, topic); |
47 |
|
|
else if (type == "matrixHomo") |
48 |
|
|
entity.add<sot::MatrixHomogeneous>(signal, topic); |
49 |
|
|
else if (type == "matrixHomoStamped") |
50 |
|
|
entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >(signal, topic); |
51 |
|
|
else if (type == "twist") |
52 |
|
|
entity.add<specific::Twist>(signal, topic); |
53 |
|
|
else if (type == "twistStamped") |
54 |
|
|
entity.add<std::pair<specific::Twist, dg::Vector> >(signal, topic); |
55 |
|
|
else if (type == "string") |
56 |
|
|
entity.add<std::string>(signal, topic); |
57 |
|
|
else |
58 |
|
|
throw std::runtime_error("bad type"); |
59 |
|
|
return Value(); |
60 |
|
|
} |
61 |
|
|
} // namespace rosSubscribe |
62 |
|
|
} // end of namespace command. |
63 |
|
|
|
64 |
|
|
const std::string RosSubscribe::docstring_( |
65 |
|
|
"Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n" |
66 |
|
|
"\n" |
67 |
|
|
" Use command \"add\" to subscribe to a new signal.\n"); |
68 |
|
|
|
69 |
|
|
RosSubscribe::RosSubscribe(const std::string& n) |
70 |
|
|
: dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() { |
71 |
|
|
std::string docstring = |
72 |
|
|
"\n" |
73 |
|
|
" Add a signal reading data from a ROS topic\n" |
74 |
|
|
"\n" |
75 |
|
|
" Input:\n" |
76 |
|
|
" - type: string among ['double', 'matrix', 'vector', 'vector3',\n" |
77 |
|
|
" 'vector3Stamped', 'matrixHomo', " |
78 |
|
|
"'matrixHomoStamped',\n" |
79 |
|
|
" 'twist', 'twistStamped'],\n" |
80 |
|
|
" - signal: the signal name in dynamic-graph,\n" |
81 |
|
|
" - topic: the topic name in ROS.\n" |
82 |
|
|
"\n"; |
83 |
|
|
addCommand("add", new command::rosSubscribe::Add(*this, docstring)); |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
RosSubscribe::~RosSubscribe() {} |
87 |
|
|
|
88 |
|
|
void RosSubscribe::display(std::ostream& os) const { |
89 |
|
|
os << CLASS_NAME << std::endl; |
90 |
|
|
} |
91 |
|
|
|
92 |
|
|
void RosSubscribe::rm(const std::string& signal) { |
93 |
|
|
std::string signalTs = signal + "Timestamp"; |
94 |
|
|
|
95 |
|
|
signalDeregistration(signal); |
96 |
|
|
bindedSignal_.erase(signal); |
97 |
|
|
|
98 |
|
|
if (bindedSignal_.find(signalTs) != bindedSignal_.end()) { |
99 |
|
|
signalDeregistration(signalTs); |
100 |
|
|
bindedSignal_.erase(signalTs); |
101 |
|
|
} |
102 |
|
|
} |
103 |
|
|
|
104 |
|
|
std::vector<std::string> RosSubscribe::list() { |
105 |
|
|
std::vector<std::string> result(bindedSignal_.size()); |
106 |
|
|
std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(), |
107 |
|
|
[](const auto& pair) { return pair.first; }); |
108 |
|
|
return result; |
109 |
|
|
} |
110 |
|
|
|
111 |
|
|
void RosSubscribe::clear() { |
112 |
|
|
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin(); |
113 |
|
|
for (; it != bindedSignal_.end();) { |
114 |
|
|
rm(it->first); |
115 |
|
|
it = bindedSignal_.begin(); |
116 |
|
|
} |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
std::string RosSubscribe::getDocString() const { return docstring_; } |
120 |
|
|
} // end of namespace dynamicgraph. |