MicroROS FreeRTOS integration - random crashes

[mjohannessen]

Issue template

• Hardware description: Raspberry Pico RP2040
• RTOS: micro_ros_raspberry_pico_sdk
• Installation type:
• Version or commit hash: foxy

Steps to reproduce the issue

Random termination

I am using the micro_ros_raspberry_pico_sdk along with FreeRTOS SMP starting with the Blinky app. It's a very simple test that just publishes a timer counter. The ROS Agent (foxy - docker) is run on a connected Raspberry Pi 4b via USB.

I start the agent on the RPi4:

docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:foxy serial --dev /dev/ttyACM0

Once this app is loaded onto the pico (I'm using VS Code with the GDB debugger on the pico's debug out), this runs as expected. However, after a random period of time (could be several minutes or more), the agent sees no further activity. I have miniterm monitoring the pico debug, and can confirm the app hangs (the runTimeStats() function fails to trigger). Below is the relevant code.

Is there something missing here that is an issue?

main.cpp

#include <stdio.h>
#include <math.h>
#include "assert.h"
#include <time.h>
#include <stdbool.h>
#include <stdlib.h>

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

extern"C"{
#include "pico/stdio/driver.h"
#include "pico/stdio.h"
#include "pico/stdio_usb.h"
#include "pico/stdio_uart.h"
#include "pico/stdlib.h"
}

#include "main.h"
#include "uRosBridge.h"

#define TASK_PRIORITY       ( tskIDLE_PRIORITY + 1UL )
#define LED_PIN 25

void mainTask(void *params){

    // to add motors, PIDs, control, odometry etc.

    uRosBridge *bridge = uRosBridge::getInstance();
    bridge->start("Bridge",  TASK_PRIORITY+2);

    //Run forever for runtime stats
    for(;;){
        vTaskDelay(10000);
        printf("Run stats\n");
        runTimeStats();
    }
}

void vLaunch( void) {
    TaskHandle_t task;
    xTaskCreate(mainTask, "MainThread", 500, NULL, TASK_PRIORITY, &task);
    vTaskStartScheduler();
}

int main( void ){
    stdio_init_all();
    stdio_filter_driver(&stdio_uart);
    sleep_ms(2000);
    printf("GO\n");
    const char *rtos_name = "FreeRTOS";
    printf("Starting %s on core 0:\n", rtos_name);
    vLaunch();

    return 0;
}

/***
 * Debug function to look at Task Stats
 */
void runTimeStats( void){

    TaskStatus_t *pxTaskStatusArray;
    volatile UBaseType_t uxArraySize, x;
    unsigned long ulTotalRunTime;

   // Get number of takss
   uxArraySize = uxTaskGetNumberOfTasks();
   printf("Number of tasks %d\n", uxArraySize);

   //Allocate a TaskStatus_t structure for each task.
   pxTaskStatusArray = (TaskStatus_t *)pvPortMalloc( uxArraySize * sizeof( TaskStatus_t ) );

   if( pxTaskStatusArray != NULL ){
      // Generate raw status information about each task.
      uxArraySize = uxTaskGetSystemState( pxTaskStatusArray,
                                 uxArraySize,
                                 &ulTotalRunTime );
     // Print stats
     for( x = 0; x < uxArraySize; x++ )
     {
         printf("Task: %d \t curr pri:%d \t base pri:%d \t hw mark:%d \ttask:%s\n",
                pxTaskStatusArray[ x ].xTaskNumber ,
                pxTaskStatusArray[ x ].uxCurrentPriority ,
                pxTaskStatusArray[ x ].uxBasePriority ,
                pxTaskStatusArray[ x ].usStackHighWaterMark ,
                pxTaskStatusArray[ x ].pcTaskName
                );
     }

      // Free array
      vPortFree( pxTaskStatusArray );
   } else {
       printf("Failed to allocate space for stats\n");
   }

   //Get heap allocation information
   HeapStats_t heapStats;
   vPortGetHeapStats(&heapStats);
   printf("HEAP avl: %d, blocks %d, alloc: %d, free: %d\n",
           heapStats.xAvailableHeapSpaceInBytes,
           heapStats.xNumberOfFreeBlocks,
           heapStats.xNumberOfSuccessfulAllocations,
           heapStats.xNumberOfSuccessfulFrees
           );
}

and uRosBridge is:

#include "uRosBridge.h"

enum states {
  WAITING_AGENT,    /**< WAITING_AGENT */
  AGENT_AVAILABLE,  /**< AGENT_AVAILABLE */
  AGENT_CONNECTED,  /**< AGENT_CONNECTED */
  AGENT_DISCONNECTED/**< AGENT_DISCONNECTED */
} ;

uRosBridge * uRosBridge::pSingleton = NULL;

uRosBridge * uRosBridge::getInstance(){
    if (pSingleton == NULL){
        pSingleton = new uRosBridge();
    }
    return pSingleton;
}

uRosBridge::uRosBridge() {
    //init items
}

uRosBridge::~uRosBridge() {
    //
}

void uRosBridge::run(){

    setLed(25);

    rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
    freeRTOS_allocator.allocate = __freertos_allocate;
    freeRTOS_allocator.deallocate = __freertos_deallocate;
    freeRTOS_allocator.reallocate = __freertos_reallocate;
    freeRTOS_allocator.zero_allocate = __freertos_zero_allocate;

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

    rmw_uros_set_custom_transport(
        true,
        NULL,
        pico_usb_transport_open,
        pico_usb_transport_close,
        pico_usb_transport_write,
        pico_usb_transport_read
    );

    states state = WAITING_AGENT;

    xMsg.data = 0;

    for(;;){
        switch (state) {
            case WAITING_AGENT:
              state = pingAgent() ? AGENT_AVAILABLE : WAITING_AGENT;
              break;
            case AGENT_AVAILABLE:
              createEntities();
              state = AGENT_CONNECTED ;
              break;
            case AGENT_CONNECTED:
              state = pingAgent() ? AGENT_CONNECTED : AGENT_DISCONNECTED;
              if (state == AGENT_CONNECTED) {
                rclc_executor_spin_some(&xExecutor, RCL_MS_TO_NS(100));
                usleep(1000);
              }
              break;
            case AGENT_DISCONNECTED:
              destroyEntities();
              state = WAITING_AGENT;
              break;
            default:
              break;
          }

        taskYIELD();
    }
}

configSTACK_DEPTH_TYPE uRosBridge::getMaxStackSize(){
    return 1024;
}

bool uRosBridge::pingAgent(){
    const int timeout_ms = 100;
    const uint8_t attempts = 1;
    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);
    if (ret != RCL_RET_OK){
        gpio_put(xLedPad, 0);
        return false;
    } else {
        gpio_put(xLedPad, 1);
    }
    return true;
}

void uRosBridge::setLed(uint8_t pad){
    xLedPad = pad;
    gpio_init(xLedPad);
    gpio_set_dir(xLedPad, GPIO_OUT);
    gpio_put(xLedPad, 0);
}

void uRosBridge::createEntities(){

    xAllocator = rcl_get_default_allocator();

    // TO DO - Set Domain ID to 1
    //printf("Setting domain ID to 1\n");
    //rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
    //rcl_ret_t dummy1 = rcl_init_options_init(&init_options, xAllocator);
    //rcl_ret_t dummy2 = rcl_init_options_set_domain_id(&init_options, 1);
    //rclc_support_init_with_options(&xSupport, 0, NULL, &init_options, &xAllocator);

    rclc_support_init(&xSupport, 0, NULL, &xAllocator);

    rclc_node_init_default(&xNode, "pico_node", "", &xSupport);

    rclc_publisher_init_default(
        &xPublisher,
        &xNode,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "pico_count");

    rclc_timer_init_default(
        &xTimer,
        &xSupport,
        RCL_MS_TO_NS(1000),
        uRosBridge::timerCallback);

    uint handles = 1;
    uint count =   2;

    printf("Created %d entities\n", count);

    rclc_executor_init(&xExecutor, &xSupport.context, handles, &xAllocator);
    rclc_executor_add_timer(&xExecutor, &xTimer);

    //Sync Time
    if (RMW_RET_OK != rmw_uros_sync_session(5000)){
        printf("ERROR Time synk failed\n");
    }

    printf("Entities Created\n");

}

void uRosBridge::destroyEntities(){
    rmw_context_t * rmw_context = rcl_context_get_rmw_context(&xSupport.context);
    (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
    rcl_publisher_fini(&xPublisher, &xNode);
    rcl_timer_fini(&xTimer);
    rclc_executor_fini(&xExecutor);
    rcl_node_fini(&xNode);
    rclc_support_fini(&xSupport);

    printf("Entities Destroyed\n");
}

void uRosBridge::timerCallback(rcl_timer_t *timer, int64_t last_call_time){
    uRosBridge::getInstance()->pubCount();
}

void uRosBridge::pubCount(){
    rcl_ret_t ret = rcl_publish(&xPublisher, &xMsg, NULL);
    //printf("Published %d\n", xMsg.data);
    xMsg.data++;
}

rclc_support_t *uRosBridge::getSupport(){
    return &xSupport;
}

And here is FreeRTOSConfig.h:

/*
 * FreeRTOS V202107.00
 * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * http://www.FreeRTOS.org
 * http://aws.amazon.com/freertos
 *
 * 1 tab == 4 spaces!
 */

#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H

/*-----------------------------------------------------------
 * Application specific definitions.
 *
 * These definitions should be adjusted for your particular hardware and
 * application requirements.
 *
 * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
 * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
 *
 * See http://www.freertos.org/a00110.html
 *----------------------------------------------------------*/

/* Scheduler Related */
#define configUSE_PREEMPTION                    1
#define configUSE_TICKLESS_IDLE                 0
#define configUSE_IDLE_HOOK                     0
#define configUSE_TICK_HOOK                     0
#define configTICK_RATE_HZ                      ( ( TickType_t ) 1000 )
#define configMAX_PRIORITIES                    32
#define configMINIMAL_STACK_SIZE                ( configSTACK_DEPTH_TYPE ) 256
#define configUSE_16_BIT_TICKS                  0

#define configIDLE_SHOULD_YIELD                 1

/* Synchronization Related */
#define configUSE_MUTEXES                       1
#define configUSE_RECURSIVE_MUTEXES             1
#define configUSE_APPLICATION_TASK_TAG          0
#define configUSE_COUNTING_SEMAPHORES           1
#define configQUEUE_REGISTRY_SIZE               8
#define configUSE_QUEUE_SETS                    1
#define configUSE_TIME_SLICING                  1
#define configUSE_NEWLIB_REENTRANT              0
#define configENABLE_BACKWARD_COMPATIBILITY     0
#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5

/* System */
#define configSTACK_DEPTH_TYPE                  uint32_t
#define configMESSAGE_BUFFER_LENGTH_TYPE        size_t

/* Memory allocation related definitions. */
#define configSUPPORT_STATIC_ALLOCATION         0
#define configSUPPORT_DYNAMIC_ALLOCATION        1
#define configTOTAL_HEAP_SIZE                   (128*1024)
#define configAPPLICATION_ALLOCATED_HEAP        0

/* Hook function related definitions. */
#define configCHECK_FOR_STACK_OVERFLOW          0
#define configUSE_MALLOC_FAILED_HOOK            0
#define configUSE_DAEMON_TASK_STARTUP_HOOK      0

/* Run time and task stats gathering related definitions. */
#define configGENERATE_RUN_TIME_STATS           0
#define configUSE_TRACE_FACILITY                1
#define configUSE_STATS_FORMATTING_FUNCTIONS    0

/* Co-routine related definitions. */
#define configUSE_CO_ROUTINES                   0
#define configMAX_CO_ROUTINE_PRIORITIES         1

/* Software timer related definitions. */
#define configUSE_TIMERS                        1
#define configTIMER_TASK_PRIORITY               ( configMAX_PRIORITIES - 1 )
#define configTIMER_QUEUE_LENGTH                10
#define configTIMER_TASK_STACK_DEPTH            1024

/* Interrupt nesting behaviour configuration. */
/*
#define configKERNEL_INTERRUPT_PRIORITY         [dependent of processor]
#define configMAX_SYSCALL_INTERRUPT_PRIORITY    [dependent on processor and application]
#define configMAX_API_CALL_INTERRUPT_PRIORITY   [dependent on processor and application]
*/

/* SMP port only */
#define configNUM_CORES                         2
#define configTICK_CORE                         0
#define configRUN_MULTIPLE_PRIORITIES           1
#define configUSE_CORE_AFFINITY                 1

/* RP2040 specific */
#define configSUPPORT_PICO_SYNC_INTEROP         1
#define configSUPPORT_PICO_TIME_INTEROP         1

#include <assert.h>
/* Define to trap errors during development. */
#define configASSERT(x)                         assert(x)

/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet                1
#define INCLUDE_uxTaskPriorityGet               1
#define INCLUDE_vTaskDelete                     1
#define INCLUDE_vTaskSuspend                    1
#define INCLUDE_vTaskDelayUntil                 1
#define INCLUDE_vTaskDelay                      1
#define INCLUDE_xTaskGetSchedulerState          1
#define INCLUDE_xTaskGetCurrentTaskHandle       1
#define INCLUDE_uxTaskGetStackHighWaterMark     1
#define INCLUDE_xTaskGetIdleTaskHandle          1
#define INCLUDE_eTaskGetState                   1
#define INCLUDE_xTimerPendFunctionCall          1
#define INCLUDE_xTaskAbortDelay                 1
#define INCLUDE_xTaskGetHandle                  1
#define INCLUDE_xTaskResumeFromISR              1
#define INCLUDE_xQueueGetMutexHolder            1

/* A header file that defines trace macro can be included here. */

#endif /* FREERTOS_CONFIG_H */

[ahirane]

I'm facing exacltly the same problem. Did you find a solution?