From f26a78d2e7c7d7163095e649b7be124a32a0e4a2 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Mon, 25 Mar 2024 23:54:12 -0400 Subject: [PATCH 1/3] Populate Transitions Signed-off-by: CursedRock17 --- rcl_lifecycle/src/com_interface.c | 6 +++++- rcl_lifecycle/src/com_interface.h | 4 +++- rcl_lifecycle/src/rcl_lifecycle.c | 3 ++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index c70e5d662..f90c6fcd6 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -324,9 +324,13 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t rcl_lifecycle_com_interface_publish_notification( - rcl_lifecycle_com_interface_t * com_interface, + rcl_lifecycle_com_interface_t * com_interface, unsigned int timestamp, + const char * transition_label, uint8_t transition_id, const rcl_lifecycle_state_t * start, const rcl_lifecycle_state_t * goal) { + com_interface->msg.timestamp = timestamp; + com_interface->msg.transition.id = transition_id; + rosidl_runtime_c__String__assign(&com_interface->msg.transition.label, transition_label); com_interface->msg.start_state.id = start->id; rosidl_runtime_c__String__assign(&com_interface->msg.start_state.label, start->label); com_interface->msg.goal_state.id = goal->id; diff --git a/rcl_lifecycle/src/com_interface.h b/rcl_lifecycle/src/com_interface.h index 2fa0c146a..faeadf2e1 100644 --- a/rcl_lifecycle/src/com_interface.h +++ b/rcl_lifecycle/src/com_interface.h @@ -21,6 +21,7 @@ extern "C" #endif #include "rcl/macros.h" +#include #include "rcl_lifecycle/data_types.h" @@ -78,7 +79,8 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t RCL_WARN_UNUSED rcl_lifecycle_com_interface_publish_notification( - rcl_lifecycle_com_interface_t * com_interface, + rcl_lifecycle_com_interface_t * com_interface, unsigned int timestamp, + const char * transition_label, uint8_t transition_id, const rcl_lifecycle_state_t * start, const rcl_lifecycle_state_t * goal); #ifdef __cplusplus diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index e9e5be06d..d4be84fbd 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -361,7 +361,8 @@ _trigger_transition( if (publish_notification) { rcl_ret_t fcn_ret = rcl_lifecycle_com_interface_publish_notification( - &state_machine->com_interface, transition->start, state_machine->current_state); + &state_machine->com_interface, 250, transition->label, transition->id, + transition->start, state_machine->current_state); if (fcn_ret != RCL_RET_OK) { rcl_error_string_t error_string = rcl_get_error_string(); rcutils_reset_error(); From 15e01e3010a840b1e279bd2db91ca02e0aeb1471 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Tue, 26 Mar 2024 00:36:05 -0400 Subject: [PATCH 2/3] Suspending timestamp until I find an ansewr Signed-off-by: CursedRock17 --- rcl_lifecycle/src/com_interface.c | 4 ++-- rcl_lifecycle/src/com_interface.h | 3 +-- rcl_lifecycle/src/rcl_lifecycle.c | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index f90c6fcd6..65ced4079 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -324,11 +324,11 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t rcl_lifecycle_com_interface_publish_notification( - rcl_lifecycle_com_interface_t * com_interface, unsigned int timestamp, + rcl_lifecycle_com_interface_t * com_interface, const char * transition_label, uint8_t transition_id, const rcl_lifecycle_state_t * start, const rcl_lifecycle_state_t * goal) { - com_interface->msg.timestamp = timestamp; + com_interface->msg.timestamp = 0u; com_interface->msg.transition.id = transition_id; rosidl_runtime_c__String__assign(&com_interface->msg.transition.label, transition_label); com_interface->msg.start_state.id = start->id; diff --git a/rcl_lifecycle/src/com_interface.h b/rcl_lifecycle/src/com_interface.h index faeadf2e1..b4704bcc1 100644 --- a/rcl_lifecycle/src/com_interface.h +++ b/rcl_lifecycle/src/com_interface.h @@ -21,7 +21,6 @@ extern "C" #endif #include "rcl/macros.h" -#include #include "rcl_lifecycle/data_types.h" @@ -79,7 +78,7 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t RCL_WARN_UNUSED rcl_lifecycle_com_interface_publish_notification( - rcl_lifecycle_com_interface_t * com_interface, unsigned int timestamp, + rcl_lifecycle_com_interface_t * com_interface, const char * transition_label, uint8_t transition_id, const rcl_lifecycle_state_t * start, const rcl_lifecycle_state_t * goal); diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index d4be84fbd..d500e8ec4 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -361,7 +361,7 @@ _trigger_transition( if (publish_notification) { rcl_ret_t fcn_ret = rcl_lifecycle_com_interface_publish_notification( - &state_machine->com_interface, 250, transition->label, transition->id, + &state_machine->com_interface, transition->label, transition->id, transition->start, state_machine->current_state); if (fcn_ret != RCL_RET_OK) { rcl_error_string_t error_string = rcl_get_error_string(); From 1f7898a594d68900fb461f2d912dd20538bc21b8 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Wed, 27 Mar 2024 20:54:58 -0400 Subject: [PATCH 3/3] Adding timestamp Signed-off-by: CursedRock17 --- rcl_lifecycle/src/com_interface.c | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index 65ced4079..410b2b699 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -328,7 +328,18 @@ rcl_lifecycle_com_interface_publish_notification( const char * transition_label, uint8_t transition_id, const rcl_lifecycle_state_t * start, const rcl_lifecycle_state_t * goal) { - com_interface->msg.timestamp = 0u; + // Get the current system time based on the rcl_clock + rcutils_time_point_value_t current_time; + rcutils_ret_t time_ret = rcutils_system_time_now(¤t_time); + if (time_ret != RCUTILS_RET_OK) { + rcutils_error_string_t error = rcutils_get_error_string(); + rcutils_reset_error(); + RCL_SET_ERROR_MSG(error.str); + time_ret = RCL_RET_ERROR; + return time_ret; + } + + com_interface->msg.timestamp = current_time; com_interface->msg.transition.id = transition_id; rosidl_runtime_c__String__assign(&com_interface->msg.transition.label, transition_label); com_interface->msg.start_state.id = start->id;