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

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

Start with apps/add_two_ints_service/app.c, particularly the request and response variables referenced at line 47, and reproduce the issue using the FreeRTOS tutorial, UDP configuration, and the shown service call. Compare the received values with the expected request and verify that the service returns sum=15; no test file is mentioned in the issue.