Micro-Ros action server for Arduino ESP32

[namInceptum]

• Hardware description: Arduino ESP32 USB connection to host
• RTOS: FreeRTOS
• Installation type: micro-ros-arduino Library in Arduino IDE
• Version: humble

I want to run an action server on ESP32 using the Arduino IDE based on the Fibonacci action server example. The problem is, that if I send a goal it hangs forever, such the action server is not answering.

Preparation:

• downloaded, modified and re-compiled the micro_ros_arduino Library with following setup of colcon.meta file:

            "-DRMW_UXRCE_XML_BUFFER_LENGTH=400",
            "-DRMW_UXRCE_MAX_NODES=4",
            "-DRMW_UXRCE_MAX_PUBLISHERS=10",
            "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=10",
            "-DRMW_UXRCE_MAX_SERVICES=10",
            "-DRMW_UXRCE_MAX_CLIENTS=4",
            "-DRMW_UXRCE_MAX_HISTORY=4",
            "-DRMW_UXRCE_TRANSPORT=custom"
  

• implementation based on issue 1129:

#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <example_interfaces/action/fibonacci.h> 

#define LEDR 26 
#define LEDG 25
#define LEDB 33

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

const char * goalResult[7] = { "", "", "", "", "succeeded", "canceled", "aborted" };

rclc_executor_t executor;
rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;
rclc_action_server_t action_server;
example_interfaces__action__Fibonacci_SendGoal_Request ros_goal_request[10];

int32_t data[50] = {0};  // Increased to avoid overflow

// FreeRTOS
TaskHandle_t fibonacciTaskHandle;
SemaphoreHandle_t mutex;
rclc_action_goal_handle_t *g_goal_handle = NULL;

void error_loop() {
  while (1) {
    digitalWrite(LEDR, !digitalRead(LEDR));
    delay(100);
  }
}

void fibonacci_worker(void *param)
{
  while (true) {
    if (g_goal_handle == NULL) {
      vTaskDelay(pdMS_TO_TICKS(10));
      continue;
    }

    xSemaphoreTake(mutex, portMAX_DELAY);
    rclc_action_goal_handle_t *goal_handle = g_goal_handle;
    g_goal_handle = NULL;
    xSemaphoreGive(mutex);

    if (goal_handle == NULL) continue;

    auto *req = (example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;

    example_interfaces__action__Fibonacci_FeedbackMessage feedback;
    example_interfaces__action__Fibonacci_GetResult_Response result = {0};
    rcl_action_goal_state_t goal_state;

    if (req->goal.order < 2 || req->goal.order > 50) {
      goal_state = GOAL_STATE_ABORTED;
      digitalWrite(LEDR, LOW);
    } else {
      feedback.feedback.sequence.capacity = req->goal.order;
      feedback.feedback.sequence.size = 2;
      feedback.feedback.sequence.data = data;
      feedback.feedback.sequence.data[0] = 0;
      feedback.feedback.sequence.data[1] = 1;

      for (size_t i = 2; i < (size_t)req->goal.order && !goal_handle->goal_cancelled; i++) {
        feedback.feedback.sequence.data[i] = feedback.feedback.sequence.data[i - 1] + feedback.feedback.sequence.data[i - 2];
        feedback.feedback.sequence.size++;

        xSemaphoreTake(mutex, portMAX_DELAY);
        rclc_action_publish_feedback(goal_handle, &feedback);
        xSemaphoreGive(mutex);

        vTaskDelay(pdMS_TO_TICKS(500));
      }

      if (!goal_handle->goal_cancelled) {
        result.result.sequence.capacity = feedback.feedback.sequence.capacity;
        result.result.sequence.size = feedback.feedback.sequence.size;
        result.result.sequence.data = feedback.feedback.sequence.data;
        goal_state = GOAL_STATE_SUCCEEDED;
        digitalWrite(LEDG, LOW);
      } else {
        goal_state = GOAL_STATE_CANCELED;
        digitalWrite(LEDB, LOW);
      }
    }

    rcl_ret_t rc;
    do {
      xSemaphoreTake(mutex, portMAX_DELAY);
      rc = rclc_action_send_result(goal_handle, goal_state, &result);
      xSemaphoreGive(mutex);
      vTaskDelay(pdMS_TO_TICKS(1000));
    } while (rc != RCL_RET_OK);

    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);
  }
}

rcl_ret_t handle_goal(rclc_action_goal_handle_t *goal_handle, void *context)
{
  (void)context;

  auto *req = (example_interfaces__action__Fibonacci_SendGoal_Request *) goal_handle->ros_goal_request;

  if (req->goal.order > 50) {
    return RCL_RET_ACTION_GOAL_REJECTED;
  }

  xSemaphoreTake(mutex, portMAX_DELAY);
  g_goal_handle = goal_handle;
  xSemaphoreGive(mutex);

  return RCL_RET_ACTION_GOAL_ACCEPTED;
}

bool handle_cancel(rclc_action_goal_handle_t *goal_handle, void *context)
{
  (void)context;
  (void)goal_handle;
  return true;
}

void setup()
{
  set_microros_transports();

  
  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  
  digitalWrite(LEDR, HIGH);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);

  mutex = xSemaphoreCreateMutex();

  allocator = rcl_get_default_allocator();

  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_init_options_init(&init_options, allocator);
  rcl_init_options_set_domain_id(&init_options, 90);
  RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));

  RCCHECK(rclc_node_init_default(&node, "fibonacci_action_server", "", &support));
  RCCHECK(rclc_action_server_init_default(
    &action_server, &node, &support,
    ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci),
    "fibonacci"));
  
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_action_server(
    &executor, &action_server, 10, ros_goal_request,
    sizeof(example_interfaces__action__Fibonacci_SendGoal_Request),
    handle_goal, handle_cancel, (void *)&action_server));

  // Launch FreeRTOS task
  xTaskCreatePinnedToCore(
    fibonacci_worker,
    "fibonacci_task",
    4096,
    NULL,
    1,
    &fibonacciTaskHandle,
    1
  );
}

void loop()
{
  delay(100);
  if (mutex) {
    xSemaphoreTake(mutex, portMAX_DELAY);
    RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
    xSemaphoreGive(mutex);
  }
}

• running micro-ros agent via ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 with output:

~/micro_ros_ws$ ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0
[1746628748.774559] info | TermiosAgentLinux.cpp | init | running... | fd: 3
[1746628748.774877] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 4
[1746628752.261443] info | Root.cpp | create_client | create | client_key: 0x5851F42D, session_id: 0x81
[1746628752.261599] info | SessionManager.hpp | establish_session | session established | client_key: 0x5851F42D, address: 0
[1746628752.286764] info | ProxyClient.cpp | create_participant | participant created | client_key: 0x5851F42D, participant_id: 0x000(1)
[1746628752.318397] info | ProxyClient.cpp | create_replier | replier created | client_key: 0x5851F42D, requester_id: 0x000(7), participant_id: 0x000(1)
[1746628752.348946] info | ProxyClient.cpp | create_replier | replier created | client_key: 0x5851F42D, requester_id: 0x001(7), participant_id: 0x000(1)
[1746628752.382808] info | ProxyClient.cpp | create_replier | replier created | client_key: 0x5851F42D, requester_id: 0x002(7), participant_id: 0x000(1)
[1746628752.402013] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x5851F42D, topic_id: 0x000(2), participant_id: 0x000(1)
[1746628752.410867] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x5851F42D, publisher_id: 0x000(3), participant_id: 0x000(1)
[1746628752.421709] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x5851F42D, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1746628752.437371] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x5851F42D, topic_id: 0x001(2), participant_id: 0x000(1)
[1746628752.446100] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x5851F42D, publisher_id: 0x001(3), participant_id: 0x000(1)
[1746628752.456670] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x5851F42D,

• calling: ros2 node list returns: /fibonacci_action_server
• calling: ros2 action list returns: /fibonacci
• sending goal via the command ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "order: 5" --feedback stuck and never ends:
  ~/ros2_ws$ ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "order: 5" --feedback
  Waiting for an action server to become available...
  Sending goal:
  order: 5

The LED's are not changing for any order or number. What I'm doing wrong?
Thanks for any help.

[hippo5329]

You may switch to platformio. Follow this wiki,

https://github.com/hippo5329/micro-ROS-demos-platformio/wiki#fibonacci-action-server-demo