From dfcffc8d4c9fe7bbd4e06f59c133302b88e02c92 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 27 Sep 2026 15:24:38 -0700 Subject: [PATCH 1/3] Added UScopeMutex::lockTry() support --- CMakeLists.txt | 2 +- package.xml | 2 +- utilite/include/rtabmap/utilite/UMutex.h | 104 +++++++++++++++++++-- utilite/test/test_umutex.cpp | 114 +++++++++++++++++++++++ 4 files changed, 214 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5783aba7..8bea0b1c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 23) -SET(RTABMAP_PATCH_VERSION 12) +SET(RTABMAP_PATCH_VERSION 13) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/package.xml b/package.xml index fb89be56..e11b6c20 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.23.12 + 0.23.13 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/utilite/include/rtabmap/utilite/UMutex.h b/utilite/include/rtabmap/utilite/UMutex.h index a10c2955..0490b460 100644 --- a/utilite/include/rtabmap/utilite/UMutex.h +++ b/utilite/include/rtabmap/utilite/UMutex.h @@ -159,28 +159,120 @@ public: * * @endcode * + * The lock can also be deferred, for example to only try locking it. The destructor + * then unlocks the mutex only if this object locked it: + * @code + * void callback() + * { + * UScopeMutex sm(m, false); // not locked yet + * if(sm.lockTry() == 0) + * { + * if(cond1) + * { + * return; // automatically unlock the mutex m + * } + * ... + * } + * // the mutex m is unlocked only if lockTry() succeeded + * } + * @endcode + * * @see UMutex */ class UScopeMutex { public: - UScopeMutex(const UMutex & mutex) : - mutex_(mutex) + /** + * @param mutex the mutex to lock. + * @param lockNow if true (default), the mutex is locked here. If false, it is not + * locked until lock() or lockTry() is called. + */ + UScopeMutex(const UMutex & mutex, bool lockNow = true) : + mutex_(mutex), + locked_(false) { - mutex_.lock(); + if(lockNow) + { + lock(); + } } // backward compatibility UScopeMutex(UMutex * mutex) : - mutex_(*mutex) + mutex_(*mutex), + locked_(false) { - mutex_.lock(); + lock(); } + /** + * Unlock the mutex, only if this object locked it. + */ ~UScopeMutex() { - mutex_.unlock(); + unlock(); } + + /** + * Lock the mutex, if this object doesn't hold it already. + * @return 0 on success, an error code otherwise. + */ + int lock() + { + if(locked_) + { + return 0; + } + int r = mutex_.lock(); + locked_ = r == 0; + return r; + } + +#if !defined(_WIN32) || (_WIN32_WINNT >= 0x0400) + /** + * Try locking the mutex, if this object doesn't hold it already. + * @return 0 if the mutex is held by this object, EBUSY (or another + * error code) otherwise. + */ + int lockTry() + { + if(locked_) + { + return 0; + } + int r = mutex_.lockTry(); + locked_ = r == 0; + return r; + } +#endif + + /** + * Unlock the mutex before this object goes out of scope, only if this object locked it. + * @return 0 on success (or if this object didn't hold the mutex), an error code otherwise. + */ + int unlock() + { + if(!locked_) + { + return 0; + } + locked_ = false; + return mutex_.unlock(); + } + + /** + * @return true if this object currently holds the mutex. + */ + bool isLocked() const + { + return locked_; + } + +private: + UScopeMutex(const UScopeMutex &); + void operator=(const UScopeMutex &); + private: const UMutex & mutex_; + bool locked_; }; #endif // UMUTEX_H diff --git a/utilite/test/test_umutex.cpp b/utilite/test/test_umutex.cpp index ca01a6eb..18f45dc8 100644 --- a/utilite/test/test_umutex.cpp +++ b/utilite/test/test_umutex.cpp @@ -149,6 +149,120 @@ TEST(UMutexTest, UScopeMutexWithPointer) t.join(); } +TEST(UMutexTest, UScopeMutexDeferredIsNotLocked) +{ + UMutex mutex; + { + UScopeMutex scopeMutex(mutex, false); + EXPECT_FALSE(scopeMutex.isLocked()); + + std::thread t([&mutex]() { + EXPECT_EQ(mutex.lockTry(), 0); // Not locked by the scope mutex + mutex.unlock(); + }); + t.join(); + } + // The destructor must not unlock a mutex the scope mutex didn't lock + mutex.lock(); + std::thread t([&mutex]() { + EXPECT_NE(mutex.lockTry(), 0); // Still locked by this thread + }); + t.join(); + mutex.unlock(); +} + +TEST(UMutexTest, UScopeMutexDeferredLock) +{ + UMutex mutex; + { + UScopeMutex scopeMutex(mutex, false); + EXPECT_EQ(scopeMutex.lock(), 0); + EXPECT_TRUE(scopeMutex.isLocked()); + + std::thread t([&mutex]() { + EXPECT_NE(mutex.lockTry(), 0); // Should fail + }); + t.join(); + } + std::thread t([&mutex]() { + EXPECT_EQ(mutex.lockTry(), 0); // Unlocked by the destructor + mutex.unlock(); + }); + t.join(); +} + +TEST(UMutexTest, UScopeMutexLockTrySucceeds) +{ + UMutex mutex; + { + UScopeMutex scopeMutex(mutex, false); + EXPECT_EQ(scopeMutex.lockTry(), 0); + EXPECT_TRUE(scopeMutex.isLocked()); + EXPECT_EQ(scopeMutex.lockTry(), 0); // Already held: not locked a second time + } + std::thread t([&mutex]() { + EXPECT_EQ(mutex.lockTry(), 0); // Unlocked once by the destructor, and free + mutex.unlock(); + }); + t.join(); +} + +TEST(UMutexTest, UScopeMutexLockTryFails) +{ + UMutex mutex; + std::atomic locked(false); + std::atomic release(false); + std::thread owner([&]() { + mutex.lock(); + locked = true; + while(!release) { std::this_thread::yield(); } + mutex.unlock(); + }); + while(!locked) { std::this_thread::yield(); } + + { + UScopeMutex scopeMutex(mutex, false); + EXPECT_NE(scopeMutex.lockTry(), 0); // Held by the other thread + EXPECT_FALSE(scopeMutex.isLocked()); + } + // The destructor didn't unlock the other thread's lock + std::thread t([&mutex]() { + EXPECT_NE(mutex.lockTry(), 0); + }); + t.join(); + + release = true; + owner.join(); + EXPECT_EQ(mutex.lockTry(), 0); + mutex.unlock(); +} + +TEST(UMutexTest, UScopeMutexEarlyUnlock) +{ + UMutex mutex; + { + UScopeMutex scopeMutex(mutex); + EXPECT_TRUE(scopeMutex.isLocked()); + EXPECT_EQ(scopeMutex.unlock(), 0); + EXPECT_FALSE(scopeMutex.isLocked()); + EXPECT_EQ(scopeMutex.unlock(), 0); // Nothing to unlock anymore + + std::thread t([&mutex]() { + EXPECT_EQ(mutex.lockTry(), 0); // Released before the end of the scope + mutex.unlock(); + }); + t.join(); + + mutex.lock(); // Locked by this thread, not by the scope mutex + } + // The destructor must not unlock it + std::thread t([&mutex]() { + EXPECT_NE(mutex.lockTry(), 0); + }); + t.join(); + mutex.unlock(); +} + TEST(UMutexTest, MultipleMutexes) { UMutex mutex1; From 8b1994cc4f6fa26b63b02b987ca937e97f23ea59 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 27 Sep 2026 15:45:32 -0700 Subject: [PATCH 2/3] lockTry should not be callable on temporary lock --- utilite/include/rtabmap/utilite/UMutex.h | 17 ++++++++++++++--- utilite/test/test_umutex.cpp | 11 +++++++++++ 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/utilite/include/rtabmap/utilite/UMutex.h b/utilite/include/rtabmap/utilite/UMutex.h index 0490b460..8442fce8 100644 --- a/utilite/include/rtabmap/utilite/UMutex.h +++ b/utilite/include/rtabmap/utilite/UMutex.h @@ -177,6 +177,17 @@ public: * } * @endcode * + * The object must be named: a temporary would be destroyed, and the mutex unlocked, + * at the end of the expression. lock(), lockTry() and unlock() can only be called on + * a named object, so that `if(UScopeMutex(m, false).lockTry() == 0)` doesn't compile. + * In C++17, the object can be scoped to an if statement instead: + * @code + * if(UScopeMutex sm(m, false); sm.lockTry() == 0) + * { + * // locked here + * } // unlocked here, only if lockTry() succeeded + * @endcode + * * @see UMutex */ class UScopeMutex @@ -215,7 +226,7 @@ public: * Lock the mutex, if this object doesn't hold it already. * @return 0 on success, an error code otherwise. */ - int lock() + int lock() & { if(locked_) { @@ -232,7 +243,7 @@ public: * @return 0 if the mutex is held by this object, EBUSY (or another * error code) otherwise. */ - int lockTry() + int lockTry() & { if(locked_) { @@ -248,7 +259,7 @@ public: * Unlock the mutex before this object goes out of scope, only if this object locked it. * @return 0 on success (or if this object didn't hold the mutex), an error code otherwise. */ - int unlock() + int unlock() & { if(!locked_) { diff --git a/utilite/test/test_umutex.cpp b/utilite/test/test_umutex.cpp index 18f45dc8..49d4e09a 100644 --- a/utilite/test/test_umutex.cpp +++ b/utilite/test/test_umutex.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include TEST(UMutexTest, Constructor) @@ -149,6 +151,15 @@ TEST(UMutexTest, UScopeMutexWithPointer) t.join(); } +// lock(), lockTry() and unlock() can only be called on a named UScopeMutex: a temporary +// would unlock the mutex at the end of the expression, before the code it should protect. +template +struct CanLockTry : std::false_type {}; +template +struct CanLockTry().lockTry()))> : std::true_type {}; +static_assert(CanLockTry::value, "lockTry() must be callable on a named UScopeMutex"); +static_assert(!CanLockTry::value, "lockTry() must not be callable on a temporary UScopeMutex"); + TEST(UMutexTest, UScopeMutexDeferredIsNotLocked) { UMutex mutex; From 90e5a21b3621826388a43952edcb65f71d91b126 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 27 Sep 2026 18:18:17 -0700 Subject: [PATCH 3/3] fixing test coverage --- utilite/test/test_umutex.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/utilite/test/test_umutex.cpp b/utilite/test/test_umutex.cpp index 49d4e09a..74b9b57b 100644 --- a/utilite/test/test_umutex.cpp +++ b/utilite/test/test_umutex.cpp @@ -202,6 +202,21 @@ TEST(UMutexTest, UScopeMutexDeferredLock) t.join(); } +TEST(UMutexTest, UScopeMutexLockWhenHeld) +{ + UMutex mutex; + { + UScopeMutex scopeMutex(mutex); // locked by the constructor + EXPECT_EQ(scopeMutex.lock(), 0); // Already held: not locked a second time + EXPECT_TRUE(scopeMutex.isLocked()); + } + std::thread t([&mutex]() { + EXPECT_EQ(mutex.lockTry(), 0); // Unlocked once by the destructor, and free + mutex.unlock(); + }); + t.join(); +} + TEST(UMutexTest, UScopeMutexLockTrySucceeds) { UMutex mutex;