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

Fix logic that moves goal handles when one expires (#360)

Browse source
This commit is contained in:
Shane Loretz 2018-12-18 15:16:21 -08:00 • committed by GitHub
parent 5162a99685
commit 6b6c0fe81d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

14
rcl_action/src/rcl_action/action_server.c
View file

@ -641,12 +641,16 @@ rcl_action_expire_goals(
} }
goal_time = _goal_info_stamp_to_nanosec(info_ptr); goal_time = _goal_info_stamp_to_nanosec(info_ptr);
if ((current_time - goal_time) > timeout) { if ((current_time - goal_time) > timeout) {
// Stop tracking goal handle // Deallocate space used to store pointer to goal handle
// Fill in any gaps left in the array with pointers from the end allocator.deallocate(action_server->impl->goal_handles[i], allocator.state);
action_server->impl->goal_handles[i] = NULL;
// Move all pointers after backwards one to fill the gap
for (size_t post_i = i; (post_i + 1) < num_goal_handles; ++post_i) {
action_server->impl->goal_handles[post_i] = action_server->impl->goal_handles[post_i + 1];
}
// decrement i to check the same index again now that it has a new goal handle
--i;
--num_goal_handles; --num_goal_handles;
action_server->impl->goal_handles[i] = action_server->impl->goal_handles[num_goal_handles];
allocator.deallocate(action_server->impl->goal_handles[num_goal_handles], allocator.state);
action_server->impl->goal_handles[num_goal_handles] = NULL;
++num_goals_expired; ++num_goals_expired;
} }
} }