Multi Threading + Multi Node

[DwayneGit]

Hello,

I am pretty new to micro-ROS and trying to create two nodes on separate cores.

I have been over the examples of of multi-threading and multiple nodes from the micro-ros demo examples but i have not seen anything on using those libraries here and there is nothing for this specific application.

I have managed to create the code below. it creates two nodes on the main thread and passes one node, the support, and allocator to core1 via queue to handle all the none shared initializations.

This is the closes i come to a working setup. the nodes seem to work fine except that you can not initialize subscribers or publishers on both nodes. so i would need to comment out rclc_timer_init_default for rclc_publisher_init_default to work. timers dont seem to be effect by this. It will abort with a RCL_RET_ERROR if both are attempted.

Please let me know what i am doing wrong or if this is even possible or if something isn't clear. Thank you

PS. I already recompiled the library with #define RMW_UXRCE_MAX_NODES 2

#include <stdio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/int16.h>
#include <geometry_msgs/msg/twist.h>
#include <rmw_microros/rmw_microros.h>

#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "pico/util/queue.h"

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n\r",__LINE__,(int)temp_rc); return 1;}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n\r",__LINE__,(int)temp_rc);}}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    printf("Last callback time: %ld\n\r",  last_call_time );
    if (timer != NULL) {
            /* publish */
    }
}

void subscription_callback(const void * msgin){}

#define FLAG_VALUE 123
queue_t node1_queue;

void core1_entry() {
    queue_node_req_t node1;
    rcl_timer_t timer;
    rclc_executor_t executor;

    multicore_fifo_push_blocking(FLAG_VALUE);

    uint32_t g = multicore_fifo_pop_blocking();
    if (g != FLAG_VALUE)
        printf("Hmm, that's not right on core 1!\n\r");
    else{
        queue_remove_blocking(&node1_queue, &node1);

        RCSOFTCHECK(rclc_executor_init(
            &executor,
            &node1.support.context, 
            1, 
            &node1.allocator
        ));

        RCSOFTCHECK(rclc_publisher_init_default(&pub, &node1.node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16), "pub_node"));

        const unsigned int timer_period = RCL_MS_TO_NS(100);
        RCSOFTCHECK(rclc_timer_init_default(&timer, &node1.support, timer_period, timer_callback));
        RCSOFTCHECK(rclc_executor_add_timer(&executor, &timer));
        printf("Its all gone well on core 1!\n\r");

        rclc_executor_spin(&executor);

        RCSOFTCHECK(rclc_executor_fini(&executor));
        RCSOFTCHECK(rcl_publisher_fini(&pub, &node1.node));  
        RCSOFTCHECK(rcl_node_fini(&node1.node));
        RCSOFTCHECK(rcl_timer_fini(&timer));
    }
}

int main()
{
    rmw_uros_set_custom_transport(
        true,
        NULL,
        pico_serial_transport_open,
        pico_serial_transport_close,
        pico_serial_transport_write,
        pico_serial_transport_read
    );

    stdio_init_all();

    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);

    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);

    queue_init(&node1_queue, sizeof(queue_node_req_t), 2);

    // // Turn off FIFO's - we want to do this character by character
    // uart_set_fifo_enabled(UART_ID, false);

    gpio_put(PICO_DEFAULT_LED_PIN, 1);

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

    multicore_launch_core1(core1_entry);
    uint32_t g = multicore_fifo_pop_blocking();

    if (g != FLAG_VALUE)
        printf("Hmm, that's not right on core 0!\n\r");
    else {
        rcl_node_t node0;
        RCCHECK(rclc_node_init_default(&node0, "node_1", "", &support));
        rcl_node_t node1;
        RCCHECK(rclc_node_init_default(&node1, "node_2", "", &support));

        multicore_fifo_push_blocking(FLAG_VALUE);
        printf("It's all gone well on core 0!\n\r");

        queue_node_req_t node_req = {node1, allocator, support};
        queue_add_blocking(&node1_queue, &node_req);

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

        rcl_subscription_t subscriber;
        geometry_msgs__msg__Twist twist_msg;
        RCCHECK(rclc_subscription_init_default( &subscriber, &node0, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),  "cmd_vel" ));
        RCCHECK(rclc_executor_add_subscription( &executor, &subscriber, &twist_msg, &subscription_callback, ON_NEW_DATA ));

        rclc_executor_spin(&executor);

        RCCHECK(rcl_node_fini(&node0));
        RCCHECK(rclc_executor_fini(&executor));
        RCCHECK(rcl_subscription_fini(&subscriber, &node0));
        RCCHECK(rcl_publisher_fini(&pubFrontRightTicks, &node0));   
        RCCHECK(rclc_support_fini(&support))
    }

    return 0;
}

[pablogs9]

Hello @DwayneGit multithreading is not supported in RPi Pico SDK, you need to implement lockings in middleware here: https://github.com/eProsima/Micro-XRCE-DDS-Client/blob/master/src/c/profile/multithread/multithread.c and rebuild the library with UCLIENT_PROFILE_MULTITHREADenabled in colcon.meta for Micro XRCE-DDS Client.

[DwayneGit]

@pablogs9 Thanks for the reply! Not sure what you mean by "you need to implement lockings in middleware". Is there more that this file needs or is it just copying the file into my project since it seems that the library already has some connection to it like here: https://github.com/micro-ROS/micro_ros_raspberrypi_pico_sdk/blob/humble/libmicroros/include/uxr/client/profile/multithread/multithread.h

[pablogs9]

In this file only locks for PLATFORM_NAME_FREERTOS, UCLIENT_PLATFORM_ZEPHYR, and UCLIENT_PLATFORM_POSIX are implemented. Does your platform support creating mutex somehow?

[DwayneGit]

Yes. looks like the pico sdk does support mutex, lock and semaphore https://raspberrypi.github.io/pico-sdk-doxygen/group__pico__sync.html. So i guess i need to implement the contents of the multithread.c using the pico sdk library?

[pablogs9]

That's it, it would be great if you open a PR to Micro XRCE-DDS Client with your changes

[DwayneGit]

Sure, I'll get on that