Add actions to examples (#53)

* Initial

* Revert rcl_action find

* Update actions examples with rclc API

* Fix warnings

* Update CI to use rolling

* Add rosdep update

Co-authored-by: Antonio Cuadros <acuadros1995@gmail.com>

Author: pablogs9

.github/workflows/ci.yml

@@ -2,23 +2,24 @@ name: CI
 
 on:
     pull_request:
-      branches: 
+      branches:
         - '**'
 
 jobs:
     micro_ros_demos:
         runs-on: ubuntu-20.04
-        container: microros/base:foxy
+        container: microros/base:rolling
         defaults:
           run:
             shell: bash
-    
+
         steps:
         - name: Download environment
           run: |
             cd /uros_ws
             apt update
-            source /opt/ros/foxy/setup.bash
+            rosdep update
+            source /opt/ros/rolling/setup.bash
             source install/local_setup.bash
             ros2 run micro_ros_setup create_firmware_ws.sh host
             rm -rf src/uros/micro-ROS-demos
@@ -28,6 +29,6 @@ jobs:
         - name: Build
           run: |
             cd /uros_ws
-            source /opt/ros/foxy/setup.bash
+            source /opt/ros/rolling/setup.bash
             source install/local_setup.bash
             ros2 run micro_ros_setup build_firmware.sh

rclc/configuration_example/configured_publisher/main.c

@@ -46,7 +46,7 @@ int main(int argc, char * const argv[])
 	size_t domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0);
 	const char * node_name = "int32_configured_publisher_rclc";
 
-	rcl_init_options_set_domain_id(&init_options, domain_id);
+	RCCHECK(rcl_init_options_set_domain_id(&init_options, domain_id));
 	printf("Initializing RCL '%s' with ROS Domain ID %ld...\n", node_name, domain_id);
 
 	// create init_options

rclc/configuration_example/configured_subscriber/main.c

@@ -42,7 +42,7 @@ int main(int argc, const char * const * argv)
 	size_t domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0);
 	const char * node_name = "int32_configured_subscriber_rclc";
 
-	rcl_init_options_set_domain_id(&init_options, domain_id);
+	RCCHECK(rcl_init_options_set_domain_id(&init_options, domain_id));
 	printf("Initializing RCL '%s' with ROS Domain ID %ld...\n", node_name, domain_id);
 
 	// create init_options

rclc/fibonacci_action_client/CMakeLists.txt

@@ -4,14 +4,14 @@ project(fibonacci_action_client LANGUAGES C)
 
 find_package(ament_cmake REQUIRED)
 find_package(rcl REQUIRED)
-find_package(rcl_action REQUIRED)
+find_package(rclc REQUIRED)
 find_package(example_interfaces REQUIRED)
 
 add_executable(${PROJECT_NAME} main.c)
 
 ament_target_dependencies(${PROJECT_NAME}
   rcl
-  rcl_action
+  rclc
   example_interfaces
   )
 

rclc/fibonacci_action_client/main.c

@@ -1,129 +1,182 @@
-#include <rcl/rcl.h>
-#include <rcl_action/rcl_action.h>
-#include <rcl/error_handling.h>
-#include <example_interfaces/action/fibonacci.h>
-
 #include <stdio.h>
 #include <unistd.h>
 #include <pthread.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);}}
-
-int main(int argc, const char * const * argv)
-{
-  rcl_init_options_t options = rcl_get_zero_initialized_init_options();
-  RCCHECK(rcl_init_options_init(&options, rcl_get_default_allocator()))
-
-  rcl_context_t context = rcl_get_zero_initialized_context();
-  RCCHECK(rcl_init(argc, argv, &options, &context))
-
-  rcl_node_options_t node_ops = rcl_node_get_default_options();
-  rcl_node_t node = rcl_get_zero_initialized_node();
-  RCCHECK(rcl_node_init(&node, "fibonacci_action_client_rcl", "", &context, &node_ops))
-
-  const char * action_name = "fibonacci";
-  const rosidl_action_type_support_t * action_type_support = ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci);
-  rcl_action_client_t action_client = rcl_action_get_zero_initialized_client();
-  rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options();
-
-  RCCHECK(rcl_action_client_init(&action_client, &node, action_type_support, action_name, &action_client_ops))
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
 
-  size_t num_subscriptions, num_guard_conditions, num_timers, num_clients, num_services;
+#include <example_interfaces/action/fibonacci.h>
 
-  RCCHECK(rcl_action_client_wait_set_get_num_entities(&action_client, &num_subscriptions, &num_guard_conditions, &num_timers, &num_clients, &num_services))
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  RCCHECK(rcl_wait_set_init(&wait_set, num_subscriptions, num_guard_conditions, num_timers, num_clients, num_services, 0, &context, rcl_get_default_allocator()))
+#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);}}
 
-  int64_t goal_sequence_number;
-  int64_t result_sequence_number;
+#define MAX_FIBONACCI_ORDER 500
+bool goal_completed = false;
+int goal_order = 10;
 
-  bool done = false;
-  bool goal_accepted = false;
-  int order = 10;
+void goal_request_callback(rclc_action_goal_handle_t * goal_handle, bool accepted, void * context)
+{
+  (void) context;
+
+  example_interfaces__action__Fibonacci_SendGoal_Request * request =
+    (example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
+  printf(
+    "Goal request (order: %d): %s\n",
+    request->goal.order,
+    accepted ? "Accepted" : "Rejected"
+  );
+
+  if (!accepted) {
+    goal_completed = true;
+  }
+}
 
-  sleep(2); // Sleep a while to ensure DDS matching before sending request
+void feedback_callback(rclc_action_goal_handle_t * goal_handle, void * ros_feedback, void * context)
+{
+  (void) context;
 
-  printf("Sending goal request\n");
-  example_interfaces__action__Fibonacci_SendGoal_Request ros_goal_request;
-  ros_goal_request.goal.order = order;
-  RCCHECK(rcl_action_send_goal_request(&action_client, &ros_goal_request, &goal_sequence_number))
-  
+  example_interfaces__action__Fibonacci_SendGoal_Request * request =
+    (example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
 
-  do {    
-    RCSOFTCHECK(rcl_wait_set_clear(&wait_set))
-    
-    size_t client_index, subscription_index;
-    RCSOFTCHECK(rcl_action_wait_set_add_action_client(&wait_set, &action_client, &client_index, &subscription_index))
+  printf(
+    "Goal Feedback (order: %d) [",
+    request->goal.order
+  );
 
-    rcl_ret_t rc = rcl_wait(&wait_set, RCL_MS_TO_NS(50));
+  example_interfaces__action__Fibonacci_FeedbackMessage * feedback =
+    (example_interfaces__action__Fibonacci_FeedbackMessage *) ros_feedback;
 
-    bool is_feedback_ready = false;
-    bool is_status_ready = false;
-    bool is_goal_response_ready = false;
-    bool is_cancel_response_ready = false;
-    bool is_result_response_ready = false;
+  for (size_t i = 0; i < feedback->feedback.sequence.size; i++) {
+    printf("%d ", feedback->feedback.sequence.data[i]);
+  }
+  printf("\b]\n");
+}
 
-    RCSOFTCHECK(rcl_action_client_wait_set_get_entities_ready(&wait_set, &action_client, &is_feedback_ready, &is_status_ready, &is_goal_response_ready, &is_cancel_response_ready, &is_result_response_ready))
+void result_request_callback(
+  rclc_action_goal_handle_t * goal_handle, void * ros_result_response,
+  void * context)
+{
+  (void) context;
 
-    if (is_goal_response_ready) {
-      printf("Goal response ready\n");
-      example_interfaces__action__Fibonacci_SendGoal_Response ros_goal_response;
-      rmw_request_id_t response_header;
+  static size_t goal_count = 1;
 
-      RCSOFTCHECK(rcl_action_take_goal_response(&action_client, &response_header, &ros_goal_response))
+  example_interfaces__action__Fibonacci_SendGoal_Request * request =
+    (example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
 
-      if (ros_goal_response.accepted) {
-          printf("Goal request accepted\n");
-          printf("Requesting response...\n");
+  printf(
+    "Goal Result (order: %d) [",
+    request->goal.order
+  );
 
-          goal_accepted = true;
+  example_interfaces__action__Fibonacci_GetResult_Response * result =
+    (example_interfaces__action__Fibonacci_GetResult_Response *) ros_result_response;
 
-          example_interfaces__action__Fibonacci_GetResult_Request ros_result_request;
-          RCSOFTCHECK(rcl_action_send_result_request(&action_client, &ros_result_request, &result_sequence_number))
-      }
+  if (result->status == GOAL_STATE_SUCCEEDED) {
+    for (size_t i = 0; i < result->result.sequence.size; i++) {
+      printf("%d ", result->result.sequence.data[i]);
     }
-    
-    if (is_feedback_ready) {
-      example_interfaces__action__Fibonacci_FeedbackMessage ros_feedback;
-
-      ros_feedback.feedback.sequence.data = (int32_t*) malloc(order * sizeof(int32_t));
-      ros_feedback.feedback.sequence.capacity = order;
+  } else if (result->status == GOAL_STATE_CANCELED) {
+    printf("CANCELED ");
+  } else {
+    printf("ABORTED ");
+  }
 
-      RCSOFTCHECK(rcl_action_take_feedback(&action_client, &ros_feedback))
+  printf("\b]\n");
 
-      printf("Feedback: [");
-      for (size_t i = 0; i < ros_feedback.feedback.sequence.size ; i++) {
-        printf("%d, ", ros_feedback.feedback.sequence.data[i]);
-      }
-      printf("]\n");
+  goal_completed = true;
+}
 
-      free(ros_feedback.feedback.sequence.data);
-    } 
-    
-    if (is_result_response_ready) {
-      example_interfaces__action__Fibonacci_GetResult_Response ros_result_response;
-      rmw_request_id_t response_header;
+void cancel_request_callback(
+  rclc_action_goal_handle_t * goal_handle, bool cancelled,
+  void * context)
+{
+  (void) context;
 
-      ros_result_response.result.sequence.data = (int32_t*) malloc(order * sizeof(int32_t));
-      ros_result_response.result.sequence.capacity = order;
+  example_interfaces__action__Fibonacci_SendGoal_Request * request =
+    (example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;
 
-      RCSOFTCHECK(rcl_action_take_result_response(&action_client, &response_header, &ros_result_response))
+  printf(
+    "Goal cancel request (order: %d): %s\n",
+    request->goal.order,
+    cancelled ? "Accepted" : "Rejected");
 
-      printf("Response: [");
-      for (size_t i = 0; i < ros_result_response.result.sequence.size ; i++) {
-        printf("%d, ", ros_result_response.result.sequence.data[i]);
-      }
-      printf("]\n");
+  if (cancelled) {
+    goal_completed = true;
+  }
+}
 
-      done = true;
-      free(ros_result_response.result.sequence.data);
-    }
-  } while ( !done );
+int main(int argc, const char * const * argv)
+{
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rclc_support_t support;
+
+  // create init_options
+  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+  // create node
+  rcl_node_t node;
+  RCCHECK(rclc_node_init_default(&node, "fibonacci_action_client_rcl", "", &support));
+
+  // Create action client
+  rclc_action_client_t action_client;
+  RCCHECK(
+    rclc_action_client_init_default(
+      &action_client,
+      &node,
+      ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci),
+      "fibonacci"
+  ));
+
+  // Create executor
+  rclc_executor_t executor;
+  rclc_executor_init(&executor, &support.context, 1, &allocator);
+
+  example_interfaces__action__Fibonacci_FeedbackMessage ros_feedback;
+  example_interfaces__action__Fibonacci_GetResult_Response ros_result_response;
+  example_interfaces__action__Fibonacci_SendGoal_Request ros_goal_request;
 
-  RCCHECK(rcl_action_client_fini(&action_client, &node))
+  // Allocate msg memory
+  ros_feedback.feedback.sequence.capacity = MAX_FIBONACCI_ORDER;
+  ros_feedback.feedback.sequence.size = 0;
+  ros_feedback.feedback.sequence.data = (int32_t *) malloc(
+    ros_feedback.feedback.sequence.capacity * sizeof(int32_t));
+
+  ros_result_response.result.sequence.capacity = MAX_FIBONACCI_ORDER;
+  ros_result_response.result.sequence.size = 0;
+  ros_result_response.result.sequence.data = (int32_t *) malloc(
+    ros_result_response.result.sequence.capacity * sizeof(int32_t));
+
+  RCCHECK(
+    rclc_executor_add_action_client(
+      &executor,
+      &action_client,
+      10,
+      &ros_result_response,
+      &ros_feedback,
+      goal_request_callback,
+      feedback_callback,
+      result_request_callback,
+      cancel_request_callback,
+      (void *) &action_client
+  ));
+
+  sleep(2);
+
+  // Send goal request
+  ros_goal_request.goal.order = goal_order;
+  RCCHECK(rclc_action_send_goal_request(&action_client, &ros_goal_request, NULL));
+
+  while (!goal_completed) {
+    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
+    usleep(100000);
+  }
+
+  RCCHECK(rclc_action_client_fini(&action_client, &node))
   RCCHECK(rcl_node_fini(&node))
 
   return 0;
 }
-

rclc/fibonacci_action_server/CMakeLists.txt

@@ -4,7 +4,7 @@ project(fibonacci_action_server LANGUAGES C)
 
 find_package(ament_cmake REQUIRED)
 find_package(rcl REQUIRED)
-find_package(rcl_action REQUIRED)
+find_package(rclc REQUIRED)
 find_package(example_interfaces REQUIRED)
 
 add_executable(${PROJECT_NAME} main.c)
@@ -14,7 +14,7 @@ SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pthread")
 
 ament_target_dependencies(${PROJECT_NAME}
   rcl
-  rcl_action
+  rclc
   example_interfaces
   )