niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

refactored time and timer, and added tests

Browse source
This commit is contained in:
William Woodall 2015-12-10 16:11:16 -08:00
parent 7ab361a5fb
commit 46551399e6
17 changed files with 1221 additions and 279 deletions
Download patch file Download diff file Expand all files Collapse all files

53
rcl/CMakeLists.txt
View file

@ -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=$<TARGET_FILE:${PROJECT_NAME}_memory_tools_interpose>)
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=$<TARGET_FILE:${PROJECT_NAME}_memory_tools_interpose>)
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=$<TARGET_FILE:${PROJECT_NAME}_memory_tools_interpose>)
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()

7
rcl/include/rcl/allocator.h
View file

@ -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();

89
rcl/include/rcl/time.h
View file

@ -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
}

65
rcl/include/rcl/timer.h
View file

@ -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 <rcl/rcl.h>
*
* 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.

181
rcl/src/rcl/time.c
View file

@ -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 <math.h>
#include <sys/time.h>
#include <time.h>
#if defined(__MACH__)
#include <mach/clock.h>
#include <mach/mach.h>
#endif
#if defined(WIN32)
#include <stdatomic.h>
#include <windows.h>
#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(&timespec_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

119
rcl/src/rcl/time_unix.c Normal file
View file

@ -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 <mach/clock.h>
#include <mach/mach.h>
#endif
#include <math.h>
#include <time.h>
#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, &timespec_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(&timespec_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

218
rcl/src/rcl/time_win32.c Normal file
View file

@ -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 <math.h>
#include <sys/time.h>
#include <time.h>
#if defined(__MACH__)
#include <mach/clock.h>
#include <mach/mach.h>
#endif
#if defined(WIN32)
#include <stdatomic.h>
#include <windows.h>
#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(&timespec_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(&timespec_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

48
rcl/src/rcl/timer.c
View file

@ -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;
}

50
rcl/src/rcl/wait.c
View file

@ -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) {

73
rcl/test/memory_tools.cpp Normal file
View file

@ -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)

50
rcl/test/memory_tools.hpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <functional>
#include <stddef.h>
typedef std::function<void()> 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_

150
rcl/test/memory_tools_common.cpp Normal file
View file

@ -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 <atomic>
#if defined(__APPLE__)
#include <malloc/malloc.h>
#define MALLOC_PRINTF malloc_printf
#else
#define MALLOC_PRINTF printf
#endif
#include "./memory_tools.hpp"
#include "./scope_exit.hpp"
static std::atomic<bool> 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;}

54
rcl/test/memory_tools_osx_interpose.cpp Normal file
View file

@ -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);

74
rcl/test/rcl/test_allocator.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#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);
}

110
rcl/test/rcl/test_time.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <chrono>
#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<std::chrono::nanoseconds>(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<std::chrono::nanoseconds>(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";
}
}

40
rcl/test/scope_exit.hpp Normal file
View file

@ -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 <functional>
template<typename Callable>
struct ScopeExit
{
explicit ScopeExit(Callable callable)
: callable_(callable) {}
~ScopeExit() {callable_(); }
private:
Callable callable_;
};
template<typename Callable>
ScopeExit<Callable>
make_scope_exit(Callable callable)
{
return ScopeExit<Callable>(callable);
}
#define SCOPE_EXIT(code) make_scope_exit([&]() {code; })
#endif // TEST__SCOPE_EXIT_HPP_

119
rcl/test/test_memory_tools.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#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);
}