Fix clock_nanosleep existence checks (#193)

CMakeLists.txt

@@ -67,6 +67,7 @@ endif(READLINE_FOUND)
 include(CheckFunctionExists)
 check_function_exists(longjmp HAVE_LONGJMP)
 check_function_exists(siglongjmp HAVE_SIGLONGJMP)
+check_function_exists(clock_nanosleep HAVE_CLOCK_NANOSLEEP)
 
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/config.h.cmake
   ${CMAKE_CURRENT_BINARY_DIR}/include/config.h
@@ -95,6 +96,7 @@ if(READLINE_FOUND)
   )
 endif(READLINE_FOUND)
 
+
 # Static link options
 
 option(ENABLE_ALL_STATIC "Static link libgcc and libstdc++." OFF)
@@ -106,6 +108,37 @@ if(ENABLE_ALL_STATIC)
 endif(ENABLE_ALL_STATIC)
 
 
+# Compile flags
+
+if(CMAKE_VERSION VERSION_LESS "3.12")
+  if(HAVE_LONGJMP)
+    add_definitions(-DHAVE_LONGJMP)
+  endif()
+  if(HAVE_SIGLONGJMP)
+    add_definitions(-DHAVE_SIGLONGJMP)
+  endif()
+  if(HAVE_CLOCK_NANOSLEEP)
+    add_definitions(-DHAVE_CLOCK_NANOSLEEP)
+  endif()
+  if(HAVE_SSM)
+    add_definitions(-DHAVE_SSM)
+  endif()
+else()
+  if(HAVE_LONGJMP)
+    add_compile_definitions(HAVE_LONGJMP)
+  endif()
+  if(HAVE_SIGLONGJMP)
+    add_compile_definitions(HAVE_SIGLONGJMP)
+  endif()
+  if(HAVE_CLOCK_NANOSLEEP)
+    add_compile_definitions(HAVE_CLOCK_NANOSLEEP)
+  endif()
+  if(HAVE_SSM)
+    add_compile_definitions(HAVE_SSM)
+  endif()
+endif()
+
+
 # Add subdirectories
 
 add_subdirectory(auxlib)

cmake/config.h.cmake

@@ -11,10 +11,4 @@
 
 #cmakedefine YP_PARAMS_DIR "@YP_PARAMS_DIR@"
 
-#cmakedefine HAVE_SSM @HAVE_SSM@
-
-#cmakedefine HAVE_LONGJMP @HAVE_LONGJMP@
-
-#cmakedefine HAVE_SIGLONGJMP @HAVE_SIGLONGJMP@
-
 #endif

src/control_vehicle.c

@@ -503,39 +503,27 @@ void control_loop(void)
   yprintf(OUTPUT_LV_INFO, "Trajectory control loop started.\n");
   pthread_cleanup_push(control_loop_cleanup, NULL);
 
-#if defined(HAVE_LIBRT)  // clock_nanosleepが利用可能
+#if defined(HAVE_CLOCK_NANOSLEEP)  // clock_nanosleepが利用可能
   struct timespec request;
 
   if (clock_gettime(CLOCK_MONOTONIC, &request) == -1)
   {
     yprintf(OUTPUT_LV_ERROR, "error on clock_gettime\n");
     exit(0);
   }
+#endif  // defined(HAVE_CLOCK_NANOSLEEP)
   while (1)
   {
+#if defined(HAVE_CLOCK_NANOSLEEP)  // clock_nanosleepが利用可能
     request.tv_nsec += (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000000);
     request.tv_sec += request.tv_nsec / 1000000000;
     request.tv_nsec = request.tv_nsec % 1000000000;
 
     clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &request, 0);
-    coordinate_synchronize(odometry, spur);
-    run_control(*odometry, spur);
-
-    if ((option(OPTION_WITHOUT_DEVICE)))
-    {
-      simulate_control(*odometry, spur);
-    }
-
-    // スレッドの停止要求チェック
-    pthread_testcancel();
-  }
 #else
-  int request;
-  request = (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000);
+    yp_usleep(p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000);
+#endif  // defined(HAVE_CLOCK_NANOSLEEP)
 
-  while (1)
-  {
-    yp_usleep(request);
     coordinate_synchronize(odometry, spur);
     run_control(*odometry, spur);
 
@@ -547,7 +535,6 @@ void control_loop(void)
     // スレッドの停止要求チェック
     pthread_testcancel();
   }
-#endif  // defined(HAVE_LIBRT)
   pthread_cleanup_pop(1);
 }