rclc_support_init() hangs

[KohlhardtC]

Issue template

• Hardware description: Teensy 4.1
• RTOS: micro_ros_arduino bare metal
• Version or commit hash: foxy

Steps to reproduce the issue

I am trying to implement a NativeEthernet transport over TCP for the Teensy 4.1 board using the WIFI transport as my starting point.

Agent Launch Command: ros2 run micro_ros_agent micro_ros_agent tcp4 -p 8080 -v 6

Teensy Code full code attached to this issue as .gz, but I'm including inline what I believe are the key parts:

examples/micro-ros_publisher_native_ethernet/micro-ros_publisher_native_ethernet.ino

#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/int32.h>

#ifndef ARDUINO_TEENSY41
#error This example has only been tested on Teensy 4.1
#endif

// The implementation will attempt to assign the client IP address using DHCP
// If DHCP fails, the following self assigned IP will be used instaed
IPAddress fallback_client_ip( 192,168,7,100 );

//For TCP connections, we need to know the agent IP address 
IPAddress agent_ip( 192,168,7,217 );

//self assigned mac address
//there are blocks of allowed self assigned mac addresses similar to IP
byte mac[] = {
    0x02, 0x47, 0x00, 0x00, 0x00, 0x01
};

//For TCP connections, we need to know the agent IP port
uint agent_port = 8080;

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define LED_PIN 13

#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)){}}

void error_loop(){

  Serial.println( "error loop a");
  while(1){

    Serial.println( "error loop b");
    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(&publisher, &msg, NULL));
    msg.data++;
  }
}

void setup() {

    if(!Serial) {
        Serial.begin( 115200  ); 
        delay(1000);
    }

    Serial.println("Setup start");

// TCP -- 
// Mac Address
// IP Address if DHCP fails
// Agent IP address
// PORT

// UDP
// TCP or UDP 
// IP Address if DHCP fails

  //set_microros_wifi_transports("WIFI SSID", "WIFI PASS", "192.168.1.57", 8888);
  //

  set_microros_native_ethernet_tcp_4_transports( mac, fallback_client_ip, agent_ip, agent_port  );

  Serial.println("Setup 1");

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

  delay(2000);

  Serial.println("Setup 2");
  allocator = rcl_get_default_allocator();

  Serial.println("Setup 2.2");

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

  Serial.println("Setup 3");
  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_native_ethernet_node", "", &support));

  Serial.println("Setup 4");

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "topic_name"));

  msg.data = 0;

    Serial.println("Setup end");
}

void loop() {
    Serial.println("publish....");
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
}

src/micro_ros_arduino.h

#ifndef MICRO_ROS_ARDUINO
#define MICRO_ROS_ARDUINO

// ---- Build fixes -----
//Removing __attribute__ not supported by gcc-arm-none-eabi-5_4
#define __attribute__(x)

#ifdef ARDUINO_SAMD_ZERO
// Avoid macro usage in https://github.com/eProsima/Micro-XRCE-DDS-Client/blob/66df0a6df20063d246dd638ac3d33eb2e652fab2/include/uxr/client/core/session/session.h#L97
// beacuse of https://github.com/arduino/ArduinoCore-samd/blob/0b60a79c4b194ed2e76fead95caf1bbce8960049/cores/arduino/sync.h#L28
#define synchronized synchronized
#endif
// ----------------------

#include <uxr/client/transport.h> 
#include <rmw_microros/rmw_microros.h>

extern "C" bool arduino_transport_open(struct uxrCustomTransport * transport);
extern "C" bool arduino_transport_close(struct uxrCustomTransport * transport);
extern "C" size_t arduino_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
extern "C" size_t arduino_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);

static inline void set_microros_transports(){
    rmw_uros_set_custom_transport(
        true,
        NULL,
        arduino_transport_open,
        arduino_transport_close,
        arduino_transport_write,
        arduino_transport_read
    );
}

#ifdef TARGET_PORTENTA_H7_M7

#include <WiFi.h>
#include <WiFiUdp.h>

extern "C" bool arduino_wifi_transport_open(struct uxrCustomTransport * transport);
extern "C" bool arduino_wifi_transport_close(struct uxrCustomTransport * transport);
extern "C" size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
extern "C" size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);

struct micro_ros_agent_locator {
    IPAddress address;
    int port;
};

static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){
    while (WiFi.begin(ssid, pass) != WL_CONNECTED)
    {
        delay(500);
    }

    static struct micro_ros_agent_locator locator;
    locator.address.fromString(agent_ip);
    locator.port = agent_port;

    rmw_uros_set_custom_transport(
        false,
        (void *) &locator,
        arduino_wifi_transport_open,
        arduino_wifi_transport_close,
        arduino_wifi_transport_write,
        arduino_wifi_transport_read
    );
}

#endif // TARGET_PORTENTA_H7_M7

#ifdef ARDUINO_TEENSY41

#include <SPI.h>
#include <NativeEthernet.h>

extern EthernetClient client;

extern "C" bool arduino_native_ethernet_tcp_4_transport_open(struct uxrCustomTransport * transport);
extern "C" bool arduino_native_ethernet_tcp_4_transport_close(struct uxrCustomTransport * transport);
extern "C" size_t arduino_native_ethernet_tcp_4_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
extern "C" size_t arduino_native_ethernet_tcp_4_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);

struct micro_ros_agent_locator {
    IPAddress address;
    int port;
}; 

// static inline void set_microros_native_ethernet_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){
static inline void set_microros_native_ethernet_tcp_4_transports(byte mac[],IPAddress fallback_client_ip, IPAddress agent_ip , uint agent_port){

    if(!Serial) {
        Serial.begin(9600);
        //wait a bit for serial to get ready
        /*
        while( !Serial.availableForWrite() ) {
            delay(100);
        }
        */
        delay(1000);
    }

    Serial.println("Initialize NativeEthernet with DHCP..."); 
    if (Ethernet.begin(mac) == 0) {
        Serial.println("Failed to configure Ethernet using DHCP");
        // Check for Ethernet hardware present
        if (Ethernet.hardwareStatus() == EthernetNoHardware) {
            Serial.println("Ethernet hardware was not found.  Sorry, can't run without hardware. :(");
            while (true) {
                delay(1); // do nothing, no point running without Ethernet hardware
            }
        }
        if (Ethernet.linkStatus() == LinkOFF) {
            Serial.println("No ethernet link detected.");
        }

        // try to congifure using IP address instead of DHCP:

        Serial.println("Using self assigned IP");
        Ethernet.begin(mac, fallback_client_ip);
    } else {
        Serial.print(" CK - DHCP assigned IP ");
        Serial.println(Ethernet.localIP());
    }

    static struct micro_ros_agent_locator locator;
    locator.address = agent_ip;
    locator.port = agent_port;

    Serial.println("Creating custom transport");
    rmw_uros_set_custom_transport(
        true,
        (void *) &locator,
        arduino_native_ethernet_tcp_4_transport_open,
        arduino_native_ethernet_tcp_4_transport_close,
        arduino_native_ethernet_tcp_4_transport_write,
        arduino_native_ethernet_tcp_4_transport_read
    ); 
    Serial.println("done Creating custom transport");
}

#endif // TARGET_ARDUINO_TEENSY41

#endif  // MICRO_ROS_ARDUINO

Expected behavior

Example code should execute cleanly and enter the main loop

Actual behavior

rclc_support_init() hangs preventing main loop from executing

Output on Serial Monitor from Teensy:

Setup start
Initialize NativeEthernet with DHCP...
 CK - DHCP assigned IP 192.168.7.185
Creating custom transport
done Creating custom transport
Setup 1
Setup 2
Setup 2.2
Open()
Open(): Connect....
 Connected!
 client NOT avail
Write()
Write() 1
Write() 2
End Write()
Read()
Read() printed about 100 times

Output from agent:

(base) chrisk@brain:~/code/micro-ros/test_ws$ ros2 run micro_ros_agent micro_ros_agent tcp4 -p 8080  -v 6
[1620401286.196109] info     | TCPv4AgentLinux.cpp | init                     | CK running...          | port: 8080
[1620401286.196242] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1620401304.816081] debug    | TCPv4AgentLinux.cpp | recv_message             | [==>> TCP <<==]        | client_key: 0x00000000, len: 126, data: 
0000: 00 18 00 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 01 ED 53 00 81 00 FC 01 B2 FD 7E 00 00
0020: 18 00 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 01 ED 53 00 81 00 FC 01 B2 FD 7E 00 00 18
0040: 00 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 01 ED 53 00 81 00 FC 01 B2 FD 7E 00 00 18 00
0060: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 01 ED 53 00 81 00 FC 01 B2 FD 7E 00 00 18

Additional information

I would like to keep debugging this, but I'm not sure how best to approach that at this point and would appreciate any/all suggestions.

native_ethernet.tar.gz

[pablogs9]

Try the same using UDP and let us know if it works.

[KohlhardtC]

rclc_support_init() also hangs in a UDP NativeEthernet transport implementation that is very similar to the WiFi transport code.

UDP agent startup command: ros2 run micro_ros_agent micro_ros_agent udp4 -p 8080 -v 6

UDP agent output:

I've checked both the UDP and TCP implementations in to my fork

[pablogs9]

You are enabling framing for the custom transport here, try with this to false.

That means that you are using a packet-oriented transport instead of a stream-oriented transport, more info here

[KohlhardtC]

Thank you for the helpful suggestion Pablo. The good news is that the UDP implementation now appears to be working thanks to your solution (I will do more testing before submitting a pull request).

Unfortunately I'm still seeing the same issue where rclc_support_init() does not return when the TCP implementation is used. Do you have any suggestions for how to debug what is happening inside rclc_support_init()? Is it possible that there is an issue with the TCP version of the agent that is causing this issue?

My latest work has been pushed to my fork.

[pablogs9]

TCP is not going to work because it is not supported in micro-ROS. Do you have a special interest in having this working over TCP?

[KohlhardtC]

I added code for a TCP transport because the Agent purports to support TCP and of course TCP is designed to be more reliable. Regardless, I don't believe I have a special requirement for TCP and I will forge ahead with the UDP implementation. I'll submit a pull request after I conduct some real world testing. Thanks for implementing micro-ros!