added mutex to fakeLocalizer to avoid race conditions

majacQ · Nov 8, 2023 · e7c0766 · e7c0766
1 parent 8c67fd1
commit e7c0766
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 5 deletions.
diff --git a/src/devices/fakeLocalizerDevice/fakeLocalizer.cpp b/src/devices/fakeLocalizerDevice/fakeLocalizer.cpp
@@ -39,6 +39,7 @@ YARP_LOG_COMPONENT(FAKELOCALIZER, "yarp.device.fakeLocalizer")
 
 bool   fakeLocalizer::getLocalizationStatus(yarp::dev::Nav2D::LocalizationStatusEnum& status)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     status = yarp::dev::Nav2D::LocalizationStatusEnum::localization_status_localized_ok;
@@ -47,6 +48,7 @@ bool   fakeLocalizer::getLocalizationStatus(yarp::dev::Nav2D::LocalizationStatus
 
 bool   fakeLocalizer::getEstimatedPoses(std::vector<Map2DLocation>& poses)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     poses.clear();
@@ -77,6 +79,7 @@ bool   fakeLocalizer::getEstimatedPoses(std::vector<Map2DLocation>& poses)
 
 bool   fakeLocalizer::getCurrentPosition(Map2DLocation& loc)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     locThread->getCurrentLoc(loc);
@@ -85,6 +88,7 @@ bool   fakeLocalizer::getCurrentPosition(Map2DLocation& loc)
 
 bool   fakeLocalizer::setInitialPose(const Map2DLocation& loc)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     locThread->initializeLocalization(loc);
@@ -93,6 +97,7 @@ bool   fakeLocalizer::setInitialPose(const Map2DLocation& loc)
 
 bool   fakeLocalizer::getCurrentPosition(Map2DLocation& loc, yarp::sig::Matrix& cov)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     locThread->getCurrentLoc(loc,cov);
@@ -101,6 +106,7 @@ bool   fakeLocalizer::getCurrentPosition(Map2DLocation& loc, yarp::sig::Matrix&
 
 bool   fakeLocalizer::setInitialPose(const Map2DLocation& loc, const yarp::sig::Matrix& cov)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     locThread->initializeLocalization(loc);
@@ -109,6 +115,7 @@ bool   fakeLocalizer::setInitialPose(const Map2DLocation& loc, const yarp::sig::
 
 bool   fakeLocalizer::getEstimatedOdometry(yarp::dev::OdometryData& odom)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status" ; return false; }
 
     Map2DLocation loc;
@@ -144,20 +151,23 @@ fakeLocalizer::~fakeLocalizer()
 
 bool fakeLocalizer::startLocalizationService()
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     return true;
 }
 
 bool fakeLocalizer::stopLocalizationService()
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (!locThread) { yCError(FAKELOCALIZER) << "Invalid status"; return false; }
 
     return true;
 }
 
 bool fakeLocalizer::close()
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (locThread)
     {
         locThread->stop();
@@ -190,7 +200,7 @@ void fakeLocalizerThread::run()
         m_last_statistics_printed = yarp::os::Time::now();
     }
 
-    std::lock_guard<std::mutex> lock(m_mutex);
+    std::lock_guard<std::mutex> lock(m_mutex_thread);
     yarp::sig::Vector loc(3);
     loc[0] = 0.0;
     loc[1] = 0.0;
@@ -226,7 +236,7 @@ void fakeLocalizerThread::run()
 bool fakeLocalizerThread::initializeLocalization(const Map2DLocation& loc)
 {
     yCInfo(FAKELOCALIZER) << "Localization init request: (" << loc.map_id << ")";
-    std::lock_guard<std::mutex> lock(m_mutex);
+    std::lock_guard<std::mutex> lock(m_mutex_thread);
     m_initial_loc.map_id = loc.map_id;
     m_initial_loc.x = loc.x;
     m_initial_loc.y = loc.y;
@@ -249,14 +259,14 @@ bool fakeLocalizerThread::initializeLocalization(const Map2DLocation& loc)
 
 bool fakeLocalizerThread::getCurrentLoc(Map2DLocation& loc)
 {
-    std::lock_guard<std::mutex> lock(m_mutex);
+    std::lock_guard<std::mutex> lock(m_mutex_thread);
     loc = m_current_loc;
     return true;
 }
 
 bool fakeLocalizerThread::getCurrentLoc(Map2DLocation& loc, yarp::sig::Matrix& cov)
 {
-    std::lock_guard<std::mutex> lock(m_mutex);
+    std::lock_guard<std::mutex> lock(m_mutex_thread);
     loc = m_current_loc;
     cov.resize(3,3);
     cov.eye();

diff --git a/src/devices/fakeLocalizerDevice/fakeLocalizer.h b/src/devices/fakeLocalizerDevice/fakeLocalizer.h
@@ -30,7 +30,7 @@ class fakeLocalizerThread :
     yarp::dev::Nav2D::Map2DLocation    m_initial_odom;
     yarp::dev::Nav2D::Map2DLocation    m_current_loc;
     yarp::dev::Nav2D::Map2DLocation    m_current_odom;
-    std::mutex                         m_mutex;
+    std::mutex                         m_mutex_thread;
     yarp::os::Searchable&              m_cfg;
     std::string                        m_local_name;
 
@@ -57,6 +57,8 @@ class fakeLocalizer :
 {
 public:
     fakeLocalizerThread         *locThread = nullptr;
+    std::mutex                  m_mutex;
+
     virtual bool open(yarp::os::Searchable& config) override;
 
     fakeLocalizer();