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 ...@@ -147,6 +147,26 @@ namespace dynamicgraph
return Value ((unsigned)entity.queueSize (signal)); 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 errorEstimator.
} // end of namespace command. } // end of namespace command.
...@@ -158,7 +178,8 @@ namespace dynamicgraph ...@@ -158,7 +178,8 @@ namespace dynamicgraph
RosQueuedSubscribe::RosQueuedSubscribe (const std::string& n) RosQueuedSubscribe::RosQueuedSubscribe (const std::string& n)
: dynamicgraph::Entity(n), : dynamicgraph::Entity(n),
nh_ (rosInit (true)), nh_ (rosInit (true)),
bindedSignal_ () bindedSignal_ (),
readQueue_ (false)
{ {
std::string docstring = std::string docstring =
"\n" "\n"
...@@ -221,6 +242,16 @@ namespace dynamicgraph ...@@ -221,6 +242,16 @@ namespace dynamicgraph
addCommand ("queueSize", addCommand ("queueSize",
new command::rosSubscribe::QueueSize new command::rosSubscribe::QueueSize
(*this, docstring)); (*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 () RosQueuedSubscribe::~RosQueuedSubscribe ()
...@@ -287,6 +318,17 @@ namespace dynamicgraph ...@@ -287,6 +318,17 @@ namespace dynamicgraph
return -1; 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 std::string RosQueuedSubscribe::getDocString () const
{ {
return docstring_; return docstring_;
......
...@@ -43,12 +43,14 @@ namespace dynamicgraph ...@@ -43,12 +43,14 @@ namespace dynamicgraph
ROS_SUBSCRIBE_MAKE_COMMAND(Rm); ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
ROS_SUBSCRIBE_MAKE_COMMAND(ClearQueue); ROS_SUBSCRIBE_MAKE_COMMAND(ClearQueue);
ROS_SUBSCRIBE_MAKE_COMMAND(QueueSize); ROS_SUBSCRIBE_MAKE_COMMAND(QueueSize);
ROS_SUBSCRIBE_MAKE_COMMAND(ReadQueue);
#undef ROS_SUBSCRIBE_MAKE_COMMAND #undef ROS_SUBSCRIBE_MAKE_COMMAND
} // end of namespace errorEstimator. } // end of namespace errorEstimator.
} // end of namespace command. } // end of namespace command.
class RosQueuedSubscribe;
namespace internal namespace internal
{ {
...@@ -58,13 +60,14 @@ namespace dynamicgraph ...@@ -58,13 +60,14 @@ namespace dynamicgraph
struct BindedSignalBase { struct BindedSignalBase {
typedef boost::shared_ptr<ros::Subscriber> Subscriber_t; typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
BindedSignalBase() {} BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
virtual ~BindedSignalBase() {} virtual ~BindedSignalBase() {}
virtual void clear () = 0; virtual void clear () = 0;
virtual std::size_t size () const = 0; virtual std::size_t size () const = 0;
Subscriber_t subscriber; Subscriber_t subscriber;
RosQueuedSubscribe* entity;
}; };
template <typename T> template <typename T>
...@@ -73,7 +76,7 @@ namespace dynamicgraph ...@@ -73,7 +76,7 @@ namespace dynamicgraph
typedef boost::shared_ptr<Signal_t> SignalPtr_t; typedef boost::shared_ptr<Signal_t> SignalPtr_t;
typedef std::queue<T> Queue_t; typedef std::queue<T> Queue_t;
BindedSignal() : BindedSignalBase () {} BindedSignal(RosQueuedSubscribe* e) : BindedSignalBase (e), init(false) {}
~BindedSignal() ~BindedSignal()
{ {
std::cout << signal->getName() << ": Delete" << std::endl; std::cout << signal->getName() << ": Delete" << std::endl;
...@@ -97,6 +100,7 @@ namespace dynamicgraph ...@@ -97,6 +100,7 @@ namespace dynamicgraph
Queue_t queue; Queue_t queue;
boost::mutex qmutex; boost::mutex qmutex;
T last; T last;
bool init;
template <typename R> void writer (const R& data); template <typename R> void writer (const R& data);
T& reader (T& val, int time); T& reader (T& val, int time);
...@@ -123,6 +127,7 @@ namespace dynamicgraph ...@@ -123,6 +127,7 @@ namespace dynamicgraph
std::string list (); std::string list ();
void clear (); void clear ();
void clearQueue (const std::string& signal); void clearQueue (const std::string& signal);
void readQueue (bool read);
std::size_t queueSize (const std::string& signal) const; std::size_t queueSize (const std::string& signal) const;
template <typename T> template <typename T>
...@@ -155,6 +160,12 @@ namespace dynamicgraph ...@@ -155,6 +160,12 @@ namespace dynamicgraph
static const std::string docstring_; static const std::string docstring_;
ros::NodeHandle& nh_; ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_; std::map<std::string, bindedSignal_t> bindedSignal_;
bool readQueue_;
// Signal<bool, int> readQueue_;
template <typename T>
friend class internal::BindedSignal;
}; };
} // end of namespace dynamicgraph. } // end of namespace dynamicgraph.
......
...@@ -29,7 +29,7 @@ namespace dynamicgraph ...@@ -29,7 +29,7 @@ namespace dynamicgraph
typedef typename BindedSignal_t::Signal_t Signal_t; typedef typename BindedSignal_t::Signal_t Signal_t;
// Initialize the bindedSignal object. // Initialize the bindedSignal object.
BindedSignal_t* bs = new BindedSignal_t(); BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
SotToRos<T>::setDefault (bs->last); SotToRos<T>::setDefault (bs->last);
// Initialize the signal. // Initialize the signal.
...@@ -61,6 +61,10 @@ namespace dynamicgraph ...@@ -61,6 +61,10 @@ namespace dynamicgraph
{ {
T value; T value;
converter (value, data); converter (value, data);
if (!init) {
last = value;
init = true;
}
qmutex.lock(); qmutex.lock();
queue.push (value); queue.push (value);
qmutex.unlock(); qmutex.unlock();
...@@ -69,15 +73,19 @@ namespace dynamicgraph ...@@ -69,15 +73,19 @@ namespace dynamicgraph
template <typename T> template <typename T>
T& BindedSignal<T>::reader (T& data, int) T& BindedSignal<T>::reader (T& data, int)
{ {
qmutex.lock(); if (!entity->readQueue_) {
if (queue.empty())
data = last; data = last;
else { } else {
data = queue.front(); qmutex.lock();
queue.pop(); if (queue.empty())
last = data; data = last;
else {
data = queue.front();
queue.pop();
last = data;
}
qmutex.unlock();
} }
qmutex.unlock();
return data; return data;
} }
} // end of namespace internal. } // 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