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

removing all references to default time sources

Browse source 
switching time_source to clock, and removing clock pointer from internal time_point and duration storage.
This commit is contained in:
Tully Foote 2017-08-24 18:49:52 -07:00
parent 2079097227
commit d273703e09
3 changed files with 256 additions and 360 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -45,70 +45,70 @@ typedef rcutils_time_point_value_t rcl_time_point_value_t;
typedef rcutils_duration_value_t rcl_duration_value_t;
/// Time source type, used to indicate the source of a time measurement.
enum rcl_time_source_type_t
typedef enum rcl_clock_type_t
{
RCL_TIME_SOURCE_UNINITIALIZED = 0,
RCL_CLOCK_UNINITIALIZED = 0,
RCL_ROS_TIME,
RCL_SYSTEM_TIME,
RCL_STEADY_TIME
};
} rcl_clock_type_t;
/// Encapsulation of a time source.
typedef struct rcl_time_source_t
typedef struct rcl_clock_t
{
enum rcl_time_source_type_t type;
enum rcl_clock_type_t type;
void (* pre_update)(void);
void (* post_update)(void);
rcl_ret_t (* get_now)(void * data, rcl_time_point_value_t * now);
// void (*set_now) (rcl_time_point_value_t);
void * data;
} rcl_time_source_t;
} rcl_clock_t;
struct rcl_ros_time_source_storage_t;
struct rcl_ros_clock_storage_t;
/// A single point in time, measured in nanoseconds, the reference point is based on the source.
typedef struct rcl_time_point_t
{
rcl_time_point_value_t nanoseconds;
rcl_time_source_t * time_source;
rcl_clock_type_t clock_type;
} rcl_time_point_t;
/// A duration of time, measured in nanoseconds and its source.
typedef struct rcl_duration_t
{
rcl_duration_value_t nanoseconds;
rcl_time_source_t * time_source;
rcl_clock_type_t clock_type;
} rcl_duration_t;
// typedef struct rcl_rate_t
// {
// rcl_time_point_value_t trigger_time;
// int64_t period;
// rcl_time_source_t * time_source;
// rcl_clock_type_t clock;;
// } rcl_rate_t;
// TODO(tfoote) integrate rate and timer implementations
/// Check if the time_source has valid values.
/// Check if the clock has valid values.
/**
* This function returns true if the time source appears to be valid.
* It will check that the type is not uninitialized, and that pointers
* are not invalid.
* Note that if data is uninitialized it may give a false positive.
*
* \param[in] time_source the handle to the time_source which is being queried
* \param[in] clock the handle to the clock which is being queried
* \return true if the source is believed to be valid, otherwise return false.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
bool
rcl_time_source_valid(rcl_time_source_t * time_source);
rcl_clock_valid(rcl_clock_t * clock);
/// Initialize a time_source based on the passed type.
/// Initialize a clock based on the passed type.
/**
* This will allocate all necessary internal structures, and initialize variables.
*
* \param[in] time_source_type the type identifying the time source to provide
* \param[in] time_source the handle to the time_source which is being initialized
* \param[in] clock_type the type identifying the time source to provide
* \param[in] clock the handle to the clock which is being initialized
* \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_ERROR` an unspecified error occur.
@ -116,19 +116,19 @@ rcl_time_source_valid(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_time_source_init(
enum rcl_time_source_type_t time_source_type, rcl_time_source_t * time_source
rcl_clock_init(
enum rcl_clock_type_t clock_type, rcl_clock_t * clock
);
/// Finalize a time_source.
/// Finalize a clock.
/**
* This will deallocate all necessary internal structures, and clean up any variables.
* It can be combined with any of the init functions.
*
* Passing a time_source with type RCL_TIME_SOURCE_UNINITIALIZED will result in
* Passing a clock with type RCL_CLOCK_UNINITIALIZED will result in
* RCL_RET_INVALID_ARGUMENT being returned.
*
* \param[in] time_source the handle to the time_source which is being finalized
* \param[in] clock the handle to the clock which is being finalized
* \return `RCL_RET_OK` if the time source was successfully finalized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -136,14 +136,14 @@ rcl_time_source_init(
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_time_source_fini(rcl_time_source_t * time_source);
rcl_clock_fini(rcl_clock_t * clock);
/// Initialize a time_source as a RCL_ROS_TIME time source.
/// Initialize a clock as a RCL_ROS_TIME time source.
/**
* This will allocate all necessary internal structures, and initialize variables.
* It is specifically setting up a RCL_ROS_TIME time source.
*
* \param[in] time_source the handle to the time_source which is being initialized
* \param[in] clock the handle to the clock which is being initialized
* \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_ERROR` an unspecified error occur.
@ -151,15 +151,15 @@ rcl_time_source_fini(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_ros_time_source_init(rcl_time_source_t * time_source);
rcl_ros_clock_init(rcl_clock_t * clock);
/// Finalize a time_source as a `RCL_ROS_TIME` time source.
/// Finalize a clock as a `RCL_ROS_TIME` time source.
/**
* This will deallocate all necessary internal structures, and clean up any variables.
* It is specifically setting up a `RCL_ROS_TIME` time source. It is expected
* to be paired with the init fuction.
*
* \param[in] time_source the handle to the time_source which is being initialized
* \param[in] clock the handle to the clock which is being initialized
* \return `RCL_RET_OK` if the time source was successfully finalized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -167,14 +167,14 @@ rcl_ros_time_source_init(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_ros_time_source_fini(rcl_time_source_t * time_source);
rcl_ros_clock_fini(rcl_clock_t * clock);
/// Initialize a time_source as a `RCL_STEADY_TIME` time source.
/// Initialize a clock as a `RCL_STEADY_TIME` time source.
/**
* This will allocate all necessary internal structures, and initialize variables.
* It is specifically setting up a `RCL_STEADY_TIME` time source.
*
* \param[in] time_source the handle to the time_source which is being initialized
* \param[in] clock the handle to the clock which is being initialized
* \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_ERROR` an unspecified error occur.
@ -182,17 +182,17 @@ rcl_ros_time_source_fini(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_steady_time_source_init(rcl_time_source_t * time_source);
rcl_steady_clock_init(rcl_clock_t * clock);
/// Finalize a time_source as a `RCL_STEADY_TIME` time source.
/// Finalize a clock as a `RCL_STEADY_TIME` time source.
/**
* Finalize the time_source as a `RCL_STEADY_TIME` time source.
* Finalize the clock as a `RCL_STEADY_TIME` time source.
*
* This will deallocate all necessary internal structures, and clean up any variables.
* It is specifically setting up a steady time source. It is expected to be
* paired with the init fuction.
*
* \param[in] time_source the handle to the time_source which is being initialized
* \param[in] clock the handle to the clock which is being initialized
* \return `RCL_RET_OK` if the time source was successfully finalized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -200,16 +200,16 @@ rcl_steady_time_source_init(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_steady_time_source_fini(rcl_time_source_t * time_source);
rcl_steady_clock_fini(rcl_clock_t * clock);
/// Initialize a time_source as a `RCL_SYSTEM_TIME` time source.
/// Initialize a clock as a `RCL_SYSTEM_TIME` time source.
/**
* Initialize the time_source as a `RCL_SYSTEM_TIME` time source.
* Initialize the clock as a `RCL_SYSTEM_TIME` time source.
*
* This will allocate all necessary internal structures, and initialize variables.
* It is specifically setting up a system time source.
*
* \param[in] time_source the handle to the time_source which is being initialized
* \param[in] clock the handle to the clock which is being initialized
* \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_ERROR` an unspecified error occur.
@ -217,17 +217,17 @@ rcl_steady_time_source_fini(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_system_time_source_init(rcl_time_source_t * time_source);
rcl_system_clock_init(rcl_clock_t * clock);
/// Finalize a time_source as a `RCL_SYSTEM_TIME` time source.
/// Finalize a clock as a `RCL_SYSTEM_TIME` time source.
/**
* Finalize the time_source as a `RCL_SYSTEM_TIME` time source.
* Finalize the clock as a `RCL_SYSTEM_TIME` time source.
*
* This will deallocate all necessary internal structures, and clean up any variables.
* It is specifically setting up a system time source. It is expected to be paired with
* the init fuction.
*
* \param[in] time_source the handle to the time_source which is being initialized.
* \param[in] clock the handle to the clock which is being initialized.
* \return `RCL_RET_OK` if the time source was successfully finalized, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -235,20 +235,20 @@ rcl_system_time_source_init(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_system_time_source_fini(rcl_time_source_t * time_source);
rcl_system_clock_fini(rcl_clock_t * clock);
/// Initialize a time point using the time_source.
/// Initialize a time point using the clock.
/**
* This function will initialize the time_point using the time_source
* This function will initialize the time_point using the clock
* as a reference.
* If the time_source is null it will use the system default time_source.
* If the clock is null it will use the system default clock.
*
* This will allocate all necessary internal structures, and initialize variables.
* The time_source may be of types `RCL_ROS_TIME`, `RCL_STEADY_TIME`, or
* The clock may be of types `RCL_ROS_TIME`, `RCL_STEADY_TIME`, or
* `RCL_SYSTEM_TIME`.
*
* \param[in] time_point the handle to the time_source which is being initialized.
* \param[in] time_source the handle to the time_source will be used for reference.
* \param[in] time_point the handle to the clock which is being initialized.
* \param[in] clock_type the type of the clock that clock will be used for reference.
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -256,7 +256,7 @@ rcl_system_time_source_fini(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_time_point_init(rcl_time_point_t * time_point, rcl_time_source_t * time_source);
rcl_time_point_init(rcl_time_point_t * time_point, rcl_clock_type_t * clock_type);
/// Finalize a time_point
/**
@ -264,7 +264,7 @@ rcl_time_point_init(rcl_time_point_t * time_point, rcl_time_source_t * time_sour
*
* This will deallocate all necessary internal structures, and clean up any variables.
*
* \param[in] time_point the handle to the time_source which is being finalized.
* \param[in] time_point the handle to the time_point which is being finalized.
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -274,16 +274,16 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_time_point_fini(rcl_time_point_t * time_point);
/// Initialize a duration using the time_source.
/// Initialize a duration using the clock.
/**
* This function will initialize the duration using the time_source as a reference.
* If the time_source is null it will use the system default time_source.
* This function will initialize the duration using the clock as a reference.
* If the clock is null it will use the system default clock.
*
* This will allocate all necessary internal structures, and initialize variables.
* The time_source may be of types ros, steady, or system.
* The clock may be of types ros, steady, or system.
*
* \param[in] duration the handle to the duration which is being initialized.
* \param[in] time_source the handle to the time_source will be used for reference.
* \param[in] clock_type The type of the clock will be used for reference.
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -291,7 +291,7 @@ rcl_time_point_fini(rcl_time_point_t * time_point);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_duration_init(rcl_duration_t * duration, rcl_time_source_t * time_source);
rcl_duration_init(rcl_duration_t * duration, rcl_clock_type_t * clock_type);
/// Finalize a duration
/**
@ -309,77 +309,6 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_duration_fini(rcl_duration_t * duration);
/// Get the default `RCL_ROS_TIME` time source
/**
* This function will get the process default time source.
* This time source is specifically of the ROS time abstraction,
* and may be overridden by updates.
*
* If the default has not yet been used it will allocate
* and initialize the time source.
*
* \return rcl_time_source_t if it successfully found or allocated a
* time source. If an error occurred it will return NULL.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_time_source_t *
rcl_get_default_ros_time_source(void);
/// Get the default `RCL_STEADY_TIME` time source
/**
* This function will get the process default time source.
* This time source is specifically of the steady time abstraction,
* it should not be able to be overridden..
*
* If the default has not yet been used it will allocate
* and initialize the time source.
*
* \return rcl_time_source_t if it successfully found or allocated a
* time source. If an error occurred it will return NULL.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_time_source_t *
rcl_get_default_steady_time_source(void);
/// Get the default `RCL_SYSTEM_TIME` time source
/**
* This function will get the process default time source.
* This time source is specifically of the system time abstraction,
* and may be overridden by updates to the system clock.
*
* If the default has not yet been used it will allocate
* and initialize the time source.
*
* \return rcl_time_source_t if it successfully found or allocated a
* time source. If an error occurred it will return NULL.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_time_source_t *
rcl_get_default_system_time_source(void);
/// Set the current time on the `RCL_ROS_TIME` time source
/**
* This function is used to set the time on a ROS time source.
* It will error if passed a different time source.
*
* This should not block, except on Windows. One caveat is that
* if the ROS time abstraction is active, it will invoke the user
* defined callbacks, for pre and post update notifications. The
* callbacks are supposed to be short running and non-blocking.
*
* \param[in] process_time_source The time source on which to set the value.
* \return `RCL_RET_OK` if the value was set successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source);
/// Compute the difference between two time points
/**
* This function takes two time points and computes the duration between them.
@ -406,7 +335,7 @@ rcl_difference_times(
/**
* This function will populate the data of the time_point object with the
* current value from it's associated time abstraction.
*
* \param[in] clock The time source from which to set the value.
* \param[out] time_point The time_point to populate.
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@ -415,7 +344,7 @@ rcl_difference_times(
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_time_point_get_now(rcl_time_point_t * time_point);
rcl_time_point_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point);
/// Enable the ROS time abstraction override.
@ -424,7 +353,7 @@ rcl_time_point_get_now(rcl_time_point_t * time_point);
* such that the time source will report the set value instead of falling
* back to system time.
*
* \param[in] time_source The time_source to enable.
* \param[in] clock The clock to enable.
* \return `RCL_RET_OK` if the time source was enabled successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -432,7 +361,7 @@ rcl_time_point_get_now(rcl_time_point_t * time_point);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_enable_ros_time_override(rcl_time_source_t * time_source);
rcl_enable_ros_time_override(rcl_clock_t * clock);
/// Disable the ROS time abstraction override.
/**
@ -440,7 +369,7 @@ rcl_enable_ros_time_override(rcl_time_source_t * time_source);
* such that the time source will report the system time even if a custom
* value has been set.
*
* \param[in] time_source The time_source to disable.
* \param[in] clock The clock to disable.
* \return `RCL_RET_OK` if the time source was disabled successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
@ -448,7 +377,7 @@ rcl_enable_ros_time_override(rcl_time_source_t * time_source);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_disable_ros_time_override(rcl_time_source_t * time_source);
rcl_disable_ros_time_override(rcl_clock_t * clock);
/// Check if the `RCL_ROS_TIME` time source has the override enabled.
@ -457,7 +386,7 @@ rcl_disable_ros_time_override(rcl_time_source_t * time_source);
* time overide is enabled. If it is enabled, the set value will be returned.
* Otherwise this time source will return the equivalent to system time abstraction.
*
* \param[in] time_source The time_source to query.
* \param[in] clock The clock to query.
* \param[out] is_enabled Whether the override is enabled..
* \return `RCL_RET_OK` if the time source was queried successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@ -476,7 +405,7 @@ rcl_is_enabled_ros_time_override(
* If queried and override enabled the time source will return this value,
* otherwise it will return the system time.
*
* \param[in] time_source The time_source to update.
* \param[in] clock The clock to update.
* \param[in] time_value The new current time.
* \return `RCL_RET_OK` if the time source was set successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or

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

@ -23,18 +23,14 @@
#include "rcl/error_handling.h"
#include "rcutils/time.h"
// Process default ROS time sources
static rcl_time_source_t * rcl_default_ros_time_source;
static rcl_time_source_t * rcl_default_steady_time_source;
static rcl_time_source_t * rcl_default_system_time_source;
// Internal storage for RCL_ROS_TIME implementation
typedef struct rcl_ros_time_source_storage_t
typedef struct rcl_ros_clock_storage_t
{
atomic_uint_least64_t current_time;
bool active;
// TODO(tfoote): store subscription here
} rcl_ros_time_source_storage_t;
} rcl_ros_clock_storage_t;
// Implementation only
rcl_ret_t
@ -52,15 +48,15 @@ rcl_get_system_time(void * data, rcl_time_point_value_t * current_time)
return rcutils_system_time_now(current_time);
}
// Internal method for zeroing values on init, assumes time_source is valid
// Internal method for zeroing values on init, assumes clock is valid
void
rcl_init_generic_time_source(rcl_time_source_t * time_source)
rcl_init_generic_clock(rcl_clock_t * clock)
{
time_source->type = RCL_TIME_SOURCE_UNINITIALIZED;
time_source->pre_update = NULL;
time_source->post_update = NULL;
time_source->get_now = NULL;
time_source->data = NULL;
clock->type = RCL_CLOCK_UNINITIALIZED;
clock->pre_update = NULL;
clock->post_update = NULL;
clock->get_now = NULL;
clock->data = NULL;
}
// The function used to get the current ros time.
@ -68,7 +64,7 @@ rcl_init_generic_time_source(rcl_time_source_t * time_source)
rcl_ret_t
rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time)
{
rcl_ros_time_source_storage_t * t = (rcl_ros_time_source_storage_t *)data;
rcl_ros_clock_storage_t * t = (rcl_ros_clock_storage_t *)data;
if (!t->active) {
return rcl_get_system_time(data, current_time);
}
@ -77,11 +73,11 @@ rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time)
}
bool
rcl_time_source_valid(rcl_time_source_t * time_source)
rcl_clock_valid(rcl_clock_t * clock)
{
if (time_source == NULL ||
time_source->type == RCL_TIME_SOURCE_UNINITIALIZED ||
time_source->get_now == NULL)
if (clock == NULL ||
clock->type == RCL_CLOCK_UNINITIALIZED ||
clock->get_now == NULL)
{
return false;
}
@ -89,38 +85,38 @@ rcl_time_source_valid(rcl_time_source_t * time_source)
}
rcl_ret_t
rcl_time_source_init(
enum rcl_time_source_type_t time_source_type, rcl_time_source_t * time_source
rcl_clock_init(
enum rcl_clock_type_t clock_type, rcl_clock_t * clock
)
{
switch (time_source_type) {
case RCL_TIME_SOURCE_UNINITIALIZED:
switch (clock_type) {
case RCL_CLOCK_UNINITIALIZED:
RCL_CHECK_ARGUMENT_FOR_NULL(
time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source);
clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_clock(clock);
return RCL_RET_OK;
case RCL_ROS_TIME:
return rcl_ros_time_source_init(time_source);
return rcl_ros_clock_init(clock);
case RCL_SYSTEM_TIME:
return rcl_system_time_source_init(time_source);
return rcl_system_clock_init(clock);
case RCL_STEADY_TIME:
return rcl_steady_time_source_init(time_source);
return rcl_steady_clock_init(clock);
default:
return RCL_RET_INVALID_ARGUMENT;
}
}
rcl_ret_t
rcl_time_source_fini(rcl_time_source_t * time_source)
rcl_clock_fini(rcl_clock_t * clock)
{
switch (time_source->type) {
switch (clock->type) {
case RCL_ROS_TIME:
return rcl_ros_time_source_fini(time_source);
return rcl_ros_clock_fini(clock);
case RCL_SYSTEM_TIME:
return rcl_system_time_source_fini(time_source);
return rcl_system_clock_fini(clock);
case RCL_STEADY_TIME:
return rcl_steady_time_source_fini(time_source);
case RCL_TIME_SOURCE_UNINITIALIZED:
return rcl_steady_clock_fini(clock);
case RCL_CLOCK_UNINITIALIZED:
// fall through
default:
return RCL_RET_INVALID_ARGUMENT;
@ -128,79 +124,76 @@ rcl_time_source_fini(rcl_time_source_t * time_source)
}
rcl_ret_t
rcl_ros_time_source_init(rcl_time_source_t * time_source)
rcl_ros_clock_init(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source);
time_source->data = calloc(1, sizeof(rcl_ros_time_source_storage_t));
time_source->get_now = rcl_get_ros_time;
time_source->type = RCL_ROS_TIME;
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_clock(clock);
clock->data = calloc(1, sizeof(rcl_ros_clock_storage_t));
clock->get_now = rcl_get_ros_time;
clock->type = RCL_ROS_TIME;
return RCL_RET_OK;
}
rcl_ret_t
rcl_ros_time_source_fini(rcl_time_source_t * time_source)
rcl_ros_clock_fini(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_ROS_TIME) {
RCL_SET_ERROR_MSG("time_source not of type RCL_ROS_TIME", rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_ROS_TIME) {
RCL_SET_ERROR_MSG("clock not of type RCL_ROS_TIME", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
free((rcl_ros_time_source_storage_t *)time_source->data);
free((rcl_ros_clock_storage_t *)clock->data);
return RCL_RET_OK;
}
rcl_ret_t
rcl_steady_time_source_init(rcl_time_source_t * time_source)
rcl_steady_clock_init(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source);
time_source->get_now = rcl_get_steady_time;
time_source->type = RCL_STEADY_TIME;
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_clock(clock);
clock->get_now = rcl_get_steady_time;
clock->type = RCL_STEADY_TIME;
return RCL_RET_OK;
}
rcl_ret_t
rcl_steady_time_source_fini(rcl_time_source_t * time_source)
rcl_steady_clock_fini(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_STEADY_TIME) {
RCL_SET_ERROR_MSG("time_source not of type RCL_STEADY_TIME", rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_STEADY_TIME) {
RCL_SET_ERROR_MSG("clock not of type RCL_STEADY_TIME", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
return RCL_RET_OK;
}
rcl_ret_t
rcl_system_time_source_init(rcl_time_source_t * time_source)
rcl_system_clock_init(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source);
time_source->get_now = rcl_get_system_time;
time_source->type = RCL_SYSTEM_TIME;
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_clock(clock);
clock->get_now = rcl_get_system_time;
clock->type = RCL_SYSTEM_TIME;
return RCL_RET_OK;
}
rcl_ret_t
rcl_system_time_source_fini(rcl_time_source_t * time_source)
rcl_system_clock_fini(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_SYSTEM_TIME) {
RCL_SET_ERROR_MSG("time_source not of type RCL_SYSTEM_TIME", rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_SYSTEM_TIME) {
RCL_SET_ERROR_MSG("clock not of type RCL_SYSTEM_TIME", rcl_get_default_allocator());
return RCL_RET_ERROR;
}
return RCL_RET_OK;
}
rcl_ret_t
rcl_time_point_init(rcl_time_point_t * time_point, rcl_time_source_t * time_source)
rcl_time_point_init(rcl_time_point_t * time_point, rcl_clock_type_t * clock_type)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (!time_source) {
time_point->time_source = rcl_get_default_ros_time_source();
return RCL_RET_OK;
}
time_point->time_source = time_source;
RCL_CHECK_ARGUMENT_FOR_NULL(clock_type, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
time_point->clock_type = *clock_type;
return RCL_RET_OK;
}
@ -214,14 +207,11 @@ rcl_time_point_fini(rcl_time_point_t * time_point)
}
rcl_ret_t
rcl_duration_init(rcl_duration_t * duration, rcl_time_source_t * time_source)
rcl_duration_init(rcl_duration_t * duration, rcl_clock_type_t * clock_type)
{
RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (!time_source) {
duration->time_source = rcl_get_default_ros_time_source();
return RCL_RET_OK;
}
duration->time_source = time_source;
RCL_CHECK_ARGUMENT_FOR_NULL(clock_type, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
duration->clock_type = *clock_type;
return RCL_RET_OK;
}
@ -234,64 +224,14 @@ rcl_duration_fini(rcl_duration_t * duration)
return RCL_RET_OK;
}
rcl_time_source_t *
rcl_get_default_ros_time_source(void)
{
if (!rcl_default_ros_time_source) {
rcl_default_ros_time_source = (rcl_time_source_t *)calloc(1, sizeof(rcl_time_source_t));
rcl_ret_t retval = rcl_ros_time_source_init(rcl_default_ros_time_source);
if (retval != RCL_RET_OK) {
return NULL;
}
}
return rcl_default_ros_time_source;
}
rcl_time_source_t *
rcl_get_default_steady_time_source(void)
{
if (!rcl_default_steady_time_source) {
rcl_default_steady_time_source = (rcl_time_source_t *)calloc(1, sizeof(rcl_time_source_t));
rcl_ret_t retval = rcl_steady_time_source_init(rcl_default_steady_time_source);
if (retval != RCL_RET_OK) {
return NULL;
}
}
return rcl_default_steady_time_source;
}
rcl_time_source_t *
rcl_get_default_system_time_source(void)
{
if (!rcl_default_system_time_source) {
rcl_default_system_time_source = (rcl_time_source_t *)calloc(1, sizeof(rcl_time_source_t));
rcl_ret_t retval = rcl_system_time_source_init(rcl_default_system_time_source);
if (retval != RCL_RET_OK) {
return NULL;
}
}
return rcl_default_system_time_source;
}
rcl_ret_t
rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source)
{
RCL_CHECK_ARGUMENT_FOR_NULL(
process_time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (rcl_default_ros_time_source) {
free(rcl_default_ros_time_source);
}
rcl_default_ros_time_source = process_time_source;
return RCL_RET_OK;
}
rcl_ret_t
rcl_difference_times(
rcl_time_point_t * start, rcl_time_point_t * finish, rcl_duration_t * delta)
{
if (start->time_source->type != finish->time_source->type) {
if (start->clock_type != finish->clock_type) {
RCL_SET_ERROR_MSG(
"Cannot difference between time points with time_sources types.",
"Cannot difference between time points with clocks types.",
rcl_get_default_allocator());
return RCL_RET_ERROR;
}
@ -304,30 +244,31 @@ rcl_difference_times(
}
rcl_ret_t
rcl_time_point_get_now(rcl_time_point_t * time_point)
rcl_time_point_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());
if (time_point->time_source && time_point->time_source->get_now) {
return time_point->time_source->get_now(time_point->time_source->data,
if (time_point->clock_type && clock->get_now) {
return clock->get_now(clock->data,
&(time_point->nanoseconds));
}
RCL_SET_ERROR_MSG(
"time_source is not initialized or does not have get_now registered.",
"clock is not initialized or does not have get_now registered.",
rcl_get_default_allocator());
return RCL_RET_ERROR;
}
rcl_ret_t
rcl_enable_ros_time_override(rcl_time_source_t * time_source)
rcl_enable_ros_time_override(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_ROS_TIME) {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_ROS_TIME) {
RCL_SET_ERROR_MSG(
"Time source is not RCL_ROS_TIME cannot enable override.", rcl_get_default_allocator())
return RCL_RET_ERROR;
}
rcl_ros_time_source_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->data;
rcl_ros_clock_storage_t * storage = \
(rcl_ros_clock_storage_t *)clock->data;
if (!storage) {
RCL_SET_ERROR_MSG("Storage not initialized, cannot enable.", rcl_get_default_allocator())
return RCL_RET_ERROR;
@ -337,14 +278,14 @@ rcl_enable_ros_time_override(rcl_time_source_t * time_source)
}
rcl_ret_t
rcl_disable_ros_time_override(rcl_time_source_t * time_source)
rcl_disable_ros_time_override(rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_ROS_TIME) {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_ROS_TIME) {
return RCL_RET_ERROR;
}
rcl_ros_time_source_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->data;
rcl_ros_clock_storage_t * storage = \
(rcl_ros_clock_storage_t *)clock->data;
if (!storage) {
RCL_SET_ERROR_MSG("Storage not initialized, cannot disable.", rcl_get_default_allocator())
return RCL_RET_ERROR;
@ -355,16 +296,16 @@ rcl_disable_ros_time_override(rcl_time_source_t * time_source)
rcl_ret_t
rcl_is_enabled_ros_time_override(
rcl_time_source_t * time_source,
rcl_clock_t * clock,
bool * is_enabled)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, 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(is_enabled, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_ROS_TIME) {
if (clock->type != RCL_ROS_TIME) {
return RCL_RET_ERROR;
}
rcl_ros_time_source_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->data;
rcl_ros_clock_storage_t * storage = \
(rcl_ros_clock_storage_t *)clock->data;
if (!storage) {
RCL_SET_ERROR_MSG("Storage not initialized, cannot query.", rcl_get_default_allocator())
return RCL_RET_ERROR;
@ -375,21 +316,21 @@ rcl_is_enabled_ros_time_override(
rcl_ret_t
rcl_set_ros_time_override(
rcl_time_source_t * time_source,
rcl_clock_t * clock,
rcl_time_point_value_t time_value)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_ROS_TIME) {
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type != RCL_ROS_TIME) {
return RCL_RET_ERROR;
}
rcl_ros_time_source_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->data;
if (storage->active && time_source->pre_update) {
time_source->pre_update();
rcl_ros_clock_storage_t * storage = \
(rcl_ros_clock_storage_t *)clock->data;
if (storage->active && clock->pre_update) {
clock->pre_update();
}
rcl_atomic_store(&(storage->current_time), time_value);
if (storage->active && time_source->post_update) {
time_source->post_update();
if (storage->active && clock->post_update) {
clock->post_update();
}
return RCL_RET_OK;
}

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

@ -56,7 +56,11 @@ public:
// Tests the rcl_set_ros_time_override() function.
TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_override) {
rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source();
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);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
assert_no_realloc_begin();
rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc).
@ -67,7 +71,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
ret = rcl_is_enabled_ros_time_override(nullptr, &result);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_is_enabled_ros_time_override(ros_time_source, nullptr);
ret = rcl_is_enabled_ros_time_override(ros_clock, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
ret = rcl_is_enabled_ros_time_override(nullptr, nullptr);
@ -75,15 +79,15 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
rcl_reset_error();
rcl_time_point_t query_now;
bool is_enabled;
ret = rcl_is_enabled_ros_time_override(ros_time_source, &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(is_enabled, false);
ret = rcl_time_point_init(&query_now, ros_time_source);
ret = rcl_time_point_init(&query_now, &(ros_clock->type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
assert_no_malloc_begin();
assert_no_free_begin();
// Check for normal operation (not allowed to alloc).
ret = rcl_time_point_get_now(&query_now);
ret = rcl_time_point_get_now(ros_clock, &query_now);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
@ -91,7 +95,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_NE(query_now.nanoseconds, 0u);
// Compare to std::chrono::system_clock time (within a second).
ret = rcl_time_point_get_now(&query_now);
ret = rcl_time_point_get_now(ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
{
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
@ -104,18 +108,18 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
// Test ros time specific APIs
rcl_time_point_value_t set_point = 1000000000ull;
// Check initialized state
ret = rcl_is_enabled_ros_time_override(ros_time_source, &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(is_enabled, false);
// set the time point
ret = rcl_set_ros_time_override(ros_time_source, set_point);
ret = rcl_set_ros_time_override(ros_clock, set_point);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check still disabled
ret = rcl_is_enabled_ros_time_override(ros_time_source, &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(is_enabled, false);
// get real
ret = rcl_time_point_get_now(&query_now);
ret = rcl_time_point_get_now(ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
{
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
@ -126,25 +130,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";
}
// enable
ret = rcl_enable_ros_time_override(ros_time_source);
ret = rcl_enable_ros_time_override(ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check enabled
ret = rcl_is_enabled_ros_time_override(ros_time_source, &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(is_enabled, true);
// get sim
ret = rcl_time_point_get_now(&query_now);
ret = rcl_time_point_get_now(ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(query_now.nanoseconds, set_point);
// disable
ret = rcl_disable_ros_time_override(ros_time_source);
ret = rcl_disable_ros_time_override(ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check disabled
ret = rcl_is_enabled_ros_time_override(ros_time_source, &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(is_enabled, false);
// get real
ret = rcl_time_point_get_now(&query_now);
ret = rcl_time_point_get_now(ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
{
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
@ -156,24 +160,29 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
}
}
TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_time_source_and_point) {
TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_clock_and_point) {
assert_no_realloc_begin();
rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc).
ret = rcl_ros_time_source_init(nullptr);
ret = rcl_ros_clock_init(nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
// Check for normal operation (not allowed to alloc).
rcl_time_source_t source;
ret = rcl_ros_time_source_init(&source);
rcl_clock_t source;
ret = rcl_ros_clock_init(&source);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_time_point_t a_time;
ret = rcl_time_point_init(&a_time, &source);
ret = rcl_time_point_init(&a_time, &(source.type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
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);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_time_point_t default_time;
ret = rcl_time_point_init(&default_time, nullptr);
ret = rcl_time_point_init(&default_time, &(ros_clock->type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// assert_no_malloc_begin();
@ -198,98 +207,112 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_time_so
// }
ret = rcl_time_point_fini(&a_time);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
ret = rcl_ros_time_source_fini(&source);
ret = rcl_ros_clock_fini(&source);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
}
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), time_source_validation) {
ASSERT_FALSE(rcl_time_source_valid(NULL));
rcl_time_source_t uninitialized;
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) {
ASSERT_FALSE(rcl_clock_valid(NULL));
rcl_clock_t uninitialized;
// Not reliably detectable due to random values.
// ASSERT_FALSE(rcl_time_source_valid(&uninitialized));
// ASSERT_FALSE(rcl_clock_valid(&uninitialized));
rcl_ret_t ret;
ret = rcl_ros_time_source_init(&uninitialized);
ret = rcl_ros_clock_init(&uninitialized);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
}
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_time_source_instanciation) {
rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source();
ASSERT_TRUE(rcl_time_source_valid(ros_time_source));
rcl_time_source_t * steady_time_source = rcl_get_default_steady_time_source();
ASSERT_TRUE(rcl_time_source_valid(steady_time_source));
rcl_time_source_t * system_time_source = rcl_get_default_system_time_source();
ASSERT_TRUE(rcl_time_source_valid(system_time_source));
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_clock_instanciation) {
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);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
ASSERT_TRUE(rcl_clock_valid(ros_clock));
rcl_clock_t * steady_clock =
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t)));
retval = rcl_steady_clock_init(steady_clock);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
ASSERT_TRUE(rcl_clock_valid(steady_clock));
rcl_clock_t * system_clock =
reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t)));
retval = rcl_system_clock_init(system_clock);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
ASSERT_TRUE(rcl_clock_valid(system_clock));
}
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_time_source_instantiation) {
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) {
{
rcl_time_source_t uninitialized_time_source;
rcl_ret_t ret = rcl_time_source_init(
RCL_TIME_SOURCE_UNINITIALIZED, &uninitialized_time_source);
rcl_clock_t uninitialized_clock;
rcl_ret_t ret = rcl_clock_init(
RCL_CLOCK_UNINITIALIZED, &uninitialized_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(uninitialized_time_source.type, RCL_TIME_SOURCE_UNINITIALIZED) <<
"Expected time source of type RCL_TIME_SOURCE_UNINITIALIZED";
EXPECT_EQ(uninitialized_clock.type, RCL_CLOCK_UNINITIALIZED) <<
"Expected time source of type RCL_CLOCK_UNINITIALIZED";
}
{
rcl_time_source_t ros_time_source;
rcl_ret_t ret = rcl_time_source_init(RCL_ROS_TIME, &ros_time_source);
rcl_clock_t ros_clock;
rcl_ret_t ret = rcl_clock_init(RCL_ROS_TIME, &ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(ros_time_source.type, RCL_ROS_TIME) <<
EXPECT_EQ(ros_clock.type, RCL_ROS_TIME) <<
"Expected time source of type RCL_ROS_TIME";
ret = rcl_time_source_fini(&ros_time_source);
ret = rcl_clock_fini(&ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
}
{
rcl_time_source_t system_time_source;
rcl_ret_t ret = rcl_time_source_init(RCL_SYSTEM_TIME, &system_time_source);
rcl_clock_t system_clock;
rcl_ret_t ret = rcl_clock_init(RCL_SYSTEM_TIME, &system_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(system_time_source.type, RCL_SYSTEM_TIME) <<
EXPECT_EQ(system_clock.type, RCL_SYSTEM_TIME) <<
"Expected time source of type RCL_SYSTEM_TIME";
ret = rcl_time_source_fini(&system_time_source);
ret = rcl_clock_fini(&system_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
}
{
rcl_time_source_t steady_time_source;
rcl_ret_t ret = rcl_time_source_init(RCL_STEADY_TIME, &steady_time_source);
rcl_clock_t steady_clock;
rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &steady_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(steady_time_source.type, RCL_STEADY_TIME) <<
EXPECT_EQ(steady_clock.type, RCL_STEADY_TIME) <<
"Expected time source of type RCL_STEADY_TIME";
ret = rcl_time_source_fini(&steady_time_source);
ret = rcl_clock_fini(&steady_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
}
}
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) {
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);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_TRUE(ros_clock != nullptr);
rcl_ret_t ret;
rcl_time_point_t a, b;
ret = rcl_time_point_init(&a, nullptr);
ret = rcl_time_point_init(&a, &(ros_clock->type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
ret = rcl_time_point_init(&b, nullptr);
ret = rcl_time_point_init(&b, &(ros_clock->type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
a.nanoseconds = 1000;
b.nanoseconds = 2000;
rcl_duration_t d;
ret = rcl_duration_init(&d, nullptr);
ret = rcl_duration_init(&d, &(ros_clock->type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
ret = rcl_difference_times(&a, &b, &d);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(d.nanoseconds, 1000);
EXPECT_EQ(d.time_source->type, RCL_ROS_TIME);
EXPECT_EQ(d.clock_type, RCL_ROS_TIME);
ret = rcl_difference_times(&b, &a, &d);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(d.nanoseconds, -1000);
EXPECT_EQ(d.time_source->type, RCL_ROS_TIME);
rcl_time_source_t * system_time_source = rcl_get_default_system_time_source();
EXPECT_TRUE(system_time_source != nullptr);
EXPECT_EQ(d.clock_type, RCL_ROS_TIME);
rcl_time_point_t e;
ret = rcl_time_point_init(&e, system_time_source);
ret = rcl_time_point_init(&e, &(ros_clock->type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
}
@ -309,45 +332,48 @@ void post_callback(void)
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source();
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);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_time_point_t query_now;
rcl_ret_t ret;
rcl_time_point_value_t set_point = 1000000000ull;
ret = rcl_time_point_init(&query_now, ros_time_source);
ret = rcl_time_point_init(&query_now, &(ros_clock->type));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// set callbacks
ros_time_source->pre_update = pre_callback;
ros_time_source->post_update = post_callback;
ros_clock->pre_update = pre_callback;
ros_clock->post_update = post_callback;
EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called);
// Query it to do something different. no changes expected
ret = rcl_time_point_get_now(&query_now);
ret = rcl_time_point_get_now(ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called);
// Set the time before it's enabled. Should be no callbacks
ret = rcl_set_ros_time_override(ros_time_source, set_point);
ret = rcl_set_ros_time_override(ros_clock, set_point);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called);
// enable
ret = rcl_enable_ros_time_override(ros_time_source);
ret = rcl_enable_ros_time_override(ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called);
// Set the time now that it's enabled, now get callbacks
ret = rcl_set_ros_time_override(ros_time_source, set_point);
ret = rcl_set_ros_time_override(ros_clock, set_point);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_TRUE(pre_callback_called);