diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index f235fbf..7c2b9d0 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -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 diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index ee298f5..1510deb 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -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; } diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 1615699..2a0dc5e 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -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(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(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(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(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(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(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(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);