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

Implement custom allocators for clock.

Browse source 
Store the allocator for the rcl_clock inside the datastructure to not require passing it into the fini.
This commit is contained in:
Tully Foote 2017-11-02 14:38:26 -07:00
parent 7b26e19900
commit 9f3bdbf99e
3 changed files with 110 additions and 89 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -20,6 +20,7 @@ extern "C"
{ {
#endif #endif
#include "rcl/allocator.h"
#include "rcl/macros.h" #include "rcl/macros.h"
#include "rcl/types.h" #include "rcl/types.h"
#include "rcl/visibility_control.h" #include "rcl/visibility_control.h"
@ -62,10 +63,9 @@ typedef struct rcl_clock_t
rcl_ret_t (* get_now)(void * data, rcl_time_point_value_t * now); rcl_ret_t (* get_now)(void * data, rcl_time_point_value_t * now);
// void (*set_now) (rcl_time_point_value_t); // void (*set_now) (rcl_time_point_value_t);
void * data; void * data;
rcl_allocator_t * allocator;
} rcl_clock_t; } rcl_clock_t;
struct rcl_ros_clock_storage_t;
/// A single point in time, measured in nanoseconds, the reference point is based on the source. /// A single point in time, measured in nanoseconds, the reference point is based on the source.
typedef struct rcl_time_point_t typedef struct rcl_time_point_t
{ {
@ -109,6 +109,7 @@ rcl_clock_valid(rcl_clock_t * clock);
* *
* \param[in] clock_type the type identifying the time source to provide * \param[in] clock_type the type identifying the time source to provide
* \param[in] clock the handle to the clock which is being initialized * \param[in] clock the handle to the clock which is being initialized
* \param[in] allocator The allocator to use for allocations
* \return `RCL_RET_OK` if the time source was successfully initialized, or * \return `RCL_RET_OK` if the time source was successfully initialized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur. * \return `RCL_RET_ERROR` an unspecified error occur.
@ -117,8 +118,8 @@ RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_clock_init( rcl_clock_init(
enum rcl_clock_type_t clock_type, rcl_clock_t * clock enum rcl_clock_type_t clock_type, rcl_clock_t * clock,
); const rcl_allocator_t * allocator);
/// Finalize a clock. /// Finalize a clock.
/** /**
@ -136,7 +137,8 @@ rcl_clock_init(
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_clock_fini(rcl_clock_t * clock); rcl_clock_fini(
rcl_clock_t * clock);
/// Initialize a clock as a RCL_ROS_TIME time source. /// Initialize a clock as a RCL_ROS_TIME time source.
/** /**
@ -144,6 +146,7 @@ rcl_clock_fini(rcl_clock_t * clock);
* It is specifically setting up a RCL_ROS_TIME time source. * It is specifically setting up a RCL_ROS_TIME time source.
* *
* \param[in] clock the handle to the clock which is being initialized * \param[in] clock the handle to the clock which is being initialized
* \param[in] allocator The allocator to use for allocations
* \return `RCL_RET_OK` if the time source was successfully initialized, or * \return `RCL_RET_OK` if the time source was successfully initialized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur. * \return `RCL_RET_ERROR` an unspecified error occur.
@ -151,7 +154,9 @@ rcl_clock_fini(rcl_clock_t * clock);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_ros_clock_init(rcl_clock_t * clock); rcl_ros_clock_init(
rcl_clock_t * clock,
const rcl_allocator_t * allocator);
/// Finalize a clock as a `RCL_ROS_TIME` time source. /// Finalize a clock as a `RCL_ROS_TIME` time source.
/** /**
@ -167,7 +172,8 @@ rcl_ros_clock_init(rcl_clock_t * clock);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_ros_clock_fini(rcl_clock_t * clock); rcl_ros_clock_fini(
rcl_clock_t * clock);
/// Initialize a clock as a `RCL_STEADY_TIME` time source. /// Initialize a clock as a `RCL_STEADY_TIME` time source.
/** /**
@ -175,6 +181,7 @@ rcl_ros_clock_fini(rcl_clock_t * clock);
* It is specifically setting up a `RCL_STEADY_TIME` time source. * It is specifically setting up a `RCL_STEADY_TIME` time source.
* *
* \param[in] clock the handle to the clock which is being initialized * \param[in] clock the handle to the clock which is being initialized
* \param[in] allocator The allocator to use for allocations
* \return `RCL_RET_OK` if the time source was successfully initialized, or * \return `RCL_RET_OK` if the time source was successfully initialized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur. * \return `RCL_RET_ERROR` an unspecified error occur.
@ -182,7 +189,9 @@ rcl_ros_clock_fini(rcl_clock_t * clock);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_steady_clock_init(rcl_clock_t * clock); rcl_steady_clock_init(
rcl_clock_t * clock,
const rcl_allocator_t * allocator);
/// Finalize a clock as a `RCL_STEADY_TIME` time source. /// Finalize a clock as a `RCL_STEADY_TIME` time source.
/** /**
@ -200,7 +209,8 @@ rcl_steady_clock_init(rcl_clock_t * clock);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_steady_clock_fini(rcl_clock_t * clock); rcl_steady_clock_fini(
rcl_clock_t * clock);
/// Initialize a clock as a `RCL_SYSTEM_TIME` time source. /// Initialize a clock as a `RCL_SYSTEM_TIME` time source.
/** /**
@ -210,6 +220,7 @@ rcl_steady_clock_fini(rcl_clock_t * clock);
* It is specifically setting up a system time source. * It is specifically setting up a system time source.
* *
* \param[in] clock the handle to the clock which is being initialized * \param[in] clock the handle to the clock which is being initialized
* \param[in] allocator The allocator to use for allocations
* \return `RCL_RET_OK` if the time source was successfully initialized, or * \return `RCL_RET_OK` if the time source was successfully initialized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur. * \return `RCL_RET_ERROR` an unspecified error occur.
@ -217,7 +228,9 @@ rcl_steady_clock_fini(rcl_clock_t * clock);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_system_clock_init(rcl_clock_t * clock); rcl_system_clock_init(
rcl_clock_t * clock,
const rcl_allocator_t * allocator);
/// Finalize a clock as a `RCL_SYSTEM_TIME` time source. /// Finalize a clock as a `RCL_SYSTEM_TIME` time source.
/** /**
@ -235,7 +248,8 @@ rcl_system_clock_init(rcl_clock_t * clock);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_system_clock_fini(rcl_clock_t * clock); rcl_system_clock_fini(
rcl_clock_t * clock);
/// Initialize a duration using the clock. /// Initialize a duration using the clock.
/** /**

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

@ -29,7 +29,6 @@ typedef struct rcl_ros_clock_storage_t
{ {
atomic_uint_least64_t current_time; atomic_uint_least64_t current_time;
bool active; bool active;
// TODO(tfoote): store subscription here
} rcl_ros_clock_storage_t; } rcl_ros_clock_storage_t;
// Implementation only // Implementation only
@ -86,9 +85,10 @@ rcl_clock_valid(rcl_clock_t * clock)
rcl_ret_t rcl_ret_t
rcl_clock_init( rcl_clock_init(
enum rcl_clock_type_t clock_type, rcl_clock_t * clock enum rcl_clock_type_t clock_type, rcl_clock_t * clock,
) const rcl_allocator_t * allocator)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
switch (clock_type) { switch (clock_type) {
case RCL_CLOCK_UNINITIALIZED: case RCL_CLOCK_UNINITIALIZED:
RCL_CHECK_ARGUMENT_FOR_NULL( RCL_CHECK_ARGUMENT_FOR_NULL(
@ -96,18 +96,19 @@ rcl_clock_init(
rcl_init_generic_clock(clock); rcl_init_generic_clock(clock);
return RCL_RET_OK; return RCL_RET_OK;
case RCL_ROS_TIME: case RCL_ROS_TIME:
return rcl_ros_clock_init(clock); return rcl_ros_clock_init(clock, allocator);
case RCL_SYSTEM_TIME: case RCL_SYSTEM_TIME:
return rcl_system_clock_init(clock); return rcl_system_clock_init(clock, allocator);
case RCL_STEADY_TIME: case RCL_STEADY_TIME:
return rcl_steady_clock_init(clock); return rcl_steady_clock_init(clock, allocator);
default: default:
return RCL_RET_INVALID_ARGUMENT; return RCL_RET_INVALID_ARGUMENT;
} }
} }
rcl_ret_t rcl_ret_t
rcl_clock_fini(rcl_clock_t * clock) rcl_clock_fini(
rcl_clock_t * clock)
{ {
switch (clock->type) { switch (clock->type) {
case RCL_ROS_TIME: case RCL_ROS_TIME:
@ -124,40 +125,52 @@ rcl_clock_fini(rcl_clock_t * clock)
} }
rcl_ret_t rcl_ret_t
rcl_ros_clock_init(rcl_clock_t * clock) rcl_ros_clock_init(
rcl_clock_t * clock,
const rcl_allocator_t * allocator)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_clock(clock); rcl_init_generic_clock(clock);
clock->data = calloc(1, sizeof(rcl_ros_clock_storage_t)); clock->data = allocator->allocate(sizeof(rcl_ros_clock_storage_t), allocator->state);
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
storage->active = false;
clock->get_now = rcl_get_ros_time; clock->get_now = rcl_get_ros_time;
clock->type = RCL_ROS_TIME; clock->type = RCL_ROS_TIME;
clock->allocator = allocator;
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t rcl_ret_t
rcl_ros_clock_fini(rcl_clock_t * clock) rcl_ros_clock_fini(
rcl_clock_t * clock)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_ROS_TIME) { if (clock->type != RCL_ROS_TIME) {
RCL_SET_ERROR_MSG("clock not of type RCL_ROS_TIME", rcl_get_default_allocator()); RCL_SET_ERROR_MSG("clock not of type RCL_ROS_TIME", rcl_get_default_allocator());
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
free((rcl_ros_clock_storage_t *)clock->data); clock->allocator->deallocate((rcl_ros_clock_storage_t *)clock->data, clock->allocator->state);
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t rcl_ret_t
rcl_steady_clock_init(rcl_clock_t * clock) rcl_steady_clock_init(
rcl_clock_t * clock,
const rcl_allocator_t * allocator)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_clock(clock); rcl_init_generic_clock(clock);
clock->get_now = rcl_get_steady_time; clock->get_now = rcl_get_steady_time;
clock->type = RCL_STEADY_TIME; clock->type = RCL_STEADY_TIME;
clock->allocator = allocator;
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t rcl_ret_t
rcl_steady_clock_fini(rcl_clock_t * clock) rcl_steady_clock_fini(
rcl_clock_t * clock)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_STEADY_TIME) { if (clock->type != RCL_STEADY_TIME) {
@ -168,17 +181,22 @@ rcl_steady_clock_fini(rcl_clock_t * clock)
} }
rcl_ret_t rcl_ret_t
rcl_system_clock_init(rcl_clock_t * clock) rcl_system_clock_init(
rcl_clock_t * clock,
const rcl_allocator_t * allocator)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_clock(clock); rcl_init_generic_clock(clock);
clock->get_now = rcl_get_system_time; clock->get_now = rcl_get_system_time;
clock->type = RCL_SYSTEM_TIME; clock->type = RCL_SYSTEM_TIME;
clock->allocator = allocator;
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t rcl_ret_t
rcl_system_clock_fini(rcl_clock_t * clock) rcl_system_clock_fini(
rcl_clock_t * clock)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_SYSTEM_TIME) { if (clock->type != RCL_SYSTEM_TIME) {
@ -229,7 +247,6 @@ rcl_difference_times(
rcl_ret_t rcl_ret_t
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point) rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point)
{ {
// TODO(tfoote) switch to use external time source
RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type && clock->get_now) { if (clock->type && clock->get_now) {
return clock->get_now(clock->data, return clock->get_now(clock->data,
@ -250,8 +267,7 @@ rcl_enable_ros_time_override(rcl_clock_t * clock)
"Time source is not RCL_ROS_TIME cannot enable override.", rcl_get_default_allocator()) "Time source is not RCL_ROS_TIME cannot enable override.", rcl_get_default_allocator())
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_ros_clock_storage_t * storage = \ rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
(rcl_ros_clock_storage_t *)clock->data;
if (!storage) { if (!storage) {
RCL_SET_ERROR_MSG("Storage not initialized, cannot enable.", rcl_get_default_allocator()) RCL_SET_ERROR_MSG("Storage not initialized, cannot enable.", rcl_get_default_allocator())
return RCL_RET_ERROR; return RCL_RET_ERROR;

111
rcl/test/rcl/test_time.cpp
View file

@ -56,9 +56,9 @@ public:
// Tests the rcl_set_ros_time_override() function. // Tests the rcl_set_ros_time_override() function.
TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_override) { TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_override) {
rcl_clock_t * ros_clock = rcl_clock_t ros_clock;
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t))); rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_ret_t retval = rcl_ros_clock_init(ros_clock); rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
assert_no_realloc_begin(); assert_no_realloc_begin();
@ -71,7 +71,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
ret = rcl_is_enabled_ros_time_override(nullptr, &result); ret = rcl_is_enabled_ros_time_override(nullptr, &result);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error(); rcl_reset_error();
ret = rcl_is_enabled_ros_time_override(ros_clock, nullptr); ret = rcl_is_enabled_ros_time_override(&ros_clock, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error(); rcl_reset_error();
ret = rcl_is_enabled_ros_time_override(nullptr, nullptr); ret = rcl_is_enabled_ros_time_override(nullptr, nullptr);
@ -79,13 +79,13 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
rcl_reset_error(); rcl_reset_error();
rcl_time_point_t query_now; rcl_time_point_t query_now;
bool is_enabled; bool is_enabled;
ret = rcl_is_enabled_ros_time_override(ros_clock, &is_enabled); ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(is_enabled, false); EXPECT_EQ(is_enabled, false);
assert_no_malloc_begin(); assert_no_malloc_begin();
assert_no_free_begin(); assert_no_free_begin();
// Check for normal operation (not allowed to alloc). // Check for normal operation (not allowed to alloc).
ret = rcl_clock_get_now(ros_clock, &query_now); ret = rcl_clock_get_now(&ros_clock, &query_now);
assert_no_malloc_end(); assert_no_malloc_end();
assert_no_realloc_end(); assert_no_realloc_end();
assert_no_free_end(); assert_no_free_end();
@ -93,7 +93,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_NE(query_now.nanoseconds, 0u); EXPECT_NE(query_now.nanoseconds, 0u);
// Compare to std::chrono::system_clock time (within a second). // Compare to std::chrono::system_clock time (within a second).
ret = rcl_clock_get_now(ros_clock, &query_now); ret = rcl_clock_get_now(&ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
{ {
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
@ -106,18 +106,18 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
// Test ros time specific APIs // Test ros time specific APIs
rcl_time_point_value_t set_point = 1000000000ull; rcl_time_point_value_t set_point = 1000000000ull;
// Check initialized state // Check initialized state
ret = rcl_is_enabled_ros_time_override(ros_clock, &is_enabled); ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(is_enabled, false); EXPECT_EQ(is_enabled, false);
// set the time point // set the time point
ret = rcl_set_ros_time_override(ros_clock, set_point); ret = rcl_set_ros_time_override(&ros_clock, set_point);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check still disabled // check still disabled
ret = rcl_is_enabled_ros_time_override(ros_clock, &is_enabled); ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(is_enabled, false); EXPECT_EQ(is_enabled, false);
// get real // get real
ret = rcl_clock_get_now(ros_clock, &query_now); ret = rcl_clock_get_now(&ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
{ {
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
@ -128,25 +128,25 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs"; EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
} }
// enable // enable
ret = rcl_enable_ros_time_override(ros_clock); ret = rcl_enable_ros_time_override(&ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check enabled // check enabled
ret = rcl_is_enabled_ros_time_override(ros_clock, &is_enabled); ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(is_enabled, true); EXPECT_EQ(is_enabled, true);
// get sim // get sim
ret = rcl_clock_get_now(ros_clock, &query_now); ret = rcl_clock_get_now(&ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(query_now.nanoseconds, set_point); EXPECT_EQ(query_now.nanoseconds, set_point);
// disable // disable
ret = rcl_disable_ros_time_override(ros_clock); ret = rcl_disable_ros_time_override(&ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check disabled // check disabled
ret = rcl_is_enabled_ros_time_override(ros_clock, &is_enabled); ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(is_enabled, false); EXPECT_EQ(is_enabled, false);
// get real // get real
ret = rcl_clock_get_now(ros_clock, &query_now); ret = rcl_clock_get_now(&ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
{ {
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
@ -159,42 +159,25 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
} }
TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_clock_and_point) { TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_clock_and_point) {
assert_no_realloc_begin();
rcl_ret_t ret; rcl_ret_t ret;
rcl_allocator_t allocator = rcl_get_default_allocator();
// Check for invalid argument error condition (allowed to alloc). // Check for invalid argument error condition (allowed to alloc).
ret = rcl_ros_clock_init(nullptr); ret = rcl_ros_clock_init(nullptr, &allocator);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
// Check for invalid argument error condition (allowed to alloc).
rcl_clock_t uninitialized_clock;
ret = rcl_ros_clock_init(&uninitialized_clock, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error(); rcl_reset_error();
// Check for normal operation (not allowed to alloc). // Check for normal operation (not allowed to alloc).
rcl_clock_t source; rcl_clock_t source;
ret = rcl_ros_clock_init(&source); ret = rcl_ros_clock_init(&source, &allocator);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_clock_t * ros_clock = rcl_clock_t ros_clock;
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t))); rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
rcl_ret_t retval = rcl_ros_clock_init(ros_clock);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
// assert_no_malloc_begin();
// assert_no_free_begin();
// // Do stuff in here
// assert_no_malloc_end();
// assert_no_realloc_end();
// assert_no_free_end();
// stop_memory_checking();
// EXPECT_NE(now.nanoseconds, 0u);
// // Compare to std::chrono::system_clock time (within a second).
// now = {0};
// ret = rcutils_system_time_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;
// const int k_tolerance_ms = 1000;
// EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs";
// }
} }
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) { TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) {
@ -202,43 +185,45 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) {
rcl_clock_t uninitialized; rcl_clock_t uninitialized;
// Not reliably detectable due to random values. // Not reliably detectable due to random values.
// ASSERT_FALSE(rcl_clock_valid(&uninitialized)); // ASSERT_FALSE(rcl_clock_valid(&uninitialized));
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_ret_t ret; rcl_ret_t ret;
ret = rcl_ros_clock_init(&uninitialized); ret = rcl_ros_clock_init(&uninitialized, &allocator);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
} }
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_clock_instanciation) { TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_clock_instanciation) {
rcl_clock_t * ros_clock = rcl_clock_t ros_clock;
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t))); rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_ret_t retval = rcl_ros_clock_init(ros_clock); rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
ASSERT_TRUE(rcl_clock_valid(ros_clock)); ASSERT_TRUE(rcl_clock_valid(&ros_clock));
rcl_clock_t * steady_clock = rcl_clock_t * steady_clock =
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t))); reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t)));
retval = rcl_steady_clock_init(steady_clock); retval = rcl_steady_clock_init(steady_clock, &allocator);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
ASSERT_TRUE(rcl_clock_valid(steady_clock)); ASSERT_TRUE(rcl_clock_valid(steady_clock));
rcl_clock_t * system_clock = rcl_clock_t * system_clock =
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t))); reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t)));
retval = rcl_system_clock_init(system_clock); retval = rcl_system_clock_init(system_clock, &allocator);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
ASSERT_TRUE(rcl_clock_valid(system_clock)); ASSERT_TRUE(rcl_clock_valid(system_clock));
} }
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) { TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) {
rcl_allocator_t allocator = rcl_get_default_allocator();
{ {
rcl_clock_t uninitialized_clock; rcl_clock_t uninitialized_clock;
rcl_ret_t ret = rcl_clock_init( rcl_ret_t ret = rcl_clock_init(
RCL_CLOCK_UNINITIALIZED, &uninitialized_clock); RCL_CLOCK_UNINITIALIZED, &uninitialized_clock, &allocator);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(uninitialized_clock.type, RCL_CLOCK_UNINITIALIZED) << EXPECT_EQ(uninitialized_clock.type, RCL_CLOCK_UNINITIALIZED) <<
"Expected time source of type RCL_CLOCK_UNINITIALIZED"; "Expected time source of type RCL_CLOCK_UNINITIALIZED";
} }
{ {
rcl_clock_t ros_clock; rcl_clock_t ros_clock;
rcl_ret_t ret = rcl_clock_init(RCL_ROS_TIME, &ros_clock); rcl_ret_t ret = rcl_clock_init(RCL_ROS_TIME, &ros_clock, &allocator);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(ros_clock.type, RCL_ROS_TIME) << EXPECT_EQ(ros_clock.type, RCL_ROS_TIME) <<
"Expected time source of type RCL_ROS_TIME"; "Expected time source of type RCL_ROS_TIME";
@ -247,7 +232,7 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) {
} }
{ {
rcl_clock_t system_clock; rcl_clock_t system_clock;
rcl_ret_t ret = rcl_clock_init(RCL_SYSTEM_TIME, &system_clock); rcl_ret_t ret = rcl_clock_init(RCL_SYSTEM_TIME, &system_clock, &allocator);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(system_clock.type, RCL_SYSTEM_TIME) << EXPECT_EQ(system_clock.type, RCL_SYSTEM_TIME) <<
"Expected time source of type RCL_SYSTEM_TIME"; "Expected time source of type RCL_SYSTEM_TIME";
@ -256,7 +241,7 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) {
} }
{ {
rcl_clock_t steady_clock; rcl_clock_t steady_clock;
rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &steady_clock); rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &steady_clock, &allocator);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(steady_clock.type, RCL_STEADY_TIME) << EXPECT_EQ(steady_clock.type, RCL_STEADY_TIME) <<
"Expected time source of type RCL_STEADY_TIME"; "Expected time source of type RCL_STEADY_TIME";
@ -266,9 +251,10 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) {
} }
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) { TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) {
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_clock_t * ros_clock = rcl_clock_t * ros_clock =
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t))); reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
rcl_ret_t retval = rcl_ros_clock_init(ros_clock); rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_TRUE(ros_clock != nullptr); EXPECT_TRUE(ros_clock != nullptr);
EXPECT_TRUE(ros_clock->data != nullptr); EXPECT_TRUE(ros_clock->data != nullptr);
@ -312,11 +298,17 @@ void post_callback(void)
post_callback_called = true; post_callback_called = true;
} }
void reset_callback_triggers(void)
{
pre_callback_called = false;
post_callback_called = false;
}
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) { TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_clock_t * ros_clock = rcl_clock_t * ros_clock =
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t))); reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
rcl_ret_t retval = rcl_ros_clock_init(ros_clock); rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_time_point_t query_now; rcl_time_point_t query_now;
rcl_ret_t ret; rcl_ret_t ret;
@ -326,7 +318,6 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
ros_clock->pre_update = pre_callback; ros_clock->pre_update = pre_callback;
ros_clock->post_update = post_callback; ros_clock->post_update = post_callback;
EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called); EXPECT_FALSE(post_callback_called);