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; typedef rcutils_duration_value_t rcl_duration_value_t;
/// Time source type, used to indicate the source of a time measurement. /// 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_ROS_TIME,
RCL_SYSTEM_TIME, RCL_SYSTEM_TIME,
RCL_STEADY_TIME RCL_STEADY_TIME
}; } rcl_clock_type_t;
/// Encapsulation of a time source. /// 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 (* pre_update)(void);
void (* post_update)(void); void (* post_update)(void);
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_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. /// 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
{ {
rcl_time_point_value_t nanoseconds; rcl_time_point_value_t nanoseconds;
rcl_time_source_t * time_source; rcl_clock_type_t clock_type;
} rcl_time_point_t; } rcl_time_point_t;
/// A duration of time, measured in nanoseconds and its source. /// A duration of time, measured in nanoseconds and its source.
typedef struct rcl_duration_t typedef struct rcl_duration_t
{ {
rcl_duration_value_t nanoseconds; rcl_duration_value_t nanoseconds;
rcl_time_source_t * time_source; rcl_clock_type_t clock_type;
} rcl_duration_t; } rcl_duration_t;
// typedef struct rcl_rate_t // typedef struct rcl_rate_t
// { // {
// rcl_time_point_value_t trigger_time; // rcl_time_point_value_t trigger_time;
// int64_t period; // int64_t period;
// rcl_time_source_t * time_source; // rcl_clock_type_t clock;;
// } rcl_rate_t; // } rcl_rate_t;
// TODO(tfoote) integrate rate and timer implementations // 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. * This function returns true if the time source appears to be valid.
* It will check that the type is not uninitialized, and that pointers * It will check that the type is not uninitialized, and that pointers
* are not invalid. * are not invalid.
* Note that if data is uninitialized it may give a false positive. * 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. * \return true if the source is believed to be valid, otherwise return false.
*/ */
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
bool 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. * 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] clock_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 the handle to the clock which is being initialized
* \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.
@ -116,19 +116,19 @@ rcl_time_source_valid(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_time_source_init( rcl_clock_init(
enum rcl_time_source_type_t time_source_type, rcl_time_source_t * time_source 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. * This will deallocate all necessary internal structures, and clean up any variables.
* It can be combined with any of the init functions. * 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. * 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_OK` if the time source was successfully finalized, 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.
@ -136,14 +136,14 @@ rcl_time_source_init(
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. * This will allocate all necessary internal structures, and initialize variables.
* It is specifically setting up a RCL_ROS_TIME time source. * 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_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,15 +151,15 @@ rcl_time_source_fini(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. * 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 * It is specifically setting up a `RCL_ROS_TIME` time source. It is expected
* to be paired with the init fuction. * 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_OK` if the time source was successfully finalized, 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.
@ -167,14 +167,14 @@ rcl_ros_time_source_init(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. * This will allocate all necessary internal structures, and initialize variables.
* It is specifically setting up a `RCL_STEADY_TIME` time source. * 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_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,17 +182,17 @@ rcl_ros_time_source_fini(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. * 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 * It is specifically setting up a steady time source. It is expected to be
* paired with the init fuction. * 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_OK` if the time source was successfully finalized, 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.
@ -200,16 +200,16 @@ rcl_steady_time_source_init(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. * This will allocate all necessary internal structures, and initialize variables.
* It is specifically setting up a system time source. * 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_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,17 +217,17 @@ rcl_steady_time_source_fini(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. * 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 * It is specifically setting up a system time source. It is expected to be paired with
* the init fuction. * 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_OK` if the time source was successfully finalized, 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.
@ -235,20 +235,20 @@ rcl_system_time_source_init(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. * 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. * 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`. * `RCL_SYSTEM_TIME`.
* *
* \param[in] time_point the handle to the time_source which is being initialized. * \param[in] time_point the handle to the clock 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 that clock will be used for reference.
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or * \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_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur. * \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_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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 /// 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. * 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_OK` if the last call time was retrieved successfully, 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.
@ -274,16 +274,16 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_time_point_fini(rcl_time_point_t * time_point); 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. * This function will initialize the duration 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. * 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] 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_OK` if the last call time was retrieved successfully, 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.
@ -291,7 +291,7 @@ rcl_time_point_fini(rcl_time_point_t * time_point);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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 /// Finalize a duration
/** /**
@ -309,77 +309,6 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_duration_fini(rcl_duration_t * duration); 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 /// Compute the difference between two time points
/** /**
* This function takes two time points and computes the duration between them. * 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 * This function will populate the data of the time_point object with the
* current value from it's associated time abstraction. * 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. * \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_OK` if the last call time was retrieved successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@ -415,7 +344,7 @@ rcl_difference_times(
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. /// 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 * such that the time source will report the set value instead of falling
* back to system time. * 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_OK` if the time source was enabled successfully, 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.
@ -432,7 +361,7 @@ rcl_time_point_get_now(rcl_time_point_t * time_point);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. /// 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 * such that the time source will report the system time even if a custom
* value has been set. * 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_OK` if the time source was disabled successfully, 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.
@ -448,7 +377,7 @@ rcl_enable_ros_time_override(rcl_time_source_t * time_source);
RCL_PUBLIC RCL_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t 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. /// 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. * 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. * 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.. * \param[out] is_enabled Whether the override is enabled..
* \return `RCL_RET_OK` if the time source was queried successfully, or * \return `RCL_RET_OK` if the time source was queried successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, 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, * If queried and override enabled the time source will return this value,
* otherwise it will return the system time. * 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. * \param[in] time_value The new current time.
* \return `RCL_RET_OK` if the time source was set successfully, or * \return `RCL_RET_OK` if the time source was set successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, 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 "rcl/error_handling.h"
#include "rcutils/time.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 // 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; atomic_uint_least64_t current_time;
bool active; bool active;
// TODO(tfoote): store subscription here // TODO(tfoote): store subscription here
} rcl_ros_time_source_storage_t; } rcl_ros_clock_storage_t;
// Implementation only // Implementation only
rcl_ret_t 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); 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 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; clock->type = RCL_CLOCK_UNINITIALIZED;
time_source->pre_update = NULL; clock->pre_update = NULL;
time_source->post_update = NULL; clock->post_update = NULL;
time_source->get_now = NULL; clock->get_now = NULL;
time_source->data = NULL; clock->data = NULL;
} }
// The function used to get the current ros time. // 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_ret_t
rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time) 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) { if (!t->active) {
return rcl_get_system_time(data, current_time); 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 bool
rcl_time_source_valid(rcl_time_source_t * time_source) rcl_clock_valid(rcl_clock_t * clock)
{ {
if (time_source == NULL || if (clock == NULL ||
time_source->type == RCL_TIME_SOURCE_UNINITIALIZED || clock->type == RCL_CLOCK_UNINITIALIZED ||
time_source->get_now == NULL) clock->get_now == NULL)
{ {
return false; return false;
} }
@ -89,38 +85,38 @@ rcl_time_source_valid(rcl_time_source_t * time_source)
} }
rcl_ret_t rcl_ret_t
rcl_time_source_init( rcl_clock_init(
enum rcl_time_source_type_t time_source_type, rcl_time_source_t * time_source enum rcl_clock_type_t clock_type, rcl_clock_t * clock
) )
{ {
switch (time_source_type) { switch (clock_type) {
case RCL_TIME_SOURCE_UNINITIALIZED: case RCL_CLOCK_UNINITIALIZED:
RCL_CHECK_ARGUMENT_FOR_NULL( RCL_CHECK_ARGUMENT_FOR_NULL(
time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source); rcl_init_generic_clock(clock);
return RCL_RET_OK; return RCL_RET_OK;
case RCL_ROS_TIME: case RCL_ROS_TIME:
return rcl_ros_time_source_init(time_source); return rcl_ros_clock_init(clock);
case RCL_SYSTEM_TIME: case RCL_SYSTEM_TIME:
return rcl_system_time_source_init(time_source); return rcl_system_clock_init(clock);
case RCL_STEADY_TIME: case RCL_STEADY_TIME:
return rcl_steady_time_source_init(time_source); return rcl_steady_clock_init(clock);
default: default:
return RCL_RET_INVALID_ARGUMENT; return RCL_RET_INVALID_ARGUMENT;
} }
} }
rcl_ret_t 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: case RCL_ROS_TIME:
return rcl_ros_time_source_fini(time_source); return rcl_ros_clock_fini(clock);
case RCL_SYSTEM_TIME: case RCL_SYSTEM_TIME:
return rcl_system_time_source_fini(time_source); return rcl_system_clock_fini(clock);
case RCL_STEADY_TIME: case RCL_STEADY_TIME:
return rcl_steady_time_source_fini(time_source); return rcl_steady_clock_fini(clock);
case RCL_TIME_SOURCE_UNINITIALIZED: case RCL_CLOCK_UNINITIALIZED:
// fall through // fall through
default: default:
return RCL_RET_INVALID_ARGUMENT; return RCL_RET_INVALID_ARGUMENT;
@ -128,79 +124,76 @@ rcl_time_source_fini(rcl_time_source_t * time_source)
} }
rcl_ret_t 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_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source); rcl_init_generic_clock(clock);
time_source->data = calloc(1, sizeof(rcl_ros_time_source_storage_t)); clock->data = calloc(1, sizeof(rcl_ros_clock_storage_t));
time_source->get_now = rcl_get_ros_time; clock->get_now = rcl_get_ros_time;
time_source->type = RCL_ROS_TIME; clock->type = RCL_ROS_TIME;
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t 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()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_ROS_TIME) { if (clock->type != RCL_ROS_TIME) {
RCL_SET_ERROR_MSG("time_source 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_time_source_storage_t *)time_source->data); free((rcl_ros_clock_storage_t *)clock->data);
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t 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_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source); rcl_init_generic_clock(clock);
time_source->get_now = rcl_get_steady_time; clock->get_now = rcl_get_steady_time;
time_source->type = RCL_STEADY_TIME; clock->type = RCL_STEADY_TIME;
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t 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()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_STEADY_TIME) { if (clock->type != RCL_STEADY_TIME) {
RCL_SET_ERROR_MSG("time_source not of type RCL_STEADY_TIME", rcl_get_default_allocator()); RCL_SET_ERROR_MSG("clock not of type RCL_STEADY_TIME", rcl_get_default_allocator());
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t 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_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
rcl_init_generic_time_source(time_source); rcl_init_generic_clock(clock);
time_source->get_now = rcl_get_system_time; clock->get_now = rcl_get_system_time;
time_source->type = RCL_SYSTEM_TIME; clock->type = RCL_SYSTEM_TIME;
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t 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()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_SYSTEM_TIME) { if (clock->type != RCL_SYSTEM_TIME) {
RCL_SET_ERROR_MSG("time_source not of type RCL_SYSTEM_TIME", rcl_get_default_allocator()); RCL_SET_ERROR_MSG("clock not of type RCL_SYSTEM_TIME", rcl_get_default_allocator());
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
return RCL_RET_OK; return RCL_RET_OK;
} }
rcl_ret_t 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()); RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (!time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(clock_type, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
time_point->time_source = rcl_get_default_ros_time_source(); time_point->clock_type = *clock_type;
return RCL_RET_OK;
}
time_point->time_source = time_source;
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -214,14 +207,11 @@ rcl_time_point_fini(rcl_time_point_t * time_point)
} }
rcl_ret_t 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()); RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (!time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(clock_type, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
duration->time_source = rcl_get_default_ros_time_source(); duration->clock_type = *clock_type;
return RCL_RET_OK;
}
duration->time_source = time_source;
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -234,64 +224,14 @@ rcl_duration_fini(rcl_duration_t * duration)
return RCL_RET_OK; 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_ret_t
rcl_difference_times( rcl_difference_times(
rcl_time_point_t * start, rcl_time_point_t * finish, rcl_duration_t * delta) 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( 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()); rcl_get_default_allocator());
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
@ -304,30 +244,31 @@ rcl_difference_times(
} }
rcl_ret_t 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()); 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) { if (time_point->clock_type && clock->get_now) {
return time_point->time_source->get_now(time_point->time_source->data, return clock->get_now(clock->data,
&(time_point->nanoseconds)); &(time_point->nanoseconds));
} }
RCL_SET_ERROR_MSG( 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()); rcl_get_default_allocator());
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_ret_t 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()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (time_source->type != RCL_ROS_TIME) { if (clock->type != RCL_ROS_TIME) {
RCL_SET_ERROR_MSG( RCL_SET_ERROR_MSG(
"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_time_source_storage_t * storage = \ rcl_ros_clock_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->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;
@ -337,14 +278,14 @@ rcl_enable_ros_time_override(rcl_time_source_t * time_source)
} }
rcl_ret_t 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()); RCL_CHECK_ARGUMENT_FOR_NULL(clock, 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; return RCL_RET_ERROR;
} }
rcl_ros_time_source_storage_t * storage = \ rcl_ros_clock_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->data; (rcl_ros_clock_storage_t *)clock->data;
if (!storage) { if (!storage) {
RCL_SET_ERROR_MSG("Storage not initialized, cannot disable.", rcl_get_default_allocator()) RCL_SET_ERROR_MSG("Storage not initialized, cannot disable.", rcl_get_default_allocator())
return RCL_RET_ERROR; return RCL_RET_ERROR;
@ -355,16 +296,16 @@ rcl_disable_ros_time_override(rcl_time_source_t * time_source)
rcl_ret_t rcl_ret_t
rcl_is_enabled_ros_time_override( rcl_is_enabled_ros_time_override(
rcl_time_source_t * time_source, rcl_clock_t * clock,
bool * is_enabled) 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()); 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; return RCL_RET_ERROR;
} }
rcl_ros_time_source_storage_t * storage = \ rcl_ros_clock_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->data; (rcl_ros_clock_storage_t *)clock->data;
if (!storage) { if (!storage) {
RCL_SET_ERROR_MSG("Storage not initialized, cannot query.", rcl_get_default_allocator()) RCL_SET_ERROR_MSG("Storage not initialized, cannot query.", rcl_get_default_allocator())
return RCL_RET_ERROR; return RCL_RET_ERROR;
@ -375,21 +316,21 @@ rcl_is_enabled_ros_time_override(
rcl_ret_t rcl_ret_t
rcl_set_ros_time_override( rcl_set_ros_time_override(
rcl_time_source_t * time_source, rcl_clock_t * clock,
rcl_time_point_value_t time_value) rcl_time_point_value_t time_value)
{ {
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());
if (time_source->type != RCL_ROS_TIME) { if (clock->type != RCL_ROS_TIME) {
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_ros_time_source_storage_t * storage = \ rcl_ros_clock_storage_t * storage = \
(rcl_ros_time_source_storage_t *)time_source->data; (rcl_ros_clock_storage_t *)clock->data;
if (storage->active && time_source->pre_update) { if (storage->active && clock->pre_update) {
time_source->pre_update(); clock->pre_update();
} }
rcl_atomic_store(&(storage->current_time), time_value); rcl_atomic_store(&(storage->current_time), time_value);
if (storage->active && time_source->post_update) { if (storage->active && clock->post_update) {
time_source->post_update(); clock->post_update();
} }
return RCL_RET_OK; 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. // 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_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(); assert_no_realloc_begin();
rcl_ret_t ret; rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc). // 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); 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_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(); 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);
@ -75,15 +79,15 @@ 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_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(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(is_enabled, false); 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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
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_time_point_get_now(&query_now); ret = rcl_time_point_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();
@ -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_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_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(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();
@ -104,18 +108,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_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(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_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_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check still disabled // 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(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_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(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();
@ -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"; EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
} }
// enable // 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_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// check enabled // 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(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_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(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_time_source); 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_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(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_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(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();
@ -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(); assert_no_realloc_begin();
rcl_ret_t ret; rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc). // 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(); 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_time_source_t source; rcl_clock_t source;
ret = rcl_ros_time_source_init(&source); ret = rcl_ros_clock_init(&source);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_time_point_t a_time; 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(); 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; 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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// assert_no_malloc_begin(); // 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); ret = rcl_time_point_fini(&a_time);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); 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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
} }
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), time_source_validation) { TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) {
ASSERT_FALSE(rcl_time_source_valid(NULL)); ASSERT_FALSE(rcl_clock_valid(NULL));
rcl_time_source_t uninitialized; rcl_clock_t uninitialized;
// Not reliably detectable due to random values. // Not reliably detectable due to random values.
// ASSERT_FALSE(rcl_time_source_valid(&uninitialized)); // ASSERT_FALSE(rcl_clock_valid(&uninitialized));
rcl_ret_t ret; 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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
} }
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_time_source_instanciation) { TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_clock_instanciation) {
rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source(); rcl_clock_t * ros_clock =
ASSERT_TRUE(rcl_time_source_valid(ros_time_source)); reinterpret_cast<rcl_clock_t *>(calloc(1, sizeof(rcl_clock_t)));
rcl_time_source_t * steady_time_source = rcl_get_default_steady_time_source(); rcl_ret_t retval = rcl_ros_clock_init(ros_clock);
ASSERT_TRUE(rcl_time_source_valid(steady_time_source)); EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_time_source_t * system_time_source = rcl_get_default_system_time_source(); ASSERT_TRUE(rcl_clock_valid(ros_clock));
ASSERT_TRUE(rcl_time_source_valid(system_time_source));
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_clock_t uninitialized_clock;
rcl_ret_t ret = rcl_time_source_init( rcl_ret_t ret = rcl_clock_init(
RCL_TIME_SOURCE_UNINITIALIZED, &uninitialized_time_source); RCL_CLOCK_UNINITIALIZED, &uninitialized_clock);
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_time_source.type, RCL_TIME_SOURCE_UNINITIALIZED) << EXPECT_EQ(uninitialized_clock.type, RCL_CLOCK_UNINITIALIZED) <<
"Expected time source of type RCL_TIME_SOURCE_UNINITIALIZED"; "Expected time source of type RCL_CLOCK_UNINITIALIZED";
} }
{ {
rcl_time_source_t ros_time_source; rcl_clock_t ros_clock;
rcl_ret_t ret = rcl_time_source_init(RCL_ROS_TIME, &ros_time_source); 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(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"; "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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
} }
{ {
rcl_time_source_t system_time_source; rcl_clock_t system_clock;
rcl_ret_t ret = rcl_time_source_init(RCL_SYSTEM_TIME, &system_time_source); 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(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"; "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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
} }
{ {
rcl_time_source_t steady_time_source; rcl_clock_t steady_clock;
rcl_ret_t ret = rcl_time_source_init(RCL_STEADY_TIME, &steady_time_source); 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(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"; "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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
} }
} }
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) { 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_ret_t ret;
rcl_time_point_t a, b; 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(); 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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
a.nanoseconds = 1000; a.nanoseconds = 1000;
b.nanoseconds = 2000; b.nanoseconds = 2000;
rcl_duration_t d; 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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
ret = rcl_difference_times(&a, &b, &d); ret = rcl_difference_times(&a, &b, &d);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(d.nanoseconds, 1000); 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); ret = rcl_difference_times(&b, &a, &d);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(d.nanoseconds, -1000); EXPECT_EQ(d.nanoseconds, -1000);
EXPECT_EQ(d.time_source->type, RCL_ROS_TIME); EXPECT_EQ(d.clock_type, RCL_ROS_TIME);
rcl_time_source_t * system_time_source = rcl_get_default_system_time_source();
EXPECT_TRUE(system_time_source != nullptr);
rcl_time_point_t e; 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(); 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) { 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_time_point_t query_now;
rcl_ret_t ret; rcl_ret_t ret;
rcl_time_point_value_t set_point = 1000000000ull; 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(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// set callbacks // set callbacks
ros_time_source->pre_update = pre_callback; ros_clock->pre_update = pre_callback;
ros_time_source->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);
// Query it to do something different. no changes expected // 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_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called); EXPECT_FALSE(post_callback_called);
// Set the time before it's enabled. Should be no callbacks // 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_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called); EXPECT_FALSE(post_callback_called);
// enable // 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_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(pre_callback_called);
EXPECT_FALSE(post_callback_called); EXPECT_FALSE(post_callback_called);
// Set the time now that it's enabled, now get callbacks // 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_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_TRUE(pre_callback_called); EXPECT_TRUE(pre_callback_called);