Provide lifecycle services in the rclc lifecycle nodes (#51) (#191)

* Adds service callback with service context Adds an additional callback type and handling that allows passing additional service context (void *) to a service callback. #35 Signed-off-by: Arne Nordmann <[email protected]> * Fixed service callback handling Signed-off-by: Arne Nordmann <[email protected]> * Adds user API and tests * Adds user API to add service with additional context * Tests (copied and adapted from service with request id tests) Signed-off-by: Arne Nordmann <[email protected]> * Proper tear-down of msgs Signed-off-by: Arne Nordmann <[email protected]> * Fixes service call with context tests Signed-off-by: Arne Nordmann <[email protected]> * Adds service callbacks to lifecycle node Declares service callbacks for lifecycle services: get state and get available states micro-ROS#40 Signed-off-by: Arne Nordmann <[email protected]> * Adds callback for GetState service micro-ROS#40 Signed-off-by: Arne Nordmann <[email protected]> * Provide GetAvailableStates service Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * Example lifecycle node proides services Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * Implemented ChangeState service Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * Linting, documentation, example Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * commnted out check for received sequence id - error in build-job for rolling https://build.ros2.org/job/Rpr__rclc__ubuntu_focal_amd64/72/ see also PR #46 and issue #49 * Cmake linting Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * Adds missing declaration of callbacks Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * Pre-initialize all strings Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * Lifecycle services working * Functions to initialize lifecycle services for nodes * Exemplary lifecycle nodes with services micro-ROS#40 Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * Tests for service initialization Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * commnted out check for received sequence id - error in build-job for rolling https://build.ros2.org/job/Rpr__rclc__ubuntu_focal_amd64/72/ see also PR #46 and issue #49 Signed-off-by: Jan Staschulat <[email protected]> * commnted out check for received sequence id - error in build-job for rolling https://build.ros2.org/job/Rpr__rclc__ubuntu_focal_amd64/72/ see also PR #46 and issue #49 Signed-off-by: Jan Staschulat <[email protected]> * Ignoring unsuccessful SERVICE_TAKE (#175) * resolved bug by ignoring RCL_RET_SERVICE_TAKE_FAILED Signed-off-by: Jan Staschulat <[email protected]> * data_available has to be reset to false, if rcl_take fails - this needs to be updated for subscriptions as well. Signed-off-by: Jan Staschulat <[email protected]> * Fixes init and cleanup Signed-off-by: Nordmann Arne (CR/ADT3) <[email protected]> * commnted out check for received sequence id - error in build-job for rolling https://build.ros2.org/job/Rpr__rclc__ubuntu_focal_amd64/72/ see also PR #46 and issue #49 Signed-off-by: Jan Staschulat <[email protected]> Co-authored-by: Jan Staschulat <[email protected]> (cherry picked from commit 865b02b) Co-authored-by: Arne Nordmann <[email protected]>
ros2 · Aug 19, 2021 · c299f0e · c299f0e
1 parent 313235d
commit c299f0e
Show file tree

Hide file tree

Showing 10 changed files with 456 additions and 108 deletions.
diff --git 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
@@ -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
@@ -26,8 +26,15 @@
 #include <lifecycle_msgs/srv/get_available_states.h>
 #include <lifecycle_msgs/srv/get_available_transitions.h>
 
+#include <rclc/executor.h>
+
 #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,15 +73,15 @@ 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");
     return -1;
   }
 
   // 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
@@ -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
@@ -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
@@ -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
@@ -23,7 +23,12 @@
 #include <rcl/error_handling.h>
 #include <rcl_lifecycle/rcl_lifecycle.h>
 
-#include "rclc/node.h"
+#include <lifecycle_msgs/srv/change_state.h>
+#include <lifecycle_msgs/srv/get_state.h>
+#include <lifecycle_msgs/srv/get_available_states.h>
+
+#include <rclc/node.h>
+#include <rclc/executor.h>
 #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);