Micro-Ros action server for Arduino ESP32

Nobody has claimed this yet.

- 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](https://github.com/micro-ROS/micro_ros_arduino/issues/1129):

```c++
#include
#include
#include
#include
#include
#include

#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.**

Start with setup(), loop(), handle_goal(), and fibonacci_worker() in the posted sketch, then run the micro-ROS agent command and the ros2 action send_goal command shown in the issue. Trace whether the goal reaches the handler and worker; done means the Fibonacci goal returns feedback and a result instead of hanging, with the LEDs changing as described.

Written by the indexing model from the issue text.