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