What about adding UDP transport?

[Hao-Lion-ZJU]

Hello Microros or ROS2 Community,
I recently tried deploying micro-ROS on STM32H743. The project requires me to establish a connection with the Agent using STM32 over the network. Unfortunately, I couldn't find relevant information online, so I attempted to use LWIP to write a custom transport.I imitated the writing style of the custom transport in this repository and referred to the micro-ROS tutorial to write my own udp_transport.c. Surprisingly, it worked successfully. I hope the developers will consider submitting UDP transport as an option.It's worth noting that when using UDP, modifications are required in the content of simple_main.c.

  rmw_uros_set_custom_transport(
    false,  //Framing disable here. Udp should Use Packet-oriented mode.
    "192.168.1.121",  //your agent's ip address
    cubemx_transport_open,
    cubemx_transport_close,
    cubemx_transport_write,
    cubemx_transport_read);

Detail in udp_transport.c:

#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){

    struct sockaddr_in addr;
    socklen_t addr_len = sizeof(addr);
    int ret = 0;
    //set timeout
    struct timeval tv_out;
    tv_out.tv_sec = 0;
    tv_out.tv_usec = timeout * 1000;
    setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO,&tv_out, sizeof(tv_out));
    //Use non-block mode
    ret = recv(sock_fd, buf, len, MSG_DONTWAIT);
    size_t readed = ret>0? ret:0;
    return readed;
}

#endif

[pablogs9]

Hello @Hao-Lion-ZJU PR are welcomed, please if you have a functional transport, contribute it.

[isomadinow]

@Hao-Lion-ZJU
Hi, I'm trying to implement UDP transport on Humble. I have an STM32F767ZI, I've set everything up, but I can't get it to work. If there's a video on this or if there's a way to send an example of a working repository, that would be great because there's little information on how to set it up so that UDP works fully...

[Hao-Lion-ZJU]

@Hao-Lion-ZJU Hi, I'm trying to implement UDP transport on Humble. I have an STM32F767ZI, I've set everything up, but I can't get it to work. If there's a video on this or if there's a way to send an example of a working repository, that would be great because there's little information on how to set it up so that UDP works fully...

Can you tell me more details? And attach a screenshot of your question.

[isomadinow]

@Hao-Lion-ZJU
Sure. Here is the connection diagram, and I have indicated the IP addresses of the devices in the diagram.

Settings on the STM32F767ZI side.

/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void argument)
{
/ init code for LWIP /
MX_LWIP_Init();
/ USER CODE BEGIN StartDefaultTask /
/ Infinite loop */
// micro-ROS configuration

rmw_uros_set_custom_transport(
false, //Framing disable here. Udp should Use Packet-oriented mode.
"172.20.76.39", //your Agent's ip address
cubemx_transport_open,
cubemx_transport_close,
cubemx_transport_write,
cubemx_transport_read);

oktransport = cubemx_transport_open;

rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
freeRTOS_allocator.allocate = microros_allocate;
freeRTOS_allocator.deallocate = microros_deallocate;
freeRTOS_allocator.reallocate = microros_reallocate;
freeRTOS_allocator.zero_allocate = microros_zero_allocate;

if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
printf("Error on default allocators (line %d)\n", LINE);
}

rcl_publisher_t publisher;

rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

allocator = rcl_get_default_allocator();

//create init_options
rclc_support_init(&support, 0, NULL, &allocator);

// create node
rclc_node_init_default(&node, "cubemx_node", "", &support);

// create publisher
rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"cubemx_publisher");

msg.data = 0;

for(;;)
{
rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
if (ret != RCL_RET_OK)
{
printf("Error publishing (line %d)\n", LINE);
}

msg.data++;
osDelay(10);

}
/* USER CODE END StartDefaultTask */
}

And I set the flags for tracking, rclc_support_init(&support, 0, NULL, &allocator); does not initialize.

[Hao-Lion-ZJU]

@isomadinow I roughly understand now. Firstly, please ensure that your Jetson/PC can ping your STM32. One issue I encountered while using the H7 series is that the MPU must be correctly enable.

By the way, why is the gateway address empty in your third screenshot?

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

You can set up an demo of UDP communication to see if the communication is working properly.

[isomadinow]

Hello again. @Hao-Lion-ZJU
The gateway address is empty because internet access is not needed, as I understand it, or should the gateway address be specified? If yes, should the router address be specified, or what? I just don't understand much about this.

[Hao-Lion-ZJU]

@isomadinow
Based on the situation in your first screenshot, your gateway address should be filled with the address of your router. Firstly, please ensure that your Jetson/PC can ping your STM32.

[isomadinow]

@Hao-Lion-ZJU
Without FreeRTOS, ping works, but when I connected FreeRTOS, the micro-agent worked once, but after attempts and changes in the microcontroller's memory sizes, and even reverting to the original, nothing worked. Maybe there's some other stack that needs to be changed?

MCU

[Hao-Lion-ZJU]

@isomadinow It seems that need you debug step by step.

[zijian-x]

I never had to work with lwIP nor did I study how it works under the hood, but the current udp_transport impl doesn't work for me properly as in, it somehow blocks the program flow in order to receive - I'd guess it's because cubemx_transport_read calls recv in blocking mode until timeout.

the udp transport implementation from the zephyr module instead works flawlessly. I assume it polls for the incoming data without blocking. Would this implementation also work for stm32 in general and can be merged?