/* * Copyright 2011 Nate Koenig * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **/#include <google/protobuf/descriptor.h>#include "transport/IOManager.hh"#include "Master.hh"#include "gazebo_config.h"usingnamespacegazebo;/////////////////////////////////////////////////Master::Master():connection(newtransport::Connection()){this->stop=false;this->runThread=NULL;this->connectionMutex=newboost::recursive_mutex();this->msgsMutex=newboost::recursive_mutex();}/////////////////////////////////////////////////Master::~Master(){this->Fini();deletethis->connectionMutex;this->connectionMutex=NULL;deletethis->msgsMutex;this->msgsMutex=NULL;deletethis->runThread;this->runThread=NULL;this->publishers.clear();this->subscribers.clear();this->connections.clear();this->connection->Shutdown();deletethis->connection;this->connection=NULL;}/////////////////////////////////////////////////voidMaster::Init(uint16_t_port){try{this->connection->Listen(_port,boost::bind(&Master::OnAccept,this,_1));}catch(std::exception&_e){gzthrow("Unable to start server["<<_e.what()<<"]\n");}}//////////////////////////////////////////////////voidMaster::OnAccept(consttransport::ConnectionPtr&new_connection){// Send the gazebo version stringmsgs::GzStringversionMsg;versionMsg.set_data(std::string("gazebo ")+GAZEBO_VERSION);new_connection->EnqueueMsg(msgs::Package("version_init",versionMsg),true);// Send all the current topic namespacesmsgs::GzString_VnamespacesMsg;std::list<std::string>::iteratoriter;for(iter=this->worldNames.begin();iter!=this->worldNames.end();++iter){namespacesMsg.add_data(*iter);}new_connection->EnqueueMsg(msgs::Package("topic_namepaces_init",namespacesMsg),true);// Send all the publishersmsgs::PublisherspublishersMsg;PubList::iteratorpubiter;for(pubiter=this->publishers.begin();pubiter!=this->publishers.end();++pubiter){msgs::Publish*pub=publishersMsg.add_publisher();pub->CopyFrom(pubiter->first);}new_connection->EnqueueMsg(msgs::Package("publishers_init",publishersMsg),true);// Add the connection to our listthis->connectionMutex->lock();intindex=this->connections.size();this->connections[index]=new_connection;// Start reading from the connectionnew_connection->AsyncRead(boost::bind(&Master::OnRead,this,index,_1));this->connectionMutex->unlock();}//////////////////////////////////////////////////voidMaster::OnRead(constunsignedint_connectionIndex,conststd::string&_data){if(this->stop)return;if(!this->connections[_connectionIndex]||!this->connections[_connectionIndex]->IsOpen())return;// Get the connectiontransport::ConnectionPtrconn=this->connections[_connectionIndex];// Read the next messageif(conn&&conn->IsOpen())conn->AsyncRead(boost::bind(&Master::OnRead,this,_connectionIndex,_1));// Store the message if it's not emptyif(!_data.empty()){this->msgsMutex->lock();this->msgs.push_back(std::make_pair(_connectionIndex,_data));this->msgsMutex->unlock();}else{gzerr<<"Master got empty data message from["<<conn->GetRemotePort()<<"]\n";}}//////////////////////////////////////////////////voidMaster::ProcessMessage(constunsignedint_connectionIndex,conststd::string&_data){if(!this->connections[_connectionIndex]||!this->connections[_connectionIndex]->IsOpen())return;transport::ConnectionPtrconn=this->connections[_connectionIndex];msgs::Packetpacket;packet.ParseFromString(_data);if(packet.type()=="register_topic_namespace"){msgs::GzStringworldNameMsg;worldNameMsg.ParseFromString(packet.serialized_data());std::list<std::string>::iteratoriter;iter=std::find(this->worldNames.begin(),this->worldNames.end(),worldNameMsg.data());if(iter==this->worldNames.end()){this->worldNames.push_back(worldNameMsg.data());this->connectionMutex->lock();Connection_M::iteratoriter2;for(iter2=this->connections.begin();iter2!=this->connections.end();++iter2){iter2->second->EnqueueMsg(msgs::Package("topic_namespace_add",worldNameMsg));}this->connectionMutex->unlock();}}elseif(packet.type()=="advertise"){msgs::Publishpub;pub.ParseFromString(packet.serialized_data());this->connectionMutex->lock();Connection_M::iteratoriter2;for(iter2=this->connections.begin();iter2!=this->connections.end();++iter2){iter2->second->EnqueueMsg(msgs::Package("publisher_add",pub));}this->connectionMutex->unlock();this->publishers.push_back(std::make_pair(pub,conn));SubList::iteratoriter;// Find all subscribers of the topicfor(iter=this->subscribers.begin();iter!=this->subscribers.end();++iter){if(iter->first.topic()==pub.topic()){iter->second->EnqueueMsg(msgs::Package("publisher_update",pub));}}}elseif(packet.type()=="unadvertise"){msgs::Publishpub;pub.ParseFromString(packet.serialized_data());this->RemovePublisher(pub);}elseif(packet.type()=="unsubscribe"){msgs::Subscribesub;sub.ParseFromString(packet.serialized_data());this->RemoveSubscriber(sub);}elseif(packet.type()=="subscribe"){msgs::Subscribesub;sub.ParseFromString(packet.serialized_data());this->subscribers.push_back(std::make_pair(sub,conn));PubList::iteratoriter;// Find all publishers of the topicfor(iter=this->publishers.begin();iter!=this->publishers.end();++iter){if(iter->first.topic()==sub.topic()){conn->EnqueueMsg(msgs::Package("publisher_update",iter->first));}}}elseif(packet.type()=="request"){msgs::Requestreq;req.ParseFromString(packet.serialized_data());if(req.request()=="get_publishers"){msgs::Publishersmsg;PubList::iteratoriter;for(iter=this->publishers.begin();iter!=this->publishers.end();++iter){msgs::Publish*pub=msg.add_publisher();pub->CopyFrom(iter->first);}conn->EnqueueMsg(msgs::Package("publisher_list",msg),true);}elseif(req.request()=="topic_info"){msgs::Publishpub=this->GetPublisher(req.data());msgs::TopicInfoti;ti.set_msg_type(pub.msg_type());PubList::iteratorpiter;SubList::iteratorsiter;// Find all publishers of the topicfor(piter=this->publishers.begin();piter!=this->publishers.end();++piter){if(piter->first.topic()==req.data()){msgs::Publish*pubPtr=ti.add_publisher();pubPtr->CopyFrom(piter->first);}}// Find all subscribers of the topicfor(siter=this->subscribers.begin();siter!=this->subscribers.end();++siter){if(siter->first.topic()==req.data()){msgs::Subscribe*sub=ti.add_subscriber();sub->CopyFrom(siter->first);}}conn->EnqueueMsg(msgs::Package("topic_info_response",ti));}elseif(req.request()=="get_topic_namespaces"){msgs::GzString_Vmsg;std::list<std::string>::iteratoriter;for(iter=this->worldNames.begin();iter!=this->worldNames.end();++iter){msg.add_data(*iter);}conn->EnqueueMsg(msgs::Package("get_topic_namespaces_response",msg));}else{gzerr<<"Unknown request["<<req.request()<<"]\n";}}elsestd::cerr<<"Master Unknown message type["<<packet.type()<<"] From["<<conn->GetRemotePort()<<"]\n";}//////////////////////////////////////////////////voidMaster::Run(){while(!this->stop){this->RunOnce();common::Time::MSleep(10);}}//////////////////////////////////////////////////voidMaster::RunThread(){this->runThread=newboost::thread(boost::bind(&Master::Run,this));}//////////////////////////////////////////////////voidMaster::RunOnce(){Connection_M::iteratoriter;// Process the incoming message queuethis->msgsMutex->lock();while(this->msgs.size()>0){this->ProcessMessage(this->msgs.front().first,this->msgs.front().second);this->msgs.pop_front();}this->msgsMutex->unlock();// Process all the connectionsthis->connectionMutex->lock();for(iter=this->connections.begin();iter!=this->connections.end();){if(iter->second->IsOpen()){iter->second->ProcessWriteQueue();++iter;}else{this->RemoveConnection(iter->first);++iter;}}this->connectionMutex->unlock();}/////////////////////////////////////////////////voidMaster::RemoveConnection(unsignedint_index){std::list<std::pair<unsignedint,std::string>>::iteratormsgIter;Connection_M::iteratorconnIter;connIter=this->connections.find(_index);if(connIter==this->connections.end()||!connIter->second)return;// Remove all messages for this connectionthis->msgsMutex->lock();msgIter=this->msgs.begin();while(msgIter!=this->msgs.end()){if((*msgIter).first==_index)this->msgs.erase(msgIter++);else++msgIter;}this->msgsMutex->unlock();// Remove all publishers for this connectionbooldone=false;while(!done){done=true;PubList::iteratorpubIter=this->publishers.begin();while(pubIter!=this->publishers.end()){if((*pubIter).second->id==connIter->second->id){this->RemovePublisher((*pubIter).first);done=false;break;}else++pubIter;}}done=false;while(!done){done=true;// Remove all subscribers for this connectionSubList::iteratorsubIter=this->subscribers.begin();while(subIter!=this->subscribers.end()){if((*subIter).second->id==connIter->second->id){this->RemoveSubscriber((*subIter).first);done=false;break;}else++subIter;}}this->connections.erase(connIter);}/////////////////////////////////////////////////voidMaster::RemovePublisher(constmsgs::Publish_pub){this->connectionMutex->lock();Connection_M::iteratoriter2;for(iter2=this->connections.begin();iter2!=this->connections.end();++iter2){iter2->second->EnqueueMsg(msgs::Package("publisher_del",_pub));}this->connectionMutex->unlock();SubList::iteratoriter;// Find all subscribers of the topicfor(iter=this->subscribers.begin();iter!=this->subscribers.end();++iter){if(iter->first.topic()==_pub.topic()){iter->second->EnqueueMsg(msgs::Package("unadvertise",_pub));}}PubList::iteratorpubIter=this->publishers.begin();while(pubIter!=this->publishers.end()){if(pubIter->first.topic()==_pub.topic()&&pubIter->first.host()==_pub.host()&&pubIter->first.port()==_pub.port()){pubIter=this->publishers.erase(pubIter);}else++pubIter;}}/////////////////////////////////////////////////voidMaster::RemoveSubscriber(constmsgs::Subscribe_sub){// Find all publishers of the topic, and remove the subscriptionsfor(PubList::iteratoriter=this->publishers.begin();iter!=this->publishers.end();++iter){if(iter->first.topic()==_sub.topic()){iter->second->EnqueueMsg(msgs::Package("unsubscribe",_sub));}}// Remove the subscribers from our listSubList::iteratorsubiter=this->subscribers.begin();while(subiter!=this->subscribers.end()){if(subiter->first.topic()==_sub.topic()&&subiter->first.host()==_sub.host()&&subiter->first.port()==_sub.port()){subiter=this->subscribers.erase(subiter);}else++subiter;}}//////////////////////////////////////////////////voidMaster::Stop(){this->stop=true;if(this->runThread){this->runThread->join();deletethis->runThread;this->runThread=NULL;}}//////////////////////////////////////////////////voidMaster::Fini(){this->Stop();}//////////////////////////////////////////////////msgs::PublishMaster::GetPublisher(conststd::string&_topic){msgs::Publishmsg;PubList::iteratoriter;// Find all publishers of the topicfor(iter=this->publishers.begin();iter!=this->publishers.end();++iter){if(iter->first.topic()==_topic){msg=iter->first;break;}}returnmsg;}//////////////////////////////////////////////////transport::ConnectionPtrMaster::FindConnection(conststd::string&_host,uint16_t_port){transport::ConnectionPtrconn;Connection_M::iteratoriter;this->connectionMutex->lock();for(iter=this->connections.begin();iter!=this->connections.end();++iter){if(iter->second->GetRemoteAddress()==_host&&iter->second->GetRemotePort()==_port){conn=iter->second;break;}}this->connectionMutex->unlock();returnconn;}