Merge pull request #213 from Field-Robotics-Lab/lock_stratified_curre…

…nt_update

Lock ocean current calculation function.

gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h

@@ -36,6 +36,7 @@
 #include <string>
 #include <vector>
 #include <thread>
+#include <mutex>
 #include "ros/callback_queue.h"
 #include "ros/subscribe_options.h"
 
@@ -108,6 +109,9 @@ namespace gazebo
     /// \brief A thread the keeps running the rosQueue
     private: std::thread databaseSubQueueThread;
 
+    /// \brief A thread mutex to lock
+    private: std::mutex lock_;
+
     /// \brief Period after which we should publish a message via ROS.
     private: gazebo::common::Time rosPublishPeriod;
 

gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc

@@ -250,6 +250,8 @@ void TransientCurrentPlugin::Update(const gazebo::common::UpdateInfo &)
 void TransientCurrentPlugin::UpdateDatabase(
   const dave_gazebo_ros_plugins::StratifiedCurrentDatabase::ConstPtr &_msg)
 {
+    this->lock_.lock();
+
     this->database.clear();
     for (int i = 0; i < _msg->depths.size(); i++)
     {
@@ -299,11 +301,15 @@ void TransientCurrentPlugin::UpdateDatabase(
       this->world_start_time[3] = _msg->worldStartTimeHour;
       this->world_start_time[4] = _msg->worldStartTimeMinute;
     }
+
+    this->lock_.unlock();
 }
 
 /////////////////////////////////////////////////
 void TransientCurrentPlugin::CalculateOceanCurrent()
 {
+  this->lock_.lock();
+
   // Update vehicle position
   double vehicleDepth = - this->model->WorldPose().Pos().Z();
 
@@ -442,6 +448,8 @@ void TransientCurrentPlugin::CalculateOceanCurrent()
     // Update time stamp
     this->lastUpdate = time;
   }
+
+  this->lock_.unlock();
 }
 
 /////////////////////////////////////////////////