Two publishers conflict in different FreeRTOS tasks

[Pansamic]

Description

• MCU:STM32F407ZGT6
• STM32CubeIDE version: 1.13.1
• FreeRTOS version: 10.3.1
• CMSIS-RTOS version 1.02
• micro-ROS communication: STM32 HAL USB CDC 8MBits/s

Aiming to publish IMU when IMU-data-ready external interrupt comes, about 100Hz and publish JointState in 20Hz.

Two best_effort publishers: 1.sensor_msgs/msg/Imu 2.sensor_msgs/msg/JointState

After successful initialization and configuration, two FreeRTOS tasks publishs the two messages separately.

rcl_publish() for IMU message returns RCL_RET_OK, micro-ROS-Agent receives IMU message successfully and ros2 topic echo /imu/raw shows correct data.

rcl_publish() for JointState message returns RCL_RET_OK, micro-ROS-Agent receives no JointState message and ros2 topic echo /imu/raw shows nothing.

Trials

1. joint_states:default | IMU:best_effort
   • JointState rcl_publish() returns RCL_RET_ERROR micro-ROS-Agent receives nothing
   • IMU rcl_publish() returns RCL_RET_OK micro-ROS-Agent receives all messages
2. joint_states:best_efforrt | IMU:default
   • JointState rcl_publish() returns RCL_RET_OK micro-ROS-Agent receives all messages
   • IMU rcl_publish() sometimes returns RCL_RET_ERROR micro-ROS-Agent receives part of messages and some messages are missing
3. joint_states:default | IMU:default
   • JointState rcl_publish() returns RCL_RET_ERROR micro-ROS-Agent receives nothing
   • IMU rcl_publish() returns RCL_RET_ERROR micro-ROS-Agent receives nothing
4. put rcl_publish(JointState) to task function that publishes IMU message both messages are published successfully and micro-ROS-Agent receives all the messages
5. use FreeRTOS timer to publish JointState message Both publisher is best_effort. IMU publishing succeeds and JointState publishing fails.
6. disable IMU publishing and publish JointState only best_effort JointState publishes successfully and micro-ROS-Agent receives all messages.

[pablogs9]

Are you protecting each task with a mutex?

[Pansamic]

No, I haven't use mutex. So, I try to add a binary semaphore (not using mutex because this lock associates with my custom external GPIO interrupt so mutex is not safe), but IMU is published well while JointState is published returning OK but micro-ros-agent doesn't receive any JointState message.

Before adding binary semaphore, I modified colcon.meta file and rebuild the lib again and I'm sure the firmware is correctly built. I've add -DUCLIENT_MAX_OUTPUT_BEST_EFFORT_STREAMS=3 and -DUCLIENT_MAX_INPUT_BEST_EFFORT_STREAMS=2 to microxrecdds_client field. I've modified -DRMW_UXRCE_MAX_PUBLISHERS=3 and -DRMW_UXRCE_MAX_SUBSCRIPTIONS=2.

The following is my colcon.meta file

{
    "names": {
        "tracetools": {
            "cmake-args": [
                "-DTRACETOOLS_DISABLED=ON",
                "-DTRACETOOLS_STATUS_CHECKING_TOOL=OFF"
            ]
        },
        "rosidl_typesupport": {
            "cmake-args": [
                "-DROSIDL_TYPESUPPORT_SINGLE_TYPESUPPORT=ON"
            ]
        },
        "rcl": {
            "cmake-args": [
                "-DBUILD_TESTING=OFF",
                "-DRCL_COMMAND_LINE_ENABLED=OFF",
                "-DRCL_LOGGING_ENABLED=OFF"
            ]
        },
        "rcutils": {
            "cmake-args": [
                "-DENABLE_TESTING=OFF",
                "-DRCUTILS_NO_FILESYSTEM=ON",
                "-DRCUTILS_NO_THREAD_SUPPORT=ON",
                "-DRCUTILS_NO_64_ATOMIC=ON",
                "-DRCUTILS_AVOID_DYNAMIC_ALLOCATION=ON"
            ]
        },
        "microxrcedds_client": {
            "cmake-args": [
                "-DUCLIENT_PIC=OFF",
                "-DUCLIENT_PROFILE_UDP=OFF",
                "-DUCLIENT_PROFILE_TCP=OFF",
                "-DUCLIENT_PROFILE_DISCOVERY=OFF",
                "-DUCLIENT_PROFILE_SERIAL=OFF",
                "-DUCLIENT_PROFILE_STREAM_FRAMING=ON",
                "-UCLIENT_PROFILE_CUSTOM_TRANSPORT=ON",
                "-DUCLIENT_CUSTOM_TRANSPORT_MTU=1024",
                "-DUCLIENT_MAX_OUTPUT_BEST_EFFORT_STREAMS=3",
                "-DUCLIENT_MAX_INPUT_BEST_EFFORT_STREAMS=2"
            ]
        },
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=3",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=2",
                "-DRMW_UXRCE_MAX_SERVICES=1",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

The following is my related FreeRTOS task functions.

void PublishJointStates(void const * argument)
{
  /* USER CODE BEGIN PublishJointStates */
  rcl_ret_t ret;
  /* Infinite loop */
  for(;;)
  {
      osSemaphoreWait(PublisherSemaphoreHandle,0);
      joint_states.position[0] = LeftFrontMotor.CurrentAngle;
      joint_states.position[1] = LeftRearMotor.CurrentAngle;
      joint_states.position[2] = RightFrontMotor.CurrentAngle;
      joint_states.position[3] = RightRearMotor.CurrentAngle;

      joint_states.velocity[0] = LeftFrontMotor.CurrentVelocity;
      joint_states.velocity[1] = LeftRearMotor.CurrentVelocity;
      joint_states.velocity[2] = RightFrontMotor.CurrentVelocity;
      joint_states.velocity[3] = RightRearMotor.CurrentVelocity;

      ret = rcl_publish(&pub_joint_states, &msg_joint_state, NULL);

      if(ret == RCL_RET_ERROR)
      {
          printf("[ERROR]publish /joint_states failed:publish error.\r\n");
      }
      else if(ret == RCL_RET_INVALID_ARGUMENT)
      {
          printf("[ERROR]publish /joint_states failed:invalid argument.\r\n");
      }
      else if(ret == RCL_RET_PUBLISHER_INVALID)
      {
          printf("[ERROR]publish /joint_states failed:publisher invalid.\r\n");
      }
      osSemaphoreRelease(PublisherSemaphoreHandle);
      osDelay(95);
  }
  /* USER CODE END PublishJointStates */
}

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. */

        osSemaphoreWait(PublisherSemaphoreHandle,0);
        ICM20602_UpdateMessage(&msg_imu);
        ret = rcl_publish(&pub_imu,&msg_imu,NULL);

        if(ret != RCL_RET_OK)
        {
            printf("[ERROR]pub imu failed.\r\n");
        }
        osSemaphoreRelease(PublisherSemaphoreHandle);
    }
  /* USER CODE END PublishIMU */
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if(GPIO_Pin==ICM20602_INT_Pin)
    {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        __HAL_GPIO_EXTI_CLEAR_IT(ICM20602_INT_Pin);
        /* Unblock the handling task so the task can perform any processing necessitated
         * by the interrupt.  xHandlingTask is the task's handle, which was obtained
         * when the task was created.  The handling task's 0th notification value
         * is bitwise ORed with the interrupt status - ensuring bits that are already
         * set are not overwritten.
         * */
        xTaskNotifyFromISR(
                 Task_PublishIMUHandle,       // notify IMU_Publish task
                 IMU_READY_SIGNAL,           // give out IMU_READY_SIGNAL
                 eSetBits,                   // default param
                 &xHigherPriorityTaskWoken); // whether interrupted by higher priority tasks

         /* Force a context switch if xHigherPriorityTaskWoken is now set to pdTRUE.
          * the macro used to do this is dependent on the port and may be called
          * portEND_SWITCHING_ISR.
          * */
         portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
    }
}

[Pansamic]

I found this issue intended to cross-compile Micro-XRCE-DDS-Client with FreeRTOS, and we have to define PLATFORM_NAME_FREERTOS. But what confused me is that Micro-ROS doesn't mention about this. Is that the problem multithread is disabled or something?

[pablogs9]

Thread safe operation is not enabled by default nor is integrated into this Cube IDE platform due to compilation issues. It is okay in this case to use a mutex at application level that protects each micro-ROS API calls in each different task.

[elgarbe]

Hi @Pansamic I'm working on something similar to your work. Can you share a little more code? I have doubt around your semaphore implementation. I just have the same IMU and make it work ok with microROS, but now I have to add encoders readings publishers.

[Pansamic]

@elgarbe For detailed code, you can refer to my issue #124. It's quite simple to deal with that problem. First create a semaphore in STM32CubeIDE called PublisherSemaphore. Then, for each rcl_publish() in different tasks, add osSemaphoreWait() before rcl_publish(), and osSemaphoreRelease() after rcl_publish(). I set only one semaphore called PublisherSemaphore to protect rcl_publish().

osSemaphoreWait(PublisherSemaphoreHandle,0);
ret = rcl_publish(&pub_joint_states, &msg_joint_state, NULL);
osSemaphoreRelease(PublisherSemaphoreHandle);

I can publish other two messages(JointStates and Odometry) in two different FreeRTOS tasks with this method. But this method doesn't work properly when I add IMU task. IMU interrupt handler function use xTaskNotifyFromISR() to notify the IMU task, the IMU task then read IMU and publish it. I've add semaphore to those three tasks and the phenomenon is STM32 enters hardfault or just publish several times and stops. Do you have any ideas?

[Pansamic]

Thread safe operation is not enabled by default nor is integrated into this Cube IDE platform due to compilation issues. It is okay in this case to use a mutex at application level that protects each micro-ROS API calls in each different task.

@pablogs9 But it still cannot publish correctly, even rcl_spinsome() is protected with semaphore. two rcl_publish() in two different tasks work well, but if I add a IMU publish task that get notified from interrupt handler function, micro-ros will send some IMU messages and send no other two type messages. It's weird that rcl_publish() return RCL_RET_OK but micro-ros-agent doesn't receive the message. That confused me a lot and I don't know how to debug it.

[elgarbe]

@Pansamic you are trying to publish IMU messages at IMU data rate? If your IMU is configured to output data at 200Hz (at least) it seems to me that it will requier to much bandwidth. Rigth now I just update IMU data on IMU_task and I have another task that takes IMU_data and published it at a convenient rate. Who is consuming IMU data on your high level computer?

[Pansamic]

@Pansamic you are trying to publish IMU messages at IMU data rate? If your IMU is configured to output data at 200Hz (at least) it seems to me that it will requier to much bandwidth. Rigth now I just update IMU data on IMU_task and I have another task that takes IMU_data and published it at a convenient rate. Who is consuming IMU data on your high level computer?

Thanks for your solution. But I actually need such a high frequency of IMU data. My IMU task has reached 970 Hz publish rate when only IMU task is working while other publish tasks are disabled. I'm using USB CDC FS which has 8Mbits/s bandwidth and it's enough for 970Hz IMU publish rate.

Inspired by you, I tried to decrease my IMU sample rate to 500Hz, and the whole system works well(but publish rate is lower than expected). My system can publish IMU in about 440 Hz and JointStates in 100 Hz.

This issue is now solved.

The main problem can be summarized as follows:

1. Semaphore is needed to protect each rcl_publish().
2. Set IMU task that takes notification from interrupt handler to highest priority.
3. Slow down publish rate if some tasks seems to fail to publish.
4. Increase publish rate by modifying cubemx_transport_write() in micro_ros_stm32cubemx_utils/extra_sources/microros_transports/usb_cdc_transport.c as shown below.

   size_t cubemx_transport_write(struct uxrCustomTransport* transport, uint8_t * buf, size_t len, uint8_t * err)
   {
   /* Original Method*/
   uint8_t ret = CDC_Transmit_FS(buf, len);
   if (USBD_OK != ret)
   {
       return 0;
   }
   while(!g_write_complete){}
   size_t writed = g_write_complete ? len : 0;
   g_write_complete = false;
   return writed;
   }

[elgarbe]

glad to hear I inspired you to find the solution.

[pablogs9]

Clsoing as solved