Add UDP transport (#134) (#135)

* Add UDP transport Signed-off-by: Lion Hao <[email protected]> * Apply suggestions from code review Co-authored-by: Pablo Garrido <[email protected]> * Update README and modify recv flag * Update udp_transport.c * Apply suggestions from code review Co-authored-by: Pablo Garrido <[email protected]> * Update udp_transport.c * Minor fixes on readme Signed-off-by: Pablo Garrido <[email protected]> --------- Signed-off-by: Lion Hao <[email protected]> Signed-off-by: Pablo Garrido <[email protected]> Co-authored-by: Pablo Garrido <[email protected]> (cherry picked from commit 0dbe8b6) Co-authored-by: Lion Hao <[email protected]>
micro-ROS · Dec 22, 2023 · 513d43c · 513d43c
1 parent bc19d66
commit 513d43c
Show file tree

Hide file tree

Showing 3 changed files with 453 additions and 1 deletion.
diff --git a/README.md b/README.md
@@ -9,11 +9,12 @@ This tool aims to ease the micro-ROS integration in a STM32CubeMX/IDE project.
   - [Middlewares available](#middlewares-available)
   - [Using this package with STM32CubeMX](#using-this-package-with-stm32cubemx)
   - [Using this package with STM32CubeIDE](#using-this-package-with-stm32cubeide)
-      - [Windows 11 (Community Contributed)](#STM32CubeIDE-Win11)
+    - [Windows 11 (Community Contributed)](#windows-11-community-contributed)
   - [Transport configuration](#transport-configuration)
     - [U(S)ART with DMA](#usart-with-dma)
     - [U(S)ART with Interrupts](#usart-with-interrupts)
     - [USB CDC](#usb-cdc)
+    - [UDP](#udp)
   - [Customizing the micro-ROS library](#customizing-the-micro-ros-library)
   - [Adding custom packages](#adding-custom-packages)
   - [Purpose of the Project](#purpose-of-the-project)
@@ -134,6 +135,31 @@ Steps to configure:
 
       **Note: The micro-ROS transport will override the autogenerated `USB_DEVICE/App/usbd_cdc_if.c` methods.**
 
+### UDP
+
+Steps to configure:
+   - Enable Ethernet in your STM32CubeMX/IDE `Connectivity` tab.
+   - Enable LwIP in your STM32CubeMX/IDE `Middleware` tab.
+   - Make sure that LwIP has the following configuration:
+
+      ```
+      Platform Setting according to your own board
+      LwIP -> General Settings -> LWIP_DHCP -> Disabled
+      LwIP -> General Settings -> IP Address Settings (Set here the board address and mask)
+      LwIP -> General Settings -> LWIP UDP -> Enabled
+      LwIP -> General Settings -> Procols Options -> MEMP_NUM_UDP_PCB -> 15
+      LwIP -> Key Options -> LWIP_SO_RCVTIMEO -> Enable
+      ```
+
+   **Note: Ensure your board and Agent are within the same LAN. The default port is 8888. You can modify it in `udp_transport.c`.If you are using a board from the STM32H7 series, please set up the MPU correctly.**
+
+   - Use `sample_main_udp.c` as a reference for writing your application code.
+   - Start the micro-ROS Agent with the following arguments:
+
+        ```
+        ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v 6
+        ```
+
 ## Customizing the micro-ROS library
 
 All the micro-ROS configuration can be done in `colcon.meta` file before step 3. You can find detailed information about how to tune the static memory usage of the library in the [Middleware Configuration tutorial](https://micro.ros.org/docs/tutorials/advanced/microxrcedds_rmw_configuration/).

diff --git a/extra_sources/microros_transports/udp_transport.c b/extra_sources/microros_transports/udp_transport.c
@@ -0,0 +1,79 @@
+#include <uxr/client/transport.h>
+
+#include <rmw_microxrcedds_c/config.h>
+
+#include "main.h"
+#include "cmsis_os.h"
+
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+
+// --- LWIP ---
+#include "lwip/opt.h"
+#include "lwip/sys.h"
+#include "lwip/api.h"
+#include <lwip/sockets.h>
+
+#ifdef RMW_UXRCE_TRANSPORT_CUSTOM
+
+// --- micro-ROS Transports ---
+#define UDP_PORT        8888
+static int sock_fd = -1;
+
+bool cubemx_transport_open(struct uxrCustomTransport * transport){
+    sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
+    struct sockaddr_in addr;
+    addr.sin_family = AF_INET;
+    addr.sin_port = htons(UDP_PORT);
+    addr.sin_addr.s_addr = htonl(INADDR_ANY);
+
+    if (bind(sock_fd, (struct sockaddr *)&addr, sizeof(addr)) == -1)
+    {
+        return false;
+    }
+
+    return true;
+}
+
+bool cubemx_transport_close(struct uxrCustomTransport * transport){
+    if (sock_fd != -1)
+    {
+        closesocket(sock_fd);
+        sock_fd = -1;
+    }
+    return true;
+}
+
+size_t cubemx_transport_write(struct uxrCustomTransport* transport, uint8_t * buf, size_t len, uint8_t * err){
+    if (sock_fd == -1)
+    {
+        return 0;
+    }
+    const char * ip_addr = (const char*) transport->args;
+    struct sockaddr_in addr;
+    addr.sin_family = AF_INET;
+    addr.sin_port = htons(UDP_PORT);
+    addr.sin_addr.s_addr = inet_addr(ip_addr);
+    int ret = 0;
+    ret = sendto(sock_fd, buf, len, 0, (struct sockaddr *)&addr, sizeof(addr));
+    size_t writed = ret>0? ret:0;
+
+    return writed;
+}
+
+size_t cubemx_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){
+
+    int ret = 0;
+    //set timeout
+    struct timeval tv_out;
+    tv_out.tv_sec = timeout / 1000;
+    tv_out.tv_usec = (timeout % 1000) * 1000;
+    setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO,&tv_out, sizeof(tv_out));
+    ret = recv(sock_fd, buf, len, MSG_WAITALL);
+    size_t readed = ret > 0 ? ret : 0;
+    return readed;
+}
+
+#endif