Difficulty to create a freertos app that publishes an array

[mateusandradepeixoto]

Hi, I'm a new in ROS world and I'm exploring the freertos apps examples of micro-ROS firmware (https://github.com/micro-ROS/freertos_apps/tree/foxy/apps) and trying to create one that publishes elements of an array. I have basic knowledge about C++ but I'm not getting how to make this works. I'm trying by diffent ways: using static alocation and dynamic alocation.

• Hardware description: ESP32S
• RTOS: FreeRTOS
• Installation type: micro_ros_setup
• Version or commit hash: ROS Foxy

Steps to reproduce the issue

Dynamic alocation:

#include <stdio.h>
#include <unistd.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int64_multi_array.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

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

#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 publisher;

std_msgs__msg__Int64MultiArray msg;

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{

    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
         msg.layout.dim.data[0].label.data++;
         msg.layout.dim.data[1].label.data++;
         msg.layout.dim.data[2].label.data++;
         msg.layout.dim.data[3].label.data++;
    }
}

void appMain(void * arg)
{
    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, "int64_array_publisher", "", &support));

    // create publisher
    RCCHECK(rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64MultiArray),
        "int64_array_publisher"));

    // create timer,
    rcl_timer_t timer;
    const unsigned int timer_timeout = 1000;
    RCCHECK(rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(timer_timeout),
        timer_callback));

    // create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));

msg.data.capacity = 100; 
msg.data.size = 0;
msg.data.data = (int64_t*) malloc(msg.data.capacity * sizeof(int64_t));

msg.layout.dim.capacity = 100;
msg.layout.dim.size = 0;
msg.layout.dim.data = (std_msgs__msg__MultiArrayDimension*) malloc(msg.layout.dim.capacity * sizeof(std_msgs__msg__MultiArrayDimension));

for(size_t i = 0; i < msg.layout.dim.capacity; i++){
    msg.layout.dim.data[i].label.capacity = 100;
    msg.layout.dim.data[i].label.size = 0;
    msg.layout.dim.data[i].label.data = (char*) malloc(msg.layout.dim.data[i].label.capacity * sizeof(char));
}

    while(1){
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
        usleep(100000);
    }

    // free resources
    RCCHECK(rcl_publisher_fini(&publisher, &node))
    RCCHECK(rcl_node_fini(&node))

        vTaskDelete(NULL);
}

Static alocation:

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

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int64_multi_array.h>

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

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

#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 publisher;
std_msgs__msg__Int64MultiArray msg[4];

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
        msg->data.data[0]++, msg->data.data[1]++, msg->data.data[2]++, msg->data.data[3]++;
    }
}

void appMain(void * arg)
{
    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, "int64_array_publisher", "", &support));

    // create publisher
    RCCHECK(rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64MultiArray),
        "int64_array_publisher"));

    // create timer,
    rcl_timer_t timer;
    const unsigned int timer_timeout = 1000;
    RCCHECK(rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(timer_timeout),
        timer_callback));

    // create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));

    msg->data.data[0] = 2;
    msg->data.data[1] = 5;
    msg->data.data[2] = 10;
    msg->data.data[3] = 16;

    while(1){
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
        usleep(100000);
    }

    // free resources
    RCCHECK(rcl_publisher_fini(&publisher, &node))
    RCCHECK(rcl_node_fini(&node))

    vTaskDelete(NULL);
}

Actual behavior

The second one compiles, using these steps: https://medium.com/@SameerT009/connect-esp32-to-ros2-foxy-5f06e0cc64df, but when I use the echo command looks like there is nothing being published on the topic. Anyway, I was using the int32_publisher example like support but I don't know how to deal with Int64MultiArray, I already read https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/ and https://github.com/micro-ROS/micro_ros_arduino/issues/413.

Additional information

[mateusandradepeixoto]

In the second image I don't know why the session is closing and opening.

[pablogs9]

In the first code, have you checked the amount of allocated memory? Maybe 100 is too high.

In the second code, you are not initializing the members of a type, probably the session keeps restarting because of a segfault. Have you checked the ESP32 serial output?

Two tips:

1. If you are familiar with ESP32, I highly recommend using micro_ros_espidf_component
2. Take a look at this tutorial. If you are using Galactic, maybe you can use micro_ros_utilities package for memory handling.

[mateusandradepeixoto]

In the first one I changed allocated memory to 4 and the results is:

I thought that the data will increase because of "msg.layout.dim.data[0].label.data++;"

[mateusandradepeixoto]

How can I initialize the members of a type in the second code?

[pablogs9]

Take a look at this tutorial.

[mateusandradepeixoto]

Ok, now I'm trying to print only the first element of the array

[...]
rcl_publisher_t publisher;
std_msgs__msg__Int64MultiArray msg;

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
        msg.data.data[0]++;
    }
}

void appMain(void * arg)
{
    [...]

    // create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));

static int64_t memory[5];
msg.data.capacity = 5;
msg.data.size = 0;
msg.data.data = memory;

msg.layout.dim.capacity = 5;
msg.layout.dim.size = 0;
msg.layout.dim.data = memory;

for(int32_t i = 0; i < 3; i++){
  msg.data.data = i;
  msg.data.size++;
}

[...]
}

and continue like this:

The other option I developed is:

static int64_t memory[5];
msg.data.capacity = 5;
msg.data.size = 0;
msg.data.data = memory;

msg.layout.dim.capacity = 5;
msg.layout.dim.size = 0;
msg.layout.dim.data = memory;

for(size_t i = 0; i < msg.layout.dim.capacity; i++){
    msg.layout.dim.data[i].label.capacity = 5;
    msg.layout.dim.data[i].label.size = 0;
    msg.layout.dim.data[i].label.data = memory;
}

But didn't work too.

[pablogs9]

DELETED. UPDATED BELOW.

[mateusandradepeixoto]

Hi. Thanks, it works. Actually, I just corrected

for(int32_t i = 0; i < 3; i++){
  msg.data.data = i;
  msg.data.size++;
}

to

for(int32_t i = 0; i < 4; i++){
    msg.data.data[i] = i;
    msg.data.size++;
    }

In your code, what is the importance of these parts?

char labels[BUFFER_SIZE][STRING_SIZE] = {};
for (size_t i = 0; i < BUFFER_SIZE; i++)
{
    msg.layout.dim[i].label.data = labels[i];
    msg.layout.dim[i].label.size = 0;
    msg.layout.dim[i].label.capacity = STRING_SIZE;
}

[...]

msg.layout.data_offset = 42;
for (size_t i = 0; i < BUFFER_SIZE; i++)
{
    snprintf(msg.layout.dim[i].label.data, STRING_SIZE, "label_%lu", i);
    msg.layout.dim[i].label.size = strlen(msg.layout.dim[i].label.data);
    msg.layout.dim[i].size = 42;
    msg.layout.dim[i].stride = 42;
}

My app published without them.

Thank you very much for the help!

[mateusandradepeixoto]

I tested your code, but I edit somethings like:

include <std_msgs/msg/multi_array_dimension.h>

std_msgsmsgMultiArrayDimension dim[BUFFER_SIZE] = {};

The complete code:

#include <stdio.h>
#include <unistd.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int64_multi_array.h>
#include <std_msgs/msg/multi_array_dimension.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#define BUFFER_SIZE 10
#define STRING_SIZE 30

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

#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 publisher;
std_msgs__msg__Int64MultiArray msg;

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
        msg.data.data[0] = msg.data.data[0] + 1;
        msg.data.data[1] = msg.data.data[1] + 2;
        msg.data.data[2] = msg.data.data[2] + 3;
        msg.data.data[3] = msg.data.data[3] + 4;

    }
}

void appMain(void * arg)
{
    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, "int64_array_publisher", "", &support));

    // create publisher
    RCCHECK(rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64MultiArray),
        "int64_array_publisher"));

    // create timer,
    rcl_timer_t timer;
    const unsigned int timer_timeout = 1000;
    RCCHECK(rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(timer_timeout),
        timer_callback));

    // create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));

    int64_t buffer[BUFFER_SIZE] = {};
    msg.data.data = buffer;
    msg.data.size = 0;
    msg.data.capacity = BUFFER_SIZE;

    std_msgs__msg__MultiArrayDimension dim[BUFFER_SIZE] = {};
    msg.layout.dim.data = dim;
    msg.layout.dim.size = 0;
    msg.layout.dim.capacity = BUFFER_SIZE;

    char labels[BUFFER_SIZE][STRING_SIZE] = {};
    for (size_t i = 0; i < BUFFER_SIZE; i++)
    {
    msg.layout.dim[i].label.data = labels[i];
    msg.layout.dim[i].label.size = 0;
    msg.layout.dim[i].label.capacity = STRING_SIZE;
    }

    // Fill the message with dummy data
    for (size_t i = 0; i < BUFFER_SIZE; i++)
    {
    msg.data.data[i] = i;
    msg.data.size++;
    }       

    msg.layout.data_offset = 42;
    for (size_t i = 0; i < BUFFER_SIZE; i++)
    {
    snprintf(msg.layout.dim[i].label.data, STRING_SIZE, "label_%lu", i);
    msg.layout.dim[i].label.size = strlen(msg.layout.dim[i].label.data);
    msg.layout.dim[i].size = 42;
    msg.layout.dim[i].stride = 42;
    }

    while(1){
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
        usleep(100000);
    }

    // free resources
    RCCHECK(rcl_publisher_fini(&publisher, &node))
    RCCHECK(rcl_node_fini(&node))

        vTaskDelete(NULL);
}

The error:

[pablogs9]

Updated and tested code:

  #include <std_msgs/msg/int64_multi_array.h>
  #include <std_msgs/msg/multi_array_dimension.h>

  #define BUFFER_SIZE 10
  #define STRING_SIZE 30

  std_msgs__msg__Int64MultiArray msg;

  // Assing memory to msg
  int64_t buffer[BUFFER_SIZE] = {};
  msg.data.data = buffer;
  msg.data.size = 0;
  msg.data.capacity = BUFFER_SIZE;

  std_msgs__msg__MultiArrayDimension dim[BUFFER_SIZE] = {};
  msg.layout.dim.data = dim;
  msg.layout.dim.size = 0;
  msg.layout.dim.capacity = BUFFER_SIZE;

  char labels[BUFFER_SIZE][STRING_SIZE] = {};
  for (size_t i = 0; i < BUFFER_SIZE; i++)
  {
    msg.layout.dim.data[i].label.data = labels[i];
    msg.layout.dim.data[i].label.size = 0;
    msg.layout.dim.data[i].label.capacity = STRING_SIZE;
  }

  // Fill the message with dummy data
  for (size_t i = 0; i < BUFFER_SIZE; i++)
  {
    msg.data.data[i] = i;
    msg.data.size++;
  }

  msg.layout.data_offset = 42;
  for (size_t i = 0; i < BUFFER_SIZE; i++)
  {
    snprintf(msg.layout.dim.data[i].label.data, STRING_SIZE, "label_%lu", i);
    msg.layout.dim.data[i].label.size = strlen(msg.layout.dim.data[i].label.data);
    msg.layout.dim.data[i].size = 42;
    msg.layout.dim.data[i].stride = 42;
  }

[mateusandradepeixoto]

It's worked, thanks!