WiFi transport and Pico W support

.gitignore

@@ -1,2 +1,3 @@
 /build
-pico-sdk
+pico-sdk
+Filelists.cmake

CMakeLists.txt

@@ -1,25 +1,36 @@
 cmake_minimum_required(VERSION 3.12)
 
+# stop the compiler tests from running
+set(CMAKE_C_COMPILER_WORKS TRUE)
+set(CMAKE_CXX_COMPILER_WORKS TRUE)
+
+set(PICO_BOARD pico_w CACHE STRING "Board type")
+
 include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake)
 
+pico_sdk_init()
+
 project(pico_micro_ros_example C CXX ASM)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_CXX_STANDARD 17)
 
-pico_sdk_init()
-
 link_directories(libmicroros)
 add_executable(pico_micro_ros_example
     pico_micro_ros_example.c
     pico_uart_transport.c
+    pico_wifi_transport.c
 )
 target_link_libraries(pico_micro_ros_example
     pico_stdlib
+    pico_cyw43_arch_lwip_threadsafe_background
+    pico_lwip_arch
+    pico_lwip_core
     microros
 )
 
 target_include_directories(pico_micro_ros_example PUBLIC
     libmicroros/include
+    ${CMAKE_CURRENT_LIST_DIR}
 )
 
 SET(CMAKE_C_FLAGS  "${CMAKE_C_FLAGS} -ffunction-sections -fdata-sections")

OLD_pico_micro_ros_example.c

@@ -0,0 +1,84 @@
+#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>
+#include <rmw_microros/rmw_microros.h>
+
+#include "pico/stdlib.h"
+#include "pico_uart_transports.h"
+
+const uint LED_PIN = 25;
+
+rcl_publisher_t publisher;
+std_msgs__msg__Int32 msg;
+
+void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
+{
+    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
+    msg.data++;
+}
+
+int main()
+{
+    rmw_uros_set_custom_transport(
+		true,
+		NULL,
+		pico_serial_transport_open,
+		pico_serial_transport_close,
+		pico_serial_transport_write,
+		pico_serial_transport_read
+	);
+
+    gpio_init(LED_PIN);
+    gpio_set_dir(LED_PIN, GPIO_OUT);
+
+    rcl_timer_t timer;
+    rcl_node_t node;
+    rcl_allocator_t allocator;
+    rclc_support_t support;
+    rclc_executor_t executor;
+
+    allocator = rcl_get_default_allocator();
+
+    // Wait for agent successful ping for 2 minutes.
+    const int timeout_ms = 1000; 
+    const uint8_t attempts = 120;
+
+    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);
+
+    if (ret != RCL_RET_OK)
+    {
+        // Unreachable agent, exiting program.
+        return ret;
+    }
+
+    rclc_support_init(&support, 0, NULL, &allocator);
+
+    rclc_node_init_default(&node, "pico_node", "", &support);
+    rclc_publisher_init_default(
+        &publisher,
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+        "pico_publisher");
+
+    rclc_timer_init_default(
+        &timer,
+        &support,
+        RCL_MS_TO_NS(1000),
+        timer_callback);
+
+    rclc_executor_init(&executor, &support.context, 1, &allocator);
+    rclc_executor_add_timer(&executor, &timer);
+
+    gpio_put(LED_PIN, 1);
+
+    msg.data = 0;
+    while (true)
+    {
+        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
+    }
+    return 0;
+}

README.md

@@ -53,18 +53,28 @@ cp pico_micro_ros_example.uf2 /media/$USER/RPI-RP2
 ### 3. Start Micro-ROS Agent
 Micro-ROS follows the client-server architecture, so you need to start the Micro-ROS Agent.
 You can do so using the [micro-ros-agent Snap](https://snapcraft.io/micro-ros-agent) (follow the link for installation details):
-
+For serial comunication:
+```bash
+micro_ros_agent serial --dev /dev/ttyACM0 -b 115200
+```
+For wireless comunication:
 ```bash
-micro-ros-agent serial --dev /dev/ttyACM0 -b 115200
+micro_ros_agent udp4 --port 4444
 ```
 
 or using the [micro-ros-agent Docker](https://hub.docker.com/r/microros/micro-ros-agent):
+For serial comunication:
 ```bash
 docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:iron serial --dev /dev/ttyACM0 -b 115200
 ```
+For wireless comunication:
+```bash
+docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:iron udp4 --port 4444
+```
 
 ## What files are relevant?
 - `pico_uart_transport.c`: Contains the board specific implementation of the serial transport (no change needed).
+- `pico_wifi_transport.c`: Contains the board specific implementation of the wifi transport (no change needed).
 - `CMakeLists.txt`: CMake file.
 - `pico_micro_ros_example.c`: The actual ROS 2 publisher.
 

WiFiCredentials.h

@@ -0,0 +1,5 @@
+// Credntials for connecting to Wifi
+static const char wifiSSID[] = "(WiFi name)";
+static const char wifiPassword[] = "(password)";
+static const char agentIP[] = "(agent IP address)";
+static const int agentPort = 4444;

lwipopts.h

@@ -0,0 +1,89 @@
+#ifndef __LWIPOPTS_H__
+#define __LWIPOPTS_H__
+
+// Common settings used in most of the pico_w examples
+// (see https://www.nongnu.org/lwip/2_1_x/group__lwip__opts.html for details)
+
+// allow override in some examples
+#ifndef NO_SYS
+#define NO_SYS                      1
+#endif
+// allow override in some examples
+#ifndef LWIP_SOCKET
+#define LWIP_SOCKET                 0
+#endif
+#if PICO_CYW43_ARCH_POLL
+#define MEM_LIBC_MALLOC             1
+#else
+// MEM_LIBC_MALLOC is incompatible with non polling versions
+#define MEM_LIBC_MALLOC             0
+#endif
+#define MEM_ALIGNMENT               4
+#define MEM_SIZE                    4000
+#define MEMP_NUM_TCP_SEG            32
+#define MEMP_NUM_ARP_QUEUE          10
+#define PBUF_POOL_SIZE              24
+#define LWIP_ARP                    1
+#define LWIP_ETHERNET               1
+#define LWIP_ICMP                   1
+#define LWIP_RAW                    1
+#define TCP_WND                     (8 * TCP_MSS)
+#define TCP_MSS                     1460
+#define TCP_SND_BUF                 (8 * TCP_MSS)
+#define TCP_SND_QUEUELEN            ((4 * (TCP_SND_BUF) + (TCP_MSS - 1)) / (TCP_MSS))
+#define LWIP_NETIF_STATUS_CALLBACK  1
+#define LWIP_NETIF_LINK_CALLBACK    1
+#define LWIP_NETIF_HOSTNAME         1
+#define LWIP_NETCONN                0
+#define MEM_STATS                   0
+#define SYS_STATS                   0
+#define MEMP_STATS                  0
+#define LINK_STATS                  0
+// #define ETH_PAD_SIZE                2
+#define LWIP_CHKSUM_ALGORITHM       3
+#define LWIP_DHCP                   1
+#define LWIP_IPV4                   1
+#define LWIP_TCP                    1
+#define LWIP_UDP                    1
+#define LWIP_DNS                    1
+#define LWIP_TCP_KEEPALIVE          1
+#define LWIP_NETIF_TX_SINGLE_PBUF   1
+#define DHCP_DOES_ARP_CHECK         0
+#define LWIP_DHCP_DOES_ACD_CHECK    0
+
+#ifndef NDEBUG
+#define LWIP_DEBUG                  1
+#define LWIP_STATS                  1
+#define LWIP_STATS_DISPLAY          1
+#endif
+
+#define ETHARP_DEBUG                LWIP_DBG_OFF
+#define NETIF_DEBUG                 LWIP_DBG_OFF
+#define PBUF_DEBUG                  LWIP_DBG_OFF
+#define API_LIB_DEBUG               LWIP_DBG_OFF
+#define API_MSG_DEBUG               LWIP_DBG_OFF
+#define SOCKETS_DEBUG               LWIP_DBG_OFF
+#define ICMP_DEBUG                  LWIP_DBG_OFF
+#define INET_DEBUG                  LWIP_DBG_OFF
+#define IP_DEBUG                    LWIP_DBG_OFF
+#define IP_REASS_DEBUG              LWIP_DBG_OFF
+#define RAW_DEBUG                   LWIP_DBG_OFF
+#define MEM_DEBUG                   LWIP_DBG_OFF
+#define MEMP_DEBUG                  LWIP_DBG_OFF
+#define SYS_DEBUG                   LWIP_DBG_OFF
+#define TCP_DEBUG                   LWIP_DBG_OFF
+#define TCP_INPUT_DEBUG             LWIP_DBG_OFF
+#define TCP_OUTPUT_DEBUG            LWIP_DBG_OFF
+#define TCP_RTO_DEBUG               LWIP_DBG_OFF
+#define TCP_CWND_DEBUG              LWIP_DBG_OFF
+#define TCP_WND_DEBUG               LWIP_DBG_OFF
+#define TCP_FR_DEBUG                LWIP_DBG_OFF
+#define TCP_QLEN_DEBUG              LWIP_DBG_OFF
+#define TCP_RST_DEBUG               LWIP_DBG_OFF
+#define UDP_DEBUG                   LWIP_DBG_OFF
+#define TCPIP_DEBUG                 LWIP_DBG_OFF
+#define PPP_DEBUG                   LWIP_DBG_OFF
+#define SLIP_DEBUG                  LWIP_DBG_OFF
+#define DHCP_DEBUG                  LWIP_DBG_OFF
+
+#endif /* __LWIPOPTS_H__ */

pico_micro_ros_example.c

@@ -8,9 +8,20 @@
 #include <rmw_microros/rmw_microros.h>
 
 #include "pico/stdlib.h"
+#include "pico/stdio.h"
+#include "pico/cyw43_arch.h"
+#include "pico_wifi_transports.h"
 #include "pico_uart_transports.h"
+#include "WiFiCredentials.h"
 
-const uint LED_PIN = 25;
+#define PICO_BOARD 1 // Set to 0 for pico, 1 for pico_w
+#define USE_WIFI 1 // Set to 0 for Serial, 1 for WIFI (only on pico_w)
+
+#if PICO_BOARD
+    const uint LED_PIN = CYW43_WL_GPIO_LED_PIN;
+#else
+    const uint LED_PIN = 25;
+#endif
 
 rcl_publisher_t publisher;
 std_msgs__msg__Int32 msg;
@@ -23,18 +34,24 @@ void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
 
 int main()
 {
-    rmw_uros_set_custom_transport(
+    #if USE_WIFI
+        set_microros_wifi_transports(wifiSSID, wifiPassword, agentIP, agentPort);
+    #else
+        mw_uros_set_custom_transport(
 		true,
 		NULL,
 		pico_serial_transport_open,
 		pico_serial_transport_close,
 		pico_serial_transport_write,
 		pico_serial_transport_read
-	);
-
-    gpio_init(LED_PIN);
-    gpio_set_dir(LED_PIN, GPIO_OUT);
-
+        );
+    #endif
+
+    #if not PICO_BOARD
+        gpio_init(LED_PIN);
+        gpio_set_dir(LED_PIN, GPIO_OUT);
+    #endif
+
     rcl_timer_t timer;
     rcl_node_t node;
     rcl_allocator_t allocator;
@@ -44,14 +61,14 @@ int main()
     allocator = rcl_get_default_allocator();
 
     // Wait for agent successful ping for 2 minutes.
-    const int timeout_ms = 1000; 
-    const uint8_t attempts = 120;
+    const int timeout_ms = 1000;
+    const uint8_t attempts = 10;
 
     rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);
 
     if (ret != RCL_RET_OK)
     {
-        // Unreachable agent, exiting program.
+        printf("Agent unreachable. Exiting...\n");
         return ret;
     }
 
@@ -72,8 +89,12 @@ int main()
 
     rclc_executor_init(&executor, &support.context, 1, &allocator);
     rclc_executor_add_timer(&executor, &timer);
-
-    gpio_put(LED_PIN, 1);
+
+    #if PICO_BOARD
+        cyw43_arch_gpio_put(LED_PIN, 1);
+    #else
+        gpio_put(LED_PIN, 1);
+    #endif
 
     msg.data = 0;
     while (true)