Topic created but no data communicating with agent using custom udp transport

[ZaGabriel]

• Hardware description: Linux Host machine
• RTOS: Linux
• Installation type: micro-ros-setup
• Version or commit hash: humble

Steps to reproduce the issue

Hi, I'm new to ros and microros. I'm trying to run microros on an resources constraint custom linux board .
And first testing it on my Linux Host. I've built the static library from the tutorial : https://micro.ros.org/docs/tutorials/advanced/create_custom_static_library/
toolchain.cmake and colcon.meta are below.

Next step I've write my custom udp transport as below.
Then compile the code as an executable file and run it.
And I run the agent on the same host machine from the tutorial :
https://micro.ros.org/docs/tutorials/core/first_application_linux/

Expected behavior

Expected to publish data.

Actual behavior

It has success create the topic ros2 topic list
but ros2 topic echo didn't show anything

Additional information

# toolchain.cmake

set(CMAKE_SYSTEM_NAME Linux)
set(CMAKE_CROSSCOMPILING 0)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)

set(CMAKE_C_COMPILER /usr/bin/gcc)
set(CMAKE_CXX_COMPILER /usr/bin/g++)

set(CMAKE_C_COMPILER_WORKS 1 CACHE INTERNAL "")
set(CMAKE_CXX_COMPILER_WORKS 1 CACHE INTERNAL "")

set(FLAGS "-O2 -ffunction-sections -fdata-sections" CACHE STRING "" FORCE)

set(CMAKE_C_FLAGS_INIT "-std=c11 ${FLAGS}" CACHE STRING "" FORCE)
set(CMAKE_CXX_FLAGS_INIT "-std=c++11 ${FLAGS}" CACHE STRING "" FORCE)

set(__BIG_ENDIAN__ 0)

# colcon.meta

{
    "names": {
        "tracetools": {
            "cmake-args": [
                "-DTRACETOOLS_DISABLED=ON",
                "-DTRACETOOLS_STATUS_CHECKING_TOOL=OFF"
            ]
        },
        "rosidl_typesupport": {
            "cmake-args": [
                "-DROSIDL_TYPESUPPORT_SINGLE_TYPESUPPORT=ON"
            ]
        },
        "rcl": {
            "cmake-args": [
                "-DBUILD_TESTING=OFF",
                "-DRCL_COMMAND_LINE_ENABLED=OFF",
                "-DRCL_LOGGING_ENABLED=OFF"
            ]
        }, 
        "rcutils": {
            "cmake-args": [
                "-DENABLE_TESTING=OFF",
                "-DRCUTILS_NO_FILESYSTEM=ON",
                "-DRCUTILS_NO_THREAD_SUPPORT=ON",
                "-DRCUTILS_NO_64_ATOMIC=ON",
                "-DRCUTILS_AVOID_DYNAMIC_ALLOCATION=ON"
            ]
        },
        "microxrcedds_client": {
            "cmake-args": [
                "-DUCLIENT_PIC=OFF",
                "-DUCLIENT_PROFILE_UDP=OFF",
                "-DUCLIENT_PROFILE_TCP=OFF",
                "-DUCLIENT_PROFILE_DISCOVERY=OFF",
                "-DUCLIENT_PROFILE_SERIAL=OFF",
                "-UCLIENT_PROFILE_STREAM_FRAMING=ON",
                "-DUCLIENT_PROFILE_CUSTOM_TRANSPORT=ON"
            ]
        },
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=5",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=1",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

// custom_transport.h

#include <iostream>
#include <string>
#include <cstring>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <unistd.h>

#include <uxr/client/transport.h>
#include <rmw_microros/rmw_microros.h>

struct custom_transport_args{
    std::string address;
    uint16_t port;
};

bool custom_transport_open(struct uxrCustomTransport *transport);
bool custom_transport_close(struct uxrCustomTransport *transport);
size_t custom_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode);
size_t custom_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode);

static inline void set_custom_udp_transports(std::string agent_ip, uint16_t agent_port){
	static struct custom_transport_args arg;
	arg.address = agent_ip;
	arg.port = agent_port;

	rmw_uros_set_custom_transport(
		false,
		(void *) &arg,
		custom_transport_open,
		custom_transport_close,
		custom_transport_write,
		custom_transport_read
	);
}

// custom_transport.cpp

#include <custom_transport/custom_transport.h>
int sock_fd;
struct sockaddr_in serverAddr;

bool custom_transport_open(struct uxrCustomTransport *transport){
    struct custom_transport_args *args = (struct custom_transport_args *) transport->args;

    if((sock_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) return false;

    memset(&serverAddr, 0, sizeof(serverAddr));

    // Filling server information 
    serverAddr.sin_family = AF_INET; 
    serverAddr.sin_port = htons(args->port); 
    int ret = inet_pton(AF_INET, args->address.c_str(), &serverAddr.sin_addr);

    if(ret <= 0) return false;

    printf("\tOpen...\n");
    return true;
}

bool custom_transport_close(struct uxrCustomTransport *transport){
    close(sock_fd);
    printf("\tClose...\n");
    return true;
}
size_t custom_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode){
    //(void)errcode;
    ssize_t sent = sendto(sock_fd, buf, len, MSG_CONFIRM, (const struct sockaddr *) &serverAddr, sizeof(serverAddr));
    if(sent < 0){
        errcode = (uint8_t *) sent;
    }

    printf("\tWrite...\n");
    return sent;
}
size_t custom_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode){
    //(void)errcode;
    socklen_t sock_len;

    struct timeval timev;
    timev.tv_sec = timeout / 1000;
    timev.tv_usec = (timeout % 1000) * 1000;
    setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, &timev, sizeof(timev));

    ssize_t read = recvfrom(sock_fd, buf, len, MSG_WAITALL, (struct sockaddr *) &serverAddr, &sock_len); 
    if(read < 0){
        errcode = (uint8_t *) read;
        return 0;
    }

    printf("\tRead...\n");
    return read;
}

// main.cpp

#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 <custom_transport/custom_transport.h>

#include <stdio.h>
#include <string>
#include <unistd.h>

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

std::string agent_ip = "127.0.0.1";
uint16_t agent_port = 8080;

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;

void timer_callback(rcl_timer_t * timer, int64_t last_call_time){
	printf("Timer call back...\n");
	(void) last_call_time;
	if (timer != NULL){
		RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
		printf("Sent: %d\n", msg.data);
		msg.data++;
	}
}

int main(){	
	// set custom transport
	set_custom_udp_transports(agent_ip, agent_port);

	rcl_allocator_t allocator = rcl_get_default_allocator();
	rclc_support_t support;

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

	// create node
	rcl_node_t node;
	RCCHECK(rclc_node_init_default(&node, "int32_publisher_rclc", "", &support));

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

	// create timer,
	rcl_timer_t timer;
	const unsigned int timer_timeout = 1000;

	RCCHECK(rclc_timer_init_default(
		&timer,
		&support,
		RCL_MS_TO_NS(timer_timeout),
		timer_callback));

	// create executor
	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
	RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
	RCCHECK(rclc_executor_add_timer(&executor, &timer));

	msg.data = 0;

  	rclc_executor_spin(&executor);

	RCCHECK(rcl_publisher_fini(&publisher, &node));
	RCCHECK(rcl_node_fini(&node));
}