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

Minor fixes to rcl clock implementation. (#688)

Browse source 
Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
This commit is contained in:
Michel Hidalgo 2020-06-17 16:27:02 -03:00 • committed by Alejandro Hernández Cordero
parent c58dd7ab3d
commit a4aa379d35
1 changed files with 15 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -171,11 +171,8 @@ rcl_ros_clock_fini(
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_clock_generic_fini(clock); rcl_clock_generic_fini(clock);
if (!clock->data) { clock->allocator.deallocate(clock->data, clock->allocator.state);
RCL_SET_ERROR_MSG("clock data invalid"); clock->data = NULL;
return RCL_RET_ERROR;
}
clock->allocator.deallocate((rcl_ros_clock_storage_t *)clock->data, clock->allocator.state);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -293,10 +290,8 @@ rcl_enable_ros_time_override(rcl_clock_t * clock)
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
if (!storage) { RCL_CHECK_FOR_NULL_WITH_MSG(
RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot enable override."); storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR);
return RCL_RET_ERROR;
}
if (!storage->active) { if (!storage->active) {
rcl_time_jump_t time_jump; rcl_time_jump_t time_jump;
time_jump.delta.nanoseconds = 0; time_jump.delta.nanoseconds = 0;
@ -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."); RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot disable override.");
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_ros_clock_storage_t * storage = \ rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
(rcl_ros_clock_storage_t *)clock->data; RCL_CHECK_FOR_NULL_WITH_MSG(
if (!storage) { storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR);
RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot disable override.");
return RCL_RET_ERROR;
}
if (storage->active) { if (storage->active) {
rcl_time_jump_t time_jump; rcl_time_jump_t time_jump;
time_jump.delta.nanoseconds = 0; time_jump.delta.nanoseconds = 0;
@ -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."); RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot query override state.");
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_ros_clock_storage_t * storage = \ rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
(rcl_ros_clock_storage_t *)clock->data; RCL_CHECK_FOR_NULL_WITH_MSG(
if (!storage) { storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR);
RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot query override state.");
return RCL_RET_ERROR;
}
*is_enabled = storage->active; *is_enabled = storage->active;
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -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."); RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot set time override.");
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
rcl_time_jump_t time_jump;
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; 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) { if (storage->active) {
time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE; time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE;
rcl_time_point_value_t current_time; rcl_time_point_value_t current_time;
@ -453,12 +444,12 @@ rcl_clock_remove_jump_callback(
} }
// Shrink size of the callback array // 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->allocator.deallocate(clock->jump_callbacks, clock->allocator.state);
clock->jump_callbacks = NULL; clock->jump_callbacks = NULL;
} else { } else {
rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate( 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); clock->allocator.state);
if (NULL == callbacks) { if (NULL == callbacks) {
RCL_SET_ERROR_MSG("Failed to shrink jump callbacks"); RCL_SET_ERROR_MSG("Failed to shrink jump callbacks");
@ -466,6 +457,5 @@ rcl_clock_remove_jump_callback(
} }
clock->jump_callbacks = callbacks; clock->jump_callbacks = callbacks;
} }
--(clock->num_jump_callbacks);
return RCL_RET_OK; return RCL_RET_OK;
} }