Adding telemetry with MicroROS on RP2040 and strange terminal characters

[mjohannessen]

I'm testing a basic MicroROS project on a Pico RP2040. I have a pico debug probe hooked up to the pico SWD debug pins and UART serial wires to pico GPIO pins 16 and 17. Running minicom:

minicom -b 115200 -o -D /dev/tty.usbmodem83202

I get this type of output:

~DD���Hello - blink 577 ~EE���Hello - blink 578 ~��PB�-Hello - blink 579 ~��QC��Hello - blink 580 ~��RD��Hello - blink 581 ~��SE�VHello - blink 582 ~JJ���Hello - blink 583 ~��UG�,Hello - blink 584 ~��VH��Hello - blink 585 ~MM���Hello - blink 586

The program publishes correctly on the topic /buster/pico_publisher using the micro ros agent which is running on a connected raspberry pi 4b. The pico's usb is connected to the raspberry pi 4, and the pico debug probe to a Mac via usb.

When I comment out "rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);" in the callback, the strange characters disappear.

Publishing seems to inject something into the serial UART port.

I'm using the pre-compiled libmicroros library from the micro_ros_raspberrypi_pico_sdk.

My main.c is:

#include stdio.h

#include rcl/rcl.h
#include rcl/error_handling.h
#include rclc/rclc.h
#include rclc/executor.h
#include std_msgs/msg/int32.h
#include rmw_microros/rmw_microros.h

#include "pico/stdlib.h"
#include "pico_uart_transports.h"
#include "pico/stdio.h"

const uint LED_PIN = 25; //25

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;

void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
    gpio_put(LED_PIN, 0);
    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
    msg.data++;
    // added
    sleep_ms(250);
    gpio_put(LED_PIN, 1);
    printf("Hello - blink %d\n", msg.data);
}

int main()
{
    //size_t uart_port = 0;  //added
    rmw_uros_set_custom_transport(
        true,
        NULL,   //was NULL , alt (void *) uart0,
        pico_serial_transport_open,
        pico_serial_transport_close,
        pico_serial_transport_write,
        pico_serial_transport_read
    );

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);

    rcl_timer_t timer;
    rcl_node_t node;
    rcl_allocator_t allocator;
    rclc_support_t support;
    rclc_executor_t executor;

    allocator = rcl_get_default_allocator();

    // Wait for agent successful ping for 2 minutes.
    const int timeout_ms = 1000; 
    const uint8_t attempts = 120;

    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);

    if (ret != RCL_RET_OK)
    {
        // Unreachable agent, exiting program.
        return ret;
    }

    // Initialize and modify options (Set DOMAIN ID to 1)
    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();

    // set a value to stop warnings
    rcl_ret_t dummy1 = rcl_init_options_init(&init_options, allocator);
    rcl_ret_t dummy2 = rcl_init_options_set_domain_id(&init_options, 1);

    // Initialize support object
    rcl_ret_t rc = rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);

    const char * node_name = "test_1_node";
    // Node namespace (Can remain empty "")
    const char * namespace = "buster";
    rclc_node_init_default(&node, node_name, namespace, &support);

    rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "pico_publisher");

    rclc_timer_init_default2(
        &timer,
        &support,
        RCL_MS_TO_NS(1000),
        timer_callback,
        true);

    rclc_executor_init(&executor, &support.context, 1, &allocator);
    rclc_executor_add_timer(&executor, &timer);

    gpio_put(LED_PIN, true);

    msg.data = 0;
    while (true)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
    }
    return 0;
}

And the CmakeLists.txt is:

cmake_minimum_required(VERSION 3.12)

set(NAME test)

include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake)

project(${NAME} C CXX ASM)
set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)

pico_sdk_init()

set(PICO_BOARD "pico" CACHE STRING "Pico board type")
set(PICO_PLATFORM "rp2040" CACHE STRING "Pico platform type")

link_directories($ENV{MICRO_ROS_PATH}/libmicroros)

add_executable(${NAME}
    test_1.c
    pico_uart_transport.c
)

target_link_libraries(${NAME}
    pico_stdlib
    microros
)

target_include_directories(${NAME} PUBLIC
    $ENV{MICRO_ROS_PATH}/libmicroros/include
)

# Generate UF2
pico_add_extra_outputs(${NAME})

pico_enable_stdio_uart(${NAME} 1)
pico_enable_stdio_usb(${NAME} 1)
add_compile_definitions(PICO_UART_ENABLE_CRLF_SUPPORT=0)
add_compile_definitions(PICO_STDIO_ENABLE_CRLF_SUPPORT=0)
add_compile_definitions(PICO_STDIO_DEFAULT_CRLF=0)

target_compile_definitions(${NAME} PRIVATE 
    PICO_DEFAULT_UART_RX_PIN=16 #yellow
    PICO_DEFAULT_UART_TX_PIN=17 #red
)

This same setup has correct serial readouts using a simple program like this, so this doesn't appear to be a connection or pin assignment issue.

#include stdio.h
#include "pico/stdlib.h"
#include "pico/stdio.h"

#define DELAY 1000

int main()
{
    int i = 0;

    stdio_init_all();

    sleep_ms(2000);
    printf("Go\n");

    for(;;){
        printf("Count: %d\n", i);
        i++;
        sleep_ms(DELAY);
    }
    
    return 0;
}

Am I missing something?

[pablogs9]

You are using micro-ROS wire protocol in the same serial port as your try to print your stdout, this cannot be done.