From 012c120bad0e56a8da83e53ec07d4b3a1c635b60 Mon Sep 17 00:00:00 2001
From: Michel Hidalgo <michel@ekumenlabs.com>
Date: Mon, 15 Jun 2020 13:01:31 -0300
Subject: [PATCH] Minor fixes to rcl clock implementation.

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
---
 rcl/src/rcl/time.c | 40 +++++++++++++++-------------------------
 1 file changed, 15 insertions(+), 25 deletions(-)

diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c
index 3b9174356..21ed84812 100644
--- a/rcl/src/rcl/time.c
+++ b/rcl/src/rcl/time.c
@@ -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;
 }
 
@@ -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;
@@ -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;
@@ -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;
 }
@@ -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;
@@ -453,12 +444,12 @@ 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");
@@ -466,6 +457,5 @@ rcl_clock_remove_jump_callback(
     }
     clock->jump_callbacks = callbacks;
   }
-  --(clock->num_jump_callbacks);
   return RCL_RET_OK;
 }