Add a helper method to lock the pages of memory in the RAM

CMakeLists.txt

@@ -37,6 +37,7 @@ target_include_directories(thread_priority PUBLIC
   $<INSTALL_INTERFACE:include/realtime_tools>
 )
 ament_target_dependencies(thread_priority PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(thread_priority PUBLIC cap)
 
 # Unit Tests
 if(BUILD_TESTING)

include/realtime_tools/thread_priority.hpp

@@ -29,6 +29,8 @@
 #ifndef REALTIME_TOOLS__THREAD_PRIORITY_HPP_
 #define REALTIME_TOOLS__THREAD_PRIORITY_HPP_
 
+#include <string>
+
 namespace realtime_tools
 {
 /**
@@ -44,6 +46,13 @@ bool has_realtime_kernel();
  */
 bool configure_sched_fifo(int priority);
 
+/**
+ * Locks the memory pages of the calling thread to prevent page faults. By calling this method, the programs locks all pages mapped into the address space of the calling process and future mappings. This means that the kernel will not swap out the pages to disk i.e., the pages are guaranteed to stay in RAM until later unlocked - which is important for realtime applications.
+ * \param[out] message a message describing the result of the operation
+ * \returns true if memory locking succeeded, false otherwise
+*/
+bool lock_memory(std::string & message);
+
 }  // namespace realtime_tools
 
 #endif  // REALTIME_TOOLS__THREAD_PRIORITY_HPP_

package.xml

@@ -21,6 +21,7 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
+  <build_depend>libcap-dev</build_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>lifecycle_msgs</test_depend>

src/thread_priority.cpp

@@ -29,6 +29,8 @@
 #include "realtime_tools/thread_priority.hpp"
 
 #include <sched.h>
+#include <sys/capability.h>
+#include <sys/mman.h>
 
 #include <cstring>
 #include <fstream>
@@ -53,4 +55,48 @@ bool configure_sched_fifo(int priority)
   return !sched_setscheduler(0, SCHED_FIFO, &schedp);
 }
 
+bool is_capable(cap_value_t v)
+{
+  bool rc = false;
+  cap_t caps;
+  if ((caps = cap_get_proc()) == NULL) {
+    return false;
+  }
+
+  if (cap_set_flag(caps, CAP_EFFECTIVE, 1, &v, CAP_SET) == -1) {
+    rc = false;
+  } else {
+    rc = (cap_set_proc(caps) == 0);
+  }
+  cap_free(caps);
+  return rc;
+}
+
+bool lock_memory(std::string & message)
+{
+  if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
+    if (!is_capable(CAP_IPC_LOCK)) {
+      message = "No proper privileges to lock the memory!";
+    } else if (errno == ENOMEM) {
+      message =
+        "The caller had a nonzero RLIMIT_MEMLOCK soft resource limit, but tried to lock more "
+        "memory than the limit permitted. Try running the application with privileges!";
+    } else if (errno == EPERM) {
+      message =
+        "The caller is not privileged, but needs privilege to perform the requested operation.";
+    } else if (errno == EINVAL) {
+      message =
+        "The result of the addition start+len was less than start (e.g., the addition may have "
+        "resulted in an overflow).";
+    } else if (errno == EAGAIN) {
+      message = "Some or all of the specified address range could not be locked.";
+    } else {
+      message = "Unknown error occurred!";
+    }
+    return false;
+  } else {
+    message = "Memory locked successfully!";
+    return true;
+  }
+}
 }  // namespace realtime_tools