Micro-ROS agent not working for a serial communication between Arduino R4 Minima and Linux computer

[Yanniscod]

Hello,

I am working on an Arduino R4 Minima with the goal to create a micro-ROS communication between this board and a Raspberry [3B+]. I am using ROS2 galactic, downloaded the micro-ROS application from source here and the micro-ROS Arduino package from here.

The code uploaded on the Arduino was tested without micro-ROS via serial communication and works well, as I am able to insert a velocity value in the stepper motor to make it move and can get the left and right encoder tick values. Now, when I try to integrate it with micro-ROS functions and that I test it using a serial communication between my PC and the arduino, most of the time it doesn't work. Here is the Arduino code adapted for Micro-ROS, where a node with one subscriber (cmd_vel) and two pulbishers (left and right ticks):

#include <MobileRobot.h>
#include <Parameter.h>
#include <Timer.h>
#include <Arduino.h>
#include <micro_ros_arduino.h>

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

#include <std_msgs/msg/int16.h>
#include <std_msgs/msg/string.h>
#include <geometry_msgs/msg/twist.h>

MobileRobot robot;

float angularSpeed = Parameter::angularSpeed;
float linearSpeed = Parameter::linearSpeed;

rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator;

// publishers
rcl_publisher_t pub_right_ticks;
std_msgs__msg__Int16 right_wheel_tick_count;

rcl_publisher_t pub_left_ticks;
std_msgs__msg__Int16 left_wheel_tick_count;

rclc_executor_t executor_pub;
rcl_timer_t timer;

// subscriber
rcl_subscription_t subscriber;
geometry_msgs__msg__Twist msg_twist;
rclc_executor_t executor_sub;

#define POWER_OPT A5
#define LED_PIN LED_BUILTIN

#define RCCHECK(fn) \
  { \
    rcl_ret_t temp_rc = fn; \
    if ((temp_rc != RCL_RET_OK)) { error_loop(); } \
  }
#define RCSOFTCHECK(fn) \
  { \
    rcl_ret_t temp_rc = fn; \
    if ((temp_rc != RCL_RET_OK)) {} \
  }

Timer timerTicks(Parameter::timerTicks);
Timer timerSpeedControl(Parameter::timerSpeed);
Timer timerPause(Parameter::timerPause);
bool speedPause = false;

//--------------------------- Interrupt Service Routines------------------------------------- 

static void leftWheelInterruptA() 
{ 
  robot.leftWheelIncrementA();

}

static void rightWheelInterruptA() 
{ 
  robot.rightWheelIncrementA(); 
}

//--------------------------- Micro-ROS Functions------------------------------------- 

// subscription callback
void twist2vel(const void *msgin) {
  const geometry_msgs__msg__Twist *cmd_vel_msg = (const geometry_msgs__msg__Twist *)msgin;
  // if velocity in x direction is 0 turn off LED, if 1 turn on LED
  digitalWrite(LED_PIN, (cmd_vel_msg->linear.x == 0) ? LOW : HIGH);
  linearSpeed  = cmd_vel_msg->linear.x;
  angularSpeed = cmd_vel_msg->angular.z*Parameter::rotationLength/(2*PI);
}

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

void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&pub_right_ticks, &right_wheel_tick_count, NULL));
    RCSOFTCHECK(rcl_publish(&pub_left_ticks, &left_wheel_tick_count, NULL));
  }
}

//--------------------------- Setup ------------------------------------- 

void setup() {
  //Serial.begin(9600);
  set_microros_transports();

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

  delay(1000);

  timerTicks.init();
  timerSpeedControl.init();
  timerPause.init();

  allocator = rcl_get_default_allocator();

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

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

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
    "cmd_vel"));

  // create publishers
  RCCHECK(rclc_publisher_init_default(
    &pub_right_ticks,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
    "right_wheel_tick_count"));

  RCCHECK(rclc_publisher_init_default(
    &pub_left_ticks,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
    "left_wheel_tick_count"));

  // create timer, called every 1000 ms to publish 
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  RCCHECK(rclc_executor_init(&executor_pub, &support.context, 2, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor_pub, &timer));

  RCCHECK(rclc_executor_init(&executor_sub, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor_sub, &subscriber, &msg_twist, &twist2vel, ON_NEW_DATA));

  right_wheel_tick_count.data = 0;
  left_wheel_tick_count.data = 0;
}

//--------------------------- Loop ------------------------------------- 

void loop() {
  if (timerSpeedControl.isTime()){
    robot.move(linearSpeed, angularSpeed / 9.39);
  }

  if (timerTicks.isTime()) {
    left_wheel_tick_count.data = robot.getDataLeftWheel();
    right_wheel_tick_count.data = robot.getDataRightWheel();
  }
  
  RCCHECK(rclc_executor_spin_some(&executor_pub, RCL_MS_TO_NS(100)));
  RCCHECK(rclc_executor_spin_some(&executor_sub, RCL_MS_TO_NS(100)));
  delay(50);
}

• Hardware: PC

• ROS version: Galactic

• RTOS: Ubuntu 20.04

• Steps to reproduce the issue: <source galactic> source install/setup.bash ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 -b 921600

The problems I encounter are the following:

1. When the USB-C cable is already plugged in, I get the following error: Although no other device is connected in this port and that I have the rights to read/write.

2. After resetting or unplugging and replugging USB-C cable, the micro-ROS agent starts but doesn't always create everything, as it sometimes stops after creating one publisher. Using the example file micro-ros_reconnection_example.ino, it seems that the client keeps being created and deleted until an error makes it stop working.

[1692607955.139677] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1692607956.148152] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1692607956.861503] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1692607956.861633] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1692607956.897901] info     | Root.cpp           | create_client            | create                 | client_key: 0x4E2D6FFA, session_id: 0x81
[1692607956.897944] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x4E2D6FFA, address: 0
[1692607956.904276] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x4E2D6FFA, participant_id: 0x000(1)
[1692607956.905845] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x4E2D6FFA, topic_id: 0x000(2), participant_id: 0x000(1)
[1692607956.907004] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x4E2D6FFA, publisher_id: 0x000(3), participant_id: 0x000(1)
[1692607956.908551] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x4E2D6FFA, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1692607957.256260] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x4E2D6FFA
[1692607957.256301] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x4E2D6FFA, address: 0
[1692607958.610806] info     | Root.cpp           | create_client            | create                 | client_key: 0x6371AD06, session_id: 0x81
[1692607958.610861] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x6371AD06, address: 0
[1692607958.617114] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x6371AD06, participant_id: 0x000(1)
[1692607958.821486] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x6371AD06, topic_id: 0x000(2), participant_id: 0x000(1)
[1692607959.326489] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x6371AD06, publisher_id: 0x000(3), participant_id: 0x000(1)
[1692607959.430504] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x6371AD06, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1692607959.550481] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x6371AD06
[1692607959.550525] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x6371AD06, address: 0
[1692607959.550570] info     | Root.cpp           | create_client            | create                 | client_key: 0x19692DD6, session_id: 0x81
[1692607959.550583] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x19692DD6, address: 0
[1692607962.003819] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x19692DD6
[1692607962.003883] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x19692DD6, address: 0
[1692607962.003905] info     | Root.cpp           | create_client            | create                 | client_key: 0x5B365ABB, session_id: 0x81
[1692607962.003914] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x5B365ABB, address: 0
[1692607962.009118] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x5B365ABB, participant_id: 0x000(1)
[1692607962.615098] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x5B365ABB, topic_id: 0x000(2), participant_id: 0x000(1)

Once, after few resets, I was able to establish a correct communication and to move the robot using ros2 run teleop_twist_keyboard teleop_twist_keyboard , and I was getting correct subscriber values, even though there were some important delays. This makes me think of the same problem than #241. I have not tried to modify the client-host-colcon.meta file located in ~/microros_ws/src/micro_ros_setup/config/host/generic as I am not sure to what modify.

Would you have an idea as to how to solve this issue?

Thank you for your time.

• EDIT When trying with the Arduino Zero, the program seems to work correctly, so this must be some sort of incompatibility between micro-ROS library and the Arduino R4 Minima.

[pablogs9]

Hello some questions:

1. Have you created a specific port for Arduino R4? It is not supported currently in this repo. How did you port it?
2. Galactic is deprecated, is there a possibility of using a newer and active ROS 2 version? In Galactic are you using Fast DDS as middleware?
3. Did you change the transport to support 921600 bauds? Or how are you ensuring this baudrate com?