Commit f5072c56 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Add command readQueue to RosQueuedSubscribe

parent df878ec7
......@@ -147,6 +147,26 @@ namespace dynamicgraph
return Value ((unsigned)entity.queueSize (signal));
}
ReadQueue::ReadQueue
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::BOOL),
docstring)
{}
Value ReadQueue::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
std::vector<Value> values = getParameterValues ();
bool read = values[0].value ();
entity.readQueue (read);
return Value ();
}
} // end of errorEstimator.
} // end of namespace command.
......@@ -158,7 +178,8 @@ namespace dynamicgraph
RosQueuedSubscribe::RosQueuedSubscribe (const std::string& n)
: dynamicgraph::Entity(n),
nh_ (rosInit (true)),
bindedSignal_ ()
bindedSignal_ (),
readQueue_ (false)
{
std::string docstring =
"\n"
......@@ -221,6 +242,16 @@ namespace dynamicgraph
addCommand ("queueSize",
new command::rosSubscribe::QueueSize
(*this, docstring));
docstring =
"\n"
" Whether signals should read values from the queues.\n"
"\n"
" Input is:\n"
" - boolean.\n"
"\n";
addCommand ("readQueue",
new command::rosSubscribe::ReadQueue
(*this, docstring));
}
RosQueuedSubscribe::~RosQueuedSubscribe ()
......@@ -287,6 +318,17 @@ namespace dynamicgraph
return -1;
}
void RosQueuedSubscribe::readQueue (bool read)
{
for (std::map<std::string, bindedSignal_t>::const_iterator it =
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
std::cout << it->first << " : " << it->second->size() << '\n';
}
std::cout << std::endl;
// TODO ensure that the signal are synchronised
readQueue_ = read;
}
std::string RosQueuedSubscribe::getDocString () const
{
return docstring_;
......
......@@ -43,12 +43,14 @@ namespace dynamicgraph
ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
ROS_SUBSCRIBE_MAKE_COMMAND(ClearQueue);
ROS_SUBSCRIBE_MAKE_COMMAND(QueueSize);
ROS_SUBSCRIBE_MAKE_COMMAND(ReadQueue);
#undef ROS_SUBSCRIBE_MAKE_COMMAND
} // end of namespace errorEstimator.
} // end of namespace command.
class RosQueuedSubscribe;
namespace internal
{
......@@ -58,13 +60,14 @@ namespace dynamicgraph
struct BindedSignalBase {
typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
BindedSignalBase() {}
BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
virtual ~BindedSignalBase() {}
virtual void clear () = 0;
virtual std::size_t size () const = 0;
Subscriber_t subscriber;
RosQueuedSubscribe* entity;
};
template <typename T>
......@@ -73,7 +76,7 @@ namespace dynamicgraph
typedef boost::shared_ptr<Signal_t> SignalPtr_t;
typedef std::queue<T> Queue_t;
BindedSignal() : BindedSignalBase () {}
BindedSignal(RosQueuedSubscribe* e) : BindedSignalBase (e), init(false) {}
~BindedSignal()
{
std::cout << signal->getName() << ": Delete" << std::endl;
......@@ -97,6 +100,7 @@ namespace dynamicgraph
Queue_t queue;
boost::mutex qmutex;
T last;
bool init;
template <typename R> void writer (const R& data);
T& reader (T& val, int time);
......@@ -123,6 +127,7 @@ namespace dynamicgraph
std::string list ();
void clear ();
void clearQueue (const std::string& signal);
void readQueue (bool read);
std::size_t queueSize (const std::string& signal) const;
template <typename T>
......@@ -155,6 +160,12 @@ namespace dynamicgraph
static const std::string docstring_;
ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
bool readQueue_;
// Signal<bool, int> readQueue_;
template <typename T>
friend class internal::BindedSignal;
};
} // end of namespace dynamicgraph.
......
......@@ -29,7 +29,7 @@ namespace dynamicgraph
typedef typename BindedSignal_t::Signal_t Signal_t;
// Initialize the bindedSignal object.
BindedSignal_t* bs = new BindedSignal_t();
BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
SotToRos<T>::setDefault (bs->last);
// Initialize the signal.
......@@ -61,6 +61,10 @@ namespace dynamicgraph
{
T value;
converter (value, data);
if (!init) {
last = value;
init = true;
}
qmutex.lock();
queue.push (value);
qmutex.unlock();
......@@ -69,15 +73,19 @@ namespace dynamicgraph
template <typename T>
T& BindedSignal<T>::reader (T& data, int)
{
qmutex.lock();
if (queue.empty())
if (!entity->readQueue_) {
data = last;
else {
data = queue.front();
queue.pop();
last = data;
} else {
qmutex.lock();
if (queue.empty())
data = last;
else {
data = queue.front();
queue.pop();
last = data;
}
qmutex.unlock();
}
qmutex.unlock();
return data;
}
} // end of namespace internal.
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment