diff --git a/rclc_examples/src/example_lifecycle_node.c b/rclc_examples/src/example_lifecycle_node.c index abb6cab0..840e6da6 100644 --- a/rclc_examples/src/example_lifecycle_node.c +++ b/rclc_examples/src/example_lifecycle_node.c @@ -87,6 +87,7 @@ int main(int argc, const char * argv[]) rc = rclc_make_node_a_lifecycle_node( &lifecycle_node, &my_node, + &support.clock, &state_machine_, &allocator, true); diff --git a/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h b/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h index 546e2c0a..87e36791 100644 --- a/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h +++ b/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h @@ -108,6 +108,7 @@ rcl_ret_t rclc_make_node_a_lifecycle_node( rclc_lifecycle_node_t * lifecycle_node, rcl_node_t * node, + rcl_clock_t * clock, rcl_lifecycle_state_machine_t * state_machine, rcl_allocator_t * allocator, bool enable_communication_interface); diff --git a/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c b/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c index 8bd9aac4..cd124802 100644 --- a/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c +++ b/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c @@ -38,6 +38,7 @@ rcl_ret_t rclc_make_node_a_lifecycle_node( rclc_lifecycle_node_t * lifecycle_node, rcl_node_t * node, + rcl_clock_t * clock, rcl_lifecycle_state_machine_t * state_machine, rcl_allocator_t * allocator, bool enable_communication_interface @@ -47,6 +48,8 @@ rclc_make_node_a_lifecycle_node( lifecycle_node, "lifecycle_node is a null pointer", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + clock, "clock is a null pointer", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( allocator, "allocator is a null pointer", return RCL_RET_INVALID_ARGUMENT); @@ -61,6 +64,7 @@ rclc_make_node_a_lifecycle_node( rcl_ret_t rcl_ret = rcl_lifecycle_state_machine_init( state_machine, node, + clock, ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState), ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetState), diff --git a/rclc_lifecycle/test/test_lifecycle.cpp b/rclc_lifecycle/test/test_lifecycle.cpp index 2e006acd..79fcf1e7 100644 --- a/rclc_lifecycle/test/test_lifecycle.cpp +++ b/rclc_lifecycle/test/test_lifecycle.cpp @@ -64,12 +64,16 @@ TEST(TestRclcLifecycle, lifecycle_node) { rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + rcl_clock_t my_clock; + res += rcl_clock_init(RCL_SYSTEM_TIME, &my_clock, &allocator); + rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); res += rclc_make_node_a_lifecycle_node( &lifecycle_node, &my_node, + &my_clock, &state_machine, &allocator, true); @@ -81,6 +85,8 @@ TEST(TestRclcLifecycle, lifecycle_node) { rcl_lifecycle_state_machine_is_initialized(lifecycle_node.state_machine)); // clean up + res = rcl_clock_fini(&my_clock); + EXPECT_EQ(RCL_RET_OK, res); res = rcl_node_fini(&my_node); EXPECT_EQ(RCL_RET_OK, res); res = rcl_node_options_fini(&node_ops); @@ -101,12 +107,16 @@ TEST(TestRclcLifecycle, lifecycle_node_transitions) { rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + rcl_clock_t my_clock; + res += rcl_clock_init(RCL_ROS_TIME, &my_clock, &allocator); + rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); res += rclc_make_node_a_lifecycle_node( &lifecycle_node, &my_node, + &my_clock, &state_machine, &allocator, false); @@ -151,6 +161,8 @@ TEST(TestRclcLifecycle, lifecycle_node_transitions) { lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED, lifecycle_node.state_machine->current_state->id); + res = rcl_clock_fini(&my_clock); + EXPECT_EQ(RCL_RET_OK, res); res = rcl_node_fini(&my_node); EXPECT_EQ(RCL_RET_OK, res); res = rcl_node_options_fini(&node_ops); @@ -171,12 +183,16 @@ TEST(TestRclcLifecycle, lifecycle_node_callbacks) { rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + rcl_clock_t my_clock; + res += rcl_clock_init(RCL_ROS_TIME, &my_clock, &allocator); + rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); res += rclc_make_node_a_lifecycle_node( &lifecycle_node, &my_node, + &my_clock, &state_machine, &allocator, true); @@ -216,6 +232,8 @@ TEST(TestRclcLifecycle, lifecycle_node_callbacks) { EXPECT_EQ(RCL_RET_OK, res); EXPECT_EQ(15, callback_mockup_counter); + res = rcl_clock_fini(&my_clock); + EXPECT_EQ(RCL_RET_OK, res); res = rcl_node_fini(&my_node); EXPECT_EQ(RCL_RET_OK, res); res = rcl_node_options_fini(&node_ops); @@ -236,12 +254,16 @@ TEST(TestRclcLifecycle, lifecycle_node_servers) { rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + rcl_clock_t my_clock; + res += rcl_clock_init(RCL_ROS_TIME, &my_clock, &allocator); + rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); res += rclc_make_node_a_lifecycle_node( &lifecycle_node, &my_node, + &my_clock, &state_machine, &allocator, true); @@ -286,6 +308,8 @@ TEST(TestRclcLifecycle, lifecycle_node_servers) { // Cleanup res = rclc_lifecycle_node_fini(&lifecycle_node, &allocator); EXPECT_EQ(RCL_RET_OK, res); + res = rcl_clock_fini(&my_clock); + EXPECT_EQ(RCL_RET_OK, res); res = rcl_node_fini(&my_node); EXPECT_EQ(RCL_RET_OK, res); res = rclc_executor_fini(&executor);