FollowJointTrajectory action server

[ted-miller]

I am having an issue when attempting to create a FollowJointTrajectory action server with MicroRos. My action server code is based on the fibonacci demo in micro-ROS-demos repo. The action client is based on this example.

Issue template

• Hardware description: Yaskawa Motoman YRC1000
• RTOS: VxWorks
• Installation type: Custom
• Version or commit hash: Galactic

uros/rmw-microxrcedds: b71c718832df11f0e5ee13fcf388fc3e62336040 uros/micro-ROS-Agent: installed with micro_ros_setup uros/rosidl_typesupport: 5afedecbf0150027e0d50aba86aef17824d4255e uros/rosidl_typesupport_microxrcedds: 35b1212db08f67c0af727dcb1803cb417b84dcba eProsima/Micro-CDR: c3420b63db2713cc9b9c2b122b46ce3e3dabac92 eProsima/Micro-XRCE-DDS-Client: private fork with customizations for hardware

Steps to reproduce the issue

I have stripped out any hardware-specific code and created an example that will run on Linux.

Server and Client: https://github.com/ted-miller/uros_action_issue

Start Agent:

export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
source galactic
cd microros_ws
source install/local_setup.bash
ros2 run micro_ros_agent micro_ros_agent udp4 --port 2018

Start Server:

source galactic
export RMW_IMPLEMENTATION=rmw_microxrcedds
cd microros_ws
source install/local_setup.bash
ros2 run action_server server

Start Client:

export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
source galactic
cd action_ws
source install/local_setup.bash
ros2 run action_tutorials_cpp example_position

Expected behavior

Console output should indicate that the action was accepted and successful.

Actual behavior

On Linux: On the server side, rcl_action_take_goal_request returns a 1 (failure) and the control_msgs__action__FollowJointTrajectory_SendGoal_Request message does not contain any positions.

On the Motoman: On the server side rcl_action_take_goal_request returns a 1 (failure) and the control_msgs__action__FollowJointTrajectory_SendGoal_Request message does not contain any positions. Additionally, the console output contains three messages stating cannot allocate received sequence in ros_message.

I searched for that error message and found it in msg__type_support_c.c.em. (Also, this is the first time I've ever seen EmberScript...) I tweaked the output message a bit to get more info about the message type and the values that were being compared.

cannot allocate received sequence in ros_message (JointTrajectory)
size=6, capacity = 0

cannot allocate received sequence in ros_message (MultiDOFJointTrajectory)
size=6, capacity = 0

cannot allocate received sequence in ros_message (FollowJointTrajectory_Goal)
size=723236272, capacity = 0

Any help with this would be greatly appreciated. I don't understand what the error message is trying to indicate. Or rather, I guess I don't understand where those message were supposed to have been allocated.

[pablogs9]

Hello @ted-miller,

thanks for using micro-ROS and this feedback.

I have seen that you are using *__create and *__destroy API for handling message memory. Those are not the correct option in micro-ROS (because of the dynamic memory-free type support) instead you have to preallocate memory for the message in use. You can have more information here: https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/

Also, you are using the RCL API, we have created recently an RCLC high-level API for actions based on RCLC real-time executor, but due to compatibility, it is only available in micro-ROS Rolling. If you want to test it you have examples here: https://github.com/micro-ROS/micro-ROS-demos/blob/main/rclc/fibonacci_action_server/main.c

In any case, I'm going to test this and check if I can replicate the issue.

[gavanderhoorn]

I have seen that you are using *__create and *__destroy API for handling message memory. Those are not the correct option in micro-ROS (because of the dynamic memory-free type support) instead you have to preallocate memory for the message in use. You can have more information here: https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/

@pablogs9: you're specifically referring to the micro_ros_utilities package mentioned in the Galactic section, correct?

[pablogs9]

@gavanderhoorn micro_ros_utilities is just a set of utility functions that uses ROS 2 type support introspection for C in order to auto-allocate (or auto-assign memory from a static buffer) memory for a type in micro-ROS. We found them handy for some situations and they work well. The flaws are that you are going to have 2 typesupports in use (more .text, more ram usage) and that in some APIs malloc will be used under the hood and the user should be aware of that.

But in general, the embedded programmer using micro-ROS is encouraged to assign its own memory to micro-ROS types in order to guarantee that all the memory (dynamic or static) is handled by the user.

[gavanderhoorn]

I"m confused, as from a cursory look I'd understood that micro_ros_utilities was provided to avoid having to write all that boilerplate code for allocating each and every (sub)field of your (sub)message.

From what you're writing here, I now get the impression that's in the end what you expect (and recommend) users do?

[pablogs9]

Well, IMHO if you want to handle memory carefully you need to generate your own memory management code.

In any case, don't get me wrong, micro_ros_utilities will work well in systems with malloc (for example you need it for "rules"), enough stack for running recursive functions, and with enough ROM for storing two types of supports.

The point is that micro-ROS need to provide instruction and recommendations both for those 32 kB RAM systems running bare-metal code without full libc support or without heap assignation in linker script and for full-fledged embedded platforms with POSIX-like interfaces and hundreds of RAM/ROM blocks.

[gavanderhoorn]

Please don't misunderstand me, my comment was not criticism, I was just looking for some clarification.

[pablogs9]

I know I know @gavanderhoorn, we always really appreciate your comments :)

[ted-miller]

I have seen that you are using __create and __destroy API for handling message memory. Those are not the correct option in micro-ROS (because of the dynamic memory-free type support) instead you have to preallocate memory for the message in use.

Ok. I'll give it a try with pre-allocating all the sequences. I guess I was thrown off by this comment in the header.

Thank you for the advice!

[ted-miller]

This resolved my issue in the PC test app. I need to restructure things before I can try on the actual hardware.

Thanks for steering me in the right direction. I appreciate the help.

[ted-miller]

So... it turns out that I prematurely closed this issue. I did not test thoroughly enough. (I also didn't realize that stderr was not being displayed on the console by default. Whoops.)

I have updated the server code to use the micro_ros_utilities to preallocate memory. But, it does not solve the root issue. (It did solve my capacity problem though. So that's good.) rcl_action_take_goal_request is still failing. I believe there is an alignment error somewhere within the (de)serialization routine.

I added some additional error output to uros\rosidl_typesupport_microxrcedds\rosidl_typesupport_microxrcedds_c\resource\msg__type_support_c.c.em. This shows the point of failure. It shows that the system is trying to allocate some size based on garbage data.

Server output (running on Ubuntu): (ros2 run action_server server 2>&1)

FollowJointTrajectory - Goal request received
1 cannot allocate received sequence in ros_message FollowJointTrajectory_Goal - path_tolerance
size = 32659, capacity = 25
Failed status on line 106: 1. Continuing.
pending_ros_goal_request->goal.trajectory.points.size = 8

I have verified on the client side that path_tolerance vector is empty before sending the action goal. Source

Client output:

sending goal with 4 points
 multidof joint names size = 0; points size = 0
 path_tolerance size = 0
 component_path_tolerance size = 0
 goal_tolerance size = 0
 component_goal_tolerance size = 0

I also got a wireshark capture of the action-goal message from the Agent to the robot. I don't know if it's helpful or not, as I'm having trouble understanding explicitly how the nested sequence structs get serialized. FJT_Goal.zip

Would you be able to try the example to see if you encounter the same error? (Specific steps are in the OP.) I'm getting this error on two systems (Ubuntu laptop and Motoman robot), so I don't think the problem is specific to my platform.

Any advice would be very helpful.

[Acuadros95]

I have replicated this, and found out that micro_ros_utilities_create_message_memory is not allocating memory properly for string sequences, so the deserialization of pending_ros_goal_request.goal.trajectory.joint_names fails and mess with the alignment.

This needs to be fixed on the micro_ros_utilities packet on our side, but as a workaround you can allocate it manually after micro_ros_utilities_create_message_memory like this:

rcl_allocator_t allocator = rcl_get_default_allocator();

// Fix sequence capacity
size_t sequence_capacity = messageAllocationConfig.max_basic_type_sequence_capacity;
pending_ros_goal_request.goal.trajectory.joint_names.capacity = sequence_capacity;

// Fix memory from string sequence
pending_ros_goal_request.goal.trajectory.joint_names.data = allocator.reallocate(pending_ros_goal_request.goal.trajectory.joint_names.data, 
sequence_capacity * sizeof(pending_ros_goal_request.goal.trajectory.joint_names.data[0]), allocator.state);

// Initialize strings
for (size_t i = 0; i < sequence_capacity; i++)
{
    pending_ros_goal_request.goal.trajectory.joint_names.data[i] = micro_ros_string_utilities_init_with_size(messageAllocationConfig.max_string_capacity);
}

and to free the memory before micro_ros_utilities_destroy_message_memory:

for (size_t i = 0; i < sequence_capacity; i++)
{
    micro_ros_string_utilities_destroy(&pending_ros_goal_request.goal.trajectory.joint_names.data[i]);
}

[ted-miller]

Thank you for finding the error and the suggested work around. I am now able to receive the trajectory points as expected.

[pablogs9]

We have prepared this patch: https://github.com/micro-ROS/micro_ros_utilities/pull/18. Could you check that it solves this issue with string sequences?

[ted-miller]

Great. I'll test on Mon or Tues.

[ted-miller]

I did a quick initial test and it worked great. I'll be hitting it harder with more testing/debug over the next week or two. Thanks for the great support!