Example app add_two_ints_service does not work on STM32: 1) Incorrect data received and 2) No server response

[TanmayDeshmukh]

Issue template

• Hardware description: Olimex STM32-E407
• RTOS: freeRTOS
• Installation type: micro_ros_setup
• Version or commit hash: galactic

Steps to reproduce the issue

• Follow steps mentioned in micro.ros.org using docker (ros:galactic)
• Configure firmware with udp transport ros2 run micro_ros_setup configure_firmware.sh add_two_ints_service --transport udp --ip <our agent ip> --port 8888
• Build firmware and flash through JTAG, just as in the tutorial.
• Create and run the agent.
• Service call on a new terminal: ros2 service call /addtwoints example_interfaces/srv/AddTwoInts "{a: 5, b: 10}"

Expected behavior

Should get a response (sum=15)

Actual behavior

1. Server receives request but there is no response.
   waiting for service to become available... requester: making request: example_interfaces.srv.AddTwoInts_Request(a=5, b=10)
2. Received request has garbage values for a, and b is always 1.

Additional information

We basically have the same issue as this but with FreeRTOS on our STM32 board.
We using a router from Teltonika- RUT240 (which a decent router), and the computer running the agent and making service calls is connected though Ethernet.

Quick note: I had to include #include <rclc/executor.h> in the example code, and change "printk"s to printf to get it to compile.

Not very well-versed with uROS or DDS stuff, but we tried the following:

1. export RMW_IMPLEMENTATION=rmw_fastrtps_cpp before starting the agent, as suggested in this thread.
2. Making the response and request variables global suggested in this thread.

Logs from the agent:

$ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6
[1646402745.031716] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1646402745.032007] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1646402745.756195] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 09 6C 59 A4 81 00 FC 03
[1646402745.756401] info     | Root.cpp           | create_client            | create                 | client_key: 0x096C59A4, session_id: 0x81
[1646402745.756505] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x096C59A4, address: 192.168.10.18:12020
[1646402745.756658] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1646402745.757560] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x096C59A4, len: 56, data: 
0000: 81 80 00 00 01 07 2E 00 00 0A 00 01 01 03 00 00 20 00 00 00 00 01 00 20 18 00 00 00 61 64 64 5F
0020: 74 77 6F 69 6E 74 73 5F 73 65 72 76 65 72 5F 72 63 6C 63 00 00 00 00 00
[1646402745.771891] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x096C59A4, participant_id: 0x000(1)
[1646402745.771982] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1646402745.771996] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1646402745.772787] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x096C59A4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1646402745.773173] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x096C59A4, len: 208, data: 
0000: 81 80 01 00 01 07 C5 00 00 0B 00 08 08 03 00 00 B7 00 00 00 0C 00 00 00 2F 61 64 64 74 77 6F 69
0020: 6E 74 73 00 33 00 00 00 65 78 61 6D 70 6C 65 5F 69 6E 74 65 72 66 61 63 65 73 3A 3A 73 72 76 3A
0040: 3A 64 64 73 5F 3A 3A 41 64 64 54 77 6F 49 6E 74 73 5F 52 65 71 75 65 73 74 5F 00 08 34 00 00 00
0060: 65 78 61 6D 70 6C 65 5F 69 6E 74 65 72 66 61 63 65 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 41
0080: 64 64 54 77 6F 49 6E 74 73 5F 52 65 73 70 6F 6E 73 65 5F 00 01 23 03 08 15 00 00 00 72 71 2F 61
00A0: 64 64 74 77 6F 69 6E 74 73 52 65 71 75 65 73 74 00 01 00 00 13 00 00 00 72 72 2F 61 64 64 74 77
00C0: 6F 69 6E 74 73 52 65 70 6C 79 00 00 01 00 00 00
[1646402745.773799] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x096C59A4, requester_id: 0x000(7), participant_id: 0x000(1)
[1646402745.773880] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 08 00 00
[1646402745.773894] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1646402745.774660] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x096C59A4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1646402745.774678] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x096C59A4, len: 24, data: 
0000: 81 80 02 00 08 01 10 00 00 0C 00 08 80 00 00 01 FF FF 00 00 00 00 00 00
[1646402745.774818] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1646402768.908714] debug    | Replier.cpp        | read_fn                  | [==>> DDS <<==]        | client_key: 0x00000000, len: 56, data: 
0000: 69 F5 10 01 90 E8 E4 52 F3 48 EC 0C 00 00 14 03 00 00 00 00 01 00 00 00 AB CE 57 FA 4A 37 B3 F7
0020: 01 00 00 00 00 00 00 00 05 00 00 00 00 00 00 00 0A 00 00 00 00 00 00 00
[1646402768.908912] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 68, data: 
0000: 81 80 02 00 09 01 3C 00 00 0C 00 08 69 F5 10 01 90 E8 E4 52 F3 48 EC 0C 00 00 14 03 00 00 00 00
0020: 01 00 00 00 AB CE 57 FA 4A 37 B3 F7 01 00 00 00 00 00 00 00 05 00 00 00 00 00 00 00 0A 00 00 00
0040: 00 00 00 00
[1646402768.973465] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x096C59A4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1646402768.977226] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x096C59A4, len: 44, data: 
0000: 81 80 03 00 07 01 24 00 00 0D 00 08 69 F5 10 01 90 E8 E4 52 F3 48 EC 0C 00 00 14 03 00 00 00 00
0020: 01 00 00 00 AC CE 57 FA 4A 37 B3 F7
[1646402768.977451] debug    | Replier.cpp        | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 32, data: 
0000: 69 F5 10 01 90 E8 E4 52 F3 48 EC 0C 00 00 14 03 00 00 00 00 01 00 00 00 AC CE 57 FA 4A 37 B3 F7
[1646402768.977570] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x096C59A4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80

[jcmonteiro]

Service Request

Looks like there is some incorrect behavior either

1. in the serialization happening on the agent, or
2. in extracting the UDP data from the UDP packet.

Running the agent in debug mode shows that the request of the AddTwoInts, two int64_t integers of 8 bytes each, is transmitted in a UDP packet containing 68 bytes (8 for the header + 60 for the data), thus 60 bytes of data instead of 16. The values of variables int64_t a and int64_t b are present in the last 16 bytes of the UDP packet. I don't know if this is intended behavior, but it is not matching the deserialization happening in the MCU side.

The MCU receives the UDP packet when the service is called and writes the last 32 bytes in a buffer. This can be validated by adding a breakpoint at rmw_microxrcedds/rmw_microxrcedds_c/src/rmw_request.c#L124 and validating the contents of static_buffer->buffer and static_buffer->length.

The buffer is then sent for deserialization, but it deserialized incorrectly since it expects the data to be at the beginning of the buffer and, in this case, the data is present after a 16 bytes offset.

Service Response

I still need to dig a little deeper, but there might be a similar issue (namely inconsistent serialization/deserialization) happening on the response. I say this because running the agent in debug mode clearly shows that the agent receives a UDP packet containing the a response. The data, int64_t sum, is present in the last 8 bytes of a 32 bytes UDP packet (8 bytes header + 16 bytes unknown + 8 bytes data).

[jcmonteiro]

I am pasting below a sample of the exchange reported by the agent when a service is called. Note that I have manually shifted the buffer in rmw_microxrcedds/rmw_microxrcedds_c/src/rmw_request.c#L122, i.e. changed the line to static_buffer->buffer + 16 to read from the correct address.

[1647331268.554430] debug    | Replier.cpp         | read_fn                  | [==>> DDS <<==]        | client_key: 0x00000000, len: 56, data: 
0000: 35 A7 10 01 E7 EC 5B 14 9F 07 04 2B 00 00 14 03 00 00 00 00 01 00 00 00 A3 3D 25 01 73 6C D9 63
0020: 01 00 00 00 00 00 00 00 05 00 00 00 00 00 00 00 07 00 00 00 00 00 00 00

[1647331268.554594] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3C4C11D9, len: 68, data: 
0000: 81 80 02 00 09 01 3C 00 00 0C 00 08 35 A7 10 01 E7 EC 5B 14 9F 07 04 2B 00 00 14 03 00 00 00 00
0020: 01 00 00 00 A3 3D 25 01 73 6C D9 63 01 00 00 00 00 00 00 00 05 00 00 00 00 00 00 00 07 00 00 00
0040: 00 00 00 00

[1647331268.555816] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3C4C11D9, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80

[1647331272.343187] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3C4C11D9, len: 44, data: 
0000: 81 80 03 00 07 01 24 00 00 0D 00 08 35 A7 10 01 E7 EC 5B 14 9F 07 04 2B 00 00 14 03 00 00 00 00
0020: 01 00 00 00 A4 3D 25 01 73 6C D9 63

[1647331272.343431] debug    | Replier.cpp         | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 32, data: 
0000: 35 A7 10 01 E7 EC 5B 14 9F 07 04 2B 00 00 14 03 00 00 00 00 01 00 00 00 A4 3D 25 01 73 6C D9 63

[1647331272.343553] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3C4C11D9, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80

[jcmonteiro]

I just noticed that the IP header is also included (minimum 20 bytes and maximum 60 bytes depending on the options). So, could it be that those "extra" bytes are part of the header being misinterpreted as data?

[jcmonteiro]

Following up on this, the latest news is that the services work as expected if we change the middleware implementation by prefixing the commands with RMW_IMPLEMENTATION=rmw_fastrtps_cpp, e.g., RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 run micro_ros_agent micro_ros_agent [options]. Naturally, the same could be achieved by changing the environment variable.

If we do not change to FastRTPS, then the default one in Galactic is used, namely Cyclone DDS. I am showing below the difference in the exchange, as reported by the agent. As it turns out, it looks like the actual issue here is that the DDS message is formatted differently when using Cyclone DDS and this is not accounted for / understood by the agent. My knowledge of DDS is limited, so I do not really know what the additional 16 bytes present in the DDS message from Cyclone DDS stand for. But it really looks like this is the root cause of both issues.

1. Incorrect deserialization in the MCU. My assumption is that the root cause is the additional 16 bytes present in the DDS message when using Cyclone DDS. You can see that the additional 16 bytes (which I can only assume are part of the header) are transferred to the data part of the UDP packet. The packet is properly deserialized in the MCU, but the data was incorrectly translated from DDS to UDP in the agent.
2. No response received by the service client. The MCU is sending the correct UDP packet to the agent. The agent then translates from UDP to DDS. However, Cyclone DDS is expecting 16 additional bytes (again, I am assuming as part of a header field). Since the bytes are not present, the communication fails and the client never gets the message.

## FastDDS
## Sending service request
[1647417016.074674] debug    | Replier.cpp        | read_fn                  | [==>> DDS <<==]        | client_key: 0x00000000, len: 40, data:
0000: 01 0F 6F 52 0E 2D 30 C4 01 00 00 00 00 00 12 03 00 00 00 00 01 00 00 00 08 00 00 00 00 00 00 00
0020: 07 00 00 00 00 00 00 00
[1647417016.074733] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x1632FAA9, len: 52, data:
0000: 81 80 03 00 09 01 2C 00 00 0C 00 08 01 0F 6F 52 0E 2D 30 C4 01 00 00 00 00 00 12 03 00 00 00 00
0020: 01 00 00 00 08 00 00 00 00 00 00 00 07 00 00 00 00 00 00 00
[1647417016.075906] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x1632FAA9, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80

## Receiving service response
[1647417058.588360] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x1632FAA9, len: 44, data:
0000: 81 80 04 00 07 01 24 00 00 0E 00 08 01 0F 6F 52 0E 2D 30 C4 01 00 00 00 00 00 12 03 00 00 00 00
0020: 01 00 00 00 0F 00 00 00 00 00 00 00
[1647417058.588563] debug    | Replier.cpp        | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 32, data:
0000: 01 0F 6F 52 0E 2D 30 C4 01 00 00 00 00 00 12 03 00 00 00 00 01 00 00 00 0F 00 00 00 00 00 00 00
[1647417058.588614] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x1632FAA9, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80

### Cyclone
## Sending service request
[1647417175.149524] debug    | Replier.cpp        | read_fn                  | [==>> DDS <<==]        | client_key: 0x00000000, len: 56, data:
0000: 21 F8 10 01 2B 13 F2 1B 13 0C 69 22 00 00 14 03 00 00 00 00 01 00 00 00 70 EF FA 99 A6 24 60 85
0020: 01 00 00 00 00 00 00 00 08 00 00 00 00 00 00 00 07 00 00 00 00 00 00 00
[1647417175.149686] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x28E48004, len: 68, data:
0000: 81 80 02 00 09 01 3C 00 00 0C 00 08 21 F8 10 01 2B 13 F2 1B 13 0C 69 22 00 00 14 03 00 00 00 00
0020: 01 00 00 00 70 EF FA 99 A6 24 60 85 01 00 00 00 00 00 00 00 08 00 00 00 00 00 00 00 07 00 00 00
0040: 00 00 00 00
[1647417175.150904] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x28E48004, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80

## Receiving service response
[1647417204.691133] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x28E48004, len: 44, data:
0000: 81 80 03 00 07 01 24 00 00 0D 00 08 21 F8 10 01 2B 13 F2 1B 13 0C 69 22 00 00 14 03 00 00 00 00
0020: 01 00 00 00 71 EF FA 99 A6 24 60 85
[1647417204.691417] debug    | Replier.cpp        | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 32, data:
0000: 21 F8 10 01 2B 13 F2 1B 13 0C 69 22 00 00 14 03 00 00 00 00 01 00 00 00 71 EF FA 99 A6 24 60 85
[1647417204.691584] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x28E48004, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80