Add command readQueue to RosQueuedSubscribe

src/ros_subscribe.cpp

@@ -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_;

src/ros_subscribe.hh

@@ -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.
 

src/ros_subscribe.hxx

@@ -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.