137 lines
4.4 KiB
C++
137 lines
4.4 KiB
C++
#include "PosixParticipant.h"
|
|
|
|
#if defined(__unix__) || defined(__APPLE__)
|
|
#include <arpa/inet.h>
|
|
#include <fcntl.h> // For fcntl
|
|
#include <netinet/in.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
#endif
|
|
|
|
namespace RoboidControl {
|
|
namespace Posix {
|
|
|
|
void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
|
|
#if defined(__unix__) || defined(__APPLE__)
|
|
|
|
// Create a UDP socket
|
|
|
|
this->sock = socket(AF_INET, SOCK_DGRAM, 0);
|
|
|
|
if (this->sock < 0) {
|
|
std::cerr << "Error creating socket" << std::endl;
|
|
return;
|
|
}
|
|
|
|
// Set the socket to non-blocking mode
|
|
#if defined(_WIN32) || defined(_WIN64)
|
|
u_long mode = 1; // 1 to enable non-blocking socket
|
|
ioctlsocket(this->sock, FIONBIO, &mode);
|
|
#elif defined(__unix__) || defined(__APPLE__)
|
|
int flags = fcntl(this->sock, F_GETFL, 0);
|
|
fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
|
|
#endif
|
|
|
|
if (remotePort != 0) {
|
|
// Set up the address to send to
|
|
memset(&remote_addr, 0, sizeof(remote_addr));
|
|
remote_addr.sin_family = AF_INET;
|
|
remote_addr.sin_port = htons(remotePort);
|
|
if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) {
|
|
std::cerr << "Invalid address" << std::endl;
|
|
close(sock);
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Set up the receiving address
|
|
memset(&server_addr, 0, sizeof(server_addr));
|
|
server_addr.sin_family = AF_INET;
|
|
server_addr.sin_port = htons(localPort);
|
|
if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) {
|
|
std::cerr << "Invalid address" << std::endl;
|
|
close(sock);
|
|
return;
|
|
}
|
|
|
|
// Bind the socket to the specified port
|
|
if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
|
|
std::cerr << "Bind failed" << std::endl;
|
|
close(sock);
|
|
}
|
|
|
|
#endif
|
|
}
|
|
|
|
void LocalParticipant::Receive() {
|
|
#if defined(__unix__) || defined(__APPLE__)
|
|
sockaddr_in client_addr;
|
|
socklen_t len = sizeof(client_addr);
|
|
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
|
|
if (packetSize > 0) {
|
|
char sender_ipAddress[INET_ADDRSTRLEN];
|
|
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
|
|
unsigned int sender_port = ntohs(client_addr.sin_port);
|
|
|
|
ReceiveData(packetSize, sender_ipAddress, sender_port);
|
|
// RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
|
|
// if (remoteParticipant == nullptr) {
|
|
// remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
|
|
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
|
|
// // << "\n";
|
|
// // std::cout << "New remote participant " << remoteParticipant->ipAddress
|
|
// // << ":" << remoteParticipant->port << " "
|
|
// // << (int)remoteParticipant->networkId << "\n";
|
|
// }
|
|
|
|
// ReceiveData(packetSize, remoteParticipant);
|
|
// std::cout << "Received data\n";
|
|
}
|
|
#endif
|
|
}
|
|
|
|
bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
|
|
#if defined(__unix__) || defined(__APPLE__)
|
|
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
|
|
// << "\n";
|
|
|
|
// Set up the destination address
|
|
struct sockaddr_in dest_addr;
|
|
memset(&dest_addr, 0, sizeof(dest_addr));
|
|
dest_addr.sin_family = AF_INET;
|
|
dest_addr.sin_port = htons(remoteParticipant->port);
|
|
dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
|
|
|
|
// Send the message
|
|
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr));
|
|
if (sent_bytes < 0) {
|
|
std::cerr << "sendto failed with error: " << sent_bytes << " " << strerror(errno) << std::endl;
|
|
close(sock);
|
|
return false;
|
|
}
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
bool LocalParticipant::Publish(IMessage* msg) {
|
|
#if defined(__unix__) || defined(__APPLE__)
|
|
int bufferSize = msg->Serialize(this->buffer);
|
|
if (bufferSize <= 0)
|
|
return true;
|
|
|
|
char ip_str[INET_ADDRSTRLEN];
|
|
inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
|
|
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
|
|
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
|
|
if (sent_bytes < 0) {
|
|
std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
|
|
close(sock);
|
|
return false;
|
|
}
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
} // namespace Posix
|
|
} // namespace RoboidControl
|