Provide lifecycle services in the rclc lifecycle nodes

rclc_examples/src/example_lifecycle_node.c

@@ -27,8 +27,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");
@@ -75,7 +82,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,60 +95,34 @@ int main(int argc, const char * argv[])
     return -1;
   }
 
-  // register callbacks
+  // Executor
+  rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
+  RCCHECK(rclc_executor_init(
+    &executor,
+    &support.context,
+    4,  // 1 for the node + 1 for each lifecycle service
+    &allocator));
+
+  unsigned int rcl_wait_timeout = 10;         // 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_add_get_state_service(&lifecycle_node, &executor));
+  RCCHECK(rclc_lifecycle_add_get_available_states_service(&lifecycle_node, &executor));
+  RCCHECK(rclc_lifecycle_add_change_state_service(&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
+  rclc_executor_spin(&executor);
 
+  // Cleanup
   printf("cleaning up...\n");
   rc = rcl_lifecycle_node_fini(&lifecycle_node, &allocator);
   rc += rclc_support_fini(&support);

rclc_lifecycle/CMakeLists.txt

@@ -8,6 +8,7 @@ endif()
 
 # 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)
@@ -40,6 +41,7 @@ ament_target_dependencies(${PROJECT_NAME}
   rclc
   rcutils
   rcl_lifecycle
+  std_msgs
   lifecycle_msgs
 )
 

rclc_lifecycle/README.md

@@ -25,7 +25,7 @@ rcl_node_t my_node = rcl_get_zero_initialized_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();
 ...
 
@@ -38,39 +38,29 @@ 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
@@ -85,4 +75,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}}"`.

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>
 
 typedef struct rclc_lifecycle_callback_map_t
 {
@@ -37,8 +42,22 @@ 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;
+
 rcl_ret_t
 rclc_make_node_a_lifecycle_node(
   rclc_lifecycle_node_t * lifecycle_node,
@@ -88,4 +107,39 @@ rcl_lifecycle_node_fini(
   rclc_lifecycle_node_t * node,
   rcl_allocator_t * allocator);
 
+/// Lifecycle services
+
+rcl_ret_t
+rclc_lifecycle_add_get_state_service(
+  rclc_lifecycle_node_t * lifecycle_node,
+  rclc_executor_t * executor);
+
+rcl_ret_t
+rclc_lifecycle_add_get_available_states_service(
+  rclc_lifecycle_node_t * lifecycle_node,
+  rclc_executor_t * executor);
+
+rcl_ret_t
+rclc_lifecycle_add_change_state_service(
+  rclc_lifecycle_node_t * lifecycle_node,
+  rclc_executor_t * executor);
+
+void
+rclc_lifecycle_get_state_callback(
+  const void * request,
+  void * response,
+  void * service_context);
+
+void
+rclc_lifecycle_get_available_states_callback(
+  const void * request,
+  void * response,
+  void * service_context);
+
+void
+rclc_lifecycle_change_state_callback(
+  const void * request,
+  void * response,
+  void * service_context);
+
 #endif  // RCLC_LIFECYCLE__RCLC_LIFECYCLE_H_

rclc_lifecycle/package.xml

@@ -7,13 +7,14 @@
   <maintainer email="[email protected]">Jan Staschulat</maintainer>
 
   <license>Apache License 2.0</license>
-  
+
   <author email="[email protected]">Arne Nordmann</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <depend>rclc</depend>
   <depend>rcl_lifecycle</depend>
+  <depend>std_msgs</depend>
   <depend>lifecycle_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>