micro_ros_stm32cubemx_utils How to increase a published topic frame rate faster than best effort publisher

Hardware description: STM32F427 on RoboMaster Development Board Type A
RTOS: FreeRTOS
Installation type: micro_ros_stm32cubemx_utils
Version or commit hash: Galactic

Steps to reproduce the issue

Creat a odometry best effort publisher

    // create odometry_publisher
    RCCHECK(rclc_publisher_init_best_effort(
            &odometry_publisher,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
            "odometry")
    );

Creat a 100 Hz timer

    rcl_timer_t publish_100hz_timer;
    RCCHECK(rclc_timer_init_default(&publish_100hz_timer, &support, RCL_MS_TO_NS(1), publish_100hz_timer_callback));
    RCCHECK(rclc_executor_add_timer(&executor, &publish_100hz_timer));

And the timer callback:

void publish_100hz_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    if(timer != NULL) {
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);

        nav_msgs__msg__Odometry odometry_msg = {};
        odometry_msg->header.stamp.sec = ts.tv_sec;
        odometry_msg->header.stamp.nanosec = ts.tv_nsec;
        static char odometry_frame_id[] = "odom";
        static char odometry_child_frame_id[] = "base_link";
        odometry_msg->header.frame_id.data = odometry_frame_id;
        odometry_msg->header.frame_id.size = strlen(odometry_msg->header.frame_id.data);
        odometry_msg->header.frame_id.capacity = sizeof(odometry_frame_id);
        odometry_msg->child_frame_id.data = odometry_child_frame_id;
        odometry_msg->child_frame_id.size = strlen(odometry_child_frame_id);
        odometry_msg->child_frame_id.capacity = sizeof(odometry_child_frame_id);
        RCSOFTCHECK(rcl_publish(&odometry_publisher, &odometry_msg, NULL));
    }
}

ros spin

    while (true) {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));
        osDelay(1);
    }

4.Start Microros agent with 1500000 baud rate on serial:

ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/serial/by-id/usb-FTDI_USB__-__Serial-if00-port0 -b 1500000

5.Use topic hz to watch

ros2 topic hz /odometry

Expected behavior

Expect receive the odometry msg in 100 Hz

Actual behavior

Actually only 50 Hz:

➜  interfaces_ws ros2 topic hz /odometry
WARNING: topic [/odometry] does not appear to be published yet
average rate: 49.933
	min: 0.014s max: 0.026s std dev: 0.00571s window: 51
average rate: 49.947
	min: 0.014s max: 0.026s std dev: 0.00577s window: 102
average rate: 49.964
	min: 0.013s max: 0.026s std dev: 0.00579s window: 153
average rate: 49.974
	min: 0.013s max: 0.027s std dev: 0.00581s window: 204
average rate: 49.978
	min: 0.013s max: 0.027s std dev: 0.00582s window: 255
average rate: 49.982
	min: 0.013s max: 0.027s std dev: 0.00583s window: 306
average rate: 49.983
	min: 0.013s max: 0.027s std dev: 0.00583s window: 357
average rate: 49.987
	min: 0.013s max: 0.027s std dev: 0.00583s window: 408
average rate: 49.988
	min: 0.013s max: 0.027s std dev: 0.00583s window: 459
average rate: 49.989
	min: 0.013s max: 0.027s std dev: 0.00583s window: 510
average rate: 49.995

Is 50 Hz the fatest rate? If not, how to increase the publish frame rate to reach the serial line's potential?

Additional information

When I used reliable publisher, it`s only 27 Hz:

    // create odometry_publisher
    RCCHECK(rclc_publisher_init_default(
            &odometry_publisher,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
            "odometry")
    );

And the output:

➜  interfaces_ws ros2 topic hz /odometry
average rate: 26.999
	min: 0.037s max: 0.038s std dev: 0.00023s window: 29
average rate: 27.006
	min: 0.037s max: 0.038s std dev: 0.00020s window: 57
average rate: 27.015
	min: 0.037s max: 0.038s std dev: 0.00018s window: 85
average rate: 27.123
	min: 0.036s max: 0.038s std dev: 0.00038s window: 113
average rate: 27.105
	min: 0.036s max: 0.038s std dev: 0.00035s window: 141
average rate: 27.085
	min: 0.036s max: 0.038s std dev: 0.00035s window: 168
average rate: 27.077
	min: 0.036s max: 0.038s std dev: 0.00033s window: 196

I'm not a native English speaker, and new to the ros. Hope my behavior not so bad.

Jul 30 '22 08:07 Puyu2934

Hello, @Puyu2934:

In reliable mode is normal to have a lower rate due to the reliability messages, if you need high rates, just use best effort mode.
Check the messages you are using, according to my calculations, this message has about 711 B per message (check those covariance vectors). If your need to send 711 B at 100 Hz: 711 B = 5688 bits -> 100 Hz -> 568800 bps. If your channel provides 115200 bauds/s that means that your bandwidth is not enough. I recommend using a lighter message type in order to increase the rate.

Aug 01 '22 06:08 pablogs9

Hello, @Puyu2934:

In reliable mode is normal to have a lower rate due to the reliability messages, if you need high rates, just use best effort mode.

Check the messages you are using, according to my calculations, this message has about 711 B per message (check those covariance vectors). If your need to send 711 B at 100 Hz: 711 B = 5688 bits -> 100 Hz -> 568800 bps. If your channel provides 115200 bauds/s that means that your bandwidth is not enough. I recommend using a lighter message type in order to increase the rate.

Thank you for your reply. I will use a lighter message type.

But I still confused. In fact, I'm using 1 500 000 bps. That should be enough according to your calculation. But I got the odometry msg only 50 Hz.

Where should I focus to fix the problem.

Aug 01 '22 06:08 Puyu2934

Try to remove the osDelay from the main loop and reduce your msg type.

Aug 01 '22 07:08 pablogs9

Try to remove the osDelay from the main loop and reduce your msg type.

Hello @pablogs9 :

When I remove osDelay, the odometry msg frame rate still only 52 Hz.

And I try to reduce the odometry msg size, as follow

float64 x
float64 y
float64 yaw

the frame rate reached 154 Hz. But now the msg is only 24 B, and a data pack is 36 B (288 bit), the rate can theoretically reach 1 500 000 bps / 288 bit = 5208.3 Hz .

And I use a -v6 option to start microros agent, the output seems noting special:

[1659531941.636583] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 36, data:                                                                                                                                  [0/1992]
0000: 81 01 8F 0F 07 01 1C 00 0F B3 00 25 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
[1659531941.636602] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.636628] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 36, data: 
0000: 81 01 90 0F 07 01 1C 00 0F B4 00 25 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
[1659531941.636653] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.636674] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 36, data: 
0000: 81 01 91 0F 07 01 1C 00 0F B5 00 25 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
[1659531941.636706] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.636777] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 36, data: 
0000: 81 01 92 0F 07 01 1C 00 0F B6 00 25 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
[1659531941.636838] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.636877] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 36, data: 
0000: 81 01 93 0F 07 01 1C 00 0F B7 00 25 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
[1659531941.636894] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.636941] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.637000] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.637041] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
[1659531941.637079] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000002, len: 24, data: 
0000: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I don't know how to solve the problem, and I'm considering use SPI.

Thank for you help

Aug 03 '22 13:08 Puyu2934

Another question, if you are in a FreeRTOS environment, how many tasks do you have? Which priority has each task?

Could you provide your whole micro-ROS code snippet?

Also, check the rate with ros2 topic hz [topic name] and with the agent with flag -v0

Aug 04 '22 07:08 pablogs9

Another question, if you are in a FreeRTOS environment, how many tasks do you have? Which priority has each task?

Could you provide your whole micro-ROS code snippet?

Also, check the rate with ros2 topic hz [topic name] and with the agent with flag -v0

7 tasks. But I have tried to delete other 6 tasks, reserved only microros task.And it's no help, remains 50 Hz.
This is my micro_ROS task:

#include "microros_task.h"
#include "microros/microros_ctl.h"
#include "cmsis_os.h"
#include <microros/extra_sources/microros_time.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include "nav_msgs/msg/odometry.h"

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

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

void publish_100hz_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    if(timer != NULL) {
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);

        nav_msgs__msg__Odometry odometry_msg = {};
        odometry_msg.header.stamp.sec = ts.tv_sec;
        odometry_msg.header.stamp.nanosec = ts.tv_nsec;
        static char odometry_frame_id[] = "odom";
        static char odometry_child_frame_id[] = "base_link";
        odometry_msg.header.frame_id.data = odometry_frame_id;
        odometry_msg.header.frame_id.size = strlen(odometry_msg.header.frame_id.data);
        odometry_msg.header.frame_id.capacity = sizeof(odometry_frame_id);
        odometry_msg.child_frame_id.data = odometry_child_frame_id;
        odometry_msg.child_frame_id.size = strlen(odometry_child_frame_id);
        odometry_msg.child_frame_id.capacity = sizeof(odometry_child_frame_id);
        RCSOFTCHECK(rcl_publish(&odometry_publisher, &odometry_msg, NULL));
    }
}

void MicrorosTask(const void *pVoid)
{
    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, "mcu_microros", "", &support));

    // create odometry_publisher
    RCCHECK(rclc_publisher_init_best_effort(
            &odometry_publisher,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
            "odometry")
    );

    //creat cmd_vel_subscriber
    rcl_subscription_t cmd_vel_subscriber = rcl_get_zero_initialized_subscription();
    RCCHECK(rclc_subscription_init_best_effort(
            &cmd_vel_subscriber,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
            "/cmd_vel")
    );


    // create timer
    rcl_timer_t publish_100hz_timer;
    RCCHECK(rclc_timer_init_default(&publish_100hz_timer, &support, RCL_MS_TO_NS(10), publish_100hz_timer_callback));

    // create executor
    rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
    RCCHECK(rclc_executor_init(&executor, &support.context, 4, &allocator));

    unsigned int rcl_wait_timeout = 10;   // in ms
    RCCHECK(rclc_executor_add_timer(&executor, &publish_100hz_timer));

    while (true) {
        rclc_executor_spin_some(&executor,RCL_MS_TO_NS(1));
        osDelay(1);
    }
    
    RCCHECK(rcl_subscription_fini(&cmd_vel_subscriber, &node));
    RCCHECK(rcl_node_fini(&node));
}

Start the agent with flag -v0:

➜  microros_ws ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/serial/by-id/usb-FTDI_USB__-__Serial-if00-port0 -b 1500000 -v0
[1659683902.642231] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3

Check the rate with ros2 topic hz [topic name]:

➜  interfaces_ws ros2 topic hz /qyun_mcu/odometry 
average rate: 49.660
	min: 0.011s max: 0.029s std dev: 0.00603s window: 52
average rate: 49.826
	min: 0.011s max: 0.029s std dev: 0.00595s window: 103
average rate: 49.878
	min: 0.011s max: 0.029s std dev: 0.00591s window: 154

Aug 07 '22 06:08 Puyu2934

Which transport are you using https://github.com/micro-ROS/micro_ros_stm32cubemx_utils/tree/humble/extra_sources/microros_transports ?

Aug 08 '22 05:08 pablogs9

dma_transport https://github.com/micro-ROS/micro_ros_stm32cubemx_utils/blob/humble/extra_sources/microros_transports/dma_transport.c

Aug 08 '22 06:08 Puyu2934

Can you try removing this lines: https://github.com/micro-ROS/micro_ros_stm32cubemx_utils/blob/58b4c8fe26c5cd695b7a33120c754a8a78d71300/extra_sources/microros_transports/dma_transport.c#L39-L41

I guess that maybe this transport is not optimized to throughput since it is setting a DMA transfer and then is waiting until it is completed:

HAL_StatusTypeDef ret;
    if (uart->gState == HAL_UART_STATE_READY){  // CHECK IF READY
        ret = HAL_UART_Transmit_DMA(uart, buf, len); // MAKE DMA TX

// PROBABLY NOT REQUIRED:
//        while (ret == HAL_OK && uart->gState != HAL_UART_STATE_READY){
//            osDelay(1);
//        }

        return (ret == HAL_OK) ? len : 0;
    }else{
        return 0;
    }

Aug 08 '22 06:08 pablogs9

Yes, I tried to remove the while:

size_t cubemx_transport_write(struct uxrCustomTransport* transport, const uint8_t *buf, size_t len, uint8_t * err){
    UART_HandleTypeDef * uart = (UART_HandleTypeDef*) transport->args;

    HAL_StatusTypeDef ret;
    if (uart->gState == HAL_UART_STATE_READY){
        ret = HAL_UART_Transmit_DMA(uart, buf, len);
//        while (ret == HAL_OK && uart->gState != HAL_UART_STATE_READY){
//            osDelay(1);
//        }
        return (ret == HAL_OK) ? len : 0;
    }else{
        return 0;
    }
}

But if remove that, the program will stuck at :

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

This is the agent output with flag -v6:

[1659947027.821985] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1659947027.822239] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1659947031.371890] info     | Root.cpp           | create_client            | create                 | client_key: 0x5851F42D, session_id: 0x81
[1659947031.372040] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x5851F42D, address: 0
[1659947031.372234] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1659947031.483376] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.483834] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.595445] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.595755] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.707365] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.707663] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.819310] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.819599] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.931195] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.931346] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.043333] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.043638] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.155342] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.155637] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.267143] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.267469] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.379204] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.379510] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80

Aug 08 '22 08:08 Puyu2934

I've experienced something similar to the problem you've experienced.

In my case, it was to raise the IMU Topic frequency by 250Hz. I checked the IMU Topic publishing function's time with oscilloscope and concluded that the IMU message type is too large to publish more fast. So I created a smaller form of message(removing msg's headers), and this solved the problem.

Here is my issue. How can I increase the publishing hertz?

Aug 08 '22 09:08 Interactics

I've experienced something similar to the problem you've experienced.

In my case, it was to raise the IMU Topic frequency by 250Hz. I checked the IMU Topic publishing function's time with oscilloscope and concluded that the IMU message type is too large to publish more fast. So I created a smaller form of message(removing msg's headers), and this solved the problem.

Here is my issue. How can I increase the publishing hertz?

Thank you for your advice.

and I have tried to use a lighter msg in this reply

And the rate still unsatisfying. I wondering what makes my device so slow :sob:

Aug 08 '22 10:08 Puyu2934

Yes, I tried to remove the while:

size_t cubemx_transport_write(struct uxrCustomTransport* transport, const uint8_t *buf, size_t len, uint8_t * err){
    UART_HandleTypeDef * uart = (UART_HandleTypeDef*) transport->args;

    HAL_StatusTypeDef ret;
    if (uart->gState == HAL_UART_STATE_READY){
        ret = HAL_UART_Transmit_DMA(uart, buf, len);
//        while (ret == HAL_OK && uart->gState != HAL_UART_STATE_READY){
//            osDelay(1);
//        }
        return (ret == HAL_OK) ? len : 0;
    }else{
        return 0;
    }
}

But if remove that, the program will stuck at :

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

This is the agent output with flag -v6:

[1659947027.821985] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1659947027.822239] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1659947031.371890] info     | Root.cpp           | create_client            | create                 | client_key: 0x5851F42D, session_id: 0x81
[1659947031.372040] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x5851F42D, address: 0
[1659947031.372234] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1659947031.483376] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.483834] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.595445] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.595755] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.707365] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.707663] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.819310] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.819599] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947031.931195] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947031.931346] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.043333] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.043638] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.155342] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.155637] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.267143] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.267469] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80
[1659947032.379204] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
[1659947032.379510] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 01 80

Well, deleting that lines is really very dangerous because you risk that successive DMA calls run into each other. I fixed that problem by introducing an additional Semaphore. So my transport write looks like this (kept the original lines and just comment them out): size_t cubemx_transport_write(struct uxrCustomTransport* transport, uint8_t * buf, size_t len, uint8_t * err){ UART_HandleTypeDef * uart = (UART_HandleTypeDef*) transport->args;

HAL_StatusTypeDef ret;
//if (uart->gState == HAL_UART_STATE_READY){
osSemaphoreAcquire(UART_Tx_readyHandle, osWaitForever);
    ret = HAL_UART_Transmit_DMA(uart, buf, len);
    __HAL_DMA_DISABLE_IT(uart->hdmatx, DMA_IT_HT );
    osSemaphoreAcquire(UART_Tx_readyHandle, osWaitForever);
    osSemaphoreRelease(UART_Tx_readyHandle);
    //while (ret == HAL_OK && uart->gState != HAL_UART_STATE_READY){
    //    osDelay(1);
 //   }

    return (ret == HAL_OK) ? len : 0;
//}else{
//    return 0;
//}

} For my application that works fine and improves the throughput. However, I have not such high demands as you have. Additionally I made a minor change for the read function by inserting a break in the first loop. This helps avoiding to wait one millisecond even if the message already arrived. It's a small chance of corrupting the message because receiving something doesn't say you received the complete message. So please be aware of this. In my application if works because I run the serial line with 921600 baud and only receive short messages.

size_t cubemx_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){ UART_HandleTypeDef * uart = (UART_HandleTypeDef*) transport->args;

int ms_used = 0;
do
{
    __disable_irq();
    dma_tail = UART_DMA_BUFFER_SIZE - __HAL_DMA_GET_COUNTER(uart->hdmarx);
    __enable_irq();
    ms_used++;
    if(dma_head != dma_tail)
    	break;
    osDelay(portTICK_RATE_MS);
} while (dma_head == dma_tail && ms_used < timeout);

size_t wrote = 0;
while ((dma_head != dma_tail) && (wrote < len)){
    buf[wrote] = dma_buffer[dma_head];
    dma_head = (dma_head + 1) % UART_DMA_BUFFER_SIZE;
    wrote++;
}

return wrote;

}

Aug 02 '23 10:08 DrMarkusKrug

micro_ros_stm32cubemx_utils micro_ros_stm32cubemx_utils copied to clipboard

How to increase a published topic frame rate faster than best effort publisher

Steps to reproduce the issue

Expected behavior

Actual behavior

Additional information

micro_ros_stm32cubemx_utils
micro_ros_stm32cubemx_utils copied to clipboard