Micro-ros-agent with raspberry pi pico

[vinc00]

Hello,

I’m currently working on a micro-ROS project. I’m trying to build a publisher-node for a ToF sensor which is connected to a Raspberry Pi Pico which again is connected to a NUC running Ubuntu 22.04.

I’m using the following tutorials for my build:

tutorial1

tutorial2

My problem is that when I use the command

$ snap interface serial-port

my Pico isn’t shown as a slot.

$ snap interface serial-port
name:    serial-port
summary: allows accessing a specific serial port
plugs:
  - micro-ros-agent

Below is my publisher-node and CMakeLists.

Publisher-Node:

#include <stdio.h>
#include <string.h>
#include <hardware/i2c.h>
#include <pico/stdlib.h>

// VL53L5CX API
extern "C" {
#include "VL53L5CX_ULD_API/inc/vl53l5cx_api.h"
#include "micro_ros/pico_uart_transports.h"
}
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_uros/options.h>
#include <sensor_msgs/msg/point_cloud2.h>
#include <sensor_msgs/msg/point_field.h>

#define MATRIX_SIZE 8 // 8x8 Matrix
#define POINT_COUNT (MATRIX_SIZE * MATRIX_SIZE)

#define I2C_PORT i2c0
#define I2C_SDA_PIN 0
#define I2C_SCL_PIN 1
#define VL53L5CX_I2C_ADDR 0x52 // Standardadresse des VL53L5CX

#define LED_PIN 25 // GPIO für die LED auf dem Raspberry Pi Pico

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) { printf("Error: %s:%d: %d\n", __FILE__, __LINE__, (int)temp_rc); return -1; }}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) { printf("Warning: %s:%d: %d\n", __FILE__, __LINE__, (int)temp_rc); }}


VL53L5CX_Configuration dev; // VL53L5CX-Konfiguration
VL53L5CX_ResultsData results; // Ergebnisse des VL53L5CX

rcl_publisher_t publisher;
sensor_msgs__msg__PointCloud2 cloud_msg;

// I2C-Transport für VL53L5CX konfigurieren
bool initialize_i2c() {
    i2c_init(I2C_PORT, 400 * 1000); // 400 kHz
    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SDA_PIN);
    gpio_pull_up(I2C_SCL_PIN);

    printf("I2C initialized on SDA: %d, SCL: %d\n", I2C_SDA_PIN, I2C_SCL_PIN);
    return true;
}

// VL53L5CX-Sensor initialisieren
bool initialize_vl53l5cx() {
    if (!initialize_i2c()) {
        printf("I2C initialization failed!\n");
        return false;
    }

    if (vl53l5cx_init(&dev) != 0) {
        printf("VL53L5CX initialization failed!\n");
        return false;
    }

    // Sensor konfigurieren
    vl53l5cx_set_i2c_address(&dev, VL53L5CX_I2C_ADDR);
    vl53l5cx_set_resolution(&dev, VL53L5CX_RESOLUTION_8X8);
    vl53l5cx_set_ranging_frequency_hz(&dev, 15);

    printf("VL53L5CX initialized successfully.\n");
    return true;
}

void timer_callback(rcl_timer_t *timer, int64_t /*last_call_time*/) {
    if (timer) {
        if (vl53l5cx_check_data_ready(&dev, NULL) == 0) {
            printf("VL53L5CX data not ready.\n");
            return;
        }

        vl53l5cx_get_ranging_data(&dev, &results);

        // PointCloud2-Daten auffüllen
        cloud_msg.header.stamp.sec = 0;  // Zeit synchronisieren, falls verfügbar
        cloud_msg.header.stamp.nanosec = 0;
        cloud_msg.width = POINT_COUNT;
        cloud_msg.height = 1; // Unstrukturierte Punktwolke
        cloud_msg.is_dense = true;

        // Punktwolken-Daten generieren
        for (int i = 0; i < MATRIX_SIZE; i++) {
            for (int j = 0; j < MATRIX_SIZE; j++) {
                float x = (j - MATRIX_SIZE / 2) * 0.05f; // Position in x (Spalten)
                float y = (i - MATRIX_SIZE / 2) * 0.05f; // Position in y (Zeilen)
                float z = results.distance_mm[(i * MATRIX_SIZE) + j] / 1000.0f; // Entfernung in Metern

                size_t index = (i * MATRIX_SIZE + j) * cloud_msg.point_step;

                memcpy(&cloud_msg.data.data[index + 0], &x, sizeof(float)); // x
                memcpy(&cloud_msg.data.data[index + 4], &y, sizeof(float)); // y
                memcpy(&cloud_msg.data.data[index + 8], &z, sizeof(float)); // z
            }
        }

        // Nachricht veröffentlichen
        RCSOFTCHECK(rcl_publish(&publisher, &cloud_msg, NULL));
    } else {
        printf("Failed to publish point cloud. Continuing.\n");
    }
}

int main() {
    stdio_init_all();

    rmw_uros_set_custom_transport(
        true,
        NULL,
        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);

    if (!initialize_vl53l5cx()) {
        printf("VL53L5CX initialization failed. Exiting.\n");
        return -1;
    }

    rcl_allocator_t allocator;
    rclc_support_t support;
    rcl_node_t node;

    rcl_timer_t timer;
    rclc_executor_t executor;

    // Agent-Ping-Timeout
    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) {
        while (true) {
            gpio_put(LED_PIN, 1);
            sleep_ms(500);
            gpio_put(LED_PIN, 0);
            sleep_ms(500);
        }
        return ret;
    }

    allocator = rcl_get_default_allocator();
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

    RCCHECK(rclc_node_init_default(&node, "vl53l5cx_pointcloud_node", "pico", &support));

    RCCHECK(
        rclc_publisher_init_best_effort(
            &publisher,
            &node,
            ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, PointCloud2),
            "pointcloud"
        )
    );

    RCCHECK(
        rclc_timer_init_default2(
            &timer,
            &support,
            RCL_MS_TO_NS(1000 / 15), // 15 Hz
            timer_callback,
            RCL_STEADY_TIME
        )
    );

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

    // PointCloud2-Parameter initialisieren
    cloud_msg.header.frame_id.data = (char *)"sensor_frame";
    cloud_msg.header.frame_id.size = strlen("sensor_frame");
    cloud_msg.header.frame_id.capacity = strlen("sensor_frame");

    cloud_msg.fields.size = 3;
    cloud_msg.fields.capacity = 3;
    cloud_msg.fields.data = (sensor_msgs__msg__PointField *)malloc(sizeof(sensor_msgs__msg__PointField) * 3);

    // Ändere die Zuweisungen für "name" auf die richtige Art und Weise
    cloud_msg.fields.data[0].name.data = (char *)"x";
    cloud_msg.fields.data[0].name.size = strlen("x");
    cloud_msg.fields.data[0].name.capacity = strlen("x");
    cloud_msg.fields.data[0].offset = 0;
    cloud_msg.fields.data[0].datatype = sensor_msgs__msg__PointField__FLOAT32;
    cloud_msg.fields.data[0].count = 1;

    cloud_msg.fields.data[1].name.data = (char *)"y";
    cloud_msg.fields.data[1].name.size = strlen("y");
    cloud_msg.fields.data[1].name.capacity = strlen("y");
    cloud_msg.fields.data[1].offset = sizeof(float);
    cloud_msg.fields.data[1].datatype = sensor_msgs__msg__PointField__FLOAT32;
    cloud_msg.fields.data[1].count = 1;

    cloud_msg.fields.data[2].name.data = (char *)"z";
    cloud_msg.fields.data[2].name.size = strlen("z");
    cloud_msg.fields.data[2].name.capacity = strlen("z");
    cloud_msg.fields.data[2].offset = 2 * sizeof(float);
    cloud_msg.fields.data[2].datatype = sensor_msgs__msg__PointField__FLOAT32;
    cloud_msg.fields.data[2].count = 1;

    cloud_msg.point_step = sizeof(float) * 3;
    cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width;
    cloud_msg.data.size = cloud_msg.row_step * cloud_msg.height;
    cloud_msg.data.capacity = cloud_msg.data.size;
    cloud_msg.data.data = (uint8_t *)malloc(cloud_msg.data.size);

    gpio_put(LED_PIN, 1);

    RCCHECK(rclc_executor_spin(&executor));

    RCCHECK(rcl_publisher_fini(&publisher, &node));
    RCCHECK(rcl_node_fini(&node));

    return EXIT_SUCCESS;
}

CMakeLists.txt:

cmake_minimum_required(VERSION 3.12)

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

project(pico_micro_ros_example C CXX ASM)
set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)

pico_sdk_init()

include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/mico_ros
${CMAKE_CURRENT_SOURCE_DIR}/micro_ros
${CMAKE_CURRENT_SOURCE_DIR}/src
${CMAKE_CURRENT_SOURCE_DIR}/VL53L5CX_ULD_API/src
${CMAKE_CURRENT_SOURCE_DIR}/VL53L5CX_ULD_API/inc
)

link_directories(libmicroros)

###########
# EXAMPLE #
###########

add_executable(pico_micro_ros_example
    pico_micro_ros_example.c
    pico_uart_transport.c
)
target_link_libraries(pico_micro_ros_example
    pico_stdlib
    microros
)

target_include_directories(pico_micro_ros_example PUBLIC
    libmicroros/include
)

SET(CMAKE_C_FLAGS  "${CMAKE_C_FLAGS} -ffunction-sections -fdata-sections")
SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -ffunction-sections -fdata-sections")

# Configure Pico
pico_enable_stdio_usb(pico_micro_ros_example 1)
pico_enable_stdio_uart(pico_micro_ros_example 0)
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)

# Generate UF2
pico_add_extra_outputs(pico_micro_ros_example)

############
# VL53L5CX #
############

# Add executable and specify the source files
add_executable(pico_vl53l5cx_publisher
    src/pico_vl53l5cx_publisher.cpp
    VL53L5CX_ULD_API/src/vl53l5cx_api.c
    platform.c
    pico_uart_transport.c
)

# Include directories
target_include_directories(pico_vl53l5cx_publisher PRIVATE
    ${CMAKE_CURRENT_LIST_DIR}
    ${CMAKE_CURRENT_LIST_DIR}/VL53L5CX_ULD_API/inc
    ${CMAKE_CURRENT_LIST_DIR}/micro_ros
    SYSTEM libmicroros/include
)

target_link_directories(pico_vl53l5cx_publisher PRIVATE libmicroros) #neu

# Link libraries
target_link_libraries(pico_vl53l5cx_publisher 
    pico_stdlib
    hardware_i2c
    microros
)

# Enable USB output, disable UART output
pico_enable_stdio_usb(pico_vl53l5cx_publisher 1)
pico_enable_stdio_uart(pico_vl53l5cx_publisher 0)

target_compile_definitions(pico_vl53l5cx_publisher
  PUBLIC PICO_UART_ENABLE_CRLF_SUPPORT=0
  PUBLIC PICO_STDIO_ENABLE_CRLF_SUPPORT=0
  PUBLIC PICO_STDIO_DEFAULT_CRLF=0
)

# Create map/bin/hex/uf2 file in addition to ELF.
 pico_add_extra_outputs(pico_vl53l5cx_publisher)

[mborzecki1]

Can you clarify how pico is connected to the host running Ubuntu 22.04? Is the pi operating as a usb gadget?

[ogra]

You might need to turn hotplug support on to get a serial-port slot from the pico to connect the micro-ros-agent plug to:

Hotplug support

Hotplug support gives snaps the ability to integrate with Linux’s USB hotplugging and Udev device management subsystem. This enables snaps to recognise specific classes of device and create corresponding system slots when a device is connected. Developers wishing to add hotplug functionality to their own interfaces should see Developing hotplug interfaces. Hotplug support is currently under active development, and will become widely available with a future release of snap…

[artivis]

Hi @vinc00,

Just a heads up, this blog post is now quite outdated and the micro-ros-agent snap is unfortunately not maintained (it might even not work anymore with the latest micro-ros codebase). The publisher (eProsima) maintain instead the vulcanexus-micro-ros-agent.