diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index c70e5d662..410b2b699 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -325,8 +325,23 @@ rcl_lifecycle_com_interface_fini( rcl_ret_t rcl_lifecycle_com_interface_publish_notification( 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) { + // 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; 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..b4704bcc1 100644 --- a/rcl_lifecycle/src/com_interface.h +++ b/rcl_lifecycle/src/com_interface.h @@ -79,6 +79,7 @@ rcl_ret_t RCL_WARN_UNUSED rcl_lifecycle_com_interface_publish_notification( 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); #ifdef __cplusplus diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index e9e5be06d..d500e8ec4 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, 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();