micro-ROS / micro-ROS/micro_ros_arduino

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

Open
#1,813 5 comments 0 reactions 0 assignees View on GitHub
Dominant language
C
Stars
576
Forks
146
PR merge metrics
No merged PRs in 30d

Description

## 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
#include

#include
#include
#include
#include

#include

#include
#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_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

#include
#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:
Zrzut ekranu 2024-07-30 o 23 59 20

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
```

Contributor guide

Open the contributing guide

Research direction

Reproduce both cases with the micro_ros_agent serial command and the two ros2 run micro_ros_demos ping_pong variants. Inspect the provided micro_ros_arduino_ping_pong.ino and clock_gettime.h code, starting around the rclc initialization and message publication paths, and compare the reported failures. Done means the Arduino DUE ping-pong application runs without either error and produces the expected responses.

Written by the indexing model from the issue text.

Assessment

Tech stack
c
Domain
embedded-iot, robotics
Issue type
Bug
Difficulty
4/5
Estimated time
3-5 days
Activity status
Stale
Clarity
Needs clarification
Newbie friendliness
25/100

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.