TopicReader* XMLRPCServer::getTopicReader(const char* topic)
{
for (uint16_t i = 0; i < MAX_TOPIC_READERS; i++) {
if (topicReaders[i] != NULL) {
TopicReader* tr = topicReaders[i];
if (!strcmp(tr->getTopic(), topic)) {
return tr;
}
}
}
return NULL;
}
TopicReader* XMLRPCServer::getTopicReader(const uint32_t connectionID)
{
for (uint16_t i = 0; i < MAX_TOPIC_READERS; i++) {
if (topicReaders[i] != NULL) {
TopicReader* tr = topicReaders[i];
if (tr->getConnectionID() == connectionID) {
return tr;
}
}
}
return NULL;
}
void TopicReader::connectPublishers(const void* obj, const char* data)
{
char text[100];
char* pos = strstr((char*)data, "Subscribed to");
if (pos != 0)
{
while(1)
{
char* pos2 = strstr((char*)pos, "<value><string>");
char* pos3 = strstr((char*)pos2, "</string></value>");
if (pos2 == NULL || pos3 == NULL)
break;
if (pos3 > pos2)
{
int offset = strlen("<value><string>");
char uri[pos3-pos2-offset+1];
strncpy (uri, pos2+offset, pos3-pos2-offset);
uri[pos3-pos2-offset] = 0;
uint16_t port;
char ip[32];
XMLRPCServer::extractURI(uri, ip, &port);
os_printf("URI: %s:::%d\n", ip, port);
//os_printf("URI: %s\n", uri);
// Check if this uri already exists in a "PublisherURIs" list.
if (strcmp(ip, "10.3.84.99")) // TODO: replace this with a method to check if ip is not equal self ip
{
TopicReader* self = (TopicReader*) obj;
self->requestTopic(SERVER_IP_ADDRESS, port);
}
}
pos = pos3;
}
}
else
os_printf("pos is NULL\n");
}
void XMLRPCServer::UDPreceive(void* params)
{
struct netconn *conn;
struct netbuf *buf;
err_t err;
// Initialize memory (in stack) for message.
char message[60]; // TODO: Set its size according to UDPMessage.
os_printf("Test!\n");
conn = netconn_new(NETCONN_UDP);
for (;;) {
// Check if connection was created successfully.
if (conn != NULL) {
err = netconn_bind(conn, IP_ADDR_ANY, UDP_RECEIVE_PORT);
// Check if we were able to bind to port.
if (err == ERR_OK) {
portTickType xLastWakeTime;
// Initialize the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
// Start periodic loop.
while (1) {
buf = netconn_recv(conn);
if (buf != NULL) {
struct ip_addr* ip;
uint16_t port;
ip = buf->addr;
port = buf->port;
//if(ip != NULL)
//os_printf("Received from %d:%d!\n", ip->addr, port);
// Copy received data into message.
uint16_t len = netbuf_len(buf);
if (len > 15) {
netbuf_copy (buf, &message, len);
uint32_t connectionID = *((uint32_t*)&message[0]);
TopicReader* tr = getTopicReader(connectionID);
if (tr != NULL) {
tr->enqueueMessage(&message[8]);
//os_printf("ConnectionID: %d, topic:%s\n", connectionID, tr->getTopic());
}
}
// Deallocate previously created memory.
netbuf_delete(buf);
}
// Use delay until to guarantee periodic execution of each loop iteration.
else {
os_printf("buf = NULL!\n");
vTaskDelayUntil(&xLastWakeTime, 30);
}
}
} else {
os_printf("cannot bind netconn\n");
}
} else {
os_printf("cannot create new UDP netconn\n");
conn = netconn_new(NETCONN_UDP);
}
// If connection failed, wait for 50 ms before retrying.
vTaskDelay(50);
}
}
void XMLRPCServer::XMLRPCServerReceiveCallback(const char* data, char* buffer)
{
os_printf("Receive callback, buffer addr: %08x!\n", buffer);
char methodName[48];
{
char* pos = strstr((char*)data, "<methodName>");
char* pos2 = strstr((char*)data, "</methodName>");
if (pos2 > pos) {
strncpy (methodName, pos + 12, pos2 - pos - 12);
methodName[pos2 - pos - 12] = 0;
}
}
os_printf("name:%s\n", methodName);
os_printf("Strlen:%d\n", strlen(data));
if (!strcmp(methodName, "requestTopic")) {
char* pos = strstr(data, "<i4>");
char* pos2 = strstr(data, "</i4>");
os_printf("pos:%d, pos2:%d\n", pos, pos2);
if (pos < pos2) {
char portStr[pos2 - pos - 3];
strncpy (portStr, pos + 4, pos2 - pos - 4);
portStr[pos2 - pos - 4] = 0;
uint16_t port = atoi(portStr);
os_printf("Port: %d\n", port);
char* pos3 = strstr((char*)data, "</value></param><param><value>/");
char* pos4;
int len = strlen("</value></param><param><value>/");
if (pos3) {
pos4 = strstr((char*)pos3 + len, "</value>");
if (pos4 > pos3) {
char topic[pos4 - pos3 - len + 1];
strncpy (topic, pos3 + len, pos4 - pos3 - len);
topic[pos4 - pos3 - len] = 0;
os_printf("topic: %s\n", topic);
// TODO: Move UDPConnection to registerPublishers. Then extract topic name from data. Afterwards, find the corresponding connection.
TopicWriter* tw = getTopicWriter(topic);
if (tw != NULL) {
UDPConnection* connection = tw->getConnection(port);
if (connection != NULL) {
os_printf("Connection ID: %d\n", connection->getID());
XMLRequest* response = new TopicResponse(IP_ADDR, UDP_LOCAL_PORT, connection->getID());
strcpy(buffer, response->getData());
}
}
}
}
}
} else if (!strcmp(methodName, "publisherUpdate")) {
os_printf("Publisher Update!\n");
char* pos = strstr(data, "<value><string>/master</string></value>");
if (pos != 0) {
char topic[MAX_TOPIC_LEN];
while (1) {
char* pos2 = strstr((char*)pos, "<value><string>");
char* pos3 = strstr((char*)pos2, "</string></value>");
//os_printf("_pos:%d, _pos2:%d\n", pos2, pos3);
if (pos2 == NULL || pos3 == NULL)
break;
if (pos3 > pos2) {
int offset = strlen("<value><string>");
char uri[pos3 - pos2 - offset + 1];
strncpy (uri, pos2 + offset, pos3 - pos2 - offset);
uri[pos3 - pos2 - offset] = 0;
if (!strcmp(uri, "/master")) {
} else if (uri[0] == '/') {
strcpy(topic, &uri[1]);
} else {
uint16_t port;
char ip[32];
extractURI(uri, ip, &port);
if (strcmp(ip, IP_ADDR)) {
os_printf("Topic:%s URI: %s:::%s:::%d\n", topic, uri, ip, port);
TopicReader* tr = getTopicReader(topic);
if (tr != NULL) {
if (!strcmp(ip, "SI-Z0M81"))
strcpy(ip, ROS_MASTER_IP);
tr->requestTopic(ip, port);
}
}
}
}
pos = pos3;
}
} else
os_printf("pos is NULL\n");
}
}
void XMLRPCServer::UDPSend(void* params)
{
UDPHandler* uh = UDPHandler::instance();
struct netconn* conn = netconn_new( NETCONN_UDP );
netconn_bind(conn, IP_ADDR_ANY, UDP_LOCAL_PORT);
static uint8_t counter = 1;
pinMode(GPIO_PD11, OUTPUT);
UDPMessage msg;
struct ip_addr ip;
ip.addr = inet_addr(ROS_MASTER_IP);
for (;;) {
//digitalWrite(GPIO_PD11, HIGH);
digitalWrite(GPIO_PD11, pin3);
uh->dequeueMessage(&msg);
//digitalWrite(GPIO_PD11, LOW);
// inter-node communication
TopicReader* tr = getTopicReader(msg.topic);
if (tr != NULL) {
tr->enqueueMessage(msg.data);
}
TopicWriter* tw = getTopicWriter(msg.topic);
if (tw != NULL) {
UDPConnection* const* connections = tw->getConnections();
if (connections != NULL) {
for (int i = 0; i < MAX_UDP_CONNECTIONS; i++) {
const UDPConnection* connection = connections[i];
if (connection && connection->isValid()) {
err_t err = netconn_connect(conn, &ip, connection->getPort());
//os_printf("1Port: %d LWIP Error:%d\n", endpoint.port, err);
os_printf("Connecting %s:%d, err:%d\n", ip, connection->getPort(), err);
struct netbuf *buf = netbuf_new();
char msgHeader[8];
uint32_t connectionID = connection->getID();
memcpy(&msgHeader[0], &connectionID, sizeof(uint32_t));
msgHeader[4] = 0;
msgHeader[5] = counter++;
msgHeader[6] = 0x01;
msgHeader[7] = 0;
uint32_t msgLen = *((uint32_t*) msg.data) + 4;
void* data = netbuf_alloc(buf, msgLen + sizeof(msgHeader)); // Also deallocated with netbuf_delete(buf)
if (data != NULL) {
memcpy (data, msgHeader, sizeof(msgHeader));
memcpy (data + sizeof(msgHeader), msg.data, msgLen);
} else {
os_printf("XMLRPCServer::UDPSend data is NULL!\n");
}
err = netconn_send(conn, buf);
//os_printf("2Port: %d LWIP Error:%d\n", endpoint.port, err);
netbuf_delete(buf);
}
}
}
}
pin3 = !pin3;
}
}
