Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Minor fixes to rcl clock implementation. #688

Merged
merged 1 commit into from
Jun 17, 2020
Merged

Minor fixes to rcl clock implementation. #688

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 15 additions & 25 deletions rcl/src/rcl/time.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,11 +171,8 @@ rcl_ros_clock_fini(
return RCL_RET_ERROR;
}
rcl_clock_generic_fini(clock);
if (!clock->data) {
RCL_SET_ERROR_MSG("clock data invalid");
return RCL_RET_ERROR;
}
clock->allocator.deallocate((rcl_ros_clock_storage_t *)clock->data, clock->allocator.state);
clock->allocator.deallocate(clock->data, clock->allocator.state);
clock->data = NULL;
return RCL_RET_OK;
}

Expand Down Expand Up @@ -293,10 +290,8 @@ rcl_enable_ros_time_override(rcl_clock_t * clock)
return RCL_RET_ERROR;
}
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
if (!storage) {
RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot enable override.");
return RCL_RET_ERROR;
}
RCL_CHECK_FOR_NULL_WITH_MSG(
storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR);
if (!storage->active) {
rcl_time_jump_t time_jump;
time_jump.delta.nanoseconds = 0;
Expand All @@ -316,12 +311,9 @@ rcl_disable_ros_time_override(rcl_clock_t * clock)
RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot disable override.");
return RCL_RET_ERROR;
}
rcl_ros_clock_storage_t * storage = \
(rcl_ros_clock_storage_t *)clock->data;
if (!storage) {
RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot disable override.");
return RCL_RET_ERROR;
}
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
RCL_CHECK_FOR_NULL_WITH_MSG(
storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR);
if (storage->active) {
rcl_time_jump_t time_jump;
time_jump.delta.nanoseconds = 0;
Expand All @@ -344,12 +336,9 @@ rcl_is_enabled_ros_time_override(
RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot query override state.");
return RCL_RET_ERROR;
}
rcl_ros_clock_storage_t * storage = \
(rcl_ros_clock_storage_t *)clock->data;
if (!storage) {
RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot query override state.");
return RCL_RET_ERROR;
}
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
RCL_CHECK_FOR_NULL_WITH_MSG(
storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR);
*is_enabled = storage->active;
return RCL_RET_OK;
}
Expand All @@ -364,8 +353,10 @@ rcl_set_ros_time_override(
RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot set time override.");
return RCL_RET_ERROR;
}
rcl_time_jump_t time_jump;
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
RCL_CHECK_FOR_NULL_WITH_MSG(
storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR);
rcl_time_jump_t time_jump;
if (storage->active) {
time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE;
rcl_time_point_value_t current_time;
Expand Down Expand Up @@ -453,19 +444,18 @@ rcl_clock_remove_jump_callback(
}

// Shrink size of the callback array
if (clock->num_jump_callbacks == 1) {
if (--(clock->num_jump_callbacks) == 0) {
clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state);
clock->jump_callbacks = NULL;
} else {
rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate(
clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * (clock->num_jump_callbacks - 1),
clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * clock->num_jump_callbacks,
clock->allocator.state);
if (NULL == callbacks) {
RCL_SET_ERROR_MSG("Failed to shrink jump callbacks");
return RCL_RET_BAD_ALLOC;
}
clock->jump_callbacks = callbacks;
}
--(clock->num_jump_callbacks);
return RCL_RET_OK;
}