Problems running ping-pong application on Arduino DUE and Serial communication

[Latek]

Issue template

• Hardware description: Macbook Pro + Arduino DUE
• Software description: Ubuntu Linux 22.04.1 installed on Virtualbox, installed ROS2 Humble according to the description: https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html, Installed Micro ROS according to the description: https://micro.ros.org/docs/tutorials/core/first_application_linux/
• RTOS: Arduino IDE
• Installation type: micro_ros_setup
• Version or commit hash: humble

Steps to reproduce the issue

1. Compiled and uploaded the following code using the Arduino IDE + micro_ro_arduino plugin using the patch for Arduino DUE:
   micro_ros_arduino_ping_pong.ino:

#include <Arduino.h>
#include <micro_ros_arduino.h>

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/header.h>

#include <stdio.h>
#include <time.h>
#include "clock_gettime.h"

#define STRING_BUFFER_LEN 100
#define LED_PIN 13

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

void error_loop()
{
    while (1)
    {
        digitalWrite(LED_PIN, !digitalRead(LED_PIN));
        delay(100);
    }
}

rclc_executor_t executor;
rcl_publisher_t ping_publisher;
rcl_publisher_t pong_publisher;
rcl_subscription_t ping_subscriber;
rcl_subscription_t pong_subscriber;

std_msgs__msg__Header incoming_ping;
std_msgs__msg__Header outcoming_ping;
std_msgs__msg__Header incoming_pong;

int device_id;
int seq_no;
int pong_count;

void ping_timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
    (void)last_call_time;

    if (timer != NULL)
    {

        seq_no = rand();
        sprintf(outcoming_ping.frame_id.data, "%d_%d", seq_no, device_id);monit
        outcoming_ping.frame_id.size = strlen(outcoming_ping.frame_id.data);

        // Fill the message timestamp
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);
        outcoming_ping.stamp.sec = ts.tv_sec;
        outcoming_ping.stamp.nanosec = ts.tv_nsec;

        // Reset the pong count and publish the ping message
        pong_count = 0;
        RCSOFTCHECK(rcl_publish(&ping_publisher, (const void *)&outcoming_ping, NULL));
        printf("Ping send seq %s\n", outcoming_ping.frame_id.data);
    }
}

void ping_subscription_callback(const void *msgin)
{
    const std_msgs__msg__Header *msg = (const std_msgs__msg__Header *)msgin;

    // Dont pong my own pings
    if (strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) != 0)
    {
        printf("Ping received with seq %s. Answering.\n", msg->frame_id.data);
        RCSOFTCHECK(rcl_publish(&pong_publisher, (const void *)msg, NULL));
    }
}

void pong_subscription_callback(const void *msgin)
{
    const std_msgs__msg__Header *msg = (const std_msgs__msg__Header *)msgin;

    if (strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) == 0)
    {
        pong_count++;
        printf("Pong for seq %s (%d)\n", msg->frame_id.data, pong_count);
    }
}

void setup()
{
    set_microros_transports();

    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, HIGH);

    delay(2000);

    rcl_allocator_t allocator = rcl_get_default_allocator();
    rclc_support_t support;

    // create init_options
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

    // create node
    rcl_node_t node;
    RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support));

    // Create a reliable ping publisher
    RCCHECK(rclc_publisher_init_default(&ping_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));

    // Create a best effort pong publisher
    RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));

    // Create a best effort ping subscriber
    RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));

    // Create a best effort  pong subscriber
    RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));

    // Create a 3 seconds ping timer timer,
    rcl_timer_t timer;
    RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback));

    // Create executor
    executor = rclc_executor_get_zero_initialized_executor();
    RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));
    RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping, &ping_subscription_callback, ON_NEW_DATA));
    RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong, &pong_subscription_callback, ON_NEW_DATA));

    // Create and allocate the pingpong messages

    char outcoming_ping_buffer[STRING_BUFFER_LEN];
    outcoming_ping.frame_id.data = outcoming_ping_buffer;
    outcoming_ping.frame_id.capacity = STRING_BUFFER_LEN;

    char incoming_ping_buffer[STRING_BUFFER_LEN];
    incoming_ping.frame_id.data = incoming_ping_buffer;
    incoming_ping.frame_id.capacity = STRING_BUFFER_LEN;

    char incoming_pong_buffer[STRING_BUFFER_LEN];
    incoming_pong.frame_id.data = incoming_pong_buffer;
    incoming_pong.frame_id.capacity = STRING_BUFFER_LEN;

    device_id = rand();

    rclc_executor_spin(&executor);

    RCCHECK(rcl_publisher_fini(&ping_publisher, &node));
    RCCHECK(rcl_publisher_fini(&pong_publisher, &node));
    RCCHECK(rcl_subscription_fini(&ping_subscriber, &node));
    RCCHECK(rcl_subscription_fini(&pong_subscriber, &node));
    RCCHECK(rcl_node_fini(&node));
}

void loop()
{
    delay(100);
    RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

clock_gettime.h:

#include <Arduino.h>

#include <sys/time.h>
#define micro_rollover_useconds 4294967295

#ifndef _POSIX_TIMERS

extern "C" int clock_gettime(clockid_t unused, struct timespec *tp)
{
    (void)unused;
    static uint32_t rollover = 0;
    static uint32_t last_measure = 0;

    uint32_t m = micros();
    rollover += (m < last_measure) ? 1 : 0;

    uint64_t real_us = (uint64_t) (m + rollover * micro_rollover_useconds);
    tp->tv_sec = real_us / 1000000;
    tp->tv_nsec = (real_us % 1000000) * 1000;
    last_measure = m;

    return 0;
}

#endif  // ifndef _POSIX_TIMERS

3. On the Ubuntu Virtualbox shared the USB device.
4. Run chmod to fix the permissions:
   sudo chmod 666 /dev/ttyACM0
5. Started agent in the first terminal:

xport ROS_DISTRO=humble
source /opt/ros/$ROS_DISTRO/setup.bash
source install/local_setup.bash
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0

Got the following output:

6. On the second terminal run:

export ROS_DISTRO=humble
source /opt/ros/$ROS_DISTRO/setup.bash
source install/local_setup.bash
ros2 topic list

without setting "export RMW_IMPLEMENTATION=rmw_microxrcedds"
see

/microROS/ping
/microROS/pong
/parameter_events
/rosout

Case 1:

1. When running

export RMW_IMPLEMENTATION=rmw_microxrcedds
ros2 run micro_ros_demos ping_pong

Expected behavior 1:

See the ping-pong responding back and forth as in the example https://micro.ros.org/docs/tutorials/core/first_application_linux/, Multiple Ping Pong nodes

Actual behavior 1:

[ERROR] [1722378449.432044514] [rclc]: [rclc_init] Error in rcl_init: error not set, at ./ erc/rcl/init.c:218

Failed status on line 82: 1 Aborting.
[ros2run]: Process exited with failure 1

Case 2:

1. When running

export RMW_IMPLEMENTATION=
ros2 run micro_ros_demos ping_pong

(RMW_IMPLEMENTATION set to an empty value)

Expected behavior 2:

See the ping-pong responding back and forth as in the example https://micro.ros.org/docs/tutorials/core/first_application_linux/, Multiple Ping Pong nodes

Actual behavior 2:

[INFO] [1722378409.760130758] [] Created a timer with period 2000mc.

Ping send seq 846930886_1804289383
realloc(): invalid pointer
[ros2run]: Aborted

[hippo5329]

Since you are using micro-ros serial transport, you cannot use serial printf at the same time. You should comment out the printf.

We are using the default fast dds. There is no need to set the RMW_IMPLEMENTATION. And rmw_microxrcedds should not be used with arduino.

Have you tried the int32 publisher? You should start with the simple one.

[hippo5329]

You may follow my wiki.
https://github.com/hippo5329/micro_ros_arduino_examples_platformio/wiki

[Latek]

Thank you for your response.

Since you are using micro-ros serial transport, you cannot use serial printf at the same time. You should comment out the printf.

You're 100% right! My mistake!

We are using the default fast dds. There is no need to set the RMW_IMPLEMENTATION. And rmw_microxrcedds should not be used with arduino.

Have you tried the int32 publisher? You should start with the simple one.

Int32 publisher works as expected. When I removed printf calls my ping-pong example still doesn't work and I got the same error: "realloc(): invalid pointer [ros2run]: Aborted". Sometimes the ping is sent before the error, sometimes not.

I've also tried the micro-ros_addtwoints_service example where the agent seems to connect to due, but the ros2 service caller waits endlessly for the service to be available.

Is the Serial transport method not designed to be used in all cases/examples? Do you think the problem is with using Arduino DUE?

I have built the robot based on the Ardumower (https://wiki.ardumower.de/) for which I'd like to rewrite the program using ROS2 + micro-ROS. The PCB 1.4 is compatible with Arduino DUE, Mega, or Adafruit Grand Central M4 Express.

[hippo5329]

arduino due $50 single core 84Mhz 512K flash 96K sram, Arm will stop mbed support soon.
esp32 $5 dual core 240MHnz 4M flash 512k sram wifi on chip, well supported freertos/arduino/micro-ros

due has very small flash and sram. it has to use the low memory colcon meta. due has very limited capability for micro-ros.
You should really switch to esp32.

[hippo5329]

https://github.com/hippo5329/linorobot2_hardware/wiki