nav_msgs msg Odometry publish error

[cocodmdr]

Hello,

I am using last software (02 March 2021) with Olimex e407, freeRTOS and transport serial. I modified the ping pong app in order to use nav_msgs msg Odometry instead of std_msgs msg Header.

Ping is published only once and then agent continously throw this error :

ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/serial/by-id/usb-FTDI_TTL232R-3V3_FTBT9XBM-if00-port0

[1614762536.858473] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1614762536.858646] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1614762505.989146] error    | OutputMessage.cpp  | log_error                | serialization error    | buffer: 
0000: 81 01 00 00 09 01 EC 02 00 14 00 06 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0020: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0040: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0060: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0080: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00A0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00C0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00E0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0100: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0120: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0140: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0160: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
0180: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
01A0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
01C0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
01E0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

Here is my app.c :

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/header.h>

#include <nav_msgs/msg/odometry.h>

#include <stdio.h>
#include <unistd.h>
#include <time.h>

#ifdef ESP_PLATFORM
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#endif

#define STRING_BUFFER_LEN 50

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); vTaskDelete(NULL);}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

rcl_publisher_t ping_publisher;
rcl_publisher_t pong_publisher;
rcl_subscription_t ping_subscriber;
rcl_subscription_t pong_subscriber;

nav_msgs__msg__Odometry incoming_ping;
nav_msgs__msg__Odometry outcoming_ping;
nav_msgs__msg__Odometry incoming_pong;

int device_id;
int seq_no;
int pong_count;

void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);

    if (timer != NULL) {

        seq_no = rand();
        sprintf(outcoming_ping.header.frame_id.data, "%d_%d", seq_no, device_id);
        outcoming_ping.header.frame_id.size = strlen(outcoming_ping.header.frame_id.data);

        // Fill the message timestamp
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);
        outcoming_ping.header.stamp.sec = ts.tv_sec;
        outcoming_ping.header.stamp.nanosec = ts.tv_nsec;

        sprintf(outcoming_ping.child_frame_id.data, "%d_%d", seq_no, device_id);
        outcoming_ping.child_frame_id.size = strlen(outcoming_ping.child_frame_id.data);

        outcoming_ping.pose.pose.orientation.x = 0.0;
        outcoming_ping.pose.pose.orientation.y = 0.0;
        outcoming_ping.pose.pose.orientation.z = 0.0;
        outcoming_ping.pose.pose.orientation.w = 1.0;

        // Reset the pong count and publish the ping message
        pong_count = 0;
        rcl_publish(&ping_publisher, (const void*)&outcoming_ping, NULL);
        printf("Ping send seq %s\n", outcoming_ping.header.frame_id.data);
    }
}

void ping_subscription_callback(const void * msgin)
{
    const nav_msgs__msg__Odometry * msg = (const nav_msgs__msg__Odometry *)msgin;

    // Dont pong my own pings
    if(strcmp(outcoming_ping.header.frame_id.data, msg->header.frame_id.data) != 0){
        printf("Ping received with seq %s. Answering.\n", msg->header.frame_id.data);
        rcl_publish(&pong_publisher, (const void*)msg, NULL);
    }
}

void pong_subscription_callback(const void * msgin)
{
    const nav_msgs__msg__Odometry * msg = (const nav_msgs__msg__Odometry *)msgin;

    if(strcmp(outcoming_ping.header.frame_id.data, msg->header.frame_id.data) == 0) {
        pong_count++;
        printf("Pong for seq %s (%d)\n", msg->header.frame_id.data, pong_count);
    }
}

void appMain(void *argument)
{
    rcl_allocator_t allocator = rcl_get_default_allocator();
    rclc_support_t support;

    // create init_options
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

    // create node
    rcl_node_t node;
    RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support));

    // Create a reliable ping publisher
    RCCHECK(rclc_publisher_init_default(&ping_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/microROS/ping"));

    // Create a best effort pong publisher
    RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/microROS/pong"));

    // Create a best effort ping subscriber
    RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/microROS/ping"));

    // Create a best effort  pong subscriber
    RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/microROS/pong"));

    // Create a 1 seconds ping timer timer,
    rcl_timer_t timer;
    RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(1000), ping_timer_callback));

    // Create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));
    RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping, &ping_subscription_callback, ON_NEW_DATA));
    RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong, &pong_subscription_callback, ON_NEW_DATA));

    // Create and allocate the pingpong messages
    char outcoming_ping_buffer[STRING_BUFFER_LEN];
    outcoming_ping.header.frame_id.data = outcoming_ping_buffer;
    outcoming_ping.header.frame_id.capacity = STRING_BUFFER_LEN;
    outcoming_ping.child_frame_id.data = outcoming_ping_buffer;
    outcoming_ping.child_frame_id.capacity = STRING_BUFFER_LEN;

    char incoming_ping_buffer[STRING_BUFFER_LEN];
    incoming_ping.header.frame_id.data = incoming_ping_buffer;
    incoming_ping.header.frame_id.capacity = STRING_BUFFER_LEN;
    incoming_ping.child_frame_id.data = incoming_ping_buffer;
    incoming_ping.child_frame_id.capacity = STRING_BUFFER_LEN;

    char incoming_pong_buffer[STRING_BUFFER_LEN];
    incoming_pong.header.frame_id.data = incoming_pong_buffer;
    incoming_pong.header.frame_id.capacity = STRING_BUFFER_LEN;
    incoming_pong.child_frame_id.data = incoming_pong_buffer;
    incoming_pong.child_frame_id.capacity = STRING_BUFFER_LEN;

    device_id = rand();

    while(1){
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
        usleep(10000);
    }

    // Free resources
    RCCHECK(rcl_publisher_fini(&ping_publisher, &node));
    RCCHECK(rcl_publisher_fini(&pong_publisher, &node));
    RCCHECK(rcl_subscription_fini(&ping_subscriber, &node));
    RCCHECK(rcl_subscription_fini(&pong_subscriber, &node));
    RCCHECK(rcl_node_fini(&node));
}

Thank you !

[pablogs9]

Ok what is happening here is a bit complex:

1. You are mixing best effort and reliable pub/subs
2. XRCE (the micro-ROS middleware) handles both of them differently, you can have more information in here
3. In summary:
   • reliable streams (used for reliable pubs and subs) have an internal memory of 4 slots of 512 B (by default). When a message is bigger than that the message is fragmented in these slots.
   • best-effort streams (used for best-effort pubs and subs) only have an internal memory of 512 B (by default). And they don't support fragmentation.
4. You are using a >700 B message and you are publishing it correctly, but when the subscriber in the agent receives the message and tries to send it back to the client it can't serialize >700 B in the 512 B stream slot.

Solutions:

1. Use a reliable subscriber:

   RCCHECK(rclc_subscription_init_default(&ping_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/microROS/ping"));

2. Increase the MTU in the app-colcon.meta (this file with:

   {
   "names": {
       "rmw_microxrcedds": {
           "cmake-args": [
   ...
           ]
       },
       "microxrcedds_client":{
           "cmake-args":[
               "-DUCLIENT_UDP_TRANSPORT_MTU=1024"
           ]
       },
   }
   }

[pablogs9]

Please close if you solve the issue.

[cocodmdr]

I tried the second solution : Increase the MTU in the app-colcon.meta It does not work : I still have the same problem.

[cocodmdr]

First solution ( Use a reliable subscriber) works only for some time and then crashes. I tried 3 times :

1. Crashed after 37 seconds
2. Crashed after 42 seconds
3. Crashed after 45 seconds (The ping pong timer runs at 1 Hz)

Thank you so much for your quick help !

[pablogs9]

Are you rebuilding the library?

[cocodmdr]

How do you rebuild the library ? I just ran these commands after making the modifications in the app.c:

ros2 run micro_ros_setup build_firmware.sh
ros2 run micro_ros_setup flash_firmware.sh

[pablogs9]

For modifying the app.c that's enough.

If you want to modify app-colcon.meta you will need to remove some more files. To be sure about increasing the MTU, could you rebuild from scratch?

[cocodmdr]

what do you mind by "rebuild from scratch" ?

• Is it to create a new workspace folder and download everything again ?
• Or remove build and install folder in mcu_ws folder ?
• Or create a new folder my_app with same app.c and app-colcon.meta files ? Then run :

  ros2 run micro_ros_setup configure_firmware.sh my_app --transport serial
  ros2 run micro_ros_setup build_firmware.sh
  ros2 run micro_ros_setup flash_firmware.sh

[pablogs9]

Safest: Is it to create a new workspace folder and download everything again ?

May work: Or remove build and install folder in mcu_ws folder ?

[cocodmdr]

Is it right to use UDP transport here, shouldn't it be SERIAL ? DUCLIENT_UDP_TRANSPORT_MTU=1024

[cocodmdr]

May work: Or remove build and install folder in mcu_ws folder ?

does not work

[pablogs9]

Is it right to use UDP transport here, shouldn't it be SERIAL ? DUCLIENT_UDP_TRANSPORT_MTU=1024

Yes sorry, I made my tests using UDP, but in fact it should be UCLIENT_CUSTOM_TRANSPORT_MTU because FreeRTOS transports are implemented as custom transports.

[cocodmdr]

Then is there a typo here ? : DUCLIENT_UDP_TRANSPORT_MTU=1024 Shoud it be ? : UCLIENT_UDP_TRANSPORT_MTU=1024

UCLIENT_UDP_TRANSPORT_MTU

[cocodmdr]

UCLIENT_CUSTOM_TRANSPORT_MTU=1024

Using this parameter instead does not work either, same problem. Here is my app-colcon.meta file :

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=2",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=2",
                "-DRMW_UXRCE_MAX_SERVICES=0",
                "-DRMW_UXRCE_MAX_CLIENTS=0",
                "-DRMW_UXRCE_MAX_HISTORY=4",
            ]
        },
        "microxrcedds_client":{
            "cmake-args": [
                "-UCLIENT_CUSTOM_TRANSPORT_MTU=1024"
            ]
        }
    }
}

[pablogs9]

You need to use the -D, beacuse they are CMake args:

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=2",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=2",
                "-DRMW_UXRCE_MAX_SERVICES=0",
                "-DRMW_UXRCE_MAX_CLIENTS=0",
                "-DRMW_UXRCE_MAX_HISTORY=4",
            ]
        },
        "microxrcedds_client":{
            "cmake-args": [
                "-DUCLIENT_CUSTOM_TRANSPORT_MTU=1024"
            ]
        }
    }
}

[cocodmdr]

This change triggers an other problem : region `RAM' overflowed by 11264 bytes These are the last log lines of ros2 run micro_ros_setup build_firmware.sh

/home/student/microros_ws/firmware/toolchain/bin/../lib/gcc/arm-none-eabi/8.3.1/../../../../arm-none-eabi/bin/ld: /home/student/microros_ws/firmware/freertos_apps/microros_olimex_e407_extensions/build/micro-ROS.
elf section `.bss' will not fit in region `RAM'                                                                                                               
/home/student/microros_ws/firmware/toolchain/bin/../lib/gcc/arm-none-eabi/8.3.1/../../../../arm-none-eabi/bin/ld: region `RAM' overflowed by 11264 bytes
collect2: error: ld returned 1 exit status                                                               
make: *** [Makefile:368: /home/student/microros_ws/firmware/freertos_apps/microros_olimex_e407_extensions/build/micro-ROS.elf] Error 1

[pablogs9]

Reduce DRMW_UXRCE_MAX_HISTORY to 2 or fit the MTU to something similar to your message size, you are allocating too much static memory, check here and here

[cocodmdr]

Thank you very much ! This seems to be working so far !

What about the first solution you proposed ? It works only for some times and then stops.

1. Use a reliable subscriber:

  RCCHECK(rclc_subscription_init_default(&ping_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/microROS/ping"));

[pablogs9]

Let me reproduce it.

[cocodmdr]

fit the MTU to something similar to your message size

How can I calculate the size of odometry message ? Trial an error with dichotomy ?

[pablogs9]

Kind of tricky but your library should have this symbol: https://github.com/micro-ROS/micro_ros_arduino/blob/811f2caee1728750a9d08e10e6309d1801771875/src/nav_msgs/msg/detail/odometry__rosidl_typesupport_microxrcedds_c.h#L21

[pablogs9]

Any updates on this?

[cocodmdr]

Last time I used it, it was working for approximately 3000 seconds and then it crashed. I have not done further measurements. It is the last week of my internship next week so I hope I can try again.

[pablogs9]

Any updates on this?

[Acuadros95]

Closing due to inactivity, reopen if needed.