ActionServer with mutex locks not working

[bryangd34]

Good day all,

ROS2 system: Ubuntu 22, Humble, RaspberryPi4

I am trying to get a basic action server working in microros on my Pico. By "basic", I mean something as simple as possible to show that the action server is set up correctly.

When I call the action server, it never completes successfully. On the client side (being called from the Ubuntu 22 system) the action request is seen as hanging indefinitely.

Snippets of my Pico code (I'll post the entire code if requested):

First:

rcl_action_server_t action_server_yaw;
mutex_t yaw_mutex;

Action handlers:

void * yaw_handler(void * args)
{
  (void) args;
  rclc_action_goal_handle_t * goal_handle = (rclc_action_goal_handle_t *) args;
  rcl_action_goal_state_t goal_state;

  gimbal_read_interface__action__GimbalYaw_SendGoal_Request * req =
    (gimbal_read_interface__action__GimbalYaw_SendGoal_Request *) goal_handle->ros_goal_request;

  gimbal_read_interface__action__GimbalYaw_GetResult_Response response = {1};

  goal_state = GOAL_STATE_SUCCEEDED;

  response.result.success = 1;

  rcl_ret_t rc;

  do {
  mutex_enter_blocking(&yaw_mutex);
  rc = rclc_action_send_result(goal_handle, goal_state, &response);
  mutex_exit(&yaw_mutex);
  sleep_us(1000000);
  } while (rc != RCL_RET_OK);
}

rcl_ret_t handle_goal_yaw(rclc_action_goal_handle_t * goal_handle, void * context)
{
  (void) context;
  gimbal_read_interface__action__GimbalYaw_SendGoal_Request * req =
    (gimbal_read_interface__action__GimbalYaw_SendGoal_Request *) goal_handle->ros_goal_request;

  yaw_handler(goal_handle);
  return RCL_RET_ACTION_GOAL_ACCEPTED;
}

Then in the main():

// Initialize the mutex
mutex_init(&yaw_mutex);

and

// Initialize action server yaw
rclc_action_server_init_default(&action_server_yaw, &node, &support, ROSIDL_GET_ACTION_TYPE_SUPPORT(gimbal_read_interface, GimbalYaw), "/move_gimbal_yaw");

rclc_executor_init(&executor, &support.context, 4, &allocator);

// Action message objects
gimbal_read_interface__action__GimbalYaw_SendGoal_Request ros_goal_request[10];

rclc_executor_add_action_server(
    &executor, &action_server_yaw, 10, ros_goal_request,
    sizeof(gimbal_read_interface__action__GimbalYaw_SendGoal_Request),
    handle_goal_yaw, handle_cancel_yaw, (void *) &action_server_yaw);

while (true)
{
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
    sleep_us(1000);
}

If anyone has some advice, or suggestions of things to try to further understand the problem, that'd be greatly appreciated!

Edit: while (rc != RCL_RET_OK); clearly gets stuck, but RCL_RET_OK should come back as true for such a simple test. Also I can remove the while loop altogether, and the client still hangs and doesn't get a meaningful return from the server.

[bryangd34]

Further debugging info that I have found since:
rc = rclc_action_send_result(goal_handle, goal_state, &response); never returns, so the Pico gets stuck here.
@pablogs9 do have any suggestions please? It would be greatly appreciated, thanks.