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

Merge pull request #12 from ros2/ros_time

Browse source 
first work toward adding ros time support
This commit is contained in:
Tully Foote 2016-02-12 08:06:29 +00:00
commit aaa773eb8b
6 changed files with 990 additions and 91 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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
}

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

@ -12,8 +12,315 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stdlib.h>
#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;
}

8
rcl/src/rcl/time_unix.c
View file

@ -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;
}

10
rcl/src/rcl/time_win32.c
View file

@ -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;
}

38
rcl/src/rcl/timer.c
View file

@ -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;
}

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

@ -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<std::chrono::nanoseconds>(now_sc.time_since_epoch());
int64_t now_ns_int = now_ns.count();
int64_t now_diff = now.nanoseconds - now_ns_int;
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<std::chrono::nanoseconds>(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<std::chrono::nanoseconds>(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<std::chrono::nanoseconds>(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<std::chrono::nanoseconds>(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<std::chrono::nanoseconds>(
// now_sc.time_since_epoch());
// int64_t now_ns_int = now_ns.count();
// int64_t now_diff = now.nanoseconds - now_ns_int;
// const int k_tolerance_ms = 1000;
// EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs";
// }
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);
}