search

micro-ROS / micro_ros_arduino

micro-ROS library for Arduino
Apache License 2.0
453 stars 117 forks source link

Possibility to run tcp transport on esp32? #1446

Closed kalaajiahmad closed 1 year ago

kalaajiahmad commented 1 year ago

Issue template

Steps to reproduce the issue

Setting the colcon.meta config file with "-DUCLIENT_PROFILE_TCP=ON" and trying to build the library using: docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:humble -p esp32

Expected behavior

Build successfully!

Actual behavior

I get this error:

--- stderr: microxrcedds_client                                                                                                                                                          
In file included from /uros_ws/firmware/mcu_ws/eProsima/Micro-XRCE-DDS-Client/include/uxr/client/transport.h:45,
                 from /uros_ws/firmware/mcu_ws/eProsima/Micro-XRCE-DDS-Client/include/uxr/client/util/ping.h:28,
                 from /uros_ws/firmware/mcu_ws/eProsima/Micro-XRCE-DDS-Client/src/c/util/ping.c:1:
/uros_ws/firmware/mcu_ws/eProsima/Micro-XRCE-DDS-Client/include/uxr/client/profile/transport/ip/tcp/tcp_transport.h:55:27: error: field 'platform' has incomplete type
     struct uxrTCPPlatform platform;
                           ^~~~~~~~
gmake[2]: *** [CMakeFiles/microxrcedds_client.dir/build.make:289: CMakeFiles/microxrcedds_client.dir/src/c/util/ping.c.obj] Error 1
gmake[2]: *** Waiting for unfinished jobs....
gmake[1]: *** [CMakeFiles/Makefile2:86: CMakeFiles/microxrcedds_client.dir/all] Error 2
gmake: *** [Makefile:139: all] Error 2

Additional information

Is this even supported?

Acuadros95 commented 1 year ago

Is this even supported?

No, its not supported.

Feel free to implement your own implementation, you can check an example here

kalaajiahmad commented 1 year ago

Thank you for your quick reply @Acuadros95 Any suggestion on what might be wrong? 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 But nothing shows on the agent side. I get the following debug logs from the agent:

ahmad@ahmad ~ $ 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

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