Added UScopeMutex::lockTry() support

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

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<bool> locked(false);
+    std::atomic<bool> 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;