From beaf46dc843dd612b95e61e86826305835785309 Mon Sep 17 00:00:00 2001 From: Woensug Choi Date: Mon, 21 Feb 2022 18:45:42 +0900 Subject: [PATCH 1/3] lock calculateoceancurrent function --- .../dave_gazebo_model_plugins/ocean_current_model_plugin.h | 5 +++++ .../src/ocean_current_model_plugin.cc | 4 ++++ 2 files changed, 9 insertions(+) diff --git a/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h b/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h index 3382a1a4..f2a8212e 100644 --- a/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h +++ b/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h @@ -39,6 +39,8 @@ #include "ros/callback_queue.h" #include "ros/subscribe_options.h" +#include + namespace gazebo { /// \brief Class for the underwater current plugin @@ -108,6 +110,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; diff --git a/gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc index 3ea91d9b..5d7bb002 100644 --- a/gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc +++ b/gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc @@ -304,6 +304,8 @@ void TransientCurrentPlugin::UpdateDatabase( ///////////////////////////////////////////////// void TransientCurrentPlugin::CalculateOceanCurrent() { + this->lock_.lock(); + // Update vehicle position double vehicleDepth = - this->model->WorldPose().Pos().Z(); @@ -442,6 +444,8 @@ void TransientCurrentPlugin::CalculateOceanCurrent() // Update time stamp this->lastUpdate = time; } + + this->lock_.unlock(); } ///////////////////////////////////////////////// From 8aa836d0f15d7dc8342df8b981639e83c0df646b Mon Sep 17 00:00:00 2001 From: Woensug Choi Date: Mon, 21 Feb 2022 19:15:16 +0900 Subject: [PATCH 2/3] fix for ci --- .../dave_gazebo_model_plugins/ocean_current_model_plugin.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h b/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h index f2a8212e..6eecf976 100644 --- a/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h +++ b/gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/ocean_current_model_plugin.h @@ -36,11 +36,10 @@ #include #include #include +#include #include "ros/callback_queue.h" #include "ros/subscribe_options.h" -#include - namespace gazebo { /// \brief Class for the underwater current plugin From 32c49558b256a33ab3cd5d56d6d947ae7008b91f Mon Sep 17 00:00:00 2001 From: Woensug Choi Date: Tue, 22 Feb 2022 13:35:34 +0900 Subject: [PATCH 3/3] additional lock to UpdateDatabase --- .../src/ocean_current_model_plugin.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc b/gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc index 5d7bb002..9f94a34d 100644 --- a/gazebo/dave_gazebo_model_plugins/src/ocean_current_model_plugin.cc +++ b/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,6 +301,8 @@ void TransientCurrentPlugin::UpdateDatabase( this->world_start_time[3] = _msg->worldStartTimeHour; this->world_start_time[4] = _msg->worldStartTimeMinute; } + + this->lock_.unlock(); } /////////////////////////////////////////////////