From 0aeb99c2b5d87a01857570a6fa6b485f2eb3fd72 Mon Sep 17 00:00:00 2001 From: Bradley Harris <47424953+BradleyHarris19@users.noreply.github.com> Date: Sun, 16 Jun 2024 23:55:41 +0100 Subject: [PATCH 1/2] WiFi transport support Adding support for the Pico W and options for wifi transport --- .gitignore | 3 +- CMakeLists.txt | 15 +++++- OLD_pico_micro_ros_example.c | 84 +++++++++++++++++++++++++++++ README.md | 14 ++++- WiFiCredentials.h | 5 ++ lwipopts.h | 89 +++++++++++++++++++++++++++++++ pico_micro_ros_example.c | 45 +++++++++++----- pico_uart_transport.c | 0 pico_uart_transports.h | 0 pico_wifi_transport.c | 100 +++++++++++++++++++++++++++++++++++ pico_wifi_transports.h | 85 +++++++++++++++++++++++++++++ 11 files changed, 423 insertions(+), 17 deletions(-) create mode 100644 OLD_pico_micro_ros_example.c create mode 100644 WiFiCredentials.h create mode 100644 lwipopts.h mode change 100755 => 100644 pico_uart_transport.c mode change 100755 => 100644 pico_uart_transports.h create mode 100644 pico_wifi_transport.c create mode 100644 pico_wifi_transports.h diff --git a/.gitignore b/.gitignore index c58dbb0c..8a6a3a5c 100755 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /build -pico-sdk \ No newline at end of file +pico-sdk +Filelists.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 54c545e3..b603615c 100755 --- a/CMakeLists.txt +++ b/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") diff --git a/OLD_pico_micro_ros_example.c b/OLD_pico_micro_ros_example.c new file mode 100644 index 00000000..4eb0e422 --- /dev/null +++ b/OLD_pico_micro_ros_example.c @@ -0,0 +1,84 @@ +#include + +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/README.md b/README.md index daef92ff..c17dc952 100755 --- a/README.md +++ b/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. diff --git a/WiFiCredentials.h b/WiFiCredentials.h new file mode 100644 index 00000000..fbf6dfb7 --- /dev/null +++ b/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; \ No newline at end of file diff --git a/lwipopts.h b/lwipopts.h new file mode 100644 index 00000000..ff2460ca --- /dev/null +++ b/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__ */ \ No newline at end of file diff --git a/pico_micro_ros_example.c b/pico_micro_ros_example.c index 4eb0e422..86b63da6 100755 --- a/pico_micro_ros_example.c +++ b/pico_micro_ros_example.c @@ -8,9 +8,20 @@ #include #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) diff --git a/pico_uart_transport.c b/pico_uart_transport.c old mode 100755 new mode 100644 diff --git a/pico_uart_transports.h b/pico_uart_transports.h old mode 100755 new mode 100644 diff --git a/pico_wifi_transport.c b/pico_wifi_transport.c new file mode 100644 index 00000000..c57ce3d2 --- /dev/null +++ b/pico_wifi_transport.c @@ -0,0 +1,100 @@ +#include +#include +#include + +#include "pico/stdlib.h" +#include "pico/cyw43_arch.h" + +#include "lwip/pbuf.h" +#include "lwip/udp.h" + +#include + +struct micro_ros_agent_locator +{ + ip_addr_t address; + int port; +}; + +struct transport_buffer +{ + uint8_t *buf; + bool packet_received; +}; + +void usleep(uint64_t us) +{ + sleep_us(us); +} + +int clock_gettime(clockid_t unused, struct timespec *tp) +{ + uint64_t m = time_us_64(); + tp->tv_sec = m / 1000000; + tp->tv_nsec = (m % 1000000) * 1000; + return 0; +} + +static struct udp_pcb *pcb; + +bool pico_wifi_transport_open(struct uxrCustomTransport *transport) +{ + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + + pcb = udp_new(); + err_t bind_error = udp_bind(pcb, IP_ADDR_ANY, locator->port); + bind_error = udp_connect(pcb, &locator->address, locator->port); + + return bind_error == ERR_OK; +} + +bool pico_wifi_transport_close(struct uxrCustomTransport *transport) +{ + udp_disconnect(pcb); + return true; +} + +size_t pico_wifi_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode) +{ + + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM); + + memcpy(p->payload, buf, p->len); + err_t er = udp_send(pcb, p); + pbuf_free(p); + + if (er != ERR_OK) + { + printf("Writing error"); + return 0; + } + + return len; +} + +static void udp_recv_callback(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + struct transport_buffer *transport_buffer = (struct transport_buffer *)arg; + memcpy(transport_buffer->buf, p->payload, p->len); + pbuf_free(p); + transport_buffer->packet_received = true; +} + +size_t pico_wifi_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) +{ + (void)errcode; + struct transport_buffer transport_buffer = {buf, false}; + + uint64_t start_time_us = time_us_64(); + int64_t elapsed_time_us = timeout * 1000 - (time_us_64() - start_time_us); + + while (!transport_buffer.packet_received && elapsed_time_us > 0) + { + udp_recv(pcb, udp_recv_callback, &transport_buffer); + sleep_ms(1); + elapsed_time_us = timeout * 1000 - (time_us_64() - start_time_us); + } + + return (elapsed_time_us < 0) ? 0 : len; +} \ No newline at end of file diff --git a/pico_wifi_transports.h b/pico_wifi_transports.h new file mode 100644 index 00000000..938c435a --- /dev/null +++ b/pico_wifi_transports.h @@ -0,0 +1,85 @@ +#ifndef MICRO_ROS_PICOSDK +#define MICRO_ROS_PICOSDK + +#include +#include +#include +#include + +#include "pico/stdlib.h" +#include "pico/cyw43_arch.h" + +#include + +bool pico_wifi_transport_open(struct uxrCustomTransport *transport); +bool pico_wifi_transport_close(struct uxrCustomTransport *transport); +size_t pico_wifi_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *err); +size_t pico_wifi_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *err); + +struct micro_ros_agent_locator +{ + ip_addr_t address; + int port; +}; + +int scan_result(void *env, const cyw43_ev_scan_result_t *result) { + if (result) { + printf("ssid: %-32s rssi: %4d chan: %3d mac: %02x:%02x:%02x:%02x:%02x:%02x sec: %u\n", + result->ssid, result->rssi, result->channel, + result->bssid[0], result->bssid[1], result->bssid[2], result->bssid[3], result->bssid[4], result->bssid[5], + result->auth_mode); + } + return 0; + } + +static inline bool set_microros_wifi_transports(char *ssid, char *pass, char *agent_ip, uint agent_port) +{ + stdio_init_all(); + stdio_usb_init(); + + while(1) + { + if (cyw43_arch_init()) + { + printf("failed to initialise\n"); + return 1; + } + sleep_ms(100); + printf("Enabling Wifi\n"); + cyw43_arch_enable_sta_mode(); + printf("Higher power mode\n"); + cyw43_wifi_pm(&cyw43_state, CYW43_PERFORMANCE_PM); + + printf("Connecting to Wi-Fi...\n"); + printf("SSID: %s, Password: %s, Agent IP: %s, Agent Port: %u\n", ssid, pass, agent_ip, agent_port); + + int ret = cyw43_arch_wifi_connect_timeout_ms(ssid, pass, CYW43_AUTH_WPA2_AES_PSK, 12000); + if (ret) + { + printf("failed to connect with error code: %d\n", ret); + cyw43_arch_deinit(); + } + else + { + printf("Connected.\n"); + break; + } + } + printf("Connected.\n"); + + static struct micro_ros_agent_locator locator; + ipaddr_aton(agent_ip, &locator.address); + locator.port = agent_port; + + rmw_uros_set_custom_transport( + false, + (void *)&locator, + pico_wifi_transport_open, + pico_wifi_transport_close, + pico_wifi_transport_write, + pico_wifi_transport_read); + + return 0; +} + +#endif // MICRO_ROS_PICOSDK \ No newline at end of file From f68c1cf5bfb53cd0f9754f0c837ccdb887ccb677 Mon Sep 17 00:00:00 2001 From: Bradley Harris <47424953+BradleyHarris19@users.noreply.github.com> Date: Mon, 15 Jul 2024 17:09:27 +0100 Subject: [PATCH 2/2] Update CMakeLists.txt fix comment --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b603615c..5537104d 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.12) -// stop the compiler tests from running +# stop the compiler tests from running set(CMAKE_C_COMPILER_WORKS TRUE) set(CMAKE_CXX_COMPILER_WORKS TRUE)