micro_ros_arduino icon indicating copy to clipboard operation
micro_ros_arduino copied to clipboard

Published 20 hours ago • verified publisher icon micro-ROS

Trying TCP Transports on ESP32

Open kalaajiahmad opened this issue 1 year ago • 0 comments

Issue template

  • Hardware description: Esp32
  • Installation type: micro ros arduino
  • Version or commit hash: Humble 2.0.7

Steps to reproduce the issue

run re-connection example with added functionality for TCP transports (seen bellow)

We implemented tcp_transports and tried connecting to the agent using the tcp flag as such: docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble tcp4 -p 30000 Any suggestion on what might be wrong here?

Changes done to add tcp custom transports are:

  1. wifi_transport.cpp
#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_WIO_TERMINAL)
#include <Arduino.h>


#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7)
#include <WiFi.h>
#include <WiFiUdp.h>
#elif defined(ARDUINO_NANO_RP2040_CONNECT)
#include <SPI.h>
#include <WiFiNINA.h>
#elif defined(ARDUINO_WIO_TERMINAL)
#include <rpcWiFi.h>
#include <WiFiUdp.h>
#endif

#include <micro_ros_arduino.h>

extern "C"
{

  static WiFiUDP udp_client;
  static WiFiClient wifi_client;

  bool arduino_udp_transport_open(struct uxrCustomTransport * transport)
  {
    struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args;
    udp_client.begin(locator->port);
    return true;
  }

  bool arduino_udp_transport_close(struct uxrCustomTransport * transport)
  {
    udp_client.stop();
    return true;
  }

  size_t arduino_udp_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode)
  {
    (void)errcode;
    struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args;

    udp_client.beginPacket(locator->address, locator->port);
    size_t sent = udp_client.write(buf, len);
    udp_client.endPacket();
    udp_client.flush();

    return sent;
  }

  size_t arduino_udp_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode)
  {
    (void) errcode;

    uint32_t start_time = millis();

    while(millis() - start_time < timeout && udp_client.parsePacket() == 0){
      delay(1);
    }

    size_t readed  = udp_client.read(buf, len);

    return (readed < 0) ? 0 : readed;
  }

  bool arduino_tcp_transport_open(struct uxrCustomTransport * transport)
  {
    struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args;
    return wifi_client.connect(locator->address, locator->port, 10000);
  }

  bool arduino_tcp_transport_close(struct uxrCustomTransport * transport)
  {
    wifi_client.stop();
    return true;
  }

  size_t arduino_tcp_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode)
  {
    (void)errcode;
    
    size_t sent = wifi_client.write(buf, len);
    wifi_client.flush();

    return sent;
  }

  size_t arduino_tcp_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode)
  {
    (void) errcode;

    size_t read  = wifi_client.read(buf, len);

    return (read < 0) ? 0 : read;
  }
}

#endif
  1. micro_ros_arduino.h
#ifndef MICRO_ROS_ARDUINO
#define MICRO_ROS_ARDUINO

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

// In GNU C < 6.0.0 __attribute__((deprecated(msg))) is not supported for enums, used in rmw/types.h
#if __GNUC__ < 6
#define aux__attribute__(x) __attribute__(x)
#define __attribute__(x)
#endif

#include <rmw_microros/rmw_microros.h>

#if __GNUC__ < 6
#undef __attribute__
#define __attribute__(x) aux__attribute__(x)
#endif

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
	);
}

#if defined(TARGET_STM32F4)

#include <Arduino.h>
#include <EthernetUdp.h>
#include <LwIP.h>
#include <STM32Ethernet.h>

#include <uxr/client/transport.h>
#include <rmw_microros/rmw_microros.h>
#include "IPAddress.h"
#endif

#ifdef ARDUINO_TEENSY41
#include <NativeEthernet.h>
#endif

#if defined(TARGET_PORTENTA_H7_M7)
#include <PortentaEthernet.h>
#endif

#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41)  || defined(TARGET_PORTENTA_H7_M7)
extern "C" bool arduino_native_ethernet_udp_transport_open(struct uxrCustomTransport * transport);
extern "C" bool arduino_native_ethernet_udp_transport_close(struct uxrCustomTransport * transport);
extern "C" size_t arduino_native_ethernet_udp_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
extern "C" size_t arduino_native_ethernet_udp_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_udp_transports(byte mac[], IPAddress client_ip, IPAddress agent_ip, uint16_t agent_port){

	static struct micro_ros_agent_locator locator;

   	Ethernet.begin(mac, client_ip);
	while (Ethernet.linkStatus() == LinkOFF){
		delay(100);
	}

	locator.address = agent_ip;
	locator.port = agent_port;

	rmw_uros_set_custom_transport(
		false,
		(void *) &locator,
		arduino_native_ethernet_udp_transport_open,
		arduino_native_ethernet_udp_transport_close,
		arduino_native_ethernet_udp_transport_write,
		arduino_native_ethernet_udp_transport_read
	);
}

#endif

#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_WIO_TERMINAL)

#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7)
#include <WiFi.h>
#include <WiFiUdp.h>
#include <WiFiClient.h>
#elif defined(ARDUINO_NANO_RP2040_CONNECT)
#include <SPI.h>
#include <WiFiNINA.h>
#elif defined(ARDUINO_WIO_TERMINAL)
#include <rpcWiFi.h>
#include <WiFiUdp.h>
#endif

extern "C" bool arduino_tcp_transport_open(struct uxrCustomTransport * transport);
extern "C" bool arduino_tcp_transport_close(struct uxrCustomTransport * transport);
extern "C" size_t arduino_tcp_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
extern "C" size_t arduino_tcp_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
#ifndef TARGET_PORTENTA_H7_M7
struct micro_ros_agent_locator {
	IPAddress address;
	int port;
};
#endif

static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){

	WiFi.begin(ssid, pass);

    while (WiFi.status() != 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_tcp_transport_open,
		arduino_tcp_transport_close,
		arduino_tcp_transport_write,
		arduino_tcp_transport_read
	);
}

#endif

#endif  // MICRO_ROS_ARDUINO

Expected behavior

Client connects to agent and creates all entities

Actual behavior

But nothing shows on the agent side. I get the following debug logs from the agent:

~ $ docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble tcp4 -p 30000 -v6
[1687977765.433973] info     | TCPv4AgentLinux.cpp | init                     | running...             | port: 30000
[1687977765.434099] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1687977805.944190] debug    | TCPv4AgentLinux.cpp | recv_message             | [==>> TCP <<==]        | client_key: 0x00000000, len: 128, data: 
0000: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0020: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0040: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0060: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
[1687977821.076395] debug    | TCPv4AgentLinux.cpp | recv_message             | [==>> TCP <<==]        | client_key: 0x00000000, len: 128, data: 
0000: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0020: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0040: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0060: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
[1687977836.196668] debug    | TCPv4AgentLinux.cpp | recv_message             | [==>> TCP <<==]        | client_key: 0x00000000, len: 128, data: 
0000: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0020: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0040: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00
0060: 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 80 00

kalaajiahmad avatar Jun 29 '23 06:06 kalaajiahmad