TopicReader::TopicReader(const char* callerID, const char* topic, const char* md5sum, const char* msgType)
{
strcpy(this->topic, topic);
strcpy(this->callerID, callerID);
strcpy(this->md5sum, md5sum);
strcpy(this->msgType, msgType);
qHandle = xQueueCreate(3, RX_QUEUE_MSG_SIZE);
connectionID = 0;
XMLRequest* req = new RegisterRequest("registerSubscriber", MASTER_URI, callerID, topic, msgType);
XMLRPCServer::sendRequest(req->getData(), 11311, connectPublishers, this);
// TODO: make a unique task name
xTaskCreate(task, (const signed char*)topic, 250, (void*)this, tskIDLE_PRIORITY + 2, NULL);
}
void TopicReader::requestTopic(const char* ip, uint16_t serverPort)
{
XMLRequest* req = new TopicRequest("requestTopic", MASTER_URI, callerID, topic, md5sum, msgType);
XMLRPCServer::sendRequest(req->getData(), serverPort, onResponse, this);
}
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");
}
}
