diff --git a/rclc/CMakeLists.txt b/rclc/CMakeLists.txt index f5f60a8d..5c6bd6ba 100644 --- a/rclc/CMakeLists.txt +++ b/rclc/CMakeLists.txt @@ -34,12 +34,14 @@ find_package(rosidl_generator_c REQUIRED) find_package(std_msgs REQUIRED) if("${rcl_VERSION}" VERSION_LESS "1.0.0") - message(STATUS "Found rcl version ${rcl_VERSION}, which belongs to Dashing or Eloquent") + message(STATUS + "Found rcl version ${rcl_VERSION}, which belongs to Dashing or Eloquent") # Later, with CMake 3.12+ use: # add_compile_definitions(USE_RCL_WAIT_SET_IS_VALID_BACKPORT) add_definitions(-DUSE_RCL_WAIT_SET_IS_VALID_BACKPORT) else() - message(STATUS "Found rcl version ${rcl_VERSION}, which belongs to Foxy or later") + message(STATUS + "Found rcl version ${rcl_VERSION}, which belongs to Foxy or later") find_package(rosidl_runtime_c REQUIRED) endif() @@ -60,7 +62,8 @@ add_library(${PROJECT_NAME} src/rclc/sleep.c ) if("${rcl_VERSION}" VERSION_LESS "1.0.0") - target_sources(${PROJECT_NAME} PRIVATE src/rclc/rcl_wait_set_is_valid_backport.c) + target_sources(${PROJECT_NAME} + PRIVATE src/rclc/rcl_wait_set_is_valid_backport.c) endif() target_include_directories(${PROJECT_NAME} @@ -119,11 +122,13 @@ endif() if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) # No copyright header check since link to NOTICE file is not recognized properly. find_package(osrf_testing_tools_cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(example_interfaces REQUIRED) + # No copyright header check since link to NOTICE file is not recognized properly. + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() ament_add_gtest(${PROJECT_NAME}_test diff --git a/rclc_examples/src/example_client_node.c b/rclc_examples/src/example_client_node.c index ac78013b..e41c870c 100644 --- a/rclc_examples/src/example_client_node.c +++ b/rclc_examples/src/example_client_node.c @@ -47,7 +47,7 @@ int main(int argc, const char * const * argv) RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); // create node - rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_t node; RCCHECK(rclc_node_init_default(&node, "add_twoints_client_rclc", "", &support)); // create client diff --git a/rclc_examples/src/example_lifecycle_node.c b/rclc_examples/src/example_lifecycle_node.c index 69e4d8d3..ad86acd0 100644 --- a/rclc_examples/src/example_lifecycle_node.c +++ b/rclc_examples/src/example_lifecycle_node.c @@ -26,8 +26,15 @@ #include #include +#include + #include "rclc_lifecycle/rclc_lifecycle.h" +#define RCCHECK(fn) {rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) {printf( \ + "Failed status on line %d: %d. Aborting.\n", __LINE__, (int)temp_rc); return 1;}} +#define RCSOFTCHECK(fn) {rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) {printf( \ + "Failed status on line %d: %d. Continuing.\n", __LINE__, (int)temp_rc);}} + rcl_ret_t my_on_configure() { printf(" >>> lifecycle_node: on_configure() callback called.\n"); @@ -66,7 +73,7 @@ int main(int argc, const char * argv[]) } // create rcl_node - rcl_node_t my_node = rcl_get_zero_initialized_node(); + rcl_node_t my_node; rc = rclc_node_init_default(&my_node, "lifecycle_node", "rclc", &support); if (rc != RCL_RET_OK) { printf("Error in rclc_node_init_default\n"); @@ -74,7 +81,7 @@ int main(int argc, const char * argv[]) } // make it a lifecycle node - printf("make it a lifecycle node...\n"); + printf("creating lifecycle node...\n"); rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); rclc_lifecycle_node_t lifecycle_node; rc = rclc_make_node_a_lifecycle_node( @@ -88,62 +95,38 @@ int main(int argc, const char * argv[]) return -1; } - // register callbacks + // Executor + rclc_executor_t executor; + RCCHECK(rclc_executor_init( + &executor, + &support.context, + 4, // 1 for the node + 1 for each lifecycle service + &allocator)); + + unsigned int rcl_wait_timeout = 1000; // in ms + RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout))); + + // Register lifecycle services + printf("registering lifecycle services...\n"); + RCCHECK(rclc_lifecycle_init_get_state_server(&lifecycle_node, &executor)); + RCCHECK(rclc_lifecycle_init_get_available_states_server(&lifecycle_node, &executor)); + RCCHECK(rclc_lifecycle_init_change_state_server(&lifecycle_node, &executor)); + + // Register lifecycle service callbacks + printf("registering callbacks...\n"); rclc_lifecycle_register_on_configure(&lifecycle_node, &my_on_configure); + rclc_lifecycle_register_on_activate(&lifecycle_node, &my_on_activate); rclc_lifecycle_register_on_deactivate(&lifecycle_node, &my_on_deactivate); + rclc_lifecycle_register_on_cleanup(&lifecycle_node, &my_on_cleanup); - printf(" >configuring lifecycle node...\n"); - rc = rclc_lifecycle_change_state( - &lifecycle_node, - lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, - true); - if (rc != RCL_RET_OK) { - printf("Error in TRANSITION_CONFIGURE\n"); - return -1; - } - - printf(" >activating lifecycle node...\n"); - rc = rclc_lifecycle_change_state( - &lifecycle_node, - lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, - true); - if (rc != RCL_RET_OK) { - printf("Error in TRANSITION_ACTIVATE\n"); - return -1; - } - - printf(" >deactivating lifecycle node...\n"); - rc = rclc_lifecycle_change_state( - &lifecycle_node, - lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, - true); - if (rc != RCL_RET_OK) { - printf("Error in TRANSITION_DEACTIVATE\n"); - return -1; - } - - printf(" >cleaning rcl node up...\n"); - rc = rclc_lifecycle_change_state( - &lifecycle_node, - lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, - true); - if (rc != RCL_RET_OK) { - printf("Error in TRANSITION_CLEANUP\n"); - return -1; - } - - printf(" >destroying lifecycle node...\n"); - rc = rclc_lifecycle_change_state( - &lifecycle_node, - lifecycle_msgs__msg__Transition__TRANSITION_UNCONFIGURED_SHUTDOWN, - true); - if (rc != RCL_RET_OK) { - printf("Error in TRANSITION_UNCONFIGURED_SHUTDOWN\n"); - return -1; - } + // Run + RCSOFTCHECK(rclc_executor_spin(&executor)); + // Cleanup printf("cleaning up...\n"); - rc = rcl_lifecycle_node_fini(&lifecycle_node, &allocator); + rc = rclc_lifecycle_node_fini(&lifecycle_node, &allocator); + rc += rcl_node_fini(&my_node); + rc += rclc_executor_fini(&executor); rc += rclc_support_fini(&support); if (rc != RCL_RET_OK) { diff --git a/rclc_examples/src/example_service_node.c b/rclc_examples/src/example_service_node.c index 69b15c34..d32b1060 100644 --- a/rclc_examples/src/example_service_node.c +++ b/rclc_examples/src/example_service_node.c @@ -54,7 +54,7 @@ int main(int argc, const char * const * argv) RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); // create node - rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_t node; RCCHECK(rclc_node_init_default(&node, "add_twoints_client_rclc", "", &support)); // create service @@ -68,7 +68,7 @@ int main(int argc, const char * const * argv) rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); - unsigned int rcl_wait_timeout = 10; // in ms + unsigned int rcl_wait_timeout = 1000; // in ms RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout))); RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback)); diff --git a/rclc_lifecycle/CMakeLists.txt b/rclc_lifecycle/CMakeLists.txt index 71129ee6..e759b747 100644 --- a/rclc_lifecycle/CMakeLists.txt +++ b/rclc_lifecycle/CMakeLists.txt @@ -6,8 +6,13 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() +# maximum string length - should cover all lifecycle state and transition names +set(RCLC_LIFECYCLE_MAX_STRING_LENGTH 20 CACHE STRING "Set the maximum length for strings.") +add_definitions(-DRCLC_LIFECYCLE_MAX_STRING_LENGTH=${RCLC_LIFECYCLE_MAX_STRING_LENGTH}) + # find dependencies find_package(ament_cmake_ros REQUIRED) +find_package(std_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(rcl_lifecycle REQUIRED) find_package(rclc REQUIRED) @@ -41,6 +46,7 @@ ament_target_dependencies(${PROJECT_NAME} rclc rcutils rcl_lifecycle + std_msgs lifecycle_msgs ) diff --git a/rclc_lifecycle/README.md b/rclc_lifecycle/README.md index d665d265..abb8ac01 100644 --- a/rclc_lifecycle/README.md +++ b/rclc_lifecycle/README.md @@ -11,7 +11,7 @@ The API of the RCLC Lifecycle Node can be divided in several phases: Initializat ### Initialization -Creation of a lifecycle node as a bundle of an rcl node and the rcl Node Lifecycle state machine. +Creation of a lifecycle node as a bundle of an rcl node and the rcl Node Lifecycle state machine: ```C #include "rclc_lifecycle/rclc_lifecycle.h" @@ -22,11 +22,11 @@ rcl_ret_t rc; // create rcl node rc = rclc_support_init(&support, argc, argv, &allocator); -rcl_node_t my_node = rcl_get_zero_initialized_node(); +rcl_node_t my_node; rc = rclc_node_init_default(&my_node, "lifecycle_node", "rclc", &support); // rcl state machine -rcl_lifecycle_state_machine_t state_machine_ = +rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); ... @@ -39,45 +39,36 @@ rcl_ret_t rc = rclc_make_node_a_lifecycle_node( &allocator); ``` -Optionally create hooks for lifecycle state changes. +Register lifecycle services and optionally create callbacks for state changes. Executor needsto be equipped with 1 handle per node _and_ per service: ```C -// declare callback -rcl_ret_t my_on_configure() { - printf(" >>> lifecycle_node: on_configure() callback called.\n"); - return RCL_RET_OK; -} +// Executor +rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); +rclc_executor_init( + &executor, + &support.context, + 4, // 1 for the node + 1 for each lifecycle service + &allocator)); ... -// register callbacks -rclc_lifecycle_register_on_configure(&lifecycle_node, &my_on_configure); -``` - -### Running +// Register lifecycle services +rclc_lifecycle_add_get_state_service(&lifecycle_node, &executor); +rclc_lifecycle_add_get_available_states_service(&lifecycle_node, &executor); +rclc_lifecycle_add_change_state_service(&lifecycle_node, &executor); -Change states of the lifecycle node, e.g. - -```C -bool publish_transition = true; -rc += rclc_lifecycle_change_state( - &lifecycle_node, - lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, - publish_transition); -rc += rclc_lifecycle_change_state( - &lifecycle_node, - lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, - publish_transition); +// Register lifecycle service callbacks +rclc_lifecycle_register_on_configure(&lifecycle_node, &my_on_configure); +rclc_lifecycle_register_on_activate(&lifecycle_node, &my_on_activate); ... ``` -Except for error processing transitions, transitions are usually triggered from outside, e.g., by ROS 2 services. - ### Cleaning Up -To clean everything up, simply do +To clean everything up, do: ```C rc += rcl_lifecycle_node_fini(&lifecycle_node, &allocator); +... ``` ## Example @@ -86,4 +77,4 @@ An example, how to use the RCLC Lifecycle Node is given in the file `lifecycle_n ## Limitations -The state machine publishes state changes, however, lifecycle services are not yet exposed via ROS 2 services (tbd). +* Lifecycle services cannot yet be called via `ros2 lifecycle` CLI, e.g., `ros2 lifecycle set /node configure`. Instead use the `ros2 service` CLI, e.g., `ros2 service call /node/change_state lifecycle_msgs/ChangeState "{transition: {id: 1, label: configure}}"`. diff --git a/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h b/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h index 4935655f..6e50d60e 100644 --- a/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h +++ b/rclc_lifecycle/include/rclc_lifecycle/rclc_lifecycle.h @@ -23,7 +23,12 @@ #include #include -#include "rclc/node.h" +#include +#include +#include + +#include +#include #include "rclc_lifecycle/visibility_control.h" typedef struct rclc_lifecycle_callback_map_t @@ -38,8 +43,61 @@ typedef struct rclc_lifecycle_node_t rcl_node_t * node; rcl_lifecycle_state_machine_t * state_machine; rclc_lifecycle_callback_map_t callbacks; + bool publish_transitions; + + lifecycle_msgs__srv__ChangeState_Request cs_req; + lifecycle_msgs__srv__ChangeState_Response cs_res; + lifecycle_msgs__srv__GetState_Request gs_req; + lifecycle_msgs__srv__GetState_Response gs_res; + lifecycle_msgs__srv__GetAvailableStates_Request gas_req; + lifecycle_msgs__srv__GetAvailableStates_Response gas_res; } rclc_lifecycle_node_t; +/// Structure which encapsulates a ROS Lifecycle Node. +typedef struct rclc_lifecycle_service_context_t +{ + rclc_lifecycle_node_t * lifecycle_node; +} rclc_lifecycle_service_context_t; + +RCLC_LIFECYCLE_PUBLIC +rcl_ret_t +rclc_lifecycle_init_get_state_server( + rclc_lifecycle_node_t * lifecycle_node, + rclc_executor_t * executor); + +RCLC_LIFECYCLE_PUBLIC +rcl_ret_t +rclc_lifecycle_init_get_available_states_server( + rclc_lifecycle_node_t * lifecycle_node, + rclc_executor_t * executor); + +RCLC_LIFECYCLE_PUBLIC +rcl_ret_t +rclc_lifecycle_init_change_state_server( + rclc_lifecycle_node_t * lifecycle_node, + rclc_executor_t * executor); + +RCLC_LIFECYCLE_PUBLIC +void +rclc_lifecycle_get_state_callback( + const void * req, + void * res, + void * context); + +RCLC_LIFECYCLE_PUBLIC +void +rclc_lifecycle_get_available_states_callback( + const void * req, + void * res, + void * context); + +RCLC_LIFECYCLE_PUBLIC +void +rclc_lifecycle_change_state_callback( + const void * req, + void * res, + void * context); + RCLC_LIFECYCLE_PUBLIC rcl_ret_t rclc_make_node_a_lifecycle_node( @@ -95,7 +153,7 @@ rclc_lifecycle_execute_callback( RCLC_LIFECYCLE_PUBLIC rcl_ret_t -rcl_lifecycle_node_fini( +rclc_lifecycle_node_fini( rclc_lifecycle_node_t * node, rcl_allocator_t * allocator); diff --git a/rclc_lifecycle/package.xml b/rclc_lifecycle/package.xml index c40bc9b5..4b0c22a4 100644 --- a/rclc_lifecycle/package.xml +++ b/rclc_lifecycle/package.xml @@ -7,13 +7,14 @@ Jan Staschulat Apache License 2.0 - + Arne Nordmann ament_cmake_ros rclc rcl_lifecycle + std_msgs lifecycle_msgs ament_cmake_gtest diff --git a/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c b/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c index 1e62a748..b6cd0aee 100644 --- a/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c +++ b/rclc_lifecycle/src/rclc_lifecycle/rclc_lifecycle.c @@ -19,9 +19,13 @@ #include #include +#include #include #include +#include +#include +#include #include #include #include @@ -39,6 +43,15 @@ rclc_make_node_a_lifecycle_node( bool enable_communication_interface ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + 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( + allocator, "allocator is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + lifecycle_node->node = node; + rcl_lifecycle_state_machine_options_t state_machine_options = rcl_lifecycle_get_default_state_machine_options(); state_machine_options.enable_com_interface = enable_communication_interface; @@ -56,16 +69,40 @@ rclc_make_node_a_lifecycle_node( &state_machine_options); if (rcl_ret != RCL_RET_OK) { // state machine not initialized, return uninitilized - // @todo(anordman): how/what to return in this case? RCUTILS_LOG_ERROR( "Unable to initialize state machine: %s", rcl_get_error_string().str); return RCL_RET_ERROR; } - lifecycle_node->node = node; lifecycle_node->state_machine = state_machine; + // Pre-init messages and strings + static char empty_string[RCLC_LIFECYCLE_MAX_STRING_LENGTH]; + memset(empty_string, ' ', RCLC_LIFECYCLE_MAX_STRING_LENGTH); + empty_string[RCLC_LIFECYCLE_MAX_STRING_LENGTH - 1] = '\0'; + + lifecycle_msgs__srv__ChangeState_Request__init(&lifecycle_node->cs_req); + lifecycle_msgs__srv__ChangeState_Response__init(&lifecycle_node->cs_res); + + lifecycle_msgs__srv__GetState_Request__init(&lifecycle_node->gs_req); + lifecycle_msgs__srv__GetState_Response__init(&lifecycle_node->gs_res); + rosidl_runtime_c__String__assign( + &lifecycle_node->gs_res.current_state.label, + (const char *) empty_string); + + lifecycle_msgs__srv__GetAvailableStates_Request__init(&lifecycle_node->gas_req); + lifecycle_msgs__srv__GetAvailableStates_Response__init(&lifecycle_node->gas_res); + lifecycle_msgs__msg__State__Sequence__init( + &lifecycle_node->gas_res.available_states, + state_machine->transition_map.states_size); + lifecycle_node->gas_res.available_states.size = 0; + for (size_t i = 0; i < state_machine->transition_map.states_size; ++i) { + rosidl_runtime_c__String__assign( + &lifecycle_node->gas_res.available_states.data[i].label, + (const char *) empty_string); + } + return RCL_RET_OK; } @@ -76,6 +113,9 @@ rclc_lifecycle_change_state( bool publish_update ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + lifecycle_node, "lifecycle_node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + if (rcl_lifecycle_state_machine_is_initialized(lifecycle_node->state_machine) != RCL_RET_OK) { RCUTILS_LOG_ERROR( "Unable to change state for state machine: %s", @@ -184,6 +224,9 @@ rclc_lifecycle_register_on_configure( int (* cb)(void) ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + return rclc_lifecycle_register_callback( node, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, @@ -196,6 +239,9 @@ rclc_lifecycle_register_on_activate( int (* cb)(void) ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + return rclc_lifecycle_register_callback( node, lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, @@ -208,6 +254,9 @@ rclc_lifecycle_register_on_deactivate( int (* cb)(void) ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + return rclc_lifecycle_register_callback( node, lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, @@ -220,6 +269,9 @@ rclc_lifecycle_register_on_cleanup( int (* cb)(void) ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + return rclc_lifecycle_register_callback( node, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, @@ -227,11 +279,16 @@ rclc_lifecycle_register_on_cleanup( } rcl_ret_t -rcl_lifecycle_node_fini( +rclc_lifecycle_node_fini( rclc_lifecycle_node_t * lifecycle_node, rcl_allocator_t * allocator ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + lifecycle_node, "lifecycle_node 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); + rcl_ret_t rcl_ret = RCL_RET_OK; RCLC_UNUSED(allocator); @@ -240,14 +297,20 @@ rcl_lifecycle_node_fini( lifecycle_node->state_machine, lifecycle_node->node); if (rcl_ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR( + "Unable to clean up state machine: %s", + rcl_get_error_string().str); + rcutils_reset_error(); return RCL_RET_ERROR; } - // Cleanup node - rcl_ret = rcl_node_fini(lifecycle_node->node); - if (rcl_ret != RCL_RET_OK) { - return RCL_RET_ERROR; - } + // Cleanup service messages + lifecycle_msgs__srv__GetState_Request__fini(&lifecycle_node->gs_req); + lifecycle_msgs__srv__GetState_Response__fini(&lifecycle_node->gs_res); + lifecycle_msgs__srv__ChangeState_Request__fini(&lifecycle_node->cs_req); + lifecycle_msgs__srv__ChangeState_Response__fini(&lifecycle_node->cs_res); + lifecycle_msgs__srv__GetAvailableStates_Request__fini(&lifecycle_node->gas_req); + lifecycle_msgs__srv__GetAvailableStates_Response__fini(&lifecycle_node->gas_res); return rcl_ret; } @@ -258,6 +321,9 @@ rclc_lifecycle_execute_callback( unsigned int transition_id ) { + RCL_CHECK_FOR_NULL_WITH_MSG( + lifecycle_node, "lifecycle_node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + if (!lifecycle_node->callbacks.goal_states[transition_id]) { // no callback, so process, all good return RCL_RET_OK; @@ -265,3 +331,168 @@ rclc_lifecycle_execute_callback( return (*lifecycle_node->callbacks.fun_ptrs[transition_id])(); } + +rcl_ret_t +rclc_lifecycle_init_get_state_server( + rclc_lifecycle_node_t * lifecycle_node, + rclc_executor_t * executor) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + lifecycle_node, "lifecycle_node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + executor, "executor is a null pointer", return RCL_RET_INVALID_ARGUMENT); + rclc_lifecycle_service_context_t context; + context.lifecycle_node = lifecycle_node; + + rcl_ret_t rc = rclc_executor_add_service_with_context( + executor, + &lifecycle_node->state_machine->com_interface.srv_get_state, + &lifecycle_node->gs_req, + &lifecycle_node->gs_res, + rclc_lifecycle_get_state_callback, + &context); + if (rc != RCL_RET_OK) { + PRINT_RCLC_ERROR(main, rclc_executor_add_service_with_context); + return RCL_RET_ERROR; + } + + return rc; +} + +void +rclc_lifecycle_get_state_callback( + const void * req, + void * res, + void * context) +{ + RCL_UNUSED(req); + lifecycle_msgs__srv__GetState_Response * res_in = + (lifecycle_msgs__srv__GetState_Response *) res; + rclc_lifecycle_service_context_t * context_in = + (rclc_lifecycle_service_context_t *) context; + + rcl_lifecycle_state_machine_t * sm = + context_in->lifecycle_node->state_machine; + + res_in->current_state.id = sm->current_state->id; + bool success = rosidl_runtime_c__String__assign( + &res_in->current_state.label, + sm->current_state->label); + if (!success) { + PRINT_RCLC_ERROR( + rclc_lifecycle_get_state_callback, + rosidl_runtime_c__String__assign); + } +} + +rcl_ret_t +rclc_lifecycle_init_get_available_states_server( + rclc_lifecycle_node_t * lifecycle_node, + rclc_executor_t * executor) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + lifecycle_node, "lifecycle_node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + executor, "executor is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + rclc_lifecycle_service_context_t context; + context.lifecycle_node = lifecycle_node; + + rcl_ret_t rc = rclc_executor_add_service_with_context( + executor, + &lifecycle_node->state_machine->com_interface.srv_get_available_states, + &lifecycle_node->gas_req, + &lifecycle_node->gas_res, + rclc_lifecycle_get_available_states_callback, + &context); + if (rc != RCL_RET_OK) { + PRINT_RCLC_ERROR(main, rclc_executor_add_service_with_context); + return RCL_RET_ERROR; + } + + return rc; +} + +void +rclc_lifecycle_get_available_states_callback( + const void * req, + void * res, + void * context) +{ + RCL_UNUSED(req); + lifecycle_msgs__srv__GetAvailableStates_Response * res_in = + (lifecycle_msgs__srv__GetAvailableStates_Response *) res; + rclc_lifecycle_service_context_t * context_in = + (rclc_lifecycle_service_context_t *) context; + + rcl_lifecycle_state_machine_t * sm = + context_in->lifecycle_node->state_machine; + + bool success = true; + res_in->available_states.size = sm->transition_map.states_size; + for (unsigned int i = 0; i < sm->transition_map.states_size; ++i) { + res_in->available_states.data[i].id = sm->transition_map.states[i].id; + success &= rosidl_runtime_c__String__assign( + &res_in->available_states.data[i].label, + sm->transition_map.states[i].label); + } + + if (!success) { + PRINT_RCLC_ERROR( + rclc_lifecycle_get_available_states_callback, + rosidl_runtime_c__String__assign); + } +} + +rcl_ret_t +rclc_lifecycle_init_change_state_server( + rclc_lifecycle_node_t * lifecycle_node, + rclc_executor_t * executor) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + lifecycle_node, "lifecycle_node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + rclc_lifecycle_service_context_t context; + context.lifecycle_node = lifecycle_node; + + rcl_ret_t rc = rclc_executor_add_service_with_context( + executor, + &lifecycle_node->state_machine->com_interface.srv_change_state, + &lifecycle_node->cs_req, + &lifecycle_node->cs_res, + rclc_lifecycle_change_state_callback, + &context); + if (rc != RCL_RET_OK) { + PRINT_RCLC_ERROR(main, rclc_executor_add_service_with_context); + return RCL_RET_ERROR; + } + + return rc; +} + +void +rclc_lifecycle_change_state_callback( + const void * req, + void * res, + void * context) +{ + lifecycle_msgs__srv__ChangeState_Request * req_in = + (lifecycle_msgs__srv__ChangeState_Request *) req; + lifecycle_msgs__srv__ChangeState_Response * res_in = + (lifecycle_msgs__srv__ChangeState_Response *) res; + rclc_lifecycle_service_context_t * context_in = + (rclc_lifecycle_service_context_t *) context; + + rclc_lifecycle_node_t * ln = context_in->lifecycle_node; + rcl_ret_t rc = rclc_lifecycle_change_state(ln, req_in->transition.id, true); + + lifecycle_msgs__srv__ChangeState_Response__init(res_in); + if (rc != RCL_RET_OK) { + PRINT_RCLC_ERROR( + rclc_lifecycle_change_state_callback, + rclc_lifecycle_change_state); + res_in->success = false; + } else { + res_in->success = true; + } +} diff --git a/rclc_lifecycle/test/test_lifecycle.cpp b/rclc_lifecycle/test/test_lifecycle.cpp index d4b481c7..ee952ac8 100644 --- a/rclc_lifecycle/test/test_lifecycle.cpp +++ b/rclc_lifecycle/test/test_lifecycle.cpp @@ -65,12 +65,12 @@ TEST(TestRclcLifecycle, lifecycle_node) { res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); rclc_lifecycle_node_t lifecycle_node; - rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + 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, - &state_machine_, + &state_machine, &allocator, true); @@ -102,12 +102,12 @@ TEST(TestRclcLifecycle, lifecycle_node_transitions) { res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); rclc_lifecycle_node_t lifecycle_node; - rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + 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, - &state_machine_, + &state_machine, &allocator, false); @@ -172,12 +172,12 @@ TEST(TestRclcLifecycle, lifecycle_node_callbacks) { res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); rclc_lifecycle_node_t lifecycle_node; - rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + 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, - &state_machine_, + &state_machine, &allocator, true); @@ -192,19 +192,27 @@ TEST(TestRclcLifecycle, lifecycle_node_callbacks) { &lifecycle_node, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, true); + EXPECT_EQ(RCL_RET_OK, res); + EXPECT_EQ(1, callback_mockup_counter); + res += rclc_lifecycle_change_state( &lifecycle_node, lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE, true); + EXPECT_EQ(RCL_RET_OK, res); + EXPECT_EQ(3, callback_mockup_counter); + res += rclc_lifecycle_change_state( &lifecycle_node, lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE, true); + EXPECT_EQ(RCL_RET_OK, res); + EXPECT_EQ(7, callback_mockup_counter); + res += rclc_lifecycle_change_state( &lifecycle_node, lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP, true); - EXPECT_EQ(RCL_RET_OK, res); EXPECT_EQ(15, callback_mockup_counter); @@ -215,3 +223,68 @@ TEST(TestRclcLifecycle, lifecycle_node_callbacks) { res = rcl_init_options_fini(&init_options); EXPECT_EQ(RCL_RET_OK, res); } + +TEST(TestRclcLifecycle, lifecycle_node_servers) { + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + + rcl_ret_t res = rcl_init_options_init(&init_options, allocator); + res += rcl_init(0, nullptr, &init_options, &context); + + rcl_node_t my_node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_ops = rcl_node_get_default_options(); + res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + + 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, + &state_machine, + &allocator, + true); + + // register callbacks + rclc_lifecycle_register_on_configure(&lifecycle_node, &callback_mockup_0); + rclc_lifecycle_register_on_activate(&lifecycle_node, &callback_mockup_1); + rclc_lifecycle_register_on_deactivate(&lifecycle_node, &callback_mockup_2); + rclc_lifecycle_register_on_cleanup(&lifecycle_node, &callback_mockup_3); + + // create lifecycle servers + rclc_executor_t executor; + res = rclc_executor_init( + &executor, + &context, + 1, // too little + &allocator); + EXPECT_EQ(RCL_RET_OK, res); + + // Too little executor handles + res = rclc_lifecycle_init_get_state_server(&lifecycle_node, &executor); + EXPECT_EQ(RCL_RET_OK, res); + res = rclc_lifecycle_init_get_available_states_server(&lifecycle_node, &executor); + EXPECT_EQ(RCL_RET_ERROR, res); + + // Now with correct number of handles + rclc_executor_init( + &executor, + &context, + 3, // 1 for each lifecycle service + &allocator); + res = rclc_lifecycle_init_get_state_server(&lifecycle_node, &executor); + EXPECT_EQ(RCL_RET_OK, res); + res = rclc_lifecycle_init_get_available_states_server(&lifecycle_node, &executor); + EXPECT_EQ(RCL_RET_OK, res); + res = rclc_lifecycle_init_change_state_server(&lifecycle_node, &executor); + EXPECT_EQ(RCL_RET_OK, res); + + // Cleanup + res = rclc_lifecycle_node_fini(&lifecycle_node, &allocator); + EXPECT_EQ(RCL_RET_OK, res); + res = rcl_node_fini(&my_node); + EXPECT_EQ(RCL_RET_OK, res); + res = rclc_executor_fini(&executor); + EXPECT_EQ(RCL_RET_OK, res); +}