int main(int argc, char** argv)
{
char *content = 0;
add_subscriber();
get_all_subscribers();
/*get_all_subscribers_for_topic("/weather/4/system.subscriptions");*/
/*content = test_endpoint_serialize();
test_endpoint_deserialize(content);*/
return 0;
}
void ROSGate::run_once(){
copy_fd = master_fd;
if(select(fdmax+1,©_fd,NULL,NULL,NULL) == -1){
//complain...
}
if(FD_ISSET(sock, ©_fd)){
//accept client
clients.emplace_back(sock, &master_fd);
fdmax = fdmax > clients.back().sock? fdmax : clients.back().sock;
}
msg_t buf;
// buf[dtype][topic][(sub|pub),content]
for(auto it = clients.begin(); it != clients.end();){
auto& client = *it;
if(FD_ISSET(client.sock, ©_fd)){
int n = client.recv((char*)&buf);
if(n <= 0){ // closed connection
//remove from subscription list
for(auto& e : sub_map){
//auto& topic = e.first;
auto& sub_cls = e.second; //list of clients to topic
//iterate over subscriber sockets
for(auto sc_it = sub_cls.begin(); sc_it != sub_cls.end(); ++sc_it){
if(*sc_it == &client){ //comparison of pointers : has to equal.
//alternatively, can compare socket values
sub_cls.erase(sc_it);
break;
}
}
}
it = clients.erase(it);
continue;
}else{
std::string topic(buf.topic);
std::cout << "TOPIC : " << topic << std::endl;
if(strncmp((char*)buf.method,"sub",4) == 0){
printf("Subscription request : [%s]\n", topic.c_str());
sub_map[topic].insert(&client); //remember the socket, but by pointer
//create if there is no router for the topic
if(std::find(s_topics.begin(), s_topics.end(), topic) == s_topics.end()){
printf("ADDING NEW SUBSCRIBER\n");
s_topics.push_back(topic);
add_subscriber(buf.t, topic);
}
}else if (strncmp((char*)buf.method,"pub",4) == 0){
printf("Publication request : [%s]\n", topic.c_str());
//create publisher for topic if there's not already one
if(std::find(p_topics.begin(), p_topics.end(), topic) == p_topics.end()){
printf("ADDING NEW PUBLISHER\n");
p_topics.push_back(topic);
add_publisher(buf.t, topic);
}
//then publish the message to ros board
for(auto& publisher : publishers){
std::cout << "PTOPIC : " << publisher.getTopic() << std::endl;
if(publisher.getTopic() == topic || publisher.getTopic() == "/" + topic){
std::cout << "PUBLISHING..." << std::endl;
publish(publisher, buf);
break;
}
}
}
}
}
++it;
}
ros::spinOnce();
}
