diff --git a/platform_code/wiringpi/socket/micro_ros_transport.cpp b/platform_code/wiringpi/socket/micro_ros_transport.cpp new file mode 100644 index 00000000..cf10344d --- /dev/null +++ b/platform_code/wiringpi/socket/micro_ros_transport.cpp @@ -0,0 +1,84 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +extern "C" { + +static int fd; + +bool platformio_transport_open(struct uxrCustomTransport * transport) +{ + const auto * locator = (const struct micro_ros_agent_locator *) transport->args; + + struct addrinfo hints; + + memset(&hints, 0, sizeof(hints)); + + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = AI_NUMERICSERV; + hints.ai_protocol = 0; + + const auto port_s = std::to_string(locator->port); + struct addrinfo * p_addrs = nullptr; + + if(getaddrinfo(locator->address, port_s.c_str(), &hints, &p_addrs) != 0) + return false; + + struct addrinfo * p_addr; + + for(p_addr = p_addrs; p_addr != nullptr; p_addr = p_addr->ai_next){ + if(0 > (fd = socket(p_addr->ai_family, p_addr->ai_socktype, p_addr->ai_protocol))) + continue; + + if(0 == connect(fd, p_addr->ai_addr, p_addr->ai_addrlen)) + break; + + close(fd); + } + + freeaddrinfo(p_addrs); + + return p_addr != nullptr; +} + +bool platformio_transport_close(struct uxrCustomTransport * transport) +{ + close(fd); + return true; +} + +size_t platformio_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *) +{ + auto ret = send(fd, buf, len, 0); + return ret < 0 ? 0 : (size_t)ret; +} + +size_t platformio_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *) +{ + struct pollfd pfd; + int ret; + + pfd.fd = fd; + pfd.events = POLLIN; + ret = poll(&pfd, 1, timeout); + switch(ret){ + case -1: + case 0: + return 0; + default: + ret = recv(fd, buf, len, 0); + return ret < 0 ? 0 : (size_t)ret; + } +} + +} diff --git a/platform_code/wiringpi/socket/micro_ros_transport.h b/platform_code/wiringpi/socket/micro_ros_transport.h new file mode 100644 index 00000000..ab3138f6 --- /dev/null +++ b/platform_code/wiringpi/socket/micro_ros_transport.h @@ -0,0 +1,21 @@ +#include + +struct micro_ros_agent_locator { + char * address; + int port; +}; + +static inline void set_microros_socket_transports(const char * agent_address, uint16_t agent_port){ + static struct micro_ros_agent_locator locator; + locator.address = strdup(agent_address); + locator.port = agent_port; + + rmw_uros_set_custom_transport( + false, + (void *) &locator, + platformio_transport_open, + platformio_transport_close, + platformio_transport_write, + platformio_transport_read + ); +}