diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 60ccf5d..0f6220d 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -28,52 +28,376 @@ extern "C" #define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000 * 1000) #define RCL_US_TO_NS(microseconds) (microseconds * 1000) -#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000 * 1000 * 1000) -#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000 * 1000) +#define RCL_NS_TO_S(nanoseconds) (nanoseconds / (1000 * 1000 * 1000)) +#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / (1000 * 1000)) #define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000) -/// Struct which encapsulates a point in time according to a system clock. -/* The system clock's point of reference is the Unix Epoch (January 1st, 1970). - * See: http://stackoverflow.com/a/32189845/671658 - * Therefore all times in this struct are positive and count the nanoseconds - * since the Unix epoch and this struct cannot be used on systems which report - * a current time before the Unix Epoch. - * Leap seconds are not considered. - * - * The struct represents time as nanoseconds in an unsigned 64-bit integer. - * The struct is capable of representing any time until the year 2554 with - * nanosecond precisions. - */ -typedef struct rcl_system_time_point_t -{ - uint64_t nanoseconds; -} rcl_system_time_point_t; -/// Struct which encapsulates a point in time according to a steady clock. -/* The steady clock's point of reference is system defined, but often the time - * that the process started plus a signed random offset. - * See: http://stackoverflow.com/a/32189845/671658 - * However, the clock is guaranteed to be monotonically increasing. - * Therefore all times in this struct are positive and count nanoseconds - * since an unspecified, but constant point in time. - * - * The struct represents time as nanoseconds in an unsigned 64-bit integer. - */ -typedef struct rcl_steady_time_point_t +enum rcl_time_source_type_t { - uint64_t nanoseconds; -} rcl_steady_time_point_t; + RCL_TIME_SOURCE_UNINITIALIZED = 0, + RCL_ROS_TIME, + RCL_SYSTEM_TIME, + RCL_STEADY_TIME +}; + +typedef uint64_t rcl_time_point_value_t; +typedef int64_t rcl_duration_value_t; + +typedef struct rcl_time_source_t +{ + enum rcl_time_source_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; + +struct rcl_ros_time_source_storage_t; +typedef struct rcl_time_point_t +{ + rcl_time_point_value_t nanoseconds; + rcl_time_source_t * time_source; +} rcl_time_point_t; -/// Struct which encapsulates a duration of time between two points in time. -/* The struct can represent any time within the range [~292 years, ~-292 years] - * with nanosecond precision. - */ typedef struct rcl_duration_t { - int64_t nanoseconds; + rcl_duration_value_t nanoseconds; + rcl_time_source_t * time_source; } rcl_duration_t; -/// Retrieve the current time as a rcl_system_time_point_t struct. +// typedef struct rcl_rate_t +// { +// rcl_time_point_value_t trigger_time; +// int64_t period; +// rcl_time_source_t * time_source; +// } rcl_rate_t; +// TODO(tfoote) integrate rate and timer implementations + +/// Check if the time_source 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 +* \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); + +/// Initialize a timesource 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 +* \return RCL_RET_OK if the time source was successfully initialized, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_init_ros_time_source(rcl_time_source_t * time_source); + +/// Finalize a timesource 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 +* \return RCL_RET_OK if the time source was successfully finalized, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_fini_ros_time_source(rcl_time_source_t * time_source); + +/// Initialize a timesource 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 +* \return RCL_RET_OK if the time source was successfully initialized, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_init_steady_time_source(rcl_time_source_t * time_source); + +/// Finalize a timesource as a RCL_STEADY_TIME time source. +/* Finalize the timesource 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 +* \return RCL_RET_OK if the time source was successfully finalized, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_fini_steady_time_source(rcl_time_source_t * time_source); + +/// Initialize a timesource as a RCL_SYSTEM_TIME time source. +/* Initialize the timesource 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 +* \return RCL_RET_OK if the time source was successfully initialized, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_init_system_time_source(rcl_time_source_t * time_source); + +/// Finalize a timesource as a RCL_SYSTEM_TIME time source. +/* Finalize the timesource 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. +* \return RCL_RET_OK if the time source was successfully finalized, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_fini_system_time_source(rcl_time_source_t * time_source); + +/// Initialize a time point using the time_source. +/* This function will initialize the time_point using the time_source +* as a reference. +* If the time_source is null it will use the system default time_source. +* +* This will allocate all necessary internal structures, and initialize variables. +* The time_source 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. +* \return RCL_RET_OK if the last call time was retrieved successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source); + +/// Finalize a time_point +/* Finalize the time_point such that it is ready for deallocation. +* +* 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. +* \return RCL_RET_OK if the last call time was retrieved successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_fini_time_point(rcl_time_point_t * time_point); + +/// Initialize a duration using the time_source. +/* 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 will allocate all necessary internal structures, and initialize variables. +* The time_source 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. +* \return RCL_RET_OK if the last call time was retrieved successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source); + +/// Finalize a duration +/* Finalize the duration such that it is ready for deallocation. +* +* This will deallocate all necessary internal structures, and clean up any variables. +* * +* \param[in] duration the handle to the duration which is being finalized. +* \return RCL_RET_OK if the last call time was retrieved successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_fini_duration(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 differnt 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 +* calbacks 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 +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* 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. +* The two time points must be using the same time abstraction.. +* And the resultant duration will also be of the same abstraction. +* +* The value will be computed as duration = finish - start. If start is after +* finish the duration will be negative. +* +* \param[in] start The time point for the start of the duration. +* \param[in] finish The time point for the end of the duration. +* \param[out] delta The duration between the start and finish. +* \return RCL_RET_OK if the difference was computed successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish, + rcl_duration_t * delta); + +/// Fill the time point with the current value of the associated clock. +/* This function will populate the data of the time_point object with the +* current value from it's associated time abstraction. +* +* \param[out] time_point The time_point to populate. +* \return RCL_RET_OK if the last call time was retrieved successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_get_time_point_now(rcl_time_point_t * time_point); + + +/// Enable the ros time abstraction override. +/* This method will enable the ros time abstraction override values, +* 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. +* \return RCL_RET_OK if the time source was enabled successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_enable_ros_time_override(rcl_time_source_t * time_source); + +/// Disable the ros time abstraction override. +/* This method will disable the RCL_ROS_TIME time abstraction override values, +* 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. +* \return RCL_RET_OK if the time source was disabled successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_disable_ros_time_override(rcl_time_source_t * time_source); + + +/// Check if the RCL_ROS_TIME time source has the override enabled. +/* This will populate the is_enabled object to indicate if the +* 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[out] is_enabled Whether the override is enabled.. +* \return RCL_RET_OK if the time source was queried successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source, + bool * is_enabled); + +/// Set the current time for this RCL_ROS_TIME time source. +/* This function will update the internal storage for the RCL_ROS_TIME time source. +* 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] time_value The new current time. +* \return RCL_RET_OK if the time source was set successfully, or +* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or +* RCL_RET_ERROR an unspecified error occur. +*/ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_set_ros_time_override(rcl_time_source_t * time_source, + rcl_time_point_value_t time_value); + +/// Retrieve the current time as a rcl_time_point_value_t (an alias for unint64_t). /* This function returns the time from a system clock. * The closest equivalent would be to std::chrono::system_clock::now(); * @@ -88,7 +412,10 @@ typedef struct rcl_duration_t * On Windows this is lock-free if the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * - * \param[out] now a struct in which the current time is stored + * TODO(tfoote) I talked with @wjwwood about possibly promoting this + * method into rmw for more reuse. + * + * \param[out] now a datafield in which the current time is stored * \return RCL_RET_OK if the current time was successfully obtained, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. @@ -96,15 +423,15 @@ typedef struct rcl_duration_t RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_system_time_point_now(rcl_system_time_point_t * now); +rcl_system_time_now(rcl_time_point_value_t * now); -/// Retrieve the current time as a rcl_steady_time_point_t struct. +/// Retrieve the current time as a rcl_time_point_value_t object.. /* This function returns the time from a monotonically increasing clock. * The closest equivalent would be to std::chrono::steady_clock::now(); * * The resolution (e.g. nanoseconds vs microseconds) is not guaranteed. * - * The now argument must point to an allocated rcl_steady_time_point_t struct, + * The now argument must point to an allocated rcl_time_point_value_t object, * as the result is copied into this variable. * * This function may allocate heap memory when an error occurs. @@ -113,6 +440,9 @@ rcl_system_time_point_now(rcl_system_time_point_t * now); * On Windows this is lock-free if the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * + * TODO(tfoote) I talked with @wjwwood about possibly promoting this + * method into rmw for more reuse. + * * \param[out] now a struct in which the current time is stored * \return RCL_RET_OK if the current time was successfully obtained, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or @@ -121,7 +451,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_steady_time_point_now(rcl_steady_time_point_t * now); +rcl_steady_time_now(rcl_time_point_value_t * now); #if __cplusplus } diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index 7226b2c..e233562 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -12,8 +12,315 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #if defined(WIN32) #include "./time_win32.c" #else // defined(WIN32) #include "./time_unix.c" #endif // defined(WIN32) + +#include "./stdatomic_helper.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 +{ + atomic_uint_least64_t current_time; + bool active; + // TODO(tfoote): store subscription here +} rcl_ros_time_source_storage_t; + +// Implementation only +rcl_ret_t rcl_get_steady_time(void * data, rcl_time_point_value_t * current_time) +{ + (void) data; // unused + return rcl_steady_time_now(current_time); +} + +// Implementation only +rcl_ret_t rcl_get_system_time(void * data, rcl_time_point_value_t * current_time) +{ + (void) data; // unused + return rcl_system_time_now(current_time); +} + +// Internal method for zeroing values on init, assumes time_source is valid +void rcl_init_generic_time_source(rcl_time_source_t * time_source) +{ + 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; +} + +// The function used to get the current ros time. +// This is in the implementation only +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; + if (!t->active) { + return rcl_get_system_time(data, current_time); + } + *current_time = rcl_atomic_load_uint64_t(&(t->current_time)); + return RCL_RET_OK; +} + +bool rcl_time_source_valid(rcl_time_source_t * time_source) +{ + if (time_source == NULL || + time_source->type == RCL_TIME_SOURCE_UNINITIALIZED || + time_source->get_now == NULL) + { + return false; + } + return true; +} + +rcl_ret_t rcl_init_ros_time_source(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + 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; + return RCL_RET_OK; +} + +rcl_ret_t rcl_fini_ros_time_source(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + if (time_source->type != RCL_ROS_TIME) { + RCL_SET_ERROR_MSG("time_source not of type RCL_ROS_TIME"); + return RCL_RET_ERROR; + } + free((rcl_ros_time_source_storage_t *)time_source->data); + return RCL_RET_OK; +} + +rcl_ret_t rcl_init_steady_time_source(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + rcl_init_generic_time_source(time_source); + time_source->get_now = rcl_get_steady_time; + time_source->type = RCL_STEADY_TIME; + return RCL_RET_OK; +} + +rcl_ret_t rcl_fini_steady_time_source(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + if (time_source->type != RCL_STEADY_TIME) { + RCL_SET_ERROR_MSG("time_source not of type RCL_STEADY_TIME"); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t rcl_init_system_time_source(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + rcl_init_generic_time_source(time_source); + time_source->get_now = rcl_get_system_time; + time_source->type = RCL_SYSTEM_TIME; + if (!time_source) { + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t rcl_fini_system_time_source(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + if (time_source->type != RCL_SYSTEM_TIME) { + RCL_SET_ERROR_MSG("time_source not of type RCL_SYSTEM_TIME"); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT); + if (!time_source) { + time_point->time_source = rcl_get_default_ros_time_source(); + return RCL_RET_OK; + } + time_point->time_source = time_source; + + return RCL_RET_OK; +} + +rcl_ret_t rcl_fini_time_point(rcl_time_point_t * time_point) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT); + (void) time_point; + return RCL_RET_OK; +} + +rcl_ret_t rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT); + if (!time_source) { + duration->time_source = rcl_get_default_ros_time_source(); + return RCL_RET_OK; + } + duration->time_source = time_source; + + return RCL_RET_OK; +} + +rcl_ret_t rcl_fini_duration(rcl_duration_t * duration) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT); + (void) 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 = calloc(1, sizeof(rcl_time_source_t)); + rcl_ret_t retval = rcl_init_ros_time_source(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 = calloc(1, sizeof(rcl_time_source_t)); + rcl_ret_t retval = rcl_init_steady_time_source(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 = calloc(1, sizeof(rcl_time_source_t)); + rcl_ret_t retval = rcl_init_system_time_source(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); + 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) { + RCL_SET_ERROR_MSG("Cannot difference between time points with time_sources types."); + return RCL_RET_ERROR; + } + if (finish->nanoseconds < start->nanoseconds) { + rcl_time_point_value_t intermediate = start->nanoseconds - finish->nanoseconds; + delta->nanoseconds = -1 * (int) intermediate; + } + delta->nanoseconds = (int)(finish->nanoseconds - start->nanoseconds); + return RCL_RET_OK; +} + +rcl_ret_t rcl_get_time_point_now(rcl_time_point_t * time_point) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT); + if (time_point->time_source && time_point->time_source->get_now) { + return time_point->time_source->get_now(time_point->time_source->data, + &(time_point->nanoseconds)); + } + RCL_SET_ERROR_MSG("time_source is not initialized or does not have get_now registered."); + return RCL_RET_ERROR; +} + +rcl_ret_t rcl_enable_ros_time_override(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + if (time_source->type != RCL_ROS_TIME) { + RCL_SET_ERROR_MSG("Time source is not RCL_ROS_TIME cannot enable override.") + return RCL_RET_ERROR; + } + rcl_ros_time_source_storage_t * storage = \ + (rcl_ros_time_source_storage_t *)time_source->data; + if (!storage) { + RCL_SET_ERROR_MSG("Storage not initialized, cannot enable.") + return RCL_RET_ERROR; + } + storage->active = true; + return RCL_RET_OK; +} + +rcl_ret_t rcl_disable_ros_time_override(rcl_time_source_t * time_source) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + if (time_source->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) { + RCL_SET_ERROR_MSG("Storage not initialized, cannot disable.") + return RCL_RET_ERROR; + } + storage->active = false; + return RCL_RET_OK; +} + +rcl_ret_t rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source, + bool * is_enabled) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_enabled, RCL_RET_INVALID_ARGUMENT); + if (time_source->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) { + RCL_SET_ERROR_MSG("Storage not initialized, cannot query.") + return RCL_RET_ERROR; + } + *is_enabled = storage->active; + return RCL_RET_OK; +} + +rcl_ret_t rcl_set_ros_time_override(rcl_time_source_t * time_source, + rcl_time_point_value_t time_value) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); + if (time_source->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_atomic_store(&(storage->current_time), time_value); + if (storage->active && time_source->post_update) { + time_source->post_update(); + } + return RCL_RET_OK; +} diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c index fd6cbc4..0191f29 100644 --- a/rcl/src/rcl/time_unix.c +++ b/rcl/src/rcl/time_unix.c @@ -45,7 +45,7 @@ extern "C" #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) rcl_ret_t -rcl_system_time_point_now(rcl_system_time_point_t * now) +rcl_system_time_now(rcl_time_point_value_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); struct timespec timespec_now; @@ -66,12 +66,12 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) RCL_SET_ERROR_MSG("unexpected negative time"); return RCL_RET_ERROR; } - now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; + *now = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; return RCL_RET_OK; } rcl_ret_t -rcl_steady_time_point_now(rcl_steady_time_point_t * now) +rcl_steady_time_now(rcl_time_point_value_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); // If clock_gettime is available or on OS X, use a timespec. @@ -97,7 +97,7 @@ rcl_steady_time_point_now(rcl_steady_time_point_t * now) RCL_SET_ERROR_MSG("unexpected negative time"); return RCL_RET_ERROR; } - now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; + *now = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; return RCL_RET_OK; } diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 919cf46..2fcd293 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -30,7 +30,7 @@ extern "C" #include "rcl/error_handling.h" rcl_ret_t -rcl_system_time_point_now(rcl_system_time_point_t * now) +rcl_system_time_now(rcl_time_point_value_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); FILETIME ft; @@ -42,12 +42,12 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) // https://support.microsoft.com/en-us/kb/167296 uli.QuadPart -= 116444736000000000; // Convert to nanoseconds from 100's of nanoseconds. - now->nanoseconds = uli.QuadPart * 100; + *now = uli.QuadPart * 100; return RCL_RET_OK; } rcl_ret_t -rcl_steady_time_point_now(rcl_steady_time_point_t * now) +rcl_steady_time_now(rcl_time_point_value_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); LARGE_INTEGER cpu_frequency, performance_count; @@ -59,8 +59,8 @@ rcl_steady_time_point_now(rcl_steady_time_point_t * now) QueryPerformanceFrequency(&cpu_frequency); QueryPerformanceCounter(&performance_count); // Convert to nanoseconds before converting from ticks to avoid precision loss. - now->nanoseconds = RCL_S_TO_NS(performance_count.QuadPart); - now->nanoseconds /= cpu_frequency.QuadPart; + rcl_time_point_value_t intermediate = RCL_S_TO_NS(performance_count.QuadPart); + *now = intermediate / cpu_frequency.QuadPart; return RCL_RET_OK; } diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 30d73c9..2423abe 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -56,15 +56,15 @@ rcl_timer_init( RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } - rcl_steady_time_point_t now_steady; - rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); + rcl_time_point_value_t now_steady; + rcl_ret_t now_ret = rcl_steady_time_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } rcl_timer_impl_t impl = { .callback = ATOMIC_VAR_INIT((uintptr_t)callback), .period = ATOMIC_VAR_INIT(period), - .last_call_time = ATOMIC_VAR_INIT(now_steady.nanoseconds), + .last_call_time = ATOMIC_VAR_INIT(now_steady), .canceled = ATOMIC_VAR_INIT(false), .allocator = allocator, }; @@ -100,14 +100,14 @@ rcl_timer_call(rcl_timer_t * timer) RCL_SET_ERROR_MSG("timer is canceled"); return RCL_RET_TIMER_CANCELED; } - rcl_steady_time_point_t now_steady; - rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); + rcl_time_point_value_t now_steady; + rcl_ret_t now_ret = rcl_steady_time_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - uint64_t previous_ns = - rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady.nanoseconds); - uint64_t since_last_call = now_steady.nanoseconds - previous_ns; + rcl_time_point_value_t previous_ns = + rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady); + uint64_t since_last_call = now_steady - previous_ns; rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback); typed_callback(timer, since_last_call); @@ -135,30 +135,32 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - rcl_steady_time_point_t now; - rcl_ret_t ret = rcl_steady_time_point_now(&now); + rcl_time_point_value_t now; + rcl_ret_t ret = rcl_steady_time_now(&now); if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); *time_until_next_call = - (rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now.nanoseconds; + (rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now; return RCL_RET_OK; } rcl_ret_t -rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call) +rcl_timer_get_time_since_last_call( + const rcl_timer_t * timer, + rcl_time_point_value_t * time_since_last_call) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - rcl_steady_time_point_t now; - rcl_ret_t ret = rcl_steady_time_point_now(&now); + rcl_time_point_value_t now; + rcl_ret_t ret = rcl_steady_time_now(&now); if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } *time_since_last_call = - now.nanoseconds - rcl_atomic_load_uint64_t(&timer->impl->last_call_time); + now - rcl_atomic_load_uint64_t(&timer->impl->last_call_time); return RCL_RET_OK; } @@ -224,12 +226,12 @@ rcl_timer_reset(rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - rcl_steady_time_point_t now; - rcl_ret_t now_ret = rcl_steady_time_point_now(&now); + rcl_time_point_value_t now; + rcl_ret_t now_ret = rcl_steady_time_now(&now); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - rcl_atomic_store(&timer->impl->last_call_time, now.nanoseconds); + rcl_atomic_store(&timer->impl->last_call_time, now); rcl_atomic_store(&timer->impl->canceled, false); return RCL_RET_OK; } diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 1acdd4f..1fda410 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -47,74 +47,334 @@ public: } }; -/* Tests the rcl_system_time_point_now() function. - */ -TEST_F(TestTimeFixture, test_rcl_system_time_point_now) { +// Tests the rcl_system_time_now() function. +TEST_F(TestTimeFixture, test_rcl_system_time_now) { assert_no_realloc_begin(); rcl_ret_t ret; // Check for invalid argument error condition (allowed to alloc). - ret = rcl_system_time_point_now(nullptr); + ret = rcl_system_time_now(nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); rcl_reset_error(); assert_no_malloc_begin(); assert_no_free_begin(); // Check for normal operation (not allowed to alloc). - rcl_system_time_point_t now = {0}; - ret = rcl_system_time_point_now(&now); + rcl_time_point_value_t now = 0; + ret = rcl_system_time_now(&now); assert_no_malloc_end(); assert_no_realloc_end(); assert_no_free_end(); stop_memory_checking(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); - EXPECT_NE(now.nanoseconds, 0u); + EXPECT_NE(now, 0u); // Compare to std::chrono::system_clock time (within a second). - now = {0}; - ret = rcl_system_time_point_now(&now); + now = 0; + ret = rcl_system_time_now(&now); { std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); int64_t now_ns_int = now_ns.count(); - int64_t now_diff = now.nanoseconds - now_ns_int; + int64_t now_diff = now - now_ns_int; const int k_tolerance_ms = 1000; EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs"; } } -/* Tests the rcl_steady_time_point_now() function. - */ -TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) { +// Tests the rcl_steady_time_now() function. +TEST_F(TestTimeFixture, test_rcl_steady_time_now) { assert_no_realloc_begin(); rcl_ret_t ret; // Check for invalid argument error condition (allowed to alloc). - ret = rcl_steady_time_point_now(nullptr); + ret = rcl_steady_time_now(nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); rcl_reset_error(); assert_no_malloc_begin(); assert_no_free_begin(); // Check for normal operation (not allowed to alloc). - rcl_steady_time_point_t now = {0}; - ret = rcl_steady_time_point_now(&now); + rcl_time_point_value_t now = 0; + ret = rcl_steady_time_now(&now); assert_no_malloc_end(); assert_no_realloc_end(); assert_no_free_end(); stop_memory_checking(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); - EXPECT_NE(now.nanoseconds, 0u); + EXPECT_NE(now, 0u); // Compare to std::chrono::steady_clock difference of two times (within a second). - now = {0}; - ret = rcl_steady_time_point_now(&now); + now = 0; + ret = rcl_steady_time_now(&now); std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); // Wait for a little while. std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Then take a new timestamp with each and compare. - rcl_steady_time_point_t later; - ret = rcl_steady_time_point_now(&later); + rcl_time_point_value_t later; + ret = rcl_steady_time_now(&later); std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); - int64_t steady_diff = later.nanoseconds - now.nanoseconds; + int64_t steady_diff = later - now; int64_t sc_diff = std::chrono::duration_cast(later_sc - now_sc).count(); const int k_tolerance_ms = 1; EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs"; } + +// Tests the rcl_set_ros_time_override() function. +TEST_F(TestTimeFixture, test_rcl_set_ros_time_override) { + rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source(); + assert_no_realloc_begin(); + rcl_ret_t ret; + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_set_ros_time_override(nullptr, 0); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); + rcl_reset_error(); + bool result; + 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); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); + rcl_reset_error(); + ret = rcl_is_enabled_ros_time_override(nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); + rcl_reset_error(); + rcl_time_point_t query_now; + bool is_enabled; + ret = rcl_is_enabled_ros_time_override(ros_time_source, &is_enabled); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_EQ(is_enabled, false); + ret = rcl_init_time_point(&query_now, ros_time_source); + 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_get_time_point_now(&query_now); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_NE(query_now.nanoseconds, 0); + // Compare to std::chrono::system_clock time (within a second). + ret = rcl_get_time_point_now(&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(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = query_now.nanoseconds - now_ns_int; + const int k_tolerance_ms = 1000; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs"; + } + // 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); + 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); + 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_EQ(is_enabled, false); + // get real + ret = rcl_get_time_point_now(&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(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = query_now.nanoseconds - now_ns_int; + const int k_tolerance_ms = 1000; + 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); + 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_EQ(is_enabled, true); + // get sim + ret = rcl_get_time_point_now(&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); + 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_EQ(is_enabled, false); + // get real + ret = rcl_get_time_point_now(&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(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = query_now.nanoseconds - now_ns_int; + const int k_tolerance_ms = 1000; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs"; + } +} + +TEST_F(TestTimeFixture, test_rcl_init_time_source_and_point) { + assert_no_realloc_begin(); + rcl_ret_t ret; + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_init_ros_time_source(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_init_ros_time_source(&source); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + rcl_time_point_t a_time; + ret = rcl_init_time_point(&a_time, &source); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + rcl_time_point_t default_time; + ret = rcl_init_time_point(&default_time, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + // assert_no_malloc_begin(); + // assert_no_free_begin(); + // // Do stuff in here + // assert_no_malloc_end(); + // assert_no_realloc_end(); + // assert_no_free_end(); + // stop_memory_checking(); + // EXPECT_NE(now.nanoseconds, 0u); + // // Compare to std::chrono::system_clock time (within a second). + // now = {0}; + // ret = rcl_system_time_now(&now); + // { + // std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); + // auto now_ns = std::chrono::duration_cast( + // now_sc.time_since_epoch()); + // int64_t now_ns_int = now_ns.count(); + // int64_t now_diff = now.nanoseconds - now_ns_int; + // const int k_tolerance_ms = 1000; + // EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs"; + // } + ret = rcl_fini_time_point(&a_time); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + ret = rcl_fini_ros_time_source(&source); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); +} + +TEST(rcl_time, time_source_validation) { + ASSERT_FALSE(rcl_time_source_valid(NULL)); + rcl_time_source_t uninitialized; + // Not reliably detectable due to random values. + // ASSERT_FALSE(rcl_time_source_valid(&uninitialized)); + rcl_ret_t ret; + ret = rcl_init_ros_time_source(&uninitialized); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); +} + +TEST(rcl_time, 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(rcl_time, rcl_time_difference) { + rcl_ret_t ret; + rcl_time_point_t a, b; + ret = rcl_init_time_point(&a, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + ret = rcl_init_time_point(&b, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + a.nanoseconds = 1000; + b.nanoseconds = 2000; + + rcl_duration_t d; + ret = rcl_init_duration(&d, nullptr); + 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); + + 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); + + rcl_time_point_t e; + ret = rcl_init_time_point(&e, system_time_source); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); +} + +static bool pre_callback_called = false; +static bool post_callback_called = false; + +void pre_callback(void) +{ + pre_callback_called = true; + EXPECT_FALSE(post_callback_called); +} +void post_callback(void) +{ + EXPECT_TRUE(pre_callback_called); + post_callback_called = true; +} + + +TEST(rcl_time, rcl_time_update_callbacks) { + rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source(); + rcl_time_point_t query_now; + rcl_ret_t ret; + rcl_time_point_value_t set_point = 1000000000ull; + + ret = rcl_init_time_point(&query_now, ros_time_source); + 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; + + + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // Query it to do something different. no changes expected + ret = rcl_get_time_point_now(&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); + 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); + 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + EXPECT_TRUE(pre_callback_called); + EXPECT_TRUE(post_callback_called); +}