Handling large action request gets failed

[metanav]

• Hardware description: Arduino Nano RP2040 Connect
• RTOS: Mbed OS
• Installation type: Arduino Library
• Version or commit hash: Humble

I am trying to create a follow_joint_trajectory action server with a joint_states publisher. I am able to publish to the join_states topic. Also, I can receive trajectory goal at the follow_joint_trajectory server. But It seems the action server cannot handle large goal request. The Arduino Nano RP2040 Connect has 264KB memory so I believe it can handle large messages around 10 to 20 KB easily.

Successful request:

$ ros2 action send_goal /arm/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "trajectory: {header: {frame_id: "me"}, joint_names: ['this_is_joint_1', 'this_is_joint_2', 'this_is_joint_3', 'this_is_joint_4', 'this_is_joint_5', 'this_is_joint_6'], points: [{positions: [1, 2, 3, 4, 5, 5]}, {positions: [1, 2, 3, 4, 5, 5]}, {positions: [1, 2, 3, 4, 5, 5]} , {positions: [1, 2, 3, 4, 5, 5]}, {positions: [1, 2, 3, 4, 5, 5]} ] }" 
Waiting for an action server to become available...
Sending goal:
     trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: me
  joint_names:
  - this_is_joint_1
  - this_is_joint_2
  - this_is_joint_3
  - this_is_joint_4
  - this_is_joint_5
  - this_is_joint_6
  points:
  - positions:
    - 1.0
    - 2.0
    - 3.0
    - 4.0
    - 5.0
    - 5.0
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
  - positions:
    - 1.0
    - 2.0
    - 3.0
    - 4.0
    - 5.0
    - 5.0
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
  - positions:
    - 1.0
    - 2.0
    - 3.0
    - 4.0
    - 5.0
    - 5.0
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
  - positions:
    - 1.0
    - 2.0
    - 3.0
    - 4.0
    - 5.0
    - 5.0
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
  - positions:
    - 1.0
    - 2.0
    - 3.0
    - 4.0
    - 5.0
    - 5.0
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
multi_dof_trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  joint_names: []
  points: []
path_tolerance: []
component_path_tolerance: []
goal_tolerance: []
component_goal_tolerance: []
goal_time_tolerance:
  sec: 0
  nanosec: 0

Goal accepted with ID: db855a256a2340e1889c5155946df2a3

Result:
    error_code: 0
error_string: ''

Goal finished with status: ABORTED

Unsuccessful goal request:
After executing the command below, it hangs with the error code = 1.

$ ros2 action send_goal /arm/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "trajectory: {header: {frame_id: "me"}, joint_names: ['this_is_joint_1', 'this_is_joint_2', 'this_is_joint_3', 'this_is_joint_4', 'this_is_joint_5', 'this_is_joint_6'], points: [{positions: [1, 2, 3, 4, 5, 5]}, {positions: [1, 2, 3, 4, 5, 5]}, {positions: [1, 2, 3, 4, 5, 5]} , {positions: [1, 2, 3, 4, 5, 5]}, {positions: [1, 2, 3, 4, 5, 5]},  {positions: [1, 2, 3, 4, 5, 5]}] }" 

The library was built with the modified configuration file colcon_verylowmem.meta (target: cortex_m0)
There are 6 services for 2 action servers but only one is implemented in the current code. The RMW_UXRCE_STREAM_HISTORY is set to 32 for large message buffer but seems not effective.
I have tried to set UCLIENT_SERIAL_TRANSPORT_MTU = 1024 but it didn't help so I removed it from the current config.

...
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=5",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=1",
                "-DRMW_UXRCE_MAX_SERVICES=6",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_STREAM_HISTORY=32",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
...

Please see the code below where memory allocation for the action server goal request is configured.
I have configured it for maximum 10 goal.trajectory.points but the goal request gets failed above 5 goal.trajectory.points. I am expecting 30 or more goal.trajectory.points in a real application.

void create_joint_trajectory_request_message_memory()
{
 // N_JOINTS = 6
  static micro_ros_utilities_memory_conf_t conf = {0};
  conf.max_string_capacity = 50;
  conf.max_basic_type_sequence_capacity = N_JOINTS;

  micro_ros_utilities_memory_rule_t rules[] = {
    {"goal.trajectory.joint_names", N_JOINTS},
    {"goal.trajectory.points", 10}, 
    {"goal.trajectory.points.positions", N_JOINTS},   
    {"goal.trajectory.points.velocities", N_JOINTS},
    {"goal.trajectory.points.accelerations", N_JOINTS},
    {"goal.trajectory.points.effort", N_JOINTS}
  };
  conf.rules = rules;
  conf.n_rules = sizeof(rules) / sizeof(rules[0]);
  
  size_t dynamic_size = micro_ros_utilities_get_dynamic_size(
                          ROSIDL_GET_MSG_TYPE_SUPPORT(control_msgs, action, FollowJointTrajectory_SendGoal_Request),
                          conf
                        );
  size_t message_total_size = dynamic_size + sizeof(control_msgs__action__FollowJointTrajectory_SendGoal_Request);
  
  Serial1.print("Message Size = ");
  Serial1.println(message_total_size);

  for (int i = 0; i < MAX_GOALS; i++) {
    bool success = micro_ros_utilities_create_message_memory(
                     ROSIDL_GET_MSG_TYPE_SUPPORT(control_msgs, action, FollowJointTrajectory_SendGoal_Request),
                     &ros_goal_request[i],
                     conf
                   );

    Serial1.print("success:"); Serial1.println(success);
  }
}

[metanav]

It works If I comment out the publisher implementation from the code and change the handle to 1.

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

So why the publisher (or timer?) does cause the issue?

[Acuadros95]

Consider adding an sleep on your void goal_handler_thread() thread and reducing the sleep on the main loop, as your timer has a period of 10 ms and you are sleeping 100 ms on each executor iteration.

So why the publisher (or timer?) does cause the issue?

Did you modify anything else? What is the output of your serial debug port?

[metanav]

Reducing the sleep ( delay(1) ) on theloop(), adding a sleep 50ms (delay(50)) on the goal_handler_thread() and increasing the timer timeout to 500ms (2Hz) which allows to receive goal.trajectory.points up to ~20. I have changed the rules to accept 50 points to see how much we can achieve {"goal.trajectory.points", 50}. I think there is not much room to adjust sleep so we will have to think of other ways as we need to handle around 40 trajectory points coming from another node.

I have not modified the code besides the above-mentioned changes. The serial debug output prints the Error: 1 when it is not able to receive goals.

I think the timer callback interrupts the message receiving process from the DDS. What is the default serial baud rate to connect to the agent? Would increasing the baud rate help? How can we change it?

[Acuadros95]

From the agent help:

Usage: 'micro_ros_agent <udp4|udp6|tcp4|tpc6|canfd|serial|multiserial|pseudoterminal> <<args>>'

Available arguments (per transport):
  * COMMON
    -h/--help.
    -m/--middleware <value> (ced, dds, rtps) [default: 'dds'].
    -r/--refs <value>.
    -v/--verbose <value> ( - ) [default: ''].
    -d/--discovery <value> [default: '7400'].
  * IPvX (udp4, udp6, tcp4, tcp6)
    -p/--port <value>.
  * SERIAL (serial, multiserial, pseudoterminal)
    -b/--baudrate <value> [default: '115200'].
    -D/--dev <value>.  * CAN FD (canfd)
    -D/--dev <value>.

You can modify the baudrate using the flag -b <value> on the agent arguments.
On Arduino side, you can modify this value on your library: https://github.com/micro-ROS/micro_ros_arduino/blob/humble/src/default_transport.cpp#L36

Anyway, we just discovered a bug regarding services reliability on the agent side. This directly affects action server communication, will keep you updated when the fix is ready.

[Acuadros95]

@metanav The fix is added, could you update your agent and test this again?

[metanav]

Hi @Acuadros95,

I have updated the agent and tested it but it does not fix the issue.

[metanav]

Hi @Acuadros95,

Is there any way to pause/stop the timer when a goal request comes in and resume/start after the goal is completed?

[metanav]

It seems working after increasing RMW_UXRCE_STREAM_HISTORY and regenerating precompiled library.

[madgrizzle]

@metanav I've run into the same issue. You mentioned earlier on that you tried 32 for the RMW_UXRCE_STREAM HISTORY and it didn't seem to have an effect, but your last post said you increased it AND regenerated the precompiled library. Did you stick with 32 and it didn't take effect until you regenerated the precompiled library, or did you end up going beyond 32, or both?

[madgrizzle]

To answer my own question, yes, both need to be done. Only make sure if you are using platform.io that you do not use a tab when editing or creating custom meta files as it will result in the meta file being ignored when you manage to force micro-ros to be rebuilt... as I discovered over the last 6 hours.

[metanav]

@madgrizzle I do not remember exactly but I guess it worked after doing both. By the way which manipulator/microcontroller are you using?

[madgrizzle]

Yeah, both are needed as well as a meta file without tabs in it :) I'm using a teensy 4.1 and was able to get well over 100 points sent with 8 joints per point. That part works well at least now.

[metanav]

Yeah, Teensy 4.1 has larger memory to handle it. I was working with Nano RP2040 Connect which has only 1/4 that of Teensy's. But I was able to test up to 60 points which was enough for my case (5 DOF).