Skip to content

Commit 918b8f6

Browse files
author
Olivier Stasse
authored
Merge pull request #38 from jmirabel/master
Add entity RosQueuedSubscribe
2 parents 496023c + 2ad227a commit 918b8f6

8 files changed

+871
-3
lines changed

Diff for: ‎CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,8 @@ endmacro()
132132
# Build Sot Entities
133133
compile_plugin(ros_publish)
134134
compile_plugin(ros_subscribe)
135+
compile_plugin(ros_queued_subscribe)
136+
compile_plugin(ros_tf_listener)
135137
compile_plugin(ros_time)
136138
compile_plugin(ros_joint_state)
137139
pkg_config_use_dependency(ros_joint_state pinocchio)

Diff for: ‎src/CMakeLists.txt

+2-1
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,3 @@
11
PYTHON_INSTALL("dynamic_graph/ros" "__init__.py" "${PYTHON_SITELIB}")
2-
PYTHON_INSTALL("dynamic_graph/ros" "ros.py" "${PYTHON_SITELIB}")
2+
PYTHON_INSTALL("dynamic_graph/ros" "ros.py" "${PYTHON_SITELIB}")
3+
PYTHON_INSTALL("dynamic_graph/ros" "dgcompleter.py" "${PYTHON_SITELIB}")

Diff for: ‎src/ros_queued_subscribe.cpp

+354
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,354 @@
1+
//
2+
// Copyright (c) 2017-2018 CNRS
3+
// Authors: Joseph Mirabel
4+
//
5+
//
6+
// This file is part of dynamic_graph_bridge
7+
// dynamic_graph_bridge is free software: you can redistribute it
8+
// and/or modify it under the terms of the GNU Lesser General Public
9+
// License as published by the Free Software Foundation, either version
10+
// 3 of the License, or (at your option) any later version.
11+
//
12+
// dynamic_graph_bridge is distributed in the hope that it will be
13+
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14+
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15+
// General Lesser Public License for more details. You should have
16+
// received a copy of the GNU Lesser General Public License along with
17+
// dynamic_graph_bridge If not, see
18+
// <http://www.gnu.org/licenses/>.
19+
20+
#include <boost/assign.hpp>
21+
#include <boost/bind.hpp>
22+
#include <boost/format.hpp>
23+
#include <boost/function.hpp>
24+
#include <boost/make_shared.hpp>
25+
26+
#include <ros/ros.h>
27+
#include <std_msgs/Float64.h>
28+
#include <std_msgs/UInt32.h>
29+
30+
#include <dynamic-graph/factory.h>
31+
32+
#include "dynamic_graph_bridge/ros_init.hh"
33+
#include "ros_queued_subscribe.hh"
34+
35+
namespace dynamicgraph
36+
{
37+
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
38+
39+
namespace command
40+
{
41+
namespace rosQueuedSubscribe
42+
{
43+
Clear::Clear
44+
(RosQueuedSubscribe& entity, const std::string& docstring)
45+
: Command
46+
(entity,
47+
std::vector<Value::Type> (),
48+
docstring)
49+
{}
50+
51+
Value Clear::doExecute ()
52+
{
53+
RosQueuedSubscribe& entity =
54+
static_cast<RosQueuedSubscribe&> (owner ());
55+
56+
entity.clear ();
57+
return Value ();
58+
}
59+
60+
ClearQueue::ClearQueue
61+
(RosQueuedSubscribe& entity, const std::string& docstring)
62+
: Command
63+
(entity,
64+
boost::assign::list_of (Value::STRING),
65+
docstring)
66+
{}
67+
68+
Value ClearQueue::doExecute ()
69+
{
70+
RosQueuedSubscribe& entity =
71+
static_cast<RosQueuedSubscribe&> (owner ());
72+
73+
std::vector<Value> values = getParameterValues ();
74+
const std::string& signal = values[0].value ();
75+
entity.clearQueue (signal);
76+
77+
return Value ();
78+
}
79+
80+
List::List
81+
(RosQueuedSubscribe& entity, const std::string& docstring)
82+
: Command
83+
(entity,
84+
std::vector<Value::Type> (),
85+
docstring)
86+
{}
87+
88+
Value List::doExecute ()
89+
{
90+
RosQueuedSubscribe& entity =
91+
static_cast<RosQueuedSubscribe&> (owner ());
92+
return Value (entity.list ());
93+
}
94+
95+
Add::Add
96+
(RosQueuedSubscribe& entity, const std::string& docstring)
97+
: Command
98+
(entity,
99+
boost::assign::list_of
100+
(Value::STRING) (Value::STRING) (Value::STRING),
101+
docstring)
102+
{}
103+
104+
Value Add::doExecute ()
105+
{
106+
RosQueuedSubscribe& entity =
107+
static_cast<RosQueuedSubscribe&> (owner ());
108+
std::vector<Value> values = getParameterValues ();
109+
110+
const std::string& type = values[0].value ();
111+
const std::string& signal = values[1].value ();
112+
const std::string& topic = values[2].value ();
113+
114+
if (type == "double")
115+
entity.add<double> (type, signal, topic);
116+
else if (type == "unsigned")
117+
entity.add<unsigned int> (type, signal, topic);
118+
else if (type == "matrix")
119+
entity.add<dg::Matrix> (type, signal, topic);
120+
else if (type == "vector")
121+
entity.add<dg::Vector> (type, signal, topic);
122+
else if (type == "vector3")
123+
entity.add<specific::Vector3> (type, signal, topic);
124+
else if (type == "matrixHomo")
125+
entity.add<sot::MatrixHomogeneous> (type, signal, topic);
126+
else if (type == "twist")
127+
entity.add<specific::Twist> (type, signal, topic);
128+
else
129+
throw std::runtime_error("bad type");
130+
return Value ();
131+
}
132+
133+
Rm::Rm
134+
(RosQueuedSubscribe& entity, const std::string& docstring)
135+
: Command
136+
(entity,
137+
boost::assign::list_of (Value::STRING),
138+
docstring)
139+
{}
140+
141+
Value Rm::doExecute ()
142+
{
143+
RosQueuedSubscribe& entity =
144+
static_cast<RosQueuedSubscribe&> (owner ());
145+
std::vector<Value> values = getParameterValues ();
146+
const std::string& signal = values[0].value ();
147+
entity.rm (signal);
148+
return Value ();
149+
}
150+
151+
QueueSize::QueueSize
152+
(RosQueuedSubscribe& entity, const std::string& docstring)
153+
: Command
154+
(entity,
155+
boost::assign::list_of (Value::STRING),
156+
docstring)
157+
{}
158+
159+
Value QueueSize::doExecute ()
160+
{
161+
RosQueuedSubscribe& entity =
162+
static_cast<RosQueuedSubscribe&> (owner ());
163+
164+
std::vector<Value> values = getParameterValues ();
165+
const std::string& signal = values[0].value ();
166+
167+
return Value ((unsigned)entity.queueSize (signal));
168+
}
169+
170+
ReadQueue::ReadQueue
171+
(RosQueuedSubscribe& entity, const std::string& docstring)
172+
: Command
173+
(entity,
174+
boost::assign::list_of (Value::INT),
175+
docstring)
176+
{}
177+
178+
Value ReadQueue::doExecute ()
179+
{
180+
RosQueuedSubscribe& entity =
181+
static_cast<RosQueuedSubscribe&> (owner ());
182+
183+
std::vector<Value> values = getParameterValues ();
184+
int read = values[0].value ();
185+
entity.readQueue (read);
186+
187+
return Value ();
188+
}
189+
} // end of errorEstimator.
190+
} // end of namespace command.
191+
192+
const std::string RosQueuedSubscribe::docstring_
193+
("Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
194+
"\n"
195+
" Use command \"add\" to subscribe to a new signal.\n");
196+
197+
RosQueuedSubscribe::RosQueuedSubscribe (const std::string& n)
198+
: dynamicgraph::Entity(n),
199+
nh_ (rosInit (true)),
200+
bindedSignal_ (),
201+
readQueue_ (-1)
202+
{
203+
std::string docstring =
204+
"\n"
205+
" Add a signal reading data from a ROS topic\n"
206+
"\n"
207+
" Input:\n"
208+
" - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
209+
" 'matrixHomo', 'twist'],\n"
210+
" - signal: the signal name in dynamic-graph,\n"
211+
" - topic: the topic name in ROS.\n"
212+
"\n";
213+
addCommand ("add",
214+
new command::rosQueuedSubscribe::Add
215+
(*this, docstring));
216+
docstring =
217+
"\n"
218+
" Remove a signal reading data from a ROS topic\n"
219+
"\n"
220+
" Input:\n"
221+
" - name of the signal to remove (see method list for the list of signals).\n"
222+
"\n";
223+
addCommand ("rm",
224+
new command::rosQueuedSubscribe::Rm
225+
(*this, docstring));
226+
docstring =
227+
"\n"
228+
" Remove all signals reading data from a ROS topic\n"
229+
"\n"
230+
" No input:\n"
231+
"\n";
232+
addCommand ("clear",
233+
new command::rosQueuedSubscribe::Clear
234+
(*this, docstring));
235+
docstring =
236+
"\n"
237+
" List signals reading data from a ROS topic\n"
238+
"\n"
239+
" No input:\n"
240+
"\n";
241+
addCommand ("list",
242+
new command::rosQueuedSubscribe::List
243+
(*this, docstring));
244+
docstring =
245+
"\n"
246+
" Empty the queue of a given signal\n"
247+
"\n"
248+
" Input is:\n"
249+
" - name of the signal (see method list for the list of signals).\n"
250+
"\n";
251+
addCommand ("clearQueue",
252+
new command::rosQueuedSubscribe::ClearQueue
253+
(*this, docstring));
254+
docstring =
255+
"\n"
256+
" Return the queue size of a given signal\n"
257+
"\n"
258+
" Input is:\n"
259+
" - name of the signal (see method list for the list of signals).\n"
260+
"\n";
261+
addCommand ("queueSize",
262+
new command::rosQueuedSubscribe::QueueSize
263+
(*this, docstring));
264+
docstring =
265+
"\n"
266+
" Whether signals should read values from the queues, and when.\n"
267+
"\n"
268+
" Input is:\n"
269+
" - int (dynamic graph time at which the reading begin).\n"
270+
"\n";
271+
addCommand ("readQueue",
272+
new command::rosQueuedSubscribe::ReadQueue
273+
(*this, docstring));
274+
}
275+
276+
RosQueuedSubscribe::~RosQueuedSubscribe ()
277+
{
278+
std::cout << getName() << ": Delete" << std::endl;
279+
}
280+
281+
void RosQueuedSubscribe::display (std::ostream& os) const
282+
{
283+
os << CLASS_NAME << std::endl;
284+
}
285+
286+
void RosQueuedSubscribe::rm (const std::string& signal)
287+
{
288+
std::string signalTs = signal+"Timestamp";
289+
290+
signalDeregistration(signal);
291+
bindedSignal_.erase (signal);
292+
293+
if(bindedSignal_.find(signalTs) != bindedSignal_.end())
294+
{
295+
signalDeregistration(signalTs);
296+
bindedSignal_.erase(signalTs);
297+
}
298+
}
299+
300+
std::string RosQueuedSubscribe::list ()
301+
{
302+
std::string result("[");
303+
for (std::map<std::string, bindedSignal_t>::const_iterator it =
304+
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
305+
result += "'" + it->first + "',";
306+
}
307+
result += "]";
308+
return result;
309+
}
310+
311+
void RosQueuedSubscribe::clear ()
312+
{
313+
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
314+
for(; it!= bindedSignal_.end(); )
315+
{
316+
rm(it->first);
317+
it = bindedSignal_.begin();
318+
}
319+
}
320+
321+
void RosQueuedSubscribe::clearQueue (const std::string& signal)
322+
{
323+
if(bindedSignal_.find(signal) != bindedSignal_.end())
324+
{
325+
bindedSignal_[signal]->clear();
326+
}
327+
}
328+
329+
std::size_t RosQueuedSubscribe::queueSize (const std::string& signal) const
330+
{
331+
std::map<std::string, bindedSignal_t>::const_iterator _bs =
332+
bindedSignal_.find(signal);
333+
if(_bs != bindedSignal_.end())
334+
{
335+
return _bs->second->size();
336+
}
337+
return -1;
338+
}
339+
340+
void RosQueuedSubscribe::readQueue (int beginReadingAt)
341+
{
342+
// Prints signal queues sizes
343+
/*for (std::map<std::string, bindedSignal_t>::const_iterator it =
344+
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
345+
std::cout << it->first << " : " << it->second->size() << '\n';
346+
}*/
347+
readQueue_ = beginReadingAt;
348+
}
349+
350+
std::string RosQueuedSubscribe::getDocString () const
351+
{
352+
return docstring_;
353+
}
354+
} // end of namespace dynamicgraph.

0 commit comments

Comments
 (0)