Microros int16MultiArray Subscriber Stops Working

[mburaozkan]

I am using microros on my board STM32H7xx that connected to my pc, and my aim is to write a publisher from my pc to control the thrusters connected to my PCA9685 via microcontroller.

The microros is working fine I have tried several publishers and subscribers, but when I try to do the task that I explained above the published messages come to my code and run the thrusters but after a few seconds the thrusters get stuck at a point where I no longer can change the pwm of the thrusters.

I thought that this might be about my subscriber buffer size etc.

This is my code:

Initialize here

int16_t disarm[6] = {0, 0, 0, 0, 0, 0};
// This is how the incoming messages should be like
int16_t arm[6] = {0, 200, 0, 0, 0, 0};
rcl_publisher_t my_pub;
std_msgs__msg__String pub_msg;
std_msgs__msg__Int16MultiArray sub_msg;
const std_msgs__msg__Int16MultiArray *msg;

#define BUFFER_SIZE 10
#define STRING_SIZE 30

My Subs Callback:

void my_subscriber_callback(const void * msgin)
{
    msg = (const std_msgs__msg__Int16MultiArray *)msgin;

    if (msg != NULL)
        motor_driver_drive(msg->data.data);
}

Timer for publisher:

void my_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  rcl_ret_t rc;
  UNUSED(last_call_time);
  if (timer != NULL) {
    rc = rcl_publish(&my_pub, &pub_msg, NULL);
    if (rc == RCL_RET_OK) {
      printf("Published message %s\n", pub_msg.data.data);
    } else {
      printf("Error in timer_callback: Message %s could not be published\n", pub_msg.data.data);
    }
  } else {
    printf("Error in timer_callback: timer parameter is NULL\n");
  }
}

Main:

void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN 5 */

    PCA9685_Init(&hi2c4);
    motor_driver_init();
    motor_driver_drive(disarm);

    rmw_uros_set_custom_transport(
        true,
        (void *) &huart2,
        cubemx_transport_open,
        cubemx_transport_close,
        cubemx_transport_write,
        cubemx_transport_read);

    rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
    freeRTOS_allocator.allocate = microros_allocate;
    freeRTOS_allocator.deallocate = microros_deallocate;
    freeRTOS_allocator.reallocate = microros_reallocate;
    freeRTOS_allocator.zero_allocate =  microros_zero_allocate;

    if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
        printf("Error on default allocators (line %d)\n", __LINE__);
    }

    rcl_allocator_t allocator = rcl_get_default_allocator();
    rclc_support_t support;
    rcl_ret_t rc;

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

    rcl_node_t my_node;
    rc = rclc_node_init_default(&my_node, "iturov_vcu", "iturov", &support);

    const char * topic_name = "topic_0";
    const char * topic_name_sub = "control";
    const rosidl_message_type_support_t * my_type_support =
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String);

    const rosidl_message_type_support_t * my_type_support_i16 =
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray);

    rc = rclc_publisher_init_default(&my_pub, &my_node, my_type_support, topic_name);

    rcl_timer_t my_timer;
    const unsigned int timer_timeout = 1000; // in ms
    rc = rclc_timer_init_default(&my_timer, &support, RCL_MS_TO_NS(timer_timeout), my_timer_callback);

    std_msgs__msg__String__init(&pub_msg);
    const unsigned int PUB_MSG_CAPACITY = 20;
    pub_msg.data.data = malloc(PUB_MSG_CAPACITY);
    pub_msg.data.capacity = PUB_MSG_CAPACITY;
    snprintf(pub_msg.data.data, pub_msg.data.capacity, "Hello World!");
    pub_msg.data.size = strlen(pub_msg.data.data);

    rcl_subscription_t my_sub = rcl_get_zero_initialized_subscription();
    rc = rclc_subscription_init_default(&my_sub, &my_node, my_type_support_i16, topic_name_sub);

    std_msgs__msg__Int16MultiArray__init(&sub_msg);

    int16_t buffer[BUFFER_SIZE] = {};
    sub_msg.data.data = buffer;
    sub_msg.data.size = 0;
    sub_msg.data.capacity = BUFFER_SIZE;

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

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

    rclc_executor_t executor;
    executor = rclc_executor_get_zero_initialized_executor();

    // total number of handles = #subscriptions + #timers
    unsigned int num_handles = 1 + 1;
    rclc_executor_init(&executor, &support.context, num_handles, &allocator);

    rclc_executor_add_timer(&executor, &my_timer);
    rc = rclc_executor_add_subscription(&executor, &my_sub, &sub_msg, &my_subscriber_callback, ON_NEW_DATA);

    for(;;)
    {
        // timeout specified in nanoseconds (here 2s)
        rclc_executor_spin_some(&executor, 100 * (1000 * 1000));
        osDelay(1000);
    }

    // clean up
    rc = rclc_executor_fini(&executor);
    rc += rcl_publisher_fini(&my_pub, &my_node);
    rc += rcl_timer_fini(&my_timer);
    rc += rcl_subscription_fini(&my_sub, &my_node);
    rc += rcl_node_fini(&my_node);
    rc += rclc_support_fini(&support);
    std_msgs__msg__String__fini(&pub_msg);
    std_msgs__msg__Int16MultiArray__fini(&sub_msg);
  /* USER CODE END 5 */
}

Also my publisher is still working even after the subscriber stops recieving messages, so I don't think the proccesor goes into hard fault etc.

Also I checked this issue: https://github.com/micro-ROS/micro_ros_arduino/issues/413

[mburaozkan]

I have wrote the publisher inside to see whether the size exceeds the limit like this

void my_subscriber_callback(const void * msgin)
{
    msg = (const std_msgs__msg__Int16MultiArray *)msgin;

    if (msg != NULL)
    {
        motor_driver_drive(msg->data.data);
        int size = msg->data.size; // Get the size of the received data array
        snprintf(pub_msg.data.data, pub_msg.data.capacity, "Received size: %d", size);
        pub_msg.data.size = strlen(pub_msg.data.data);
        rcl_publish(&my_pub, &pub_msg, NULL);
    }
}

The output is:

->ros2 topic echo /iturov/topic_0

data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---
data: 'Received size: 6'
---

But as I said after some point subscriber callback is not called so the text 'data: 'Received size: 6'' stops publishing

[pablogs9]

using std_msgs__msg__Int16MultiArray__init is not recommended. Probably this is a bad memory initialization of the subscription message, you can read further about this topic here: https://docs.vulcanexus.org/en/latest/rst/tutorials/micro/memory_management/memory_management.html#message-memory

[mburaozkan]

I solved it, the problem was that I did not do the following in IOC file:

-> Set the DMA mode to Circular for Rx