udpmulti_manager.cpp
Go to the documentation of this file.
00001 
00002 #include <ros/ros.h>
00003 #include <boost/thread.hpp>
00004 
00005 #include "udpmulti_transport/UDPMultRegisterTopic.h"
00006 #include "udpmulti_transport/UDPMultClearAll.h"
00007 #include "udpmulti_transport/UDPMultGetTopicList.h"
00008 
00009 
00010 typedef std::map< std::string, unsigned int ,std::less<std::string> > TopicPortMap;
00011 unsigned int availablePort = 1024;
00012 std::string multicast_address = "239.255.0.1";
00013 TopicPortMap registeredTopic;
00014 boost::mutex main_mutex;
00015 
00016 
00017 bool register_topic(udpmulti_transport::UDPMultRegisterTopic::Request &req, 
00018                 udpmulti_transport::UDPMultRegisterTopic::Response &res)
00019 {
00020         boost::lock_guard<boost::mutex> guard(main_mutex);//auto-lock unlock, even on exception
00021         TopicPortMap::iterator it = registeredTopic.find(req.topic);
00022     if (it != registeredTopic.end()) {
00023         res.multicast_address = multicast_address;
00024         res.port = it->second;
00025     } else {
00026         res.multicast_address = multicast_address;
00027         res.port = availablePort++;
00028         registeredTopic.insert(std::pair<std::string,unsigned int>(req.topic,res.port));
00029     }
00030         ROS_INFO("Registered port %d for topic %s", res.port,req.topic.c_str());
00031         
00032         return true;
00033 }
00034 
00035 bool clear_all_topics(udpmulti_transport::UDPMultClearAll::Request &req, 
00036                 udpmulti_transport::UDPMultClearAll::Response &res)
00037 {
00038         boost::lock_guard<boost::mutex> guard(main_mutex);//auto-lock unlock, even on exception
00039     registeredTopic.clear();
00040     availablePort = 1024;
00041         ROS_INFO("Deleted all topic-port association");
00042         res.result = 0;
00043         return true;
00044 }
00045 
00046 bool get_topic_list(udpmulti_transport::UDPMultGetTopicList::Request &req, 
00047                 udpmulti_transport::UDPMultGetTopicList::Response &res)
00048 {
00049         boost::lock_guard<boost::mutex> guard(main_mutex);//auto-lock unlock, even on exception
00050     TopicPortMap::const_iterator it;
00051     unsigned int i=0;
00052     res.multicast_address = multicast_address;
00053     res.topics.resize(registeredTopic.size());
00054     for (it=registeredTopic.begin();it!=registeredTopic.end();it++,i++) {
00055         udpmulti_transport::UDPMultTopic tpc;
00056         tpc.topic = it->first;
00057         tpc.port = it->second;
00058         res.topics[i] = tpc;
00059     }
00060         return true;
00061 }
00062 
00063 
00064 int main(int argc,char *argv[])
00065 {
00066         ros::init(argc, argv, "udpmulti_manager");
00067         ros::NodeHandle n("udpmulti_manager");
00068     n.param<std::string>("multicast_address",multicast_address,"239.255.0.1");
00069 
00070    ros::ServiceServer regTopicSrv = n.advertiseService("register_topic",register_topic);
00071    ros::ServiceServer clearAllSrv = n.advertiseService("clear_all_topics",clear_all_topics);
00072    ros::ServiceServer getListSrv = n.advertiseService("get_topics",get_topic_list);
00073    ROS_INFO("udpmulti_manager started");
00074 
00075    ros::spin();
00076 
00077 }
00078 
00079 
00080 


udpmulti_transport
Author(s): Cedric Pradalier
autogenerated on Sun Oct 5 2014 23:49:15