Fix RP2040 WiFi (#7)

* Update

Signed-off-by: Pablo Garrido <[email protected]>

* Update

Signed-off-by: Pablo Garrido <[email protected]>

* Update

Signed-off-by: Pablo Garrido <[email protected]>

Author: pablogs9

.github/workflows/ci.yml

@@ -14,7 +14,7 @@ jobs:
         strategy:
           fail-fast: false
           matrix:
-            platform: [teensy41, teensy40, teensy36, teensy35, teensy31, due, zero, olimex_e407, esp32dev, nanorp2040connect, portenta_h7_m7, teensy41_eth, nanorp2040connect_eth, portenta_h7_m7_wifi, esp32dev_wifi]
+            platform: [teensy41, teensy40, teensy36, teensy35, teensy31, due, zero, olimex_e407, esp32dev, nanorp2040connect, portenta_h7_m7, teensy41_eth, nanorp2040connect_wifi, portenta_h7_m7_wifi, esp32dev_wifi]
 
         steps:
         - uses: actions/checkout@v3

ci/platformio.ini

@@ -100,16 +100,6 @@ lib_deps =
     arduino-libraries/Ethernet@^2.0.0
     SPI
 
-[env:nanorp2040connect_eth]
-platform = raspberrypi
-board = nanorp2040connect
-framework = arduino
-microros_transport = native_ethernet
-lib_deps =
-    ../
-    arduino-libraries/Ethernet@^2.0.0
-    SPI
-
 ; WiFi platforms
 
 [env:portenta_h7_m7_wifi]
@@ -132,4 +122,14 @@ microros_transport = wifi
 lib_deps =
     ../
     arduino-libraries/WiFi@^1.2.7
+    SPI
+
+[env:nanorp2040connect_wifi]
+platform = raspberrypi
+board = nanorp2040connect
+framework = arduino
+microros_transport = wifi_nina
+lib_deps =
+    ../
+    arduino-libraries/WiFiNINA@^1.8.13
     SPI

ci/src/main.cpp

@@ -52,7 +52,7 @@ void setup() {
   size_t agent_port = 8888;
 
   set_microros_native_ethernet_transports(local_mac, local_ip, agent_ip, agent_port);
-#elif defined(MICRO_ROS_TRANSPORT_ARDUINO_WIFI)
+#elif defined(MICRO_ROS_TRANSPORT_ARDUINO_WIFI) || defined(MICRO_ROS_TRANSPORT_ARDUINO_WIFI_NINA)
   IPAddress agent_ip(192, 168, 1, 113);
   size_t agent_port = 8888;
 

platform_code/arduino/wifi/micro_ros_transport.h

@@ -2,27 +2,25 @@
 #include <WiFiUdp.h>
 
 struct micro_ros_agent_locator {
-	IPAddress address;
-	int port;
+    IPAddress address;
+    int port;
 };
 
 static inline void set_microros_wifi_transports(char * ssid, char * pass, IPAddress agent_ip, uint16_t agent_port){
-	WiFi.begin(ssid, pass);
-
-    while (WiFi.status() != WL_CONNECTED) {
+    while (WiFi.begin(ssid, pass) != WL_CONNECTED) {
       delay(500);
     }
 
-	static struct micro_ros_agent_locator locator;
-	locator.address = agent_ip;
-	locator.port = agent_port;
+    static struct micro_ros_agent_locator locator;
+    locator.address = agent_ip;
+    locator.port = agent_port;
 
-	rmw_uros_set_custom_transport(
-		false,
-		(void *) &locator,
-		platformio_transport_open,
-		platformio_transport_close,
-		platformio_transport_write,
-		platformio_transport_read
-	);
+    rmw_uros_set_custom_transport(
+        false,
+        (void *) &locator,
+        platformio_transport_open,
+        platformio_transport_close,
+        platformio_transport_write,
+        platformio_transport_read
+    );
 }

platform_code/arduino/wifi_nina/micro_ros_transport.cpp

@@ -0,0 +1,62 @@
+#include <Arduino.h>
+
+#include <micro_ros_platformio.h>
+
+#include <WiFi.h>
+#include <WiFiUdp.h>
+
+#include <uxr/client/util/time.h>
+#include <uxr/client/profile/transport/custom/custom_transport.h>
+
+extern "C" {
+
+static WiFiUDP udp_client;
+
+bool platformio_transport_open(struct uxrCustomTransport * transport)
+{
+  struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args;
+  return true == udp_client.begin(locator->port);
+}
+
+bool platformio_transport_close(struct uxrCustomTransport * transport)
+{
+  udp_client.stop();
+  return true;
+}
+
+size_t platformio_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;
+
+  size_t sent = 0;
+  if(true == udp_client.beginPacket(locator->address, locator->port)){
+    sent = udp_client.write(buf, len);
+    sent = true == udp_client.endPacket() ? sent : 0;
+  }
+
+  udp_client.flush();
+
+  return sent;
+}
+
+size_t platformio_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode)
+{
+  (void)errcode;
+
+  int64_t start_time = uxr_millis();
+
+  while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) {
+    delay(1);
+  }
+
+  size_t available = 0;
+  if(udp_client.available()){
+    available = udp_client.read(buf, len);
+  }
+
+  return (available < 0) ? 0 : available;
+}
+
+}

platform_code/arduino/wifi_nina/micro_ros_transport.h

@@ -0,0 +1,27 @@
+#include <SPI.h>
+#include <WiFiNINA.h>
+#include <WiFiUdp.h>
+
+struct micro_ros_agent_locator {
+    IPAddress address;
+    int port;
+};
+
+static inline void set_microros_wifi_transports(char * ssid, char * pass, IPAddress agent_ip, uint16_t agent_port){
+    while (WiFi.begin(ssid, pass) != WL_CONNECTED) {
+      delay(500);
+    }
+
+    static struct micro_ros_agent_locator locator;
+    locator.address = agent_ip;
+    locator.port = agent_port;
+
+    rmw_uros_set_custom_transport(
+        false,
+        (void *) &locator,
+        platformio_transport_open,
+        platformio_transport_close,
+        platformio_transport_write,
+        platformio_transport_read
+    );
+}