rcl_publish() fails even everything configured properly

Pansamic commented 1 year ago

issue description

publishing nav_msgs/msg/odometry failed. rcl_publish() returns RCL_RET_ERROR. but there's no error when i publish sensor_msgs/msg/Imu. successfully initialize support, node, publisher and subcriber, but just failed to publish odometry. using USB and haven't try UART.

brief config

microros cross compile PC: Ubuntu22.04 + ROS2 Humble
stm32cubemx_utils branch distro: Humble
microros communication interface: 8MHz USB CDC
MCU: STM32F407ZGT6

trials

1. increase FreeRTOS task stack size 截图 2023-10-12 10-45-52

2. increase USB CDC transmission buffer increase buffer size from 2048 to 4096 截图 2023-10-12 10-47-06

3. disable imu publishing and publish odom only

4. publish constant odometry message(don't update it with new data)

nav_msgs__msg__Odometry msg_odometry={0};
void StartDefaultTask(void const * argument){
    ......
    /***************************************/
    /*      Odometry publisher init        */
    /***************************************/
    nav_msgs__msg__Odometry__init(&msg_odometry);
    ret = rclc_publisher_init_default(&pub_odometry,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),"odom/unfiltered");
    if (ret == RCL_RET_OK)
    {
        printf("[INFO] odometry publisher init OK!\r\n");
        msg_odometry.header.frame_id.data = "odom";
        msg_odometry.header.frame_id.capacity = 4;
        msg_odometry.header.frame_id.size = 4;
    }
    else
    {
        printf("[ERROR]odometry publisher init failed!\r\n");
        MicroROS_SysInit = 0;
    }
        ......
}

shows in code snippet below.

5. increase value of RMW_UXRCE_MAX_HISTORY micro_ros_stm32cubemx_utils/microros_static_library_ide/library_generation/colcon.meta

i increase value of DRMW_UXRCE_MAX_PUBLISHERS from default to 6. i increase value of DRMW_UXRCE_MAX_HISTORY from default 4 to 6.

{
    "names": {
        ...,
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=6",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=1",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=6",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

6. check whether USB transmission failure causes odometry publishing failure breakpoint here, as shown in screenshot below. unfortunately, never reach breakpoint. 截图 2023-10-12 10-44-31

code snippet

part of freertos.c in this code, i initialized an odometry message content and never modify it and publish this odometry msg every 1s. for imu, publish in about 122Hz and it works very well.

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* C standard library */
#include <stdio.h>
#include <time.h>
#include <math.h>

/* STM32 Peripherals */
#include "rtc.h"
#include "tim.h"

/* Board Peripheral drivers */
#include "motor.h"
#include "led.h"
#include "icm20602.h"
#include "mecanum.h"

/* micro ros kernel */
#include "geometry_msgs/msg/twist.h"
#include "nav_msgs/msg/odometry.h"
#include "rcl/rcl.h"
#include "rclc/executor.h"
#include "rclc/rclc.h"
#include "rmw_microros/rmw_microros.h"
#include "sensor_msgs/msg/imu.h"
#include "sensor_msgs/msg/time_reference.h"

/* USER CODE END Includes */

#define IMU_READY_SIGNAL (0x00000001)

/*****************************/
/* micro ros core components */
/*****************************/
rclc_support_t support={0};
rcl_allocator_t allocator={0};
rcl_allocator_t freeRTOS_allocator={0};
rclc_executor_t executor={0};

/*****************************/
/*  micro ros communication  */
/*****************************/
rcl_node_t node={0};
rcl_subscription_t sub_twist={0};
rcl_subscription_t sub_time_ref={0};
rcl_publisher_t pub_imu={0};
rcl_publisher_t pub_odometry={0};

/*****************************/
/*     micro ros messages    */
/*****************************/
nav_msgs__msg__Odometry msg_odometry={0};
sensor_msgs__msg__Imu msg_imu={0};
geometry_msgs__msg__Twist msg_twist={0};
sensor_msgs__msg__TimeReference msg_time_ref={0};

osThreadId defaultTaskHandle;
osThreadId Task_PublishIMUHandle;
osThreadId Task_PublishOdomHandle;
osTimerId Timer_MotorAdjustHandle;
osTimerId Timer_PingAgentHandle;

/*****************************/
/*    port comm interface    */
/*****************************/
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);

/*****************************/
/*     memory management     */
/*****************************/
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);

void StartDefaultTask(void const * argument);
void PublishIMU(void const * argument);
void PublishOdom(void const * argument);
void MotorAdjustcb(void const * argument);
void Ping_Agent_cb(void const * argument);

extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

void MX_FREERTOS_Init(void) {
  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 2048);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

  /* definition and creation of Task_PublishIMU */
  osThreadDef(Task_PublishIMU, PublishIMU, osPriorityBelowNormal, 0, 2048);
  Task_PublishIMUHandle = osThreadCreate(osThread(Task_PublishIMU), NULL);

  /* definition and creation of Task_PublishOdom */
  osThreadDef(Task_PublishOdom, PublishOdom, osPriorityBelowNormal, 0, 2048);
  Task_PublishOdomHandle = osThreadCreate(osThread(Task_PublishOdom), NULL);

  osThreadSuspend(Task_PublishIMUHandle);
  osThreadSuspend(Task_PublishOdomHandle);
}

/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
  /* init code for USB_DEVICE */
  MX_USB_DEVICE_Init();
  ......(initialization)
    rcl_ret_t ret; // a return value container
    uint8_t MicroROS_SysInit = 1;

    /***************************************/
    /*           allocator init            */
    /***************************************/
    rmw_uros_set_custom_transport(
          true,
          NULL,
          cubemx_transport_open,
          cubemx_transport_close,
          cubemx_transport_write,
          cubemx_transport_read);
    // detect micro ros agent
    printf("[INFO] waiting for micro ros agent.\r\n");
    while(rmw_uros_ping_agent(100,1)!=RMW_RET_OK)
    {
    }
    /* Here you also give to the allocator the functions
    * that are going to be used in order to allocate memory etc.
    */
    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]set default allocators failed!\r\n");
    }

    allocator = rcl_get_default_allocator();

    /***************************************/
    /*            support init             */
    /***************************************/
    // create init_options
    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
    ret = rcl_init_options_init(&init_options, allocator);
    ret = rcl_init_options_set_domain_id(&init_options, 3); // set ROS_DOMAIN_ID=3
    ret = rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
    if(ret == RCL_RET_OK)
    {
        printf("[INFO] init support OK!\r\n");
    }
    else
    {
        printf("[ERROR]init support failed!\r\n");
        MicroROS_SysInit = 0;
    }

    /***************************************/
    /*              node init              */
    /***************************************/
    // create node
    ret = rclc_node_init_default(&node, "linorobot_base_node", "", &support);
    if(ret == RCL_RET_OK)
    {
        printf("[INFO] init node OK!\r\n");
    }
    else
    {
        printf("[ERROR]init node failed!\r\n");
        MicroROS_SysInit = 0;
    }

    /***************************************/
    /*            executor init            */
    /***************************************/
    executor = rclc_executor_get_zero_initialized_executor();
    /* 2 handles, time_ref subscription and cmd_vel subscription */
    ret = rclc_executor_init(&executor, &support.context, 2, &allocator);
    if(ret == RCL_RET_OK)
        printf("[INFO] init executor OK\r\n");
    else
    {
        printf("[ERROR]init executor failed!\r\n");
        MicroROS_SysInit = 0;
    }

    /***************************************/
    /*        IMU publisher  init          */
    /***************************************/
    // create publisher
    ret = rclc_publisher_init_best_effort(&pub_imu,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),"imu/data");
    if (ret == RCL_RET_OK)
        printf("[INFO] imu publisher init OK!\r\n");
    else
    {
        printf("[ERROR]imu publisher init failed!\r\n");
        MicroROS_SysInit = 0;
    }
    msg_imu.header.frame_id.data = "base_link";
    msg_imu.header.frame_id.size = 9;
    msg_imu.header.frame_id.capacity = 9;
    for(uint8_t i=0 ; i<9 ; i++)
        msg_imu.angular_velocity_covariance[i]=-1;
    for(uint8_t i=0 ; i<9 ; i++)
        msg_imu.linear_acceleration_covariance[i]=-1;
    for(uint8_t i=0 ; i<9 ; i++)
        msg_imu.orientation_covariance[i]=-1;

    /***************************************/
    /*      Odometry publisher init        */
    /***************************************/
    nav_msgs__msg__Odometry__init(&msg_odometry);
    ret = rclc_publisher_init_default(&pub_odometry,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),"odom/unfiltered");
    if (ret == RCL_RET_OK)
    {
        printf("[INFO] odometry publisher init OK!\r\n");
//      msg_odometry.header.frame_id.data = "odom";
//      msg_odometry.header.frame_id.capacity = 4;
//      msg_odometry.header.frame_id.size = 4;
    }
    else
    {
        printf("[ERROR]odometry publisher init failed!\r\n");
        MicroROS_SysInit = 0;
    }
    /* start other freertos tasks and timers */
    osThreadResume(Task_PublishIMUHandle);
    /* uncomment the Odom task to enable odometry
     * but there is a bug in odometry publishment. */
    osThreadResume(Task_PublishOdomHandle);

    for(;;)
    {
        // Spin executor to receive messages
        rclc_executor_spin_some(&executor,100);

        osDelay(10);
    }

void PublishIMU(void const * argument)
{
  /* USER CODE BEGIN PublishIMU */
    uint32_t NotifySignal;
    rcl_ret_t ret;
    /* Infinite loop */
    for(;;)
    {
        /* Block indefinitely (without a timeout, so no need to check the function's
        * return value) to wait for a notification.  NOTE!  Real applications
        * should not block indefinitely, but instead time out occasionally in order
        * to handle error conditions that may prevent the interrupt from sending
        * any more notifications.
        *  */
        xTaskNotifyWait(
                      0x00,               /* Don't clear any bits on entry. */
                      0xFFFFFFFF,         /* Clear all bits on exit. */
                      &NotifySignal,      /* Receives the notification value. */
                      portMAX_DELAY );    /* Block indefinitely. */

        ICM20602_UpdateMessage(&msg_imu);
        ret = rcl_publish(&pub_imu,&msg_imu,NULL);
        if(ret != RCL_RET_OK)
        {
            printf("[ERROR]pub imu failed.\r\n");
        }
    }
}

void PublishOdom(void const * argument)
{
  /* USER CODE BEGIN PublishOdom */
    TickType_t xLastWakeTime;
    const TickType_t xFrequency = 500;
    xLastWakeTime = xTaskGetTickCount();
    /* Infinite loop */
    for(;;)
    {
        vTaskDelayUntil( &xLastWakeTime, xFrequency );
//      Odometry_Update(&msg_odometry, (float)xFrequency/1000.0f, Car.CurrentXVelocity, Car.CurrentYVelocity, Car.CurrentAngularVelocity);
        rcl_ret_t ret = rcl_publish(&pub_odometry, &msg_odometry, NULL);
        if(ret == RCL_RET_ERROR)
        {
            printf("[ERROR]publish odometry failed:publish error.\r\n");
        }
        else if(ret == RCL_RET_INVALID_ARGUMENT)
        {
            printf("[ERROR]publish odometry failed:invalid argument.\r\n");
        }
        else if(ret == RCL_RET_PUBLISHER_INVALID)
        {
            printf("[ERROR]publish odometry failed:publisher invalid.\r\n");
        }
    }
}

phenomenon

UART debug message as follows. it prints publish odometry failed. it's in code snippet "PublishOdom()" FreeRTOS task function. 截图 2023-10-12 10-29-33 micro-ROS-Agent terminal shows that it is receiving IMU topic but none of odometry topic data. 截图 2023-10-12 10-31-13

pablogs9 commented 1 year ago

Do not rely on functions like nav_msgs__msg__Odometry__init, just initialize the memory yourself.

You can read some more details here: https://docs.vulcanexus.org/en/latest/rst/tutorials/micro/memory_management/memory_management.html#message-memory

For example for your nav_msgs/Odom:

You need to init header.frame_id string
You need to init child_frame_id string
You need to take into account that strings are NULL terminated, so capacity shall be bigger than size
Also assigning a literal to a pointer (msg_odometry.header.frame_id.data = "odom") may present problems on some platforms. It is recommended to assign heap/stack/static memory to that pointer and then copy the string.

Pansamic commented 1 year ago

Do not rely on functions like nav_msgs__msg__Odometry__init, just initialize the memory yourself.

You can read some more details here: https://docs.vulcanexus.org/en/latest/rst/tutorials/micro/memory_management/memory_management.html#message-memory

For example for your nav_msgs/Odom:

You need to init header.frame_id string

You need to init child_frame_id string

You need to take into account that strings are NULL terminated, so capacity shall be bigger than size

Also assigning a literal to a pointer (msg_odometry.header.frame_id.data = "odom") may present problems on some platforms. It is recommended to assign heap/stack/static memory to that pointer and then copy the string.

thank you for your reply. I tried to initialize odometry message myself without nav_msgs__msg__Odometry__init, and the error remained the same. I even created a new project to publish only odometry message and unfortunately failed. the following is my new project code (main part of it).

main.c

#include "main.h"
#include "cmsis_os.h"
#include "usb_device.h"

#include <stdio.h>

#include "usbd_cdc_if.h"
#include "usbd_cdc.h"

/* micro ros kernel */
#include "rcl/rcl.h"
#include "rclc/executor.h"
#include "rclc/rclc.h"
#include "rmw_microros/rmw_microros.h"

/* micro ros messages */
#include "sensor_msgs/msg/imu.h"
#include "sensor_msgs/msg/time_reference.h"
#include "sensor_msgs/msg/joint_state.h"
#include "geometry_msgs/msg/twist.h"
#include "nav_msgs/msg/odometry.h"

UART_HandleTypeDef huart1;

osThreadId defaultTaskHandle;

/*****************************/
/* micro ros core components */
/*****************************/
rclc_support_t support={0};
rcl_allocator_t allocator={0};
rcl_allocator_t freeRTOS_allocator={0};
rclc_executor_t executor={0};

/*****************************/
/*  micro ros communication  */
/*****************************/
rcl_node_t node={0};
rcl_publisher_t pub_odometry={0};

/*****************************/
/*     micro ros messages    */
/*****************************/
nav_msgs__msg__Odometry msg_odometry={0};

void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
void StartDefaultTask(void const * argument);

/*****************************/
/*    port comm interface    */
/*****************************/
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);

/*****************************/
/*     memory management     */
/*****************************/
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);

int main(void)
{
  HAL_Init();

  SystemClock_Config();

  MX_GPIO_Init();
  MX_USART1_UART_Init();

  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 1024);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

  osKernelStart();

  while (1)
  {

  }

}

void StartDefaultTask(void const * argument)
{
    /* init code for USB_DEVICE */
    MX_USB_DEVICE_Init();
    /* USER CODE BEGIN 5 */
    rcl_ret_t ret; // a return value container

    /***************************************/
    /*           allocator init            */
    /***************************************/
    rmw_uros_set_custom_transport(
          true,
          NULL,
          cubemx_transport_open,
          cubemx_transport_close,
          cubemx_transport_write,
          cubemx_transport_read);
    // detect micro ros agent
    printf("[INFO] waiting for micro ros agent.\r\n");
    while(rmw_uros_ping_agent(100,1)!=RMW_RET_OK)
    {
    }
    /* Here you also give to the allocator the functions
    * that are going to be used in order to allocate memory etc.
    */
    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]set default allocators failed!\r\n");
    }

    allocator = rcl_get_default_allocator();

    /***************************************/
    /*            support init             */
    /***************************************/
    // create init_options
    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
    ret = rcl_init_options_init(&init_options, allocator);
    ret = rcl_init_options_set_domain_id(&init_options, 3); // set ROS_DOMAIN_ID=3
    ret = rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
    if(ret == RCL_RET_OK)
    {
        printf("[INFO] init support OK!\r\n");
    }
    else
    {
        printf("[ERROR]init support failed!\r\n");
    }

    /***************************************/
    /*              node init              */
    /***************************************/
    // create node
    ret = rclc_node_init_default(&node, "nav_mecanum_node", "", &support);
    if(ret == RCL_RET_OK)
    {
        printf("[INFO] init node OK!\r\n");
    }
    else
    {
        printf("[ERROR]init node failed!\r\n");
    }

    /***************************************/
    /*            executor init            */
    /***************************************/
    executor = rclc_executor_get_zero_initialized_executor();
    /* 2 handles, time_ref subscription and cmd_vel subscription */
    ret = rclc_executor_init(&executor, &support.context, 2, &allocator);
    if(ret == RCL_RET_OK)
        printf("[INFO] init executor OK\r\n");
    else
    {
        printf("[ERROR]init executor failed!\r\n");
    }

    /***************************************/
    /*      Odometry publisher init        */
    /***************************************/
//  nav_msgs__msg__Odometry__init(&msg_odometry);
    ret = rclc_publisher_init_best_effort(&pub_odometry,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),"odom/unfiltered");
    if (ret == RCL_RET_OK)
    {
        printf("[INFO] odometry publisher init OK!\r\n");
        static char odometry_frame_id[] = "odom";
        static char odometry_child_frame_id[] = "base_link";
        msg_odometry.header.stamp.sec = 19996542;
        msg_odometry.header.stamp.nanosec = 3254165;

        msg_odometry.header.frame_id.data = odometry_frame_id;
        msg_odometry.header.frame_id.capacity = 5;
        msg_odometry.header.frame_id.size = 5;
        msg_odometry.child_frame_id.data = odometry_child_frame_id;
        msg_odometry.child_frame_id.capacity = 10;
        msg_odometry.child_frame_id.size = 10;
        for(uint8_t i=0 ; i<36 ; i++)
        {
            msg_odometry.pose.covariance[i] = 0.03+(double)i/8.22;
        }

        msg_odometry.pose.pose.orientation.w = 1.0;
        msg_odometry.pose.pose.orientation.x = 1.0;
        msg_odometry.pose.pose.orientation.y = 1.0;
        msg_odometry.pose.pose.orientation.z = 1.0;

        msg_odometry.pose.pose.position.x = 1.23;
        msg_odometry.pose.pose.position.y = 2.35;
        msg_odometry.pose.pose.position.z = 0.12;

        for(uint8_t i=0 ; i<36 ; i++)
        {
            msg_odometry.twist.covariance[i] = 0.03+(double)i/8.22;
        }

        msg_odometry.twist.twist.angular.x = 0.15;
        msg_odometry.twist.twist.angular.y = 0.15;
        msg_odometry.twist.twist.angular.z = 0.15;

        msg_odometry.twist.twist.linear.x = 0.23;
        msg_odometry.twist.twist.linear.y = 0.23;
        msg_odometry.twist.twist.linear.z = 0.23;

    }
    else
    {
        printf("[ERROR]odometry publisher init failed!\r\n");
    }

    TickType_t xLastWakeTime;
    const TickType_t xFrequency = 500;
    xLastWakeTime = xTaskGetTickCount();
    /* Infinite loop */
    for(;;)
    {
        vTaskDelayUntil( &xLastWakeTime, xFrequency );
//      Odometry_Update(&msg_odometry, (float)xFrequency/1000.0f, Car.CurrentXVelocity, Car.CurrentYVelocity, Car.CurrentAngularVelocity);
        rcl_ret_t ret = rcl_publish(&pub_odometry, &msg_odometry, NULL);
        if(ret == RCL_RET_ERROR)
        {
            printf("[ERROR]publish odometry failed:publish error.\r\n");
        }
        else if(ret == RCL_RET_INVALID_ARGUMENT)
        {
            printf("[ERROR]publish odometry failed:invalid argument.\r\n");
        }
        else if(ret == RCL_RET_PUBLISHER_INVALID)
        {
            printf("[ERROR]publish odometry failed:publisher invalid.\r\n");
        }
    }
  /* USER CODE END 5 */
}

pablogs9 commented 1 year ago

You need to take into account that strings are NULL terminated, so capacity shall be bigger than size

Pansamic commented 1 year ago

yes I've tried to set capacity bigger than size, but many trials all failed. I even change the code like the following but failed to publish:

static char odometry_frame_id[5] = {'o','d','o','m',0};
static char odometry_child_frame_id[10] = {'b','a','s','e','_','l','i','n','k',0};
msg_odometry.header.frame_id.data = odometry_frame_id;
msg_odometry.header.frame_id.capacity = 5;
msg_odometry.header.frame_id.size = 4;
msg_odometry.child_frame_id.data = odometry_child_frame_id;
msg_odometry.child_frame_id.capacity = 10;
msg_odometry.child_frame_id.size = 9;

btw, IMU message publish successfully with initial configuration like this:

    /***************************************/
    /*        IMU publisher  init          */
    /***************************************/
    // create publisher
    ret = rclc_publisher_init_best_effort(&pub_imu,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),"imu/data");
    if (ret == RCL_RET_OK)
        printf("[INFO] imu publisher init OK!\r\n");
    else
    {
        printf("[ERROR]imu publisher init failed!\r\n");
        MicroROS_SysInit = 0;
    }
    msg_imu.header.frame_id.data = "base_link";
    msg_imu.header.frame_id.size = 9;
    msg_imu.header.frame_id.capacity = 9;
    for(uint8_t i=0 ; i<9 ; i++)
        msg_imu.angular_velocity_covariance[i]=-1;
    for(uint8_t i=0 ; i<9 ; i++)
        msg_imu.linear_acceleration_covariance[i]=-1;
    for(uint8_t i=0 ; i<9 ; i++)
        msg_imu.orientation_covariance[i]=-1;

so i think frame_id strings are not the culprit.

pablogs9 commented 1 year ago

Another issue that may be happening is that XRCE does not allow fragmentation in best effort and nav_msgs/Odometry is quite big.

Does it work if you configure the publisher as reliable?
Does it work if you rebuild the library using a bigger MTU by means of configuring this parameter to a size that can fit the message size?

Pansamic commented 1 year ago

I publish sensor_msgs/msg/Odometry successfully by increasing this parameter from 512 to 1024. Micro-ROS-Agent shows that odometry message is 732Bytes which is larger than default MTU 512 bytes.

pablogs9 commented 1 year ago

So, that was the problem. Thanks for reporting back @Pansamic.

micro-ROS / micro_ros_stm32cubemx_utils