How to improve frequency over 60Hz

[MingshanHe]

Issue template

• Hardware description: STM32F446RET6
• RTOS: FreeRTOS
• Installation type: micro_ros_setup
• Version or commit hash: foxy

• Expected behavior

  I want to get the communication frequency over 200Hz, Thus I make the timer count number to be 5 (5ms per time). But it has not worked well as expected. How can I solve this problem?

  Actual behavior

  

Additional information

`/* USER CODE BEGIN 4 */
bool cubemx_transport_open(struct uxrCustomTransport * transport);
bool cubemx_transport_close(struct uxrCustomTransport * transport);
size_t cubemx_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
size_t cubemx_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);

void * microros_allocate(size_t size, void * state);
void microros_deallocate(void * pointer, void * state);
void * microros_reallocate(void * pointer, size_t size, void * state);
void * microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void * state);
rcl_publisher_t sensor_data_publisher;
std_msgs__msg__Int64MultiArray sensors_array_msg;
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        rcl_publish(&sensor_data_publisher, &sensors_array_msg, NULL);
        sensors_array_msg.data.data[0] = 0;
        sensors_array_msg.data.data[0] = sensors_array_msg.data.data[0] + 1;
        sensors_array_msg.data.data[1] = sensors_array_msg.data.data[1] + 2;
        sensors_array_msg.data.data[2] = sensors_array_msg.data.data[2] + 3;
        sensors_array_msg.data.data[3] = sensors_array_msg.data.data[3] + 4;

    }
}

/* USER CODE END 4 */

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN 5 */

  // micro-ROS configuration

  rmw_uros_set_custom_transport(
    true,
    (void *) &huart1,
    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__); 
  }

  // micro-ROS app

  rcl_publisher_t publisher;
  std_msgs__msg__Int32 msg;
  rclc_support_t support;
  rcl_allocator_t allocator;
  rcl_node_t node;

  allocator = rcl_get_default_allocator();

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

  // create node
  rclc_node_init_default(&node, "cubemx_node", "", &support);

  // create publisher
  rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "cubemx_publisher");

  // Int64MultiArray Part:
  // Create Publisher
  // rcl_publisher_t sensor_data_publisher;
  rclc_publisher_init_default(
    &sensor_data_publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64MultiArray),
    "capacitance_sensor_data"
  );
  // Create Timer
  rcl_timer_t timer;
  const unsigned int timer_timeout = 1;
  rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback
  );
  // Create Executor
  rclc_executor_t executor;
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  rclc_executor_add_timer(&executor, &timer);

  // Int64MultiArray Part:
  // Init Message
  // std_msgs__msg__Int64MultiArray sensors_array_msg;
  // Assing Memory to Message
  int64_t buffer[BUFFER_SIZE] = {};
  sensors_array_msg.data.data = buffer;
  sensors_array_msg.data.size = 0;
  sensors_array_msg.data.capacity = BUFFER_SIZE;

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

  char labels[BUFFER_SIZE][STRING_SIZE] = {};
  for(size_t i = 0; i < BUFFER_SIZE; i++){
    sensors_array_msg.layout.dim.data[i].label.data = labels[i];
    sensors_array_msg.layout.dim.data[i].label.size = 0;
    sensors_array_msg.layout.dim.data[i].label.capacity = STRING_SIZE;
  }
  // Fill the messgae with dummy data;
  for(size_t i = 0; i<BUFFER_SIZE; i++){
    sensors_array_msg.data.data[i] = i;
    sensors_array_msg.data.size++;
  }

  sensors_array_msg.layout.data_offset = 42;
  for(size_t i = 0; i<BUFFER_SIZE; i++){
    sensors_array_msg.layout.dim.data[i].label.size = strlen(sensors_array_msg.layout.dim.data[i].label.data);
    sensors_array_msg.layout.dim.data[i].size = 42;
    sensors_array_msg.layout.dim.data[i].stride = 42;
  }

  uint8_t meas1 = 0x01;
  uint32_t sensor_meas = 0;
  FDC1004_readmeas(&hi2c1, &meas1, &sensor_meas);
  msg.data = sensor_meas;

  while(1)
  {
//    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
//    if (ret != RCL_RET_OK)
//    {
//      printf("Error publishing (line %d)\n", __LINE__);
//    }
//
//    FDC1004_readmeas(&hi2c1, &meas1, &sensor_meas);
//    msg.data = sensor_meas;
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(0));
//    osDelay(10);
  }
  /* USER CODE END 5 */
}

[pablogs9]

• Which transport are you using?
• What is the size of sensors_array_msg in Bytes taking into account the allocated memory?

[MingshanHe]

I am using UART Totally 28 int numbers will be sent as array

[pablogs9]

Assuming BUFFER_SIZE = 28:

• sensors_array_msg.data.data -> 28 * 8 B/int64 = 224 B
• sensors_array_msg.layout.dim.data[i].label.data -> 28 * STRING_SIZE

Assuming STRING_SIZE, let's say 10 Bytes we have messages of 504 Bytes.

If you have a baud rate of 115200 bauds, this is approx 115 kbps = 14,3 kBps.

In a channel with 14,3 kBps, you will be able to send packages of 504 B at a rate of 14,3 kBps / 504 B = 28.5 Hz.

Probably you will not be able to reach 200 Hz using a serial port and this message size.

[MingshanHe]

So what is the string size and string used in the multiarray publisher, is it like some fixed layout needed to send the topic message in ROS2? And How about using more UART ports to improve or other comments for improvement in my situation, like other ports?

[pablogs9]

The string used is ROS 2 type dependent, probably you will find more details about the type you are using in ROS 2 documentation.

In any case, you always can create your own type

Regarding the transport, probably a transport with a higher bandwidth would be a better solution if you need those rates with those message sizes.