diff --git a/.github/workflows/docker-image-perception-develop.yml b/.github/workflows/docker-image-perception-develop.yml
index 012b0fb..d1c36dd 100644
--- a/.github/workflows/docker-image-perception-develop.yml
+++ b/.github/workflows/docker-image-perception-develop.yml
@@ -81,4 +81,7 @@ jobs:
       - name: Push Image to Github Packages Registry
         run: |
           docker push --all-tags ${GITHUB_REMOTE_URL}/${IMAGE_NAME}
+      - name: Start a cronjob
+        run: |
+          curl -XPOST -u "${{ secrets.WORKFLOW_USERNAME}}:${{secrets.WORKFLOW_TOKEN}}" -H "Accept: application/vnd.github.everest-preview+json" -H "Content-Type: application/json" https://api.github.com/repos/sonia-auv/provider_actuators/dispatches --data '{"event_type": "build_application"}'
 
diff --git a/package.xml b/package.xml
index 7356c0f..69814db 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>provider_power</name>
-  <version>2.0.3</version>
+  <version>2.0.5</version>
   <description>The provider_provider package</description>
 
   <maintainer email="francisalonzo29@gmail.com">Francis Alonzo</maintainer>
diff --git a/src/provider_power/provider_power_node.cc b/src/provider_power/provider_power_node.cc
index 204c2ac..1763c77 100644
--- a/src/provider_power/provider_power_node.cc
+++ b/src/provider_power/provider_power_node.cc
@@ -28,6 +28,8 @@
  * along with S.O.N.I.A. software. If not, see <http://www.gnu.org/licenses/>.
  */
 
+#define TIME_BETWEEN_FLUSH 15 // seconds
+
 #include "provider_power_node.h"
 
 namespace provider_power {
@@ -81,7 +83,7 @@ namespace provider_power {
 // M E T H O D   S E C T I O N
 
     void ProviderPowerNode::Spin(){
-        ros::Rate r(5); // 5 hz
+        ros::Rate r(RATE_HZ); // 5 hz
 
         while(ros::ok())
         {
@@ -99,6 +101,8 @@ namespace provider_power {
             switch (receivedData->cmd)
             {
                 case sonia_common::SendRS485Msg::CMD_VOLTAGE:
+                {
+                    std::unique_lock<std::mutex> mlock(voltageMutex);
                     switch (receivedData->slave)
                     {
                         case sonia_common::SendRS485Msg::SLAVE_PSU0:
@@ -109,7 +113,7 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueVoltageSlave0.push_back(msg);
+                                parsedQueueVoltageSlave0 = msg;
                             }
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU1:
@@ -120,7 +124,7 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueVoltageSlave1.push_back(msg);
+                                parsedQueueVoltageSlave1 = msg;
                             }
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU2:
@@ -131,7 +135,7 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueVoltageSlave2.push_back(msg);
+                                parsedQueueVoltageSlave2 = msg;
                             }
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU3:
@@ -142,15 +146,18 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueVoltageSlave3.push_back(msg);
+                                parsedQueueVoltageSlave3 = msg;
                             }
                             break;
                         default:
                             ROS_WARN_STREAM("Unknown SLAVE to provider_power");
                             break;
                     }
+                }
                     break;
                 case sonia_common::SendRS485Msg::CMD_CURRENT:
+                {
+                    std::unique_lock<std::mutex> mlock(currentMutex);
                     switch (receivedData->slave)
                     {
                         case sonia_common::SendRS485Msg::SLAVE_PSU0:
@@ -161,7 +168,7 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueCurrentSlave0.push_back(msg);
+                                parsedQueueCurrentSlave0 = msg;
                             }
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU1:
@@ -172,7 +179,7 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueCurrentSlave1.push_back(msg);
+                                parsedQueueCurrentSlave1 = msg;
                             }
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU2:
@@ -183,7 +190,7 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueCurrentSlave2.push_back(msg);
+                                parsedQueueCurrentSlave2 = msg;
                             }
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU3:
@@ -194,33 +201,39 @@ namespace provider_power {
                             }
                             else
                             {
-                                parsedQueueCurrentSlave3.push_back(msg);
+                                parsedQueueCurrentSlave3 = msg;
                             }
                             break;
                         default:
                             ROS_WARN_STREAM("Unknown SLAVE to provider_power");
                             break;
                     }
+                }
                     break;
                 case sonia_common::SendRS485Msg::CMD_READ_MOTOR:
+                {
+                    std::unique_lock<std::mutex> mlock(motorMutex);
                     switch (receivedData->slave)
                     {
                         case sonia_common::SendRS485Msg::SLAVE_PSU0:
-                            readQueueMotorSlave0.push_back(receivedData->data);
+                            readQueueMotorSlave0 = receivedData->data;
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU1:
-                            readQueueMotorSlave1.push_back(receivedData->data);
+                            readQueueMotorSlave1 = receivedData->data;
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU2:
-                            readQueueMotorSlave2.push_back(receivedData->data);
+                            readQueueMotorSlave2 = receivedData->data;
                             break;
                         case sonia_common::SendRS485Msg::SLAVE_PSU3:
-                            readQueueMotorSlave3.push_back(receivedData->data);
+                            readQueueMotorSlave3 = receivedData->data;
                             break;
                         default:
                             ROS_WARN_STREAM("Unknown SLAVE to provider_power");
                             break;
                     }
+                }
+                    break;
+                case sonia_common::SendRS485Msg::CMD_KEEP_ALIVE:
                     break;
                 default:
                     ROS_WARN_STREAM("Unknow CMD to provider_power");
@@ -243,8 +256,10 @@ namespace provider_power {
             case sonia_common::SendRS485Msg::CMD_READ_MOTOR:
                 ReadMotorCMD(receivedData->data, nb_motor);
                 break;
+            case sonia_common::SendRS485Msg::CMD_KEEP_ALIVE:
+                break;
             default:
-                //ROS_WARN_STREAM("Unknow CMD to provider_power");
+                ROS_WARN_STREAM("Unknow CMD to provider_power");
                 break;
             }
         }
@@ -368,98 +383,125 @@ namespace provider_power {
 
     void ProviderPowerNode::writeVoltageData()
     {
+        ros::Rate r(RATE_HZ_MESSAGE);
+
         while(!ros::isShuttingDown())
         {
-            ros::Duration(0.1).sleep();
-            while(!parsedQueueVoltageSlave0.empty() && !parsedQueueVoltageSlave1.empty() && !parsedQueueVoltageSlave2.empty() && !parsedQueueVoltageSlave3.empty())
-            {
-                std_msgs::Float64MultiArray msg_16V;
-                std_msgs::Float64MultiArray msg_12V;
-
-                std::vector<double> msg_slave0 = parsedQueueVoltageSlave0.get_n_pop_front();
-                std::vector<double> msg_slave1 = parsedQueueVoltageSlave1.get_n_pop_front();
-                std::vector<double> msg_slave2 = parsedQueueVoltageSlave2.get_n_pop_front();
-                std::vector<double> msg_slave3 = parsedQueueVoltageSlave3.get_n_pop_front();
-
-                // Motors Voltage
-                msg_16V.data.push_back(msg_slave0[0]);
-                msg_16V.data.push_back(msg_slave1[0]);
-                msg_16V.data.push_back(msg_slave2[0]);
-                msg_16V.data.push_back(msg_slave3[0]);
-                msg_16V.data.push_back(msg_slave0[1]);
-                msg_16V.data.push_back(msg_slave1[1]);
-                msg_16V.data.push_back(msg_slave2[1]);
-                msg_16V.data.push_back(msg_slave3[1]);
-
-                // Batteries Voltage
-                msg_16V.data.push_back((msg_slave0[3]+msg_slave1[3])/2);
-                msg_16V.data.push_back((msg_slave2[3]+msg_slave3[3])/2);
-
-                // 12V Voltage
-                msg_12V.data.push_back(msg_slave0[2]);
-                msg_12V.data.push_back(msg_slave1[2]);
-                msg_12V.data.push_back(msg_slave2[2]);
-                msg_12V.data.push_back(msg_slave3[2]);
-
-                voltage16V_publisher_.publish(msg_16V);
-                voltage12V_publisher_.publish(msg_12V);
+            std_msgs::Float64MultiArray msg_16V;
+            std_msgs::Float64MultiArray msg_12V;
 
+            std::unique_lock<std::mutex> mlock(voltageMutex);
+
+            std::vector<double> msg_slave0 = parsedQueueVoltageSlave0;
+            std::vector<double> msg_slave1 = parsedQueueVoltageSlave1;
+            std::vector<double> msg_slave2 = parsedQueueVoltageSlave2;
+            std::vector<double> msg_slave3 = parsedQueueVoltageSlave3;
+
+            mlock.unlock();
+
+            if(msg_slave0.empty() || msg_slave1.empty() || msg_slave2.empty() || msg_slave3.empty())
+            {
+                r.sleep();
+                continue;
             }
+            
+            // Motors Voltage
+            msg_16V.data.push_back(msg_slave0[0]);
+            msg_16V.data.push_back(msg_slave1[0]);
+            msg_16V.data.push_back(msg_slave2[0]);
+            msg_16V.data.push_back(msg_slave3[0]);
+            msg_16V.data.push_back(msg_slave0[1]);
+            msg_16V.data.push_back(msg_slave1[1]);
+            msg_16V.data.push_back(msg_slave2[1]);
+            msg_16V.data.push_back(msg_slave3[1]);
+
+            // Batteries Voltage
+            msg_16V.data.push_back((msg_slave0[3]+msg_slave1[3])/2);
+            msg_16V.data.push_back((msg_slave2[3]+msg_slave3[3])/2);
+
+            // 12V Voltage
+            msg_12V.data.push_back(msg_slave0[2]);
+            msg_12V.data.push_back(msg_slave1[2]);
+            msg_12V.data.push_back(msg_slave2[2]);
+            msg_12V.data.push_back(msg_slave3[2]);
+
+            voltage16V_publisher_.publish(msg_16V);
+            voltage12V_publisher_.publish(msg_12V);
+
+            r.sleep();
         }
     }
 
     void ProviderPowerNode::writeCurrentData()
     {
+        ros::Rate r(RATE_HZ_MESSAGE);
         while(!ros::isShuttingDown())
-        {
-            ros::Duration(0.1).sleep();
-            while(!parsedQueueCurrentSlave0.empty() && !parsedQueueCurrentSlave1.empty() && !parsedQueueCurrentSlave2.empty() && !parsedQueueCurrentSlave3.empty())
-            {
-                std_msgs::Float64MultiArray msg;
+        {   
+            std_msgs::Float64MultiArray msg;
 
-                std::vector<double> msg_slave0 = parsedQueueCurrentSlave0.get_n_pop_front();
-                std::vector<double> msg_slave1 = parsedQueueCurrentSlave1.get_n_pop_front();
-                std::vector<double> msg_slave2 = parsedQueueCurrentSlave2.get_n_pop_front();
-                std::vector<double> msg_slave3 = parsedQueueCurrentSlave3.get_n_pop_front();
+            std::unique_lock<std::mutex> mlock(currentMutex);
 
-                for (int i = 0; i <= 2; i++)
-                {
-                    msg.data.push_back(msg_slave0[i]);
-                    msg.data.push_back(msg_slave1[i]);
-                    msg.data.push_back(msg_slave2[i]);
-                    msg.data.push_back(msg_slave3[i]);
-                }
+            std::vector<double> msg_slave0 = parsedQueueCurrentSlave0;
+            std::vector<double> msg_slave1 = parsedQueueCurrentSlave1;
+            std::vector<double> msg_slave2 = parsedQueueCurrentSlave2;
+            std::vector<double> msg_slave3 = parsedQueueCurrentSlave3;
+
+            mlock.unlock();
 
-                current_publisher_.publish(msg);
+            if(msg_slave0.empty() || msg_slave1.empty() || msg_slave2.empty() || msg_slave3.empty())
+            {
+                r.sleep();
+                continue;
+            }
+
+            for (int i = 0; i <= 2; i++)
+            {
+                msg.data.push_back(msg_slave0[i]);
+                msg.data.push_back(msg_slave1[i]);
+                msg.data.push_back(msg_slave2[i]);
+                msg.data.push_back(msg_slave3[i]);
             }
+
+            current_publisher_.publish(msg);
+
+            r.sleep();
         }
     }
 
     void ProviderPowerNode::writeMotorData()
     {
-         while(!ros::isShuttingDown())
+        ros::Rate r(RATE_HZ_MESSAGE);
+
+        while(!ros::isShuttingDown())
         {
-            ros::Duration(0.1).sleep();
-            while(!readQueueMotorSlave0.empty() && !readQueueMotorSlave1.empty() && !readQueueMotorSlave2.empty() && !readQueueMotorSlave3.empty())
-            {
-                std_msgs::UInt8MultiArray msg;
+            std_msgs::UInt8MultiArray msg;
 
-                std::vector<uint8_t> msg_slave0 = readQueueMotorSlave0.get_n_pop_front();
-                std::vector<uint8_t> msg_slave1 = readQueueMotorSlave1.get_n_pop_front();
-                std::vector<uint8_t> msg_slave2 = readQueueMotorSlave2.get_n_pop_front();
-                std::vector<uint8_t> msg_slave3 = readQueueMotorSlave3.get_n_pop_front();
+            std::unique_lock<std::mutex> mlock(motorMutex);
 
-                for (int i = 0; i < 2; i++)
-                {
-                    msg.data.push_back(msg_slave0[i]);
-                    msg.data.push_back(msg_slave1[i]);
-                    msg.data.push_back(msg_slave2[i]);
-                    msg.data.push_back(msg_slave3[i]);
-                }
+            std::vector<uint8_t> msg_slave0 = readQueueMotorSlave0;
+            std::vector<uint8_t> msg_slave1 = readQueueMotorSlave1;
+            std::vector<uint8_t> msg_slave2 = readQueueMotorSlave2;
+            std::vector<uint8_t> msg_slave3 = readQueueMotorSlave3;
+
+            mlock.unlock();
+
+            if(msg_slave0.empty() || msg_slave1.empty() || msg_slave2.empty() || msg_slave3.empty())
+            {
+                r.sleep();
+                continue;
+            }
 
-                motor_publisher_.publish(msg);
+            for (int i = 0; i < 2; i++)
+            {
+                msg.data.push_back(msg_slave0[i]);
+                msg.data.push_back(msg_slave1[i]);
+                msg.data.push_back(msg_slave2[i]);
+                msg.data.push_back(msg_slave3[i]);
             }
+
+            motor_publisher_.publish(msg);
+
+            r.sleep();
         }
     }
-
 }
diff --git a/src/provider_power/provider_power_node.h b/src/provider_power/provider_power_node.h
index 49aff40..c4a99d8 100644
--- a/src/provider_power/provider_power_node.h
+++ b/src/provider_power/provider_power_node.h
@@ -37,12 +37,15 @@
 #include <std_msgs/UInt8MultiArray.h>
 #include <std_msgs/Bool.h>
 #include <thread>
-#include "sharedQueue.h"
+#include <mutex>
 
 namespace provider_power {
 
     class ProviderPowerNode {
     public:
+        const double RATE_HZ = 5.0;
+        const double RATE_HZ_MESSAGE = 2.0;
+
         //============================================================================
         // P U B L I C   C / D T O R S
         ProviderPowerNode(ros::NodeHandlePtr &nh);
@@ -93,21 +96,22 @@ namespace provider_power {
         std::thread writerCurrent;
         std::thread writerMotor;
 
-        SharedQueue<std_msgs::Float64MultiArray::ConstPtr> writerQueueVoltage;
-        SharedQueue<std_msgs::Float64MultiArray::ConstPtr> writerQueueCurrent;
-        SharedQueue<std_msgs::Float64MultiArray::ConstPtr> writerQueueMotor;
-        SharedQueue<std::vector<double>> parsedQueueVoltageSlave0;
-        SharedQueue<std::vector<double>> parsedQueueVoltageSlave1;
-        SharedQueue<std::vector<double>> parsedQueueVoltageSlave2;
-        SharedQueue<std::vector<double>> parsedQueueVoltageSlave3;
-        SharedQueue<std::vector<double>> parsedQueueCurrentSlave0;
-        SharedQueue<std::vector<double>> parsedQueueCurrentSlave1;
-        SharedQueue<std::vector<double>> parsedQueueCurrentSlave2;
-        SharedQueue<std::vector<double>> parsedQueueCurrentSlave3;
-        SharedQueue<std::vector<uint8_t>> readQueueMotorSlave0;
-        SharedQueue<std::vector<uint8_t>> readQueueMotorSlave1;
-        SharedQueue<std::vector<uint8_t>> readQueueMotorSlave2;
-        SharedQueue<std::vector<uint8_t>> readQueueMotorSlave3;
+        std::mutex voltageMutex;
+        std::mutex currentMutex;
+        std::mutex motorMutex;
+
+        std::vector<double> parsedQueueVoltageSlave0;
+        std::vector<double> parsedQueueVoltageSlave1;
+        std::vector<double> parsedQueueVoltageSlave2;
+        std::vector<double> parsedQueueVoltageSlave3;
+        std::vector<double> parsedQueueCurrentSlave0;
+        std::vector<double> parsedQueueCurrentSlave1;
+        std::vector<double> parsedQueueCurrentSlave2;
+        std::vector<double> parsedQueueCurrentSlave3;
+        std::vector<uint8_t> readQueueMotorSlave0;
+        std::vector<uint8_t> readQueueMotorSlave1;
+        std::vector<uint8_t> readQueueMotorSlave2;
+        std::vector<uint8_t> readQueueMotorSlave3;
 
         union powerData {
             uint8_t Bytes[4];
diff --git a/src/provider_power/sharedQueue.h b/src/provider_power/sharedQueue.h
deleted file mode 100644
index a024437..0000000
--- a/src/provider_power/sharedQueue.h
+++ /dev/null
@@ -1,117 +0,0 @@
-
-#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