Merge pull request #30 from sonia-auv/develop

Develop

config/compe2022.yaml

@@ -1,3 +1,4 @@
 #********** Driver Parameters ************
 
-/proc_underwater_com/settings/number_mission : 16
+/proc_underwater_com/settings/number_mission : 11
+/proc_underwater_com/settings/delay_ack : 5

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>proc_underwater_com</name>
-  <version>1.0.1</version>
+  <version>1.1.0</version>
   <description>The proc_underwater_com package</description>
 
   <maintainer email="francisalonzo29@gmail.com">Francis Alonzo</maintainer>

src/Configuration.cc

@@ -31,7 +31,8 @@ namespace proc_underwater_com
     Configuration::Configuration(const ros::NodeHandlePtr &nh)
         : nh(nh),
           nbmissions(16),
-          id(8)
+          id(8),
+          delay_ack(5)
     {
         Deserialize();
     }
@@ -44,6 +45,7 @@ namespace proc_underwater_com
 
         FindParameter("/settings/number_mission", nbmissions);
         FindParameter("/settings/id", id);
+        FindParameter("/settings/delay_ack", delay_ack);
 
         ROS_INFO("End deserialize params");
     }

src/Configuration.h

@@ -41,13 +41,15 @@ namespace proc_underwater_com
 
         int getNumberMission() const {return nbmissions;}
         int getNumberid() const {return id;}
+        int getDelayAck() const {return delay_ack;}
 
     private:
 
         ros::NodeHandlePtr nh;
 
         int nbmissions;
         int id;
+        int delay_ack;
 
         void Deserialize();
         void SetParameter();

src/SharedQueue.h

@@ -0,0 +1,116 @@
+#ifndef INTERFACE_RS485_SHAREDQUEUE_H
+#define INTERFACE_RS485_SHAREDQUEUE_H
+
+#include <queue>
+#include <mutex>
+#include <condition_variable>
+
+template <typename T>
+class SharedQueue
+{
+public:
+    SharedQueue();
+    ~SharedQueue();
+
+    T& front();
+    void pop_front();
+
+    T get_n_pop_front();
+
+    void push_back(const T& item);
+    void push_back(T&& item);
+
+
+    unsigned long size();
+    bool empty();
+
+private:
+    std::deque<T> queue_;
+    std::mutex mutex_;
+    std::condition_variable cond_;
+};
+
+template <typename T>
+SharedQueue<T>::SharedQueue(){}
+
+template <typename T>
+SharedQueue<T>::~SharedQueue(){}
+
+template <typename T>
+T& SharedQueue<T>::front()
+{
+    std::unique_lock<std::mutex> mlock(mutex_);
+    while (queue_.empty())
+    {
+        cond_.wait(mlock);
+    }
+    return queue_.front();
+}
+
+template <typename T>
+void SharedQueue<T>::pop_front()
+{
+    std::unique_lock<std::mutex> mlock(mutex_);
+    while (queue_.empty())
+    {
+        cond_.wait(mlock);
+    }
+    queue_.pop_front();
+}
+
+template <typename T>
+T SharedQueue<T>::get_n_pop_front()
+{
+    std::unique_lock<std::mutex> mlock(mutex_);
+    while (queue_.empty())
+    {
+        cond_.wait(mlock);
+    }
+    T temp = queue_.front();
+    queue_.pop_front();
+    mlock.unlock();
+    cond_.notify_one();
+    return temp;
+}
+
+template <typename T>
+void SharedQueue<T>::push_back(const T& item)
+{
+    std::unique_lock<std::mutex> mlock(mutex_);
+    queue_.push_back(item);
+    mlock.unlock();     // unlock before notificiation to minimize mutex con
+    cond_.notify_one(); // notify one waiting thread
+
+}
+
+template <typename T>
+void SharedQueue<T>::push_back(T&& item)
+{
+    std::unique_lock<std::mutex> mlock(mutex_);
+    queue_.push_back(std::move(item));
+    mlock.unlock();     // unlock before notificiation to minimize mutex con
+    cond_.notify_one(); // notify one waiting thread
+
+}
+
+template <typename T>
+unsigned long SharedQueue<T>::size()
+{
+    std::unique_lock<std::mutex> mlock(mutex_);
+    unsigned long size = queue_.size();
+    mlock.unlock();
+    cond_.notify_one();
+    return size;
+}
+
+template <typename T>
+bool SharedQueue<T>::empty()
+{
+    std::unique_lock<std::mutex> mlock(mutex_);
+    bool empty = queue_.empty();
+    mlock.unlock();
+    cond_.notify_one();
+    return empty;
+}
+
+#endif //INTERFACE_RS485_SHAREDQUEUE_H