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; } }