diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 46f65b2..4a7d20c 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -13,7 +13,7 @@ ament_export_include_directories(include) include_directories(include) if(NOT WIN32) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c11 -Wall -Wextra") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") endif() set(${PROJECT_NAME}_sources @@ -34,10 +34,61 @@ ament_target_dependencies(${PROJECT_NAME} "rmw" "rosidl_generator_c" ) +set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c11") if(AMENT_ENABLE_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + set(extra_test_libraries ) + ament_find_gtest() # For GTEST_LIBRARIES + if(APPLE) + add_library(${PROJECT_NAME}_memory_tools_interpose SHARED test/memory_tools_osx_interpose.cpp) + target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) + set_target_properties(${PROJECT_NAME}_memory_tools_interpose PROPERTIES COMPILE_FLAGS "-std=c++11") + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) + endif() + add_library(${PROJECT_NAME}_memory_tools SHARED test/memory_tools.cpp) + set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(${PROJECT_NAME}_memory_tools ${GTEST_LIBRARIES} ${extra_test_libraries}) + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) + + ament_add_gtest(test_memory_tools + test/test_memory_tools.cpp + ENV DYLD_INSERT_LIBRARIES=$) + if(TARGET test_memory_tools) + target_include_directories(test_memory_tools PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) + endif() + + ament_add_gtest(test_allocator + test/rcl/test_allocator.cpp + ENV DYLD_INSERT_LIBRARIES=$) + if(TARGET test_allocator) + target_include_directories(test_allocator PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries}) + endif() + + ament_add_gtest(test_time + test/rcl/test_time.cpp + ENV DYLD_INSERT_LIBRARIES=$) + if(TARGET test_time) + target_include_directories(test_time PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) + endif() endif() ament_package() diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 7742c50..b47313f 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -23,7 +23,7 @@ extern "C" #include "rcl/types.h" /// Encapsulation of an allocator. -/* NULL for allocate implies that the system should use malloc and free. */ +/* To use malloc, free, and realloc use rcl_get_default_allocator */ typedef struct rcl_allocator_t { /// Allocate memory, given a size and state structure. /* An error should be indicated by returning null. */ @@ -44,7 +44,10 @@ typedef struct rcl_allocator_t { } rcl_allocator_t; /// Return a properly initialized rcl_allocator_t with default values. -/* This function does not allocate memory. */ +/* This function does not allocate memory. + * This function is thread-safe. + * This function is lock-free. + */ rcl_allocator_t rcl_get_default_allocator(); diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index bffb8be..fddbce4 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -21,7 +21,6 @@ extern "C" #endif #include "rcl/types.h" -#include "rmw/rmw.h" #define RCL_S_TO_NS(seconds) (seconds * 1000000000) #define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000000) @@ -31,55 +30,89 @@ extern "C" #define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000000) #define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000) -typedef rmw_time_t rcl_time_t; - -/// Create a rcl_time_t struct with a given signed number of nanoseconds. -rcl_time_t -rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds); - -/// Create a rcl_time_t struct with a given unsigned number of nanoseconds. -rcl_time_t -rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds); - -/// Convert a rcl_time_t struct into an unsigned number of nanoseconds. -/* This function will return 0 if the time argument is NULL. */ -uint64_t -rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * time); - -/// Retrieve the current time as a rcl_time_t struct. -/* The now argument must point to an allocated rcl_time_t struct, as the - * result is copied into this variable. +/// Struct which encapsulates a point in time according to a system clock. +/* The system clock's point of reference is the Unix Epoch (January 1st, 1970). + * See: http://stackoverflow.com/a/32189845/671658 + * Therefore all times in this struct are positive and count the nanoseconds + * since the Unix epoch and this struct cannot be used on systems which report + * a current time before the Unix Epoch. + * Leap seconds are not considered. * + * The struct represents time as nanoseconds in an unsigned 64-bit integer. + * The struct is capable of representing any time until the year 2554 with + * nanosecond precisions. + */ +typedef struct rcl_system_time_point_t { + uint64_t nanoseconds; +} rcl_system_time_point_t; + +/// Struct which encapsulates a point in time according to a steady clock. +/* The steady clock's point of reference is system defined, but often the time + * that the process started plus a signed random offset. + * See: http://stackoverflow.com/a/32189845/671658 + * However, the clock is guaranteed to be monotonically increasing. + * Therefore all times in this struct are positive and count nanoseconds + * since an unspecified, but constant point in time. + * + * The struct represents time as nanoseconds in an unsigned 64-bit integer. + */ +typedef struct rcl_steady_time_point_t { + uint64_t nanoseconds; +} rcl_steady_time_point_t; + +/// Struct which encapsulates a duration of time between two points in time. +/* The struct can represent any time within the range [~292 years, ~-292 years] + * with nanosecond precision. + */ +typedef struct rcl_duration_t { + int64_t nanoseconds; +} rcl_duration_t; + +/// Retrieve the current time as a rcl_system_time_point_t struct. +/* This function returns the time from a system clock. + * The closest equivalent would be to std::chrono::system_clock::now(); + * + * The resolution (e.g. nanoseconds vs microseconds) is not guaranteed. + * + * The now argument must point to an allocated rcl_system_time_point_t struct, + * as the result is copied into this variable. + * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free, with an exception on Windows. * On Windows this is lock-free if the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * - * \param[out] now a rcl_time_t struct in which the current time is stored + * \param[out] now a struct in which the current time is stored * \return RCL_RET_OK if the current time was successfully obtained, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t -rcl_time_now(rcl_time_t * now); +rcl_system_time_point_now(rcl_system_time_point_t * now); -/// Normalize a time struct. -/* If there are more than 10^9 nanoseconds in the nsec field, the extra seconds - * are removed from the nsec field and added to the sec field. - * Overflow of the sec field due to this normalization is ignored. +/// Retrieve the current time as a rcl_steady_time_point_t struct. +/* This function returns the time from a monotonically increasing clock. + * The closest equivalent would be to std::chrono::steady_clock::now(); * + * The resolution (e.g. nanoseconds vs microseconds) is not guaranteed. + * + * The now argument must point to an allocated rcl_steady_time_point_t struct, + * as the result is copied into this variable. + * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free, with an exception on Windows. * On Windows this is lock-free if the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * - * \param[out] now a rcl_time_t struct to be normalized - * \return RCL_RET_OK if the struct was normalized successfully, or + * \param[out] now a struct in which the current time is stored + * \return RCL_RET_OK if the current time was successfully obtained, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t -rcl_time_normalize(rcl_time_t * now); +rcl_steady_time_point_now(rcl_steady_time_point_t * now); #if __cplusplus } diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index ba62062..ca921e4 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -37,15 +37,16 @@ typedef struct rcl_timer_t { /// User callback signature for timers. /* The first argument the callback gets is a pointer to the timer. - * This can be used to cancel the timer, query about the time until the next + * This can be used to cancel the timer, query the time until the next * timer callback, exchange the callback with a different one, etc. * - * The only caveat is that the function rcl_timer_get_last_call_time will - * return the time just before the callback was called. - * Therefore the second argument given is the time when the previous callback - * was called, since that information is no longer accessible via the timer. + * The only caveat is that the function rcl_timer_get_time_since_last_call will + * return the time since just before this callback was called, not the last. + * Therefore the second argument given is the time since the previous callback + * was called, because that information is no longer accessible via the timer. + * The time since the last callback call is given in nanoseconds. */ -typedef void (* rcl_timer_callback_t)(rcl_timer_t *, rcl_time_t); +typedef void (* rcl_timer_callback_t)(rcl_timer_t *, uint64_t); /// Return a zero initialized timer. rcl_timer_t @@ -56,32 +57,32 @@ rcl_get_zero_initialized_timer(); * A timer can be added to a wait set and waited on, such that the wait set * will wake up when a timer is ready to be executed. * - * A timer simply holds state and does not actively call periodically. - * It does not create any threads or register interrupts or signals. + * A timer simply holds state and does not automatically call callbacks. + * It does not create any threads, register interrupts, or consume signals. * For blocking behavior it can be used in conjunction with a wait set and * rcl_wait(). * When rcl_timer_is_ready() returns true, the timer must still be called * explicitly using rcl_timer_call(). * * The timer handle must be a pointer to an allocated and zero initialized - * timer struct. + * rcl_timer_t struct. * Calling this function on an already initialized timer will fail. * Calling this function on a timer struct which has been allocated but not * zero initialized is undefined behavior. * - * The period is a timer duration (rather an absolute time in the future). - * If the period is 0 seconds and 0 nanoseconds then it will always be ready. + * The period is a duration (rather an absolute time in the future). + * If the period is 0 then it will always be ready. * * The callback must be a function which returns void and takes two arguments, - * the first being a pointer to the associated timer, and the second a time - * struct which is the time of the previous call or when the timer was created + * the first being a pointer to the associated timer, and the second a uint64_t + * which is the time since the previous call, or since the timer was created * if it is the first call to the callback. * * Expected usage: * * #include * - * void my_timer_callback(rcl_timer_t * timer, rcl_time_t last_call_time) + * void my_timer_callback(rcl_timer_t * timer, uint64_t last_call_time) * { * // Do timer work... * // Optionally reconfigure, cancel, or reset the timer... @@ -94,8 +95,9 @@ rcl_get_zero_initialized_timer(); * ret = rcl_timer_fini(&timer); * // ... error handling * + * This function does allocate heap memory. * This function is not thread-safe. - * This function is lock-free. + * This function is not lock-free. * * \param[inout] timer the timer handle to be initialized * \param[in] period the duration between calls to the callback in nanoseconds @@ -122,8 +124,9 @@ rcl_timer_init( * This function is not thread-safe with any rcl_timer_* functions used on the * same timer object. * + * This function may allocate heap memory when an error occurs. * This function is not thread-safe. - * This function is lock-free. + * This function is not lock-free. * * \param[inout] timer the handle to the timer to be finalized. * \return RCL_RET_OK if the timer was finalized successfully, or @@ -132,7 +135,7 @@ rcl_timer_init( rcl_ret_t rcl_timer_fini(rcl_timer_t * timer); -/// Call the timer's callback and set the last call time to the current time. +/// Call the timer's callback and set the last call time. /* This function will call the callback and change the last call time even if * the timer's period has not yet elapsed. * It is up to the calling code to make sure the period has elapsed by first @@ -140,14 +143,15 @@ rcl_timer_fini(rcl_timer_t * timer); * The order of operations in this command are as follows: * * - Ensure the timer has not been canceled. - * - Get the current time into a temporary rcl_time_t. + * - Get the current time into a temporary rcl_steady_time_point_t. * - Exchange the current time with the last call time of the timer. - * - Call the callback, with this timer and the last call time as arguments. + * - Call the callback, passing this timer and the time since the last call. * - Return after the callback has completed. * * During the callback the timer can be canceled or have its period and/or * callback modified. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe, but the user's callback may not be. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t, but the user's @@ -166,11 +170,12 @@ rcl_timer_call(rcl_timer_t * timer); /// Calculates whether or not the timer should be called. /* The result is true if the time until next call is less than, or equal to, 0 * and the timer has not been canceled. - * Otherwise the result is false, indicating the period has not elapsed. + * Otherwise the result is false, indicating the timer should not be called. * * The is_ready argument must point to an allocated bool object, as the result * is copied into it. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. @@ -198,6 +203,7 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); * The time_until_next_call argument must point to an allocated int64_t, as the * the time until is coped into that instance. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. @@ -212,29 +218,30 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); -/// Retrieve the time of when the previous call to rcl_timer_call() occurred. -/* This function retrieves the internal time and copies it into the given - * rcl_time_t struct. +/// Retrieve the time since the previous call to rcl_timer_call() occurred. +/* This function calculates the time since the last call and copies it into + * the given uint64_t variable. * - * Calling this function within a callback will not return the previous time - * but instead the time of when the current callback was called. + * Calling this function within a callback will not return the time since the + * previous call but instead the time since the current callback was called. * - * The last_call_time argument must be a pointer to an already allocated - * rcl_time_t struct. + * The time_since_last_call argument must be a pointer to an already allocated + * uint64_t. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * * \param[in] timer the handle to the timer which is being queried - * \param[out] last_call_time the rcl_time_t struct in which the time is stored + * \param[out] time_since_last_call the struct in which the time is stored * \return RCL_RET_OK if the last call time was retrieved successfully, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t -rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time); +rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call); /// Retrieve the period of the timer. /* This function retrieves the period and copies it into the give variable. diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index be4c557..ab08eda 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -12,183 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#if __cplusplus -extern "C" -{ -#endif - -// Appropriate check accorind to: -// http://man7.org/linux/man-pages/man2/clock_gettime.2.html -#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) - -#include "rcl/time.h" - -#include -#include -#include - -#if defined(__MACH__) -#include -#include -#endif -#if defined(WIN32) -#include -#include -#endif - -#include "./common.h" -#include "rcl/error_handling.h" - -rcl_time_t -rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds) -{ - rcl_time_t result; - result.sec = RCL_NS_TO_S(nanoseconds); - result.nsec = nanoseconds % 1000000000; - return result; -} - -rcl_time_t -rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds) -{ - rcl_time_t result; - result.sec = RCL_NS_TO_S(nanoseconds); - result.nsec = nanoseconds % 1000000000; - return result; -} - -uint64_t -rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * rcl_time) -{ - if (!rcl_time) { - return 0; - } - return RCL_S_TO_NS(rcl_time->sec) + rcl_time->nsec; -} - -static void -__timespec_get_now(struct timespec * timespec_now) -{ -#if defined(__MACH__) - // On OS X use clock_get_time. - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - timespec_now->tv_sec = mts.tv_sec; - timespec_now->tv_nsec = mts.tv_nsec; -#else // if defined(__MACH__) - // Otherwise use clock_gettime. - clock_gettime(CLOCK_REALTIME, timespec_now); -#endif // if defined(__MACH__) -} - -rcl_ret_t -rcl_time_now(rcl_time_t * now) -{ - RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); -#ifndef WIN32 - // Unix implementations -#if HAS_CLOCK_GETTIME || defined(__MACH__) - // If clock_gettime is available or on OS X, use a timespec. - struct timespec timespec_now; - __timespec_get_now(×pec_now); - now->sec = timespec_now.tv_sec; - now->nsec = timespec_now.tv_nsec; -#else // if HAS_CLOCK_GETTIME || defined(__MACH__) - // Otherwise we have to fallback to gettimeofday. - struct timeval timeofday; - gettimeofday(&timeofday, NULL); - now->sec = timeofday.tv_sec; - now->nsec = timeofday.tv_usec * 1000; -#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#ifdef WIN32 +#include "./time_win32.c" #else - // Windows implementation adapted from roscpp_core (3-clause BSD), see: - // https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 - // Win32 implementation - // unless I've missed something obvious, the only way to get high-precision - // time on Windows is via the QueryPerformanceCounter() call. However, - // this is somewhat problematic in Windows XP on some processors, especially - // AMD, because the Windows implementation can freak out when the CPU clocks - // down to save power. Time can jump or even go backwards. Microsoft has - // fixed this bug for most systems now, but it can still show up if you have - // not installed the latest CPU drivers (an oxymoron). They fixed all these - // problems in Windows Vista, and this API is by far the most accurate that - // I know of in Windows, so I'll use it here despite all these caveats - LARGE_INTEGER cpu_freq; - static LARGE_INTEGER cpu_freq_global; - LARGE_INTEGER init_cpu_time; - static LARGE_INTEGER init_cpu_time_global; - static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); - rcl_time_t start = {0, 0}; - // If start_ns (static/global) is 0, then set it up on the first call. - uint64_t start_ns_loaded = atomic_load(&start_ns); - if (start_ns_loaded == 0) { - QueryPerformanceFrequency(&cpu_freq); - if (cpu_freq.QuadPart == 0) { - RCL_SET_ERROR_MSG("no high performance timer found"); - return RCL_RET_ERROR; - } - QueryPerformanceCounter(&init_cpu_time); - // compute an offset from the Epoch using the lower-performance timer API - FILETIME ft; - GetSystemTimeAsFileTime(&ft); - LARGE_INTEGER start_li; - start_li.LowPart = ft.dwLowDateTime; - start_li.HighPart = ft.dwHighDateTime; - // why did they choose 1601 as the time zero, instead of 1970? - // there were no outstanding hard rock bands in 1601. -#ifdef _MSC_VER - start_li.QuadPart -= 116444736000000000Ui64; -#else - start_li.QuadPart -= 116444736000000000ULL; -#endif - start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. - start.nsec = (start_li.LowPart % 10000000) * 100; - if (atomic_compare_exchange_strong(&start_ns, 0, (start.sec * 1000000000) + start.nsec)) { - // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. - init_cpu_time_global = init_cpu_time; - cpu_freq_global = cpu_freq; - } else { - // Another concurrent first call managed to set this up first; reset start so it gets set. - start = {0, 0}; - } - } - if (start.sec == 0 && start.nsec == 0) { - start.sec = RCL_NS_TO_S(start_ns_loaded); - start.nsec = start_ns_loaded % 1000000000; - } - LARGE_INTEGER cur_time; - QueryPerformanceCounter(&cur_time); - LARGE_INTEGER delta_cpu_time; - delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time_global.QuadPart; - double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq_global.QuadPart; - uint64_t delta_sec = (uint64_t) floor(d_delta_cpu_time); - uint64_t delta_nsec = (uint64_t) floor((d_delta_cpu_time - delta_sec) * 1e9); - - *now = start; - now->sec += delta_sec; - now->nsec += delta_nsec; - - // Normalize the time struct. - rcl_ret_t ret = rcl_time_normalize(now); - if (ret != RCL_RET_OK) { - return ret; // rcl error state should already be set. - } -#endif - return RCL_RET_OK; -} - -rcl_ret_t -rcl_time_normalize(rcl_time_t * now) -{ - RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); - now->sec += now->nsec / 1000000000; - now->nsec = now->nsec % 1000000000; - return RCL_RET_OK; -} - -#if __cplusplus -} +#include "./time_unix.c" #endif diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c new file mode 100644 index 0000000..5565d47 --- /dev/null +++ b/rcl/src/rcl/time_unix.c @@ -0,0 +1,119 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef WIN32 +#error time_unix.c is not intended to be used with win32 based systems +#endif + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/time.h" + +#if defined(__MACH__) +#include +#include +#endif +#include +#include + +#include "./common.h" +#include "rcl/error_handling.h" + +// This id an appropriate check for clock_gettime() according to: +// http://man7.org/linux/man-pages/man2/clock_gettime.2.html +#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) +#if !HAS_CLOCK_GETTIME && !defined(__MACH__) +#error no monotonic clock function available +#endif + +static void +__timespec_get_now_steady(struct timespec * timespec_now) +{ +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now->tv_sec = mts.tv_sec; + timespec_now->tv_nsec = mts.tv_nsec; +#else // if defined(__MACH__) + // Otherwise use clock_gettime. +#ifdef CLOCK_MONOTONIC_RAW + clock_gettime(CLOCK_MONOTONIC_RAW, timespec_now); +#else + clock_gettime(CLOCK_MONOTONIC, timespec_now); +#endif // CLOCK_MONOTONIC_RAW +#endif // if defined(__MACH__) +} + +#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) + +rcl_ret_t +rcl_system_time_point_now(rcl_system_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); + struct timespec timespec_now; +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now.tv_sec = mts.tv_sec; + timespec_now.tv_nsec = mts.tv_nsec; +#else + // Otherwise use clock_gettime. + clock_gettime(CLOCK_REALTIME, ×pec_now); +#endif // if defined(__MACH__) + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_steady_time_point_now(rcl_steady_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); +#ifndef WIN32 + // Unix implementations +#if HAS_CLOCK_GETTIME || defined(__MACH__) + // If clock_gettime is available or on OS X, use a timespec. + struct timespec timespec_now; + __timespec_get_now_steady(×pec_now); + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; +#else // if HAS_CLOCK_GETTIME || defined(__MACH__) + // Cannot fallback to gettimeofday because it is not monotonic. +#error No monotonic clock detected. +#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#else +#endif + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c new file mode 100644 index 0000000..dcc8c20 --- /dev/null +++ b/rcl/src/rcl/time_win32.c @@ -0,0 +1,218 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef WIN32 +#error time_win32.c is only intended to be used with win32 based systems +#endif + +#if __cplusplus +extern "C" +{ +#endif + +// Appropriate check accorind to: +// http://man7.org/linux/man-pages/man2/clock_gettime.2.html +#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) + +#include "rcl/time.h" + +#include +#include +#include + +#if defined(__MACH__) +#include +#include +#endif +#if defined(WIN32) +#include +#include +#endif + +#include "./common.h" +#include "rcl/error_handling.h" + +static void +__timespec_get_now_system(struct timespec * timespec_now) +{ +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now->tv_sec = mts.tv_sec; + timespec_now->tv_nsec = mts.tv_nsec; +#else // if defined(__MACH__) + // Otherwise use clock_gettime. + clock_gettime(CLOCK_REALTIME, timespec_now); +#endif // if defined(__MACH__) +} + +static void +__timespec_get_now_monotonic(struct timespec * timespec_now) +{ +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now->tv_sec = mts.tv_sec; + timespec_now->tv_nsec = mts.tv_nsec; +#else // if defined(__MACH__) + // Otherwise use clock_gettime. +#ifdef CLOCK_MONOTONIC_RAW + clock_gettime(CLOCK_MONOTONIC_RAW, timespec_now); +#else + clock_gettime(CLOCK_MONOTONIC, timespec_now); +#endif // CLOCK_MONOTONIC_RAW +#endif // if defined(__MACH__) +} + +#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) + +rcl_ret_t +rcl_system_time_point_now(rcl_system_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); +#ifndef WIN32 + // Unix implementations +#if HAS_CLOCK_GETTIME || defined(__MACH__) + // If clock_gettime is available or on OS X, use a timespec. + struct timespec timespec_now; + __timespec_get_now_system(×pec_now); + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; +#else // if HAS_CLOCK_GETTIME || defined(__MACH__) + // Otherwise we have to fallback to gettimeofday. + struct timeval timeofday; + gettimeofday(&timeofday, NULL); + if (__WOULD_BE_NEGATIVE(timeofday.tv_sec, timeofday.tv_usec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timeofday.tv_sec) + timeofday.tv_usec * 1000; +#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#else + /* Windows implementation adapted from roscpp_core (3-clause BSD), see: + * https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 + * + * > Win32 implementation + * unless I've missed something obvious, the only way to get high-precision + * time on Windows is via the QueryPerformanceCounter() call. However, + * this is somewhat problematic in Windows XP on some processors, especially + * AMD, because the Windows implementation can freak out when the CPU clocks + * down to save power. Time can jump or even go backwards. Microsoft has + * fixed this bug for most systems now, but it can still show up if you have + * not installed the latest CPU drivers (an oxymoron). They fixed all these + * problems in Windows Vista, and this API is by far the most accurate that + * I know of in Windows, so I'll use it here despite all these caveats + * + * I've further modified it to be thread safe using a atomic_uint_least64_t. + */ + LARGE_INTEGER cpu_freq; + static LARGE_INTEGER cpu_freq_global; + LARGE_INTEGER init_cpu_time; + static LARGE_INTEGER init_cpu_time_global; + static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); + rcl_time_t start = {0, 0}; + // If start_ns (static/global) is 0, then set it up on the first call. + uint64_t start_ns_loaded = atomic_load(&start_ns); + if (start_ns_loaded == 0) { + QueryPerformanceFrequency(&cpu_freq); + if (cpu_freq.QuadPart == 0) { + RCL_SET_ERROR_MSG("no high performance timer found"); + return RCL_RET_ERROR; + } + QueryPerformanceCounter(&init_cpu_time); + // compute an offset from the Epoch using the lower-performance timer API + FILETIME ft; + GetSystemTimeAsFileTime(&ft); + LARGE_INTEGER start_li; + start_li.LowPart = ft.dwLowDateTime; + start_li.HighPart = ft.dwHighDateTime; + // why did they choose 1601 as the time zero, instead of 1970? + // there were no outstanding hard rock bands in 1601. +#ifdef _MSC_VER + start_li.QuadPart -= 116444736000000000Ui64; +#else + start_li.QuadPart -= 116444736000000000ULL; +#endif + start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. + start.nsec = (start_li.LowPart % 10000000) * 100; + if (atomic_compare_exchange_strong(&start_ns, 0, RCL_S_TO_NS(start.sec) + start.nsec)) { + // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. + init_cpu_time_global = init_cpu_time; + cpu_freq_global = cpu_freq; + } else { + // Another concurrent first call managed to set this up first; reset start so it gets set. + start = {0, 0}; + } + } + if (start.sec == 0 && start.nsec == 0) { + start.sec = RCL_NS_TO_S(start_ns_loaded); + start.nsec = start_ns_loaded % 1000000000; + } + LARGE_INTEGER cur_time; + QueryPerformanceCounter(&cur_time); + LARGE_INTEGER delta_cpu_time; + delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time_global.QuadPart; + double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq_global.QuadPart; + uint64_t delta_sec = (uint64_t) floor(d_delta_cpu_time); + uint64_t delta_nsec = (uint64_t) floor((d_delta_cpu_time - delta_sec) * 1e9); + + *now = start; + now->sec += delta_sec; + now->nsec += delta_nsec; + + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } +#endif + return RCL_RET_OK; +} + +rcl_ret_t +rcl_steady_time_point_now(rcl_steady_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); +#ifndef WIN32 + // Unix implementations +#if HAS_CLOCK_GETTIME || defined(__MACH__) + // If clock_gettime is available or on OS X, use a timespec. + struct timespec timespec_now; + __timespec_get_now_monotonic(×pec_now); + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; +#else // if HAS_CLOCK_GETTIME || defined(__MACH__) + // Cannot fallback to gettimeofday because it is not monotonic. +#error No monotonic clock detected. +#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#else +#endif + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index a8df7ad..cea2edb 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -28,7 +28,7 @@ typedef struct rcl_timer_impl_t { atomic_uintptr_t callback; // This is a duration in nanoseconds. atomic_uint_least64_t period; - // This is an absolute time in nanoseconds since the unix epoch. + // This is a time in nanoseconds since an unspecified time. atomic_uint_least64_t last_call_time; // A flag which indicates if the timer is canceled. atomic_bool canceled; @@ -56,15 +56,15 @@ rcl_timer_init( RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } - rcl_time_t now; - rcl_ret_t now_ret = rcl_time_now(&now); + rcl_steady_time_point_t now_steady; + rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } rcl_timer_impl_t impl = { .callback = ATOMIC_VAR_INIT((uintptr_t)callback), .period = ATOMIC_VAR_INIT(period), - .last_call_time = ATOMIC_VAR_INIT(rcl_time_to_uint64_t_nanoseconds(&now)), + .last_call_time = ATOMIC_VAR_INIT(now_steady.nanoseconds), .canceled = ATOMIC_VAR_INIT(false), .allocator = allocator, }; @@ -100,16 +100,15 @@ rcl_timer_call(rcl_timer_t * timer) RCL_SET_ERROR_MSG("timer is canceled"); return RCL_RET_TIMER_CANCELED; } - rcl_time_t now; - rcl_ret_t now_ret = rcl_time_now(&now); + rcl_steady_time_point_t now_steady; + rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - uint64_t previous_ns = - atomic_exchange(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); - rcl_time_t previous = rcl_time_from_uint64_t_nanoseconds(previous_ns); + uint64_t previous_ns = atomic_exchange(&timer->impl->last_call_time, now_steady.nanoseconds); + uint64_t since_last_call = now_steady.nanoseconds - previous_ns; rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)atomic_load(&timer->impl->callback); - typed_callback(timer, previous); + typed_callback(timer, since_last_call); return RCL_RET_OK; } @@ -134,29 +133,28 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - rcl_time_t now, last_call_time; - rcl_ret_t ret = rcl_time_now(&now); - if (ret != RCL_RET_OK) { - return ret; // rcl error state should already be set. - } - ret = rcl_timer_get_last_call_time(timer, &last_call_time); + rcl_steady_time_point_t now; + rcl_ret_t ret = rcl_steady_time_point_now(&now); if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } uint64_t period = atomic_load(&timer->impl->period); - int64_t next_call = rcl_time_to_uint64_t_nanoseconds(&last_call_time) + period; - *time_until_next_call = next_call - rcl_time_to_uint64_t_nanoseconds(&now); + *time_until_next_call = (atomic_load(&timer->impl->last_call_time) + period) - now.nanoseconds; return RCL_RET_OK; } rcl_ret_t -rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time) +rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(last_call_time, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - uint64_t last_call_time_ns = atomic_load(&timer->impl->last_call_time); - *last_call_time = rcl_time_from_uint64_t_nanoseconds(last_call_time_ns); + rcl_steady_time_point_t now; + rcl_ret_t ret = rcl_steady_time_point_now(&now); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + *time_since_last_call = (now.nanoseconds - atomic_load(&timer->impl->last_call_time)); return RCL_RET_OK; } @@ -221,12 +219,12 @@ rcl_timer_reset(rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - rcl_time_t now; - rcl_ret_t now_ret = rcl_time_now(&now); + rcl_steady_time_point_t now; + rcl_ret_t now_ret = rcl_steady_time_point_now(&now); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - atomic_store(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); + atomic_store(&timer->impl->last_call_time, now.nanoseconds); atomic_store(&timer->impl->canceled, false); return RCL_RET_OK; } diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index c13cf38..d7aff5e 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -355,31 +355,49 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Create dummy sets for currently unsupported wait-ables. static rmw_services_t dummy_services = {0, NULL}; static rmw_clients_t dummy_clients = {0, NULL}; - // Calculate the timeout. - int64_t min_timeout = timeout; - if (min_timeout == 0) { // Do not consider timer timeouts if non-blocking. - for (size_t i = 0; i < wait_set->size_of_timers; ++i) { - if (!wait_set->timers[i]) { - continue; // Skip NULL timers. - } - int64_t timer_timeout; - rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); - if (ret != RCL_RET_OK) { - return ret; // The rcl error state should already be set. - } - if (timer_timeout < min_timeout) { - min_timeout = timer_timeout; + // Calculate the timeout argument. + rmw_time_t * timeout_argument; + rmw_time_t temporary_timeout_storage; + if (timeout < 0) { + // Pass NULL to wait to indicate block indefinitely. + timeout_argument = NULL; + } + if (timeout > 0) { + // Determine the nearest timeout (given or a timer). + uint64_t min_timeout = timeout; + if (min_timeout > 0) { // Do not consider timer timeouts if non-blocking. + for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + if (!wait_set->timers[i]) { + continue; // Skip NULL timers. + } + int64_t timer_timeout; + rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (timer_timeout < min_timeout) { + min_timeout = timer_timeout; + } } } + // Set that in the temporary storage and point to that for the argument. + temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout); + temporary_timeout_storage.nsec = min_timeout % 1000000000; + timeout_argument = &temporary_timeout_storage; + } + if (timeout == 0) { + // Then it is non-blocking, so set the temporary storage to 0, 0 and pass it. + temporary_timeout_storage.sec = 0; + temporary_timeout_storage.nsec = 0; + timeout_argument = &temporary_timeout_storage; } - rmw_time_t rmw_timeout = rcl_time_from_int64_t_nanoseconds(min_timeout); // Wait. rmw_ret_t ret = rmw_wait( &wait_set->impl->rmw_subscriptions, &wait_set->impl->rmw_guard_conditions, &dummy_services, &dummy_clients, - &rmw_timeout + timeout_argument ); // Check for error. if (ret != RMW_RET_OK) { diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools.cpp new file mode 100644 index 0000000..0a5c1e2 --- /dev/null +++ b/rcl/test/memory_tools.cpp @@ -0,0 +1,73 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if defined(__linux__) +/****************************************************************************** + * Begin Linux + *****************************************************************************/ + +// TODO(wjwwood): install custom malloc (and others) for Linux. + +#include "./memory_tools_common.cpp" + +/****************************************************************************** + * End Linux + *****************************************************************************/ +#elif defined(__APPLE__) +/****************************************************************************** + * Begin Apple + *****************************************************************************/ + +// The apple implementation is in a separate shared library, loaded with DYLD_INSERT_LIBRARIES. +// Therefore we do not include the common cpp file here. + +void osx_start_memory_checking(); +void osx_stop_memory_checking(); + +void start_memory_checking() +{ + return osx_start_memory_checking(); +} + +void stop_memory_checking() +{ + return osx_stop_memory_checking(); +} + +/****************************************************************************** + * End Apple + *****************************************************************************/ +#elif defined(WIN32) +/****************************************************************************** + * Begin Windows + *****************************************************************************/ + +// TODO(wjwwood): install custom malloc (and others) for Windows. + +/****************************************************************************** + * End Windows + *****************************************************************************/ +#else +// Default case: do nothing. + +void start_memory_checking() +{ + MALLOC_PRINTF("starting memory checking... not available\n"); +} +void stop_memory_checking() +{ + MALLOC_PRINTF("stopping memory checking... not available\n"); +} + +#endif // !defined(WIN32) diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools.hpp new file mode 100644 index 0000000..ac20a65 --- /dev/null +++ b/rcl/test/memory_tools.hpp @@ -0,0 +1,50 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Code to do replacing of malloc/free/etc... inspired by: +// https://dxr.mozilla.org/mozilla-central/rev/ +// cc9c6cd756cb744596ba039dcc5ad3065a7cc3ea/memory/build/replace_malloc.c + +#ifndef TEST__MEMORY_TOOLS_HPP_ +#define TEST__MEMORY_TOOLS_HPP_ + +#include + +#include +#include + +typedef std::function UnexpectedCallbackType; + +void start_memory_checking(); + +#define ASSERT_NO_MALLOC(statements) assert_no_malloc_begin(); statements; assert_no_malloc_end(); +void assert_no_malloc_begin(); +void assert_no_malloc_end(); +void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback); + +#define ASSERT_NO_REALLOC(statements) assert_no_realloc_begin(); statements; assert_no_realloc_end(); +void assert_no_realloc_begin(); +void assert_no_realloc_end(); +void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback); + +#define ASSERT_NO_FREE(statements) assert_no_free_begin(); statements; assert_no_free_end(); +void assert_no_free_begin(); +void assert_no_free_end(); +void set_on_unepexcted_free_callback(UnexpectedCallbackType callback); + +void stop_memory_checking(); + +void memory_checking_thread_init(); + +#endif // TEST__MEMORY_TOOLS_HPP_ diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp new file mode 100644 index 0000000..731db36 --- /dev/null +++ b/rcl/test/memory_tools_common.cpp @@ -0,0 +1,150 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#if defined(__APPLE__) +#include +#define MALLOC_PRINTF malloc_printf +#else +#define MALLOC_PRINTF printf +#endif + +#include "./memory_tools.hpp" +#include "./scope_exit.hpp" + +static std::atomic enabled(false); + +static __thread bool malloc_expected = true; +static __thread UnexpectedCallbackType * unexpected_malloc_callback = nullptr; +void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback) +{ + if (unexpected_malloc_callback) { + unexpected_malloc_callback->~UnexpectedCallbackType(); + free(unexpected_malloc_callback); + unexpected_malloc_callback = nullptr; + } + if (!callback) { + return; + } + if (!unexpected_malloc_callback) { + unexpected_malloc_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + if (!unexpected_malloc_callback) { + throw std::bad_alloc(); + } + new (unexpected_malloc_callback) UnexpectedCallbackType(); + } + *unexpected_malloc_callback = callback; +} + +void * +custom_malloc(size_t size) +{ + if (!enabled.load()) return malloc(size); + auto foo = SCOPE_EXIT(enabled.store(true);); + enabled.store(false); + if (!malloc_expected) { + if (unexpected_malloc_callback) { + (*unexpected_malloc_callback)(); + } + } + void * memory = malloc(size); + MALLOC_PRINTF( + "malloc expected(%s): %p %d\n", malloc_expected ? "true " : "false", memory, size); + return memory; +} + +static __thread bool realloc_expected = true; +static __thread UnexpectedCallbackType * unexpected_realloc_callback = nullptr; +void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback) +{ + if (unexpected_realloc_callback) { + unexpected_realloc_callback->~UnexpectedCallbackType(); + free(unexpected_realloc_callback); + unexpected_realloc_callback = nullptr; + } + if (!callback) { + return; + } + if (!unexpected_realloc_callback) { + unexpected_realloc_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + if (!unexpected_realloc_callback) { + throw std::bad_alloc(); + } + new (unexpected_realloc_callback) UnexpectedCallbackType(); + } + *unexpected_realloc_callback = callback; +} + +void * +custom_realloc(void * memory_in, size_t size) +{ + if (!enabled.load()) return realloc(memory_in, size); + auto foo = SCOPE_EXIT(enabled.store(true);); + enabled.store(false); + if (!realloc_expected) { + if (unexpected_realloc_callback) { + (*unexpected_realloc_callback)(); + } + } + void * memory = realloc(memory_in, size); + MALLOC_PRINTF( + "realloc expected(%s): %p %p %d\n", + malloc_expected ? "true " : "false", memory_in, memory, size); + return memory; +} + +static __thread bool free_expected = true; +static __thread UnexpectedCallbackType * unexpected_free_callback = nullptr; +void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) +{ + if (unexpected_free_callback) { + unexpected_free_callback->~UnexpectedCallbackType(); + free(unexpected_free_callback); + unexpected_free_callback = nullptr; + } + if (!callback) { + return; + } + if (!unexpected_free_callback) { + unexpected_free_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + if (!unexpected_free_callback) { + throw std::bad_alloc(); + } + new (unexpected_free_callback) UnexpectedCallbackType(); + } + *unexpected_free_callback = callback; +} + +void +custom_free(void * memory) +{ + if (!enabled.load()) return free(memory); + auto foo = SCOPE_EXIT(enabled.store(true);); + enabled.store(false); + if (!free_expected) { + if (unexpected_free_callback) { + (*unexpected_free_callback)(); + } + } + free(memory); + MALLOC_PRINTF("free expected(%s): %p\n", malloc_expected ? "true " : "false", memory); +} + +void assert_no_malloc_begin() {malloc_expected = false;} +void assert_no_malloc_end() {malloc_expected = true;} +void assert_no_realloc_begin() {realloc_expected = false;} +void assert_no_realloc_end() {realloc_expected = true;} +void assert_no_free_begin() {free_expected = false;} +void assert_no_free_end() {free_expected = true;} diff --git a/rcl/test/memory_tools_osx_interpose.cpp b/rcl/test/memory_tools_osx_interpose.cpp new file mode 100644 index 0000000..980b005 --- /dev/null +++ b/rcl/test/memory_tools_osx_interpose.cpp @@ -0,0 +1,54 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Pulled from: +// https://github.com/emeryberger/Heap-Layers/blob/ +// 076e9e7ef53b66380b159e40473b930f25cc353b/wrappers/macinterpose.h + +// The interposition data structure (just pairs of function pointers), +// used an interposition table like the following: +// + +typedef struct interpose_s { + void * new_func; + void * orig_func; +} interpose_t; + +#define OSX_INTERPOSE(newf,oldf) \ + __attribute__((used)) static const interpose_t \ + macinterpose##newf##oldf __attribute__ ((section("__DATA, __interpose"))) = { \ + (void *)newf, \ + (void *)oldf, \ + } + +// End Interpose. + +#include "./memory_tools_common.cpp" + +void osx_start_memory_checking() +{ + // No loading required, it is handled by DYLD_INSERT_LIBRARIES and dynamic library interposing. + MALLOC_PRINTF("starting memory checking...\n"); + enabled.store(true); +} + +void osx_stop_memory_checking() +{ + MALLOC_PRINTF("stopping memory checking...\n"); + enabled.store(false); +} + +OSX_INTERPOSE(custom_malloc, malloc); +OSX_INTERPOSE(custom_realloc, realloc); +OSX_INTERPOSE(custom_free, free); diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp new file mode 100644 index 0000000..f63ec18 --- /dev/null +++ b/rcl/test/rcl/test_allocator.cpp @@ -0,0 +1,74 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/allocator.h" + +#include "../memory_tools.hpp" + +class TestAllocatorFixture : public ::testing::Test { +public: + TestAllocatorFixture() + { + start_memory_checking(); + stop_memory_checking(); + } + void SetUp() + { + set_on_unepexcted_malloc_callback([]() {EXPECT_FALSE(true) << "unexpected malloc";}); + set_on_unepexcted_realloc_callback([]() {EXPECT_FALSE(true) << "unexpected realloc";}); + set_on_unepexcted_free_callback([]() {EXPECT_FALSE(true) << "unexpected free";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unepexcted_malloc_callback(nullptr); + set_on_unepexcted_realloc_callback(nullptr); + set_on_unepexcted_free_callback(nullptr); + } +}; + +/* Tests the default allocator. + */ +TEST_F(TestAllocatorFixture, test_default_allocator_normal) +{ + ASSERT_NO_MALLOC( + rcl_allocator_t allocator = rcl_get_default_allocator(); + ) + size_t mallocs = 0; + size_t reallocs = 0; + size_t frees = 0; + set_on_unepexcted_malloc_callback([&mallocs]() {mallocs++;}); + set_on_unepexcted_realloc_callback([&reallocs]() {reallocs++;}); + set_on_unepexcted_free_callback([&frees]() {frees++;}); + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + void * allocated_memory = allocator.allocate(1024, allocator.state); + EXPECT_EQ(mallocs, 1); + EXPECT_NE(allocated_memory, nullptr); + allocated_memory = allocator.reallocate(allocated_memory, 2048, allocator.state); + EXPECT_EQ(reallocs, 1); + EXPECT_NE(allocated_memory, nullptr); + allocator.deallocate(allocated_memory, allocator.state); + EXPECT_EQ(mallocs, 1); + EXPECT_EQ(reallocs, 1); + EXPECT_EQ(frees, 1); +} diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp new file mode 100644 index 0000000..c86b74a --- /dev/null +++ b/rcl/test/rcl/test_time.cpp @@ -0,0 +1,110 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" + +#include "../memory_tools.hpp" + +class TestTimeFixture : public ::testing::Test { +public: + void SetUp() + { + set_on_unepexcted_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unepexcted_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unepexcted_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unepexcted_malloc_callback(nullptr); + set_on_unepexcted_realloc_callback(nullptr); + set_on_unepexcted_free_callback(nullptr); + } +}; + +/* Tests the rcl_system_time_point_now() function. + */ +TEST_F(TestTimeFixture, test_rcl_system_time_point_now) +{ + assert_no_realloc_begin(); + rcl_ret_t ret = RCL_RET_OK; + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_system_time_point_now(nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); + rcl_reset_error(); + assert_no_malloc_begin(); + assert_no_free_begin(); + // Check for normal operation (not allowed to alloc). + rcl_system_time_point_t now = {0}; + ret = rcl_system_time_point_now(&now); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_NE(now.nanoseconds, 0); + // Compare to std::chrono::system_clock time (within a second). + now = {0}; + ret = rcl_system_time_point_now(&now); + { + std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = now.nanoseconds - now_ns_int; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(100)) << "system_clock differs"; + } +} + +/* Tests the rcl_steady_time_point_now() function. + */ +TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) +{ + assert_no_realloc_begin(); + rcl_ret_t ret = RCL_RET_OK; + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_steady_time_point_now(nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); + rcl_reset_error(); + assert_no_malloc_begin(); + assert_no_free_begin(); + // Check for normal operation (not allowed to alloc). + rcl_steady_time_point_t now = {0}; + ret = rcl_steady_time_point_now(&now); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_NE(now.nanoseconds, 0); + // Compare to std::chrono::steady_clock time (within a second). + now = {0}; + ret = rcl_steady_time_point_now(&now); + { + std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = now.nanoseconds - now_ns_int; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(100)) << "steady_clock differs"; + } +} diff --git a/rcl/test/scope_exit.hpp b/rcl/test/scope_exit.hpp new file mode 100644 index 0000000..cb12fce --- /dev/null +++ b/rcl/test/scope_exit.hpp @@ -0,0 +1,40 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST__SCOPE_EXIT_HPP_ +#define TEST__SCOPE_EXIT_HPP_ + +#include + +template +struct ScopeExit +{ + explicit ScopeExit(Callable callable) + : callable_(callable) {} + ~ScopeExit() {callable_(); } + +private: + Callable callable_; +}; + +template +ScopeExit +make_scope_exit(Callable callable) +{ + return ScopeExit(callable); +} + +#define SCOPE_EXIT(code) make_scope_exit([&]() {code; }) + +#endif // TEST__SCOPE_EXIT_HPP_ diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/test_memory_tools.cpp new file mode 100644 index 0000000..770991e --- /dev/null +++ b/rcl/test/test_memory_tools.cpp @@ -0,0 +1,119 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "./memory_tools.hpp" + +/* Tests the allocatation checking tools. + */ +TEST(TestMemoryTools, test_allocation_checking_tools) +{ + size_t unexpected_mallocs = 0; + auto on_unexpected_malloc = [&unexpected_mallocs]() {unexpected_mallocs++;}; + set_on_unepexcted_malloc_callback(on_unexpected_malloc); + size_t unexpected_reallocs = 0; + auto on_unexpected_realloc = [&unexpected_reallocs]() {unexpected_reallocs++;}; + set_on_unepexcted_realloc_callback(on_unexpected_realloc); + size_t unexpected_frees = 0; + auto on_unexpected_free = [&unexpected_frees]() {unexpected_frees++;}; + set_on_unepexcted_free_callback(on_unexpected_free); + void * mem = nullptr; + // First try before enabling, should have no effect. + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 0); + EXPECT_EQ(unexpected_reallocs, 0); + EXPECT_EQ(unexpected_frees, 0); + // Enable checking, but no assert, should have no effect. + start_memory_checking(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 0); + EXPECT_EQ(unexpected_reallocs, 0); + EXPECT_EQ(unexpected_frees, 0); + // Enable no_* asserts, should increment all once. + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + mem = malloc(1024); + assert_no_malloc_end(); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + assert_no_realloc_end(); + ASSERT_NE(mem, nullptr); + free(mem); + assert_no_free_end(); + EXPECT_EQ(unexpected_mallocs, 1); + EXPECT_EQ(unexpected_reallocs, 1); + EXPECT_EQ(unexpected_frees, 1); + // Enable on malloc assert, only malloc should increment. + assert_no_malloc_begin(); + mem = malloc(1024); + assert_no_malloc_end(); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 1); + EXPECT_EQ(unexpected_frees, 1); + // Enable on realloc assert, only realloc should increment. + assert_no_realloc_begin(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + assert_no_realloc_end(); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 1); + // Enable on free assert, only free should increment. + assert_no_free_begin(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + assert_no_free_end(); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 2); + // Go again, after disabling asserts, should have no effect. + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 2); + // Go once more after disabling everything, should have no effect. + stop_memory_checking(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 2); +}