Latek commented 1 month ago

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

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

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_msgsmsgHeader incoming_ping; std_msgsmsgHeader outcoming_ping; std_msgsmsgHeader 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_msgsmsgHeader msg = (const std_msgsmsgHeader *)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_msgsmsgHeader msg = (const std_msgsmsgHeader *)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

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:
<img width="939" alt="Zrzut ekranu 2024-07-30 o 23 59 20" src="https://github.com/user-attachments/assets/cc962012-6727-4b00-8419-c4df58cf9d33">

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

hippo5329 commented 1 month ago

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 commented 1 month ago

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

Latek commented 1 month ago

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 commented 1 month ago

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 commented 1 month ago

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

micro-ROS / micro_ros_arduino

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

Issue template

Steps to reproduce the issue

include <rcl/rcl.h>

include <rcl/error_handling.h>

include <rclc/rclc.h>

include <rclc/executor.h>

include <std_msgs/msg/header.h>

include

include

include "clock_gettime.h"

define STRING_BUFFER_LEN 100

define LED_PIN 13

define RCCHECK(fn) \

define RCSOFTCHECK(fn) \

include

include <sys/time.h>

define micro_rollover_useconds 4294967295

ifndef _POSIX_TIMERS

endif // ifndef _POSIX_TIMERS