Add nanosleep function

microros_esp32_extensions/main/CMakeLists.txt

@@ -1,5 +1,5 @@
 file(GLOB SRCS ${UROS_APP_FOLDER}/*.c*)
-idf_component_register(SRCS main.c microros_transports.c ${SRCS}
+idf_component_register(SRCS main.c microros_transports.c nanosleep.c ${SRCS}
                        INCLUDE_DIRS ".")
 
 set(EXTENSIONS_DIR ${COMPONENT_DIR}/..)
@@ -63,3 +63,7 @@ set_directory_properties( PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES
 add_dependencies(${COMPONENT_LIB} microros)
 target_include_directories(${COMPONENT_LIB} PUBLIC microros)
 target_link_libraries(${COMPONENT_LIB} PUBLIC microros)
+
+# libmicroros.a is linked after this component, so force the linker to pull
+# in the nanosleep() shim before rclc references it
+target_link_libraries(${COMPONENT_LIB} INTERFACE "-u nanosleep")

microros_esp32_extensions/main/nanosleep.c

@@ -0,0 +1,31 @@
+#include <errno.h>
+#include <time.h>
+#include <unistd.h>
+
+// ESP-IDF defines _POSIX_TIMERS but does not implement nanosleep()
+// Declared weak so that a native ESP-IDF implementation takes precedence
+__attribute__((weak)) int nanosleep(const struct timespec * req, struct timespec * rem)
+{
+    (void) rem; // Never interrupted: FreeRTOS has no signals
+
+    if (req == NULL ||
+        req->tv_sec < 0 ||
+        req->tv_nsec < 0 ||
+        req->tv_nsec >= 1000000000L) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    // Sleep the whole seconds without overflowing useconds_t
+    for (time_t s = 0; s < req->tv_sec; s++) {
+        usleep(1000000);
+    }
+
+    // Round the remaining nanoseconds up to microseconds
+    useconds_t us = (req->tv_nsec + 999) / 1000;
+    if (us > 0) {
+        usleep(us);
+    }
+
+    return 0;
+}