diff --git a/extras/library_generation/library_generation.sh b/extras/library_generation/library_generation.sh index 630c493d6..136878b87 100755 --- a/extras/library_generation/library_generation.sh +++ b/extras/library_generation/library_generation.sh @@ -22,6 +22,7 @@ if [ $OPTIND -eq 1 ]; then PLATFORMS+=("portenta-m7") PLATFORMS+=("kakutef7-m7") PLATFORMS+=("esp32") + PLATFORMS+=("esp32_5_2_0") fi shift $((OPTIND-1)) @@ -239,7 +240,7 @@ fi if [[ " ${PLATFORMS[@]} " =~ " esp32 " ]]; then rm -rf firmware/build - export TOOLCHAIN_PREFIX=/uros_ws/xtensa-esp32-elf/bin/xtensa-esp32-elf- + export TOOLCHAIN_PREFIX=/uros_ws/xtensa-esp32-elf-gcc8_4_0-esp-2021r2/bin/xtensa-esp32-elf- ros2 run micro_ros_setup build_firmware.sh /project/extras/library_generation/esp32_toolchain.cmake /project/extras/library_generation/colcon.meta find firmware/build/include/ -name "*.c" -delete @@ -249,6 +250,20 @@ if [[ " ${PLATFORMS[@]} " =~ " esp32 " ]]; then cp -R firmware/build/libmicroros.a /project/src/esp32/libmicroros.a fi +######## Build for ESP32 with toolchain v5.2.0 ######## +if [[ " ${PLATFORMS[@]} " =~ " esp32_5_2_0 " ]]; then + rm -rf firmware/build + + export TOOLCHAIN_PREFIX=/uros_ws/xtensa-esp32-elf-linux64-1.22/bin/xtensa-esp32-elf- + ros2 run micro_ros_setup build_firmware.sh /project/extras/library_generation/esp32_toolchain.cmake /project/extras/library_generation/colcon.meta + + find firmware/build/include/ -name "*.c" -delete + cp -R firmware/build/include/* /project/src/ + + mkdir -p /project/src/esp32_5_2_0 + cp -R firmware/build/libmicroros.a /project/src/esp32_5_2_0/libmicroros.a +fi + ######## Generate extra files ######## find firmware/mcu_ws/ros2 \( -name "*.srv" -o -name "*.msg" -o -name "*.action" \) | awk -F"/" '{print $(NF-2)"/"$NF}' > /project/available_ros2_types find firmware/mcu_ws/extra_packages \( -name "*.srv" -o -name "*.msg" -o -name "*.action" \) | awk -F"/" '{print $(NF-2)"/"$NF}' >> /project/available_ros2_types diff --git a/src/husarnet_transport.cpp b/src/husarnet_transport.cpp new file mode 100644 index 000000000..53a70f18b --- /dev/null +++ b/src/husarnet_transport.cpp @@ -0,0 +1,127 @@ +#if defined(ESP32) && defined(HUSARNET) +#include +#include +#include +#include + +extern "C" { + +static HusarnetClient client; + +bool arduino_husarnet_transport_open(struct uxrCustomTransport *transport) { + struct micro_ros_agent_locator *locator = + (struct micro_ros_agent_locator *)transport->args; + + + /* Try to connect to a server on port 8888 on your laptop */ + if (!client.connect(locator->hostname, locator->port)) { + return false; + } + + return true; +} + +bool arduino_husarnet_transport_close(struct uxrCustomTransport *transport) { + client.stop(); + return true; +} + +size_t arduino_husarnet_transport_write(struct uxrCustomTransport *transport, + const uint8_t *buf, size_t len, + uint8_t *errcode) { + (void)errcode; + + // As we are using a TCP stream connection we should indicate the size of the message with the first two bytes of the stream. + static uint8_t buffer_size[2]; + buffer_size[0] = (uint8_t)(0x00FF & len); + buffer_size[1] = (uint8_t)((0xFF00 & len) >> 8); + size_t sent = client.write(buffer_size, 2); + + // Then we send the payload + if (sent == 2) { + sent = client.write(buf, len); + } else { + sent = 0; + } + + return sent; +} + +// Sample state machine for receiving data +typedef enum { + STATE_WAIT_FOR_SIZE = 0, + STATE_WAIT_FOR_DATA, + STATE_MESSAGE_AVAILABLE +} husarnet_tcp_states_t; + +typedef struct { + uint8_t buffer[UXR_CONFIG_CUSTOM_TRANSPORT_MTU]; + + uint8_t length_buffer[2]; + + uint16_t message_size; + uint16_t message_size_received; + + husarnet_tcp_states_t state; +} husarnet_tcp_receiver_t; + +static husarnet_tcp_receiver_t receiver = {}; + +void read_tcp_data(husarnet_tcp_receiver_t & r) { + switch(r.state) { + case STATE_WAIT_FOR_SIZE: + if (client.available() >= 2) + { + client.read(r.length_buffer, 2); + r.message_size = (r.length_buffer[0] | (r.length_buffer[1] << 8)); + r.message_size_received = 0; + r.state = STATE_WAIT_FOR_DATA; + } + break; + case STATE_WAIT_FOR_DATA: + if(client.available()) + { + size_t to_read = (r.message_size - r.message_size_received) < client.available() ? r.message_size - r.message_size_received : client.available(); + size_t readed = client.read(&r.buffer[r.message_size_received], to_read); + r.message_size_received += readed; + if(r.message_size_received == r.message_size){ + r.state = STATE_MESSAGE_AVAILABLE; + } + } + break; + case STATE_MESSAGE_AVAILABLE: + break; + } +} + +size_t arduino_husarnet_transport_read(struct uxrCustomTransport *transport, + uint8_t *buf, size_t len, int timeout, + uint8_t *errcode) { + (void)errcode; + + client.setTimeout(timeout); + + do + { + int64_t time_init = uxr_millis(); + read_tcp_data(receiver); + timeout -= (int)(uxr_millis() - time_init); + } + while ((STATE_MESSAGE_AVAILABLE != receiver.state) && (0 < timeout)); + + if (STATE_MESSAGE_AVAILABLE == receiver.state) + { + size_t readed = receiver.message_size; + memcpy(buf, receiver.buffer, readed); + receiver.state = STATE_WAIT_FOR_SIZE; + return readed; + } + else + { + return 0; + } +} + +} + +#endif diff --git a/src/micro_ros_arduino.h b/src/micro_ros_arduino.h index 189d9a0d6..396d5c165 100755 --- a/src/micro_ros_arduino.h +++ b/src/micro_ros_arduino.h @@ -3,33 +3,34 @@ #define MICRO_ROS_ARDUINO // ---- Build fixes ----- -//Removing __attribute__ not supported by gcc-arm-none-eabi-5_4 +// 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 +// 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 +#endif // ARDUINO_SAMD_ZERO // ---------------------- -#include #include +#include -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 - ); +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) @@ -38,50 +39,98 @@ static inline void set_microros_transports(){ #include #include #include - -#include #include +#include + #include "IPAddress.h" -#endif +#endif // defined(TARGET_STM32F4) #ifdef ARDUINO_TEENSY41 #include -#endif +#endif // ARDUINO_TEENSY41 #if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41) -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); +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; + 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; +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); - delay(1000); + Ethernet.begin(mac, client_ip); + delay(1000); - locator.address = agent_ip; - locator.port = agent_port; + 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 - ); + 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 +#endif // defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41) -#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) +#if defined(ESP32) && defined(HUSARNET) +#include + +extern "C" bool arduino_husarnet_transport_open( + struct uxrCustomTransport* transport); +extern "C" bool arduino_husarnet_transport_close( + struct uxrCustomTransport* transport); +extern "C" size_t arduino_husarnet_transport_write( + struct uxrCustomTransport* transport, const uint8_t* buf, size_t len, + uint8_t* err); +extern "C" size_t arduino_husarnet_transport_read( + struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, + uint8_t* err); + +struct micro_ros_agent_locator { + char* hostname; + uint16_t port; + IPv6Address addr; +}; + +static inline void set_microros_husarnet_transports(char* agent_hostname, + uint16_t agent_port) { + bool agentAvailable = 0; + static struct micro_ros_agent_locator locator; + locator.hostname = agent_hostname; + locator.port = agent_port; + + while (false == agentAvailable) { + for (auto const& host : Husarnet.listPeers()) { + if (host.second == locator.hostname) { + locator.addr = host.first; + agentAvailable = true; + break; + } + } + delay(1); + } + + rmw_uros_set_custom_transport( + false, (void*)&locator, arduino_husarnet_transport_open, + arduino_husarnet_transport_close, arduino_husarnet_transport_write, + arduino_husarnet_transport_read); +} + +#elif defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || \ + defined(ARDUINO_NANO_RP2040_CONNECT) #if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) #include @@ -89,40 +138,43 @@ static inline void set_microros_native_ethernet_udp_transports(byte mac[], IPAdd #elif defined(ARDUINO_NANO_RP2040_CONNECT) #include #include -#endif - -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); +#endif // defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) + +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; + IPAddress address; + int port; }; -static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){ +static inline void set_microros_wifi_transports(char* ssid, char* pass, + char* agent_ip, + uint agent_port) { + WiFi.begin(ssid, pass); - WiFi.begin(ssid, pass); + while (WiFi.status() != WL_CONNECTED) { + delay(500); + } - while (WiFi.status() != WL_CONNECTED) { - delay(500); - } + static struct micro_ros_agent_locator locator; + locator.address.fromString(agent_ip); + locator.port = agent_port; - 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 - ); + 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 +#endif // defined(ESP32) && defined(HUSARNET) #endif // MICRO_ROS_ARDUINO diff --git a/src/wifi_transport.cpp b/src/wifi_transport.cpp index 7c7d9c42c..729c239d3 100755 --- a/src/wifi_transport.cpp +++ b/src/wifi_transport.cpp @@ -1,4 +1,4 @@ -#if defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) +#if !defined(HUSARNET) && defined(ESP32) || defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT) #include