added safety lockguards and cleanups

randaz81 · randaz81 · commit f275d581bbf9 · 2025-01-28T22:37:14.000+01:00
diff --git a/src/devices/networkWrappers/localization2D_nws_yarp/Localization2D_nws_yarp.cpp b/src/devices/networkWrappers/localization2D_nws_yarp/Localization2D_nws_yarp.cpp
@@ -48,6 +48,8 @@ Localization2D_nws_yarp::Localization2D_nws_yarp() : PeriodicThread(DEFAULT_THRE
 
 bool Localization2D_nws_yarp::attach(PolyDriver* driver)
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
+
     if (driver->isValid())
     {
         driver->view(iLoc);
@@ -83,15 +85,17 @@ bool Localization2D_nws_yarp::attach(PolyDriver* driver)
         return false;
     }
 
-    PeriodicThread::setPeriod(m_GENERAL_period);
-    return PeriodicThread::start();
+    this->setPeriod(m_GENERAL_period);
+    return this->start();
 }
 
 bool Localization2D_nws_yarp::detach()
 {
-    if (PeriodicThread::isRunning())
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    if (this->isRunning())
     {
-        PeriodicThread::stop();
+        this->stop();
     }
     m_rpcPort.interrupt();
     m_rpcPort.close();
@@ -186,8 +190,11 @@ bool Localization2D_nws_yarp::close()
 
 bool Localization2D_nws_yarp::read(yarp::os::ConnectionReader& connection)
 {
+    if (!connection.isValid()) { return false;}
     if (!m_RPC) { return false;}
 
+    std::lock_guard<std::mutex> lock(m_mutex);
+
     bool b = m_RPC->read(connection);
     if (b)
     {
@@ -202,6 +209,8 @@ bool Localization2D_nws_yarp::read(yarp::os::ConnectionReader& connection)
 
 void Localization2D_nws_yarp::run()
 {
+    std::lock_guard<std::mutex> lock(m_mutex);
+
     double m_stats_time_curr = yarp::os::Time::now();
     if (m_stats_time_curr - m_stats_time_last > 5.0)
     {
diff --git a/src/devices/networkWrappers/localization2D_nws_yarp/Localization2D_nws_yarp.h b/src/devices/networkWrappers/localization2D_nws_yarp/Localization2D_nws_yarp.h
@@ -57,6 +57,7 @@ class Localization2D_nws_yarp :
     //drivers and interfaces
     yarp::dev::PolyDriver                   pLoc;
     yarp::dev::Nav2D::ILocalization2D*      iLoc = nullptr;
+    std::mutex                              m_mutex;
 
     double                                  m_stats_time_last;
     bool                                    m_getdata_using_periodic_thread=true;
diff --git a/src/devices/networkWrappers/map2D_nws_yarp/Map2D_nws_yarp.cpp b/src/devices/networkWrappers/map2D_nws_yarp/Map2D_nws_yarp.cpp
@@ -42,7 +42,10 @@ Map2D_nws_yarp::~Map2D_nws_yarp() = default;
 
 bool Map2D_nws_yarp::read(yarp::os::ConnectionReader& connection)
 {
-    if (!m_RPC) {return false;}
+    if (!connection.isValid()) { return false;}
+    if (!m_RPC) { return false;}
+
+    std::lock_guard<std::mutex> lock (m_mutex);
 
     bool b = m_RPC->read(connection);
     if (b)
@@ -77,11 +80,15 @@ bool Map2D_nws_yarp::close()
     m_rpcPort.interrupt();
     m_rpcPort.close();
 
+    detach();
+
     return true;
 }
 
 bool Map2D_nws_yarp::detach()
 {
+    std::lock_guard lock (m_mutex);
+
     m_iMap2D = nullptr;
 
     if (m_RPC)
@@ -95,6 +102,8 @@ bool Map2D_nws_yarp::detach()
 
 bool Map2D_nws_yarp::attach(PolyDriver* driver)
 {
+    std::lock_guard lock(m_mutex);
+
     if (driver->isValid())
     {
         driver->view(m_iMap2D);
diff --git a/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.cpp b/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.cpp
@@ -79,11 +79,15 @@ bool MobileBaseVelocityControl_nws_yarp::close()
 {
     m_rpc_port_navigation_server.close();
     m_StreamingInput.close();
+
+    detach();
     return true;
 }
 
 bool MobileBaseVelocityControl_nws_yarp::detach()
 {
+    std::lock_guard lock (m_mutex);
+
     m_iNavVel = nullptr;
 
     if (m_RPC)
@@ -97,6 +101,8 @@ bool MobileBaseVelocityControl_nws_yarp::detach()
 
 bool MobileBaseVelocityControl_nws_yarp::attach(PolyDriver* driver)
 {
+    std::lock_guard lock (m_mutex);
+
     if (driver->isValid())
     {
         driver->view(m_iNavVel);
@@ -117,7 +123,10 @@ bool MobileBaseVelocityControl_nws_yarp::attach(PolyDriver* driver)
 
 bool MobileBaseVelocityControl_nws_yarp::read(yarp::os::ConnectionReader& connection)
 {
-    if (!m_RPC) { return false; }
+    if (!connection.isValid()) { return false;}
+    if (!m_RPC) { return false;}
+
+    std::lock_guard<std::mutex> lock(m_mutex);
 
     bool b = m_RPC->read(connection);
     if (b)
diff --git a/src/devices/networkWrappers/navigation2D_nws_yarp/Navigation2D_nws_yarp.cpp b/src/devices/networkWrappers/navigation2D_nws_yarp/Navigation2D_nws_yarp.cpp
@@ -33,6 +33,8 @@ Navigation2D_nws_yarp::Navigation2D_nws_yarp() : PeriodicThread(DEFAULT_THREAD_P
 
 bool Navigation2D_nws_yarp::attach(PolyDriver* driver)
 {
+    std::lock_guard lock (m_mutex);
+
     if (driver->isValid())
     {
         driver->view(iNav_target);
@@ -60,6 +62,8 @@ bool Navigation2D_nws_yarp::attach(PolyDriver* driver)
 
 bool Navigation2D_nws_yarp::detach()
 {
+    std::lock_guard lock (m_mutex);
+
     if (PeriodicThread::isRunning())
     {
         PeriodicThread::stop();
@@ -127,7 +131,10 @@ bool Navigation2D_nws_yarp::close()
 
 bool Navigation2D_nws_yarp::read(yarp::os::ConnectionReader& connection)
 {
-    if (!m_RPC) { return false; }
+    if (!connection.isValid()) { return false;}
+    if (!m_RPC) { return false;}
+
+    std::lock_guard<std::mutex> lock(m_mutex);
 
     bool b = m_RPC->read(connection);
     if (b)
@@ -143,6 +150,8 @@ bool Navigation2D_nws_yarp::read(yarp::os::ConnectionReader& connection)
 
 void Navigation2D_nws_yarp::run()
 {
+    std::lock_guard lock(m_mutex);
+
     bool ok = iNav_ctrl->getNavigationStatus(m_navigation_status);
 
     double m_stats_time_curr = yarp::os::Time::now();
diff --git a/src/devices/networkWrappers/navigation2D_nws_yarp/Navigation2D_nws_yarp.h b/src/devices/networkWrappers/navigation2D_nws_yarp/Navigation2D_nws_yarp.h
@@ -45,6 +45,7 @@ class Navigation2D_nws_yarp : public yarp::dev::DeviceDriver,
 protected:
     //thrift
     INavigation2DRPCd*                           m_RPC=nullptr;
+    std::mutex                                   m_mutex;
 
     yarp::os::Port                               m_rpcPort;
     yarp::os::Port                               m_statusPort;
diff --git a/src/devices/networkWrappers/odometry2D_nws_yarp/Odometry2D_nws_yarp.cpp b/src/devices/networkWrappers/odometry2D_nws_yarp/Odometry2D_nws_yarp.cpp
@@ -26,6 +26,7 @@ Odometry2D_nws_yarp::~Odometry2D_nws_yarp()
 
 bool Odometry2D_nws_yarp::attach(yarp::dev::PolyDriver* driver)
 {
+    std::lock_guard lock(m_mutex);
 
     if (driver->isValid())
     {
@@ -48,6 +49,8 @@ bool Odometry2D_nws_yarp::attach(yarp::dev::PolyDriver* driver)
 
 bool Odometry2D_nws_yarp::detach()
 {
+    std::lock_guard lock(m_mutex);
+
     if (PeriodicThread::isRunning())
     {
         PeriodicThread::stop();
@@ -120,6 +123,8 @@ void Odometry2D_nws_yarp::threadRelease()
 
 void Odometry2D_nws_yarp::run()
 {
+    std::lock_guard lock(m_mutex);
+
     if (m_odometry2D_interface!=nullptr)
     {
         yarp::dev::OdometryData odometryData;
@@ -190,6 +195,10 @@ bool Odometry2D_nws_yarp::close()
 
 bool Odometry2D_nws_yarp::read(yarp::os::ConnectionReader& connection)
 {
+    if (!connection.isValid()) { return false;}
+    if (!m_RPC) { return false;}
+
+    std::lock_guard<std::mutex> lock(m_mutex);
     if (m_RPC)
     {
         bool b = m_RPC->read(connection);
diff --git a/src/devices/networkWrappers/odometry2D_nws_yarp/Odometry2D_nws_yarp.h b/src/devices/networkWrappers/odometry2D_nws_yarp/Odometry2D_nws_yarp.h
@@ -57,6 +57,8 @@ class Odometry2D_nws_yarp :
     void run() override;
 
 private:
+    std::mutex        m_mutex;
+
     //thrift
     IOdometry2DRPCd* m_RPC=nullptr;
 
diff --git a/src/devices/networkWrappers/speechSynthesizer_nws_yarp/SpeechSynthesizer_nws_yarp.cpp b/src/devices/networkWrappers/speechSynthesizer_nws_yarp/SpeechSynthesizer_nws_yarp.cpp
@@ -99,6 +99,8 @@ bool SpeechSynthesizer_nws_yarp::closeMain()
 
 bool SpeechSynthesizer_nws_yarp::read(yarp::os::ConnectionReader& connection)
 {
+    if (!connection.isValid()) { return false;}
+
     bool b = m_rpc.read(connection);
     if (b)
     {
diff --git a/src/devices/networkWrappers/speechTranscription_nws_yarp/SpeechTranscription_nws_yarp.cpp b/src/devices/networkWrappers/speechTranscription_nws_yarp/SpeechTranscription_nws_yarp.cpp
@@ -99,6 +99,8 @@ bool SpeechTranscription_nws_yarp::closeMain()
 
 bool SpeechTranscription_nws_yarp::read(yarp::os::ConnectionReader& connection)
 {
+    if (!connection.isValid()) { return false;}
+
     bool b = m_rpc.read(connection);
     if (b)
     {

Original file line number	Diff line number	Diff line change
`@@ -48,6 +48,8 @@ Localization2D_nws_yarp::Localization2D_nws_yarp() : PeriodicThread(DEFAULT_THRE`
`48`	`48`
`49`	`49`	`bool Localization2D_nws_yarp::attach(PolyDriver* driver)`
`50`	`50`	`{`
	`51`	`+ std::lock_guard<std::mutex> lock(m_mutex);`
	`52`	`+`
`51`	`53`	`if (driver->isValid())`
`52`	`54`	`{`
`53`	`55`	`driver->view(iLoc);`
`@@ -83,15 +85,17 @@ bool Localization2D_nws_yarp::attach(PolyDriver* driver)`
`83`	`85`	`return false;`
`84`	`86`	`}`
`85`	`87`
`86`		`- PeriodicThread::setPeriod(m_GENERAL_period);`
`87`		`- return PeriodicThread::start();`
	`88`	`+ this->setPeriod(m_GENERAL_period);`
	`89`	`+ return this->start();`
`88`	`90`	`}`
`89`	`91`
`90`	`92`	`bool Localization2D_nws_yarp::detach()`
`91`	`93`	`{`
`92`		`- if (PeriodicThread::isRunning())`
	`94`	`+ std::lock_guard<std::mutex> lock(m_mutex);`
	`95`	`+`
	`96`	`+ if (this->isRunning())`
`93`	`97`	`{`
`94`		`- PeriodicThread::stop();`
	`98`	`+ this->stop();`
`95`	`99`	`}`
`96`	`100`	`m_rpcPort.interrupt();`
`97`	`101`	`m_rpcPort.close();`
`@@ -186,8 +190,11 @@ bool Localization2D_nws_yarp::close()`
`186`	`190`
`187`	`191`	`bool Localization2D_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`188`	`192`	`{`
	`193`	`+ if (!connection.isValid()) { return false;}`
`189`	`194`	`if (!m_RPC) { return false;}`
`190`	`195`
	`196`	`+ std::lock_guard<std::mutex> lock(m_mutex);`
	`197`	`+`
`191`	`198`	`bool b = m_RPC->read(connection);`
`192`	`199`	`if (b)`
`193`	`200`	`{`
`@@ -202,6 +209,8 @@ bool Localization2D_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`202`	`209`
`203`	`210`	`void Localization2D_nws_yarp::run()`
`204`	`211`	`{`
	`212`	`+ std::lock_guard<std::mutex> lock(m_mutex);`
	`213`	`+`
`205`	`214`	`double m_stats_time_curr = yarp::os::Time::now();`
`206`	`215`	`if (m_stats_time_curr - m_stats_time_last > 5.0)`
`207`	`216`	`{`
Original file line number	Diff line number	Diff line change
`@@ -42,7 +42,10 @@ Map2D_nws_yarp::~Map2D_nws_yarp() = default;`
`42`	`42`
`43`	`43`	`bool Map2D_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`44`	`44`	`{`
`45`		`- if (!m_RPC) {return false;}`
	`45`	`+ if (!connection.isValid()) { return false;}`
	`46`	`+ if (!m_RPC) { return false;}`
	`47`	`+`
	`48`	`+ std::lock_guard<std::mutex> lock (m_mutex);`
`46`	`49`
`47`	`50`	`bool b = m_RPC->read(connection);`
`48`	`51`	`if (b)`
`@@ -77,11 +80,15 @@ bool Map2D_nws_yarp::close()`
`77`	`80`	`m_rpcPort.interrupt();`
`78`	`81`	`m_rpcPort.close();`
`79`	`82`
	`83`	`+ detach();`
	`84`	`+`
`80`	`85`	`return true;`
`81`	`86`	`}`
`82`	`87`
`83`	`88`	`bool Map2D_nws_yarp::detach()`
`84`	`89`	`{`
	`90`	`+ std::lock_guard lock (m_mutex);`
	`91`	`+`
`85`	`92`	`m_iMap2D = nullptr;`
`86`	`93`
`87`	`94`	`if (m_RPC)`
`@@ -95,6 +102,8 @@ bool Map2D_nws_yarp::detach()`
`95`	`102`
`96`	`103`	`bool Map2D_nws_yarp::attach(PolyDriver* driver)`
`97`	`104`	`{`
	`105`	`+ std::lock_guard lock(m_mutex);`
	`106`	`+`
`98`	`107`	`if (driver->isValid())`
`99`	`108`	`{`
`100`	`109`	`driver->view(m_iMap2D);`
Original file line number	Diff line number	Diff line change
`@@ -79,11 +79,15 @@ bool MobileBaseVelocityControl_nws_yarp::close()`
`79`	`79`	`{`
`80`	`80`	`m_rpc_port_navigation_server.close();`
`81`	`81`	`m_StreamingInput.close();`
	`82`	`+`
	`83`	`+ detach();`
`82`	`84`	`return true;`
`83`	`85`	`}`
`84`	`86`
`85`	`87`	`bool MobileBaseVelocityControl_nws_yarp::detach()`
`86`	`88`	`{`
	`89`	`+ std::lock_guard lock (m_mutex);`
	`90`	`+`
`87`	`91`	`m_iNavVel = nullptr;`
`88`	`92`
`89`	`93`	`if (m_RPC)`
`@@ -97,6 +101,8 @@ bool MobileBaseVelocityControl_nws_yarp::detach()`
`97`	`101`
`98`	`102`	`bool MobileBaseVelocityControl_nws_yarp::attach(PolyDriver* driver)`
`99`	`103`	`{`
	`104`	`+ std::lock_guard lock (m_mutex);`
	`105`	`+`
`100`	`106`	`if (driver->isValid())`
`101`	`107`	`{`
`102`	`108`	`driver->view(m_iNavVel);`
`@@ -117,7 +123,10 @@ bool MobileBaseVelocityControl_nws_yarp::attach(PolyDriver* driver)`
`117`	`123`
`118`	`124`	`bool MobileBaseVelocityControl_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`119`	`125`	`{`
`120`		`- if (!m_RPC) { return false; }`
	`126`	`+ if (!connection.isValid()) { return false;}`
	`127`	`+ if (!m_RPC) { return false;}`
	`128`	`+`
	`129`	`+ std::lock_guard<std::mutex> lock(m_mutex);`
`121`	`130`
`122`	`131`	`bool b = m_RPC->read(connection);`
`123`	`132`	`if (b)`
Original file line number	Diff line number	Diff line change
`@@ -33,6 +33,8 @@ Navigation2D_nws_yarp::Navigation2D_nws_yarp() : PeriodicThread(DEFAULT_THREAD_P`
`33`	`33`
`34`	`34`	`bool Navigation2D_nws_yarp::attach(PolyDriver* driver)`
`35`	`35`	`{`
	`36`	`+ std::lock_guard lock (m_mutex);`
	`37`	`+`
`36`	`38`	`if (driver->isValid())`
`37`	`39`	`{`
`38`	`40`	`driver->view(iNav_target);`
`@@ -60,6 +62,8 @@ bool Navigation2D_nws_yarp::attach(PolyDriver* driver)`
`60`	`62`
`61`	`63`	`bool Navigation2D_nws_yarp::detach()`
`62`	`64`	`{`
	`65`	`+ std::lock_guard lock (m_mutex);`
	`66`	`+`
`63`	`67`	`if (PeriodicThread::isRunning())`
`64`	`68`	`{`
`65`	`69`	`PeriodicThread::stop();`
`@@ -127,7 +131,10 @@ bool Navigation2D_nws_yarp::close()`
`127`	`131`
`128`	`132`	`bool Navigation2D_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`129`	`133`	`{`
`130`		`- if (!m_RPC) { return false; }`
	`134`	`+ if (!connection.isValid()) { return false;}`
	`135`	`+ if (!m_RPC) { return false;}`
	`136`	`+`
	`137`	`+ std::lock_guard<std::mutex> lock(m_mutex);`
`131`	`138`
`132`	`139`	`bool b = m_RPC->read(connection);`
`133`	`140`	`if (b)`
`@@ -143,6 +150,8 @@ bool Navigation2D_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`143`	`150`
`144`	`151`	`void Navigation2D_nws_yarp::run()`
`145`	`152`	`{`
	`153`	`+ std::lock_guard lock(m_mutex);`
	`154`	`+`
`146`	`155`	`bool ok = iNav_ctrl->getNavigationStatus(m_navigation_status);`
`147`	`156`
`148`	`157`	`double m_stats_time_curr = yarp::os::Time::now();`
Original file line number	Diff line number	Diff line change
`@@ -26,6 +26,7 @@ Odometry2D_nws_yarp::~Odometry2D_nws_yarp()`
`26`	`26`
`27`	`27`	`bool Odometry2D_nws_yarp::attach(yarp::dev::PolyDriver* driver)`
`28`	`28`	`{`
	`29`	`+ std::lock_guard lock(m_mutex);`
`29`	`30`
`30`	`31`	`if (driver->isValid())`
`31`	`32`	`{`
`@@ -48,6 +49,8 @@ bool Odometry2D_nws_yarp::attach(yarp::dev::PolyDriver* driver)`
`48`	`49`
`49`	`50`	`bool Odometry2D_nws_yarp::detach()`
`50`	`51`	`{`
	`52`	`+ std::lock_guard lock(m_mutex);`
	`53`	`+`
`51`	`54`	`if (PeriodicThread::isRunning())`
`52`	`55`	`{`
`53`	`56`	`PeriodicThread::stop();`
`@@ -120,6 +123,8 @@ void Odometry2D_nws_yarp::threadRelease()`
`120`	`123`
`121`	`124`	`void Odometry2D_nws_yarp::run()`
`122`	`125`	`{`
	`126`	`+ std::lock_guard lock(m_mutex);`
	`127`	`+`
`123`	`128`	`if (m_odometry2D_interface!=nullptr)`
`124`	`129`	`{`
`125`	`130`	`yarp::dev::OdometryData odometryData;`
`@@ -190,6 +195,10 @@ bool Odometry2D_nws_yarp::close()`
`190`	`195`
`191`	`196`	`bool Odometry2D_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`192`	`197`	`{`
	`198`	`+ if (!connection.isValid()) { return false;}`
	`199`	`+ if (!m_RPC) { return false;}`
	`200`	`+`
	`201`	`+ std::lock_guard<std::mutex> lock(m_mutex);`
`193`	`202`	`if (m_RPC)`
`194`	`203`	`{`
`195`	`204`	`bool b = m_RPC->read(connection);`
Original file line number	Diff line number	Diff line change
`@@ -99,6 +99,8 @@ bool SpeechSynthesizer_nws_yarp::closeMain()`
`99`	`99`
`100`	`100`	`bool SpeechSynthesizer_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`101`	`101`	`{`
	`102`	`+ if (!connection.isValid()) { return false;}`
	`103`	`+`
`102`	`104`	`bool b = m_rpc.read(connection);`
`103`	`105`	`if (b)`
`104`	`106`	`{`
Original file line number	Diff line number	Diff line change
`@@ -99,6 +99,8 @@ bool SpeechTranscription_nws_yarp::closeMain()`
`99`	`99`
`100`	`100`	`bool SpeechTranscription_nws_yarp::read(yarp::os::ConnectionReader& connection)`
`101`	`101`	`{`
	`102`	`+ if (!connection.isValid()) { return false;}`
	`103`	`+`
`102`	`104`	`bool b = m_rpc.read(connection);`
`103`	`105`	`if (b)`
`104`	`106`	`{`