camera_dispatcher.cpp
Go to the documentation of this file.
00001 // yliu, Jul 26, 2011
00002 
00003 // camera_dispatcher, relay selected topics
00004 
00005 // camera_a/image_rect -
00006 // camera_b/image_rect - 
00007 // camera_c/image_rect -
00008 // ...
00009 
00010 
00011 #include <cstdio>
00012 #include <vector>
00013 #include <list>
00014 #include "ros/console.h"
00015 #include "std_msgs/String.h"
00016 #include "camera_pose_toolkits/Switch.h"
00017 #include "ros/ros.h"
00018 #include "ros/topic.h"
00019 #include <sensor_msgs/CameraInfo.h>
00020 #include <sensor_msgs/Image.h>
00021 //#include <boost/bind.hpp>
00022 
00023 
00024 static ros::NodeHandle *g_node = NULL;
00025 static bool g_advertised1 = false;
00026 static bool g_advertised2 = false;
00027 
00028 static std::string g_output_topic1;
00029 static std::string g_output_topic2;
00030 
00031 static ros::Publisher g_pub_1;
00032 static ros::Publisher g_pub_2;
00033 
00034 static bool save_bandwidth = false;
00035 
00036 
00037 static ros::Publisher g_pub_selected; //selected camera namespace
00038 
00039 ros::Subscriber sub_1;
00040 ros::Subscriber sub_2;
00041 
00042 struct sub_pair
00043 {
00044   ros::Subscriber sub1; //image_rect
00045   ros::Subscriber sub2; //camera_info
00046 };
00047 
00048 static std::list<struct sub_pair> g_sub_pairs;
00049 static std::list<struct sub_pair>::iterator g_selected = g_sub_pairs.end();
00050 
00051 
00052 
00053 void in_cb1(const sensor_msgs::CameraInfo::ConstPtr& msg, std::string & s1)
00054 {
00055   if (!g_advertised1)
00056   {
00057     ROS_INFO("advertising 1");
00058     g_pub_1 = g_node->advertise<sensor_msgs::CameraInfo>(g_output_topic1, 10); //not latched
00059     g_advertised1 = true;
00060   }
00061   if (s1 == g_selected->sub1.getTopic() )
00062   {
00063     //printf("relaying 1...");
00064     g_pub_1.publish(msg);
00065   }
00066 }
00067 
00068 void in_cb2(const sensor_msgs::Image::ConstPtr& msg, std::string & s2)
00069 { 
00070   if (!g_advertised2)
00071   {
00072     ROS_INFO("advertising 2");
00073     g_pub_2 = g_node->advertise<sensor_msgs::Image>(g_output_topic2, 10); //not latched
00074     g_advertised2 = true;
00075   }
00076   if (s2 == g_selected->sub2.getTopic() )
00077   {
00078     //printf("relaying 2...");
00079     g_pub_2.publish(msg);
00080   }
00081 }
00082 
00083 
00084 bool switch_srv_cb(camera_pose_toolkits::Switch::Request& req,
00085                   camera_pose_toolkits::Switch::Response& res)
00086 {
00087  
00088   // Check that it's not already in our list
00089   // spin through our vector of inputs and find this guy
00090   // if already there, select it
00091   // if not there, add it, and then select it
00092   for (std::list<sub_pair>::iterator it = g_sub_pairs.begin(); it != g_sub_pairs.end(); ++it)
00093   {
00094     if (ros::names::resolve(it->sub2.getTopic()) == ros::names::resolve(req.camera_ns + "/image_rect"))
00095     {
00096       printf(" subscribers to [%s] and [%s] aready in the list\n", it->sub1.getTopic().c_str(), it->sub2.getTopic().c_str());
00097       g_selected = it;
00098       printf("now listening to [%s] and [%s]\n", it->sub1.getTopic().c_str(), it->sub2.getTopic().c_str());
00099 
00100       std_msgs::String t;
00101       t.data = req.camera_ns;
00102       g_pub_selected.publish(t);
00103       return true;
00104     }
00105   }
00106   
00107   printf("adding subscribers to [%s/camera_info] and [%s/image_rect] to the list\n", req.camera_ns.c_str(), req.camera_ns.c_str());
00108   std::string s1 = req.camera_ns+"/camera_info";
00109   std::string s2 = req.camera_ns+"/image_rect";
00110  
00111   sub_pair sp;
00112   try
00113   {//sub_1
00114    sp.sub1  = g_node->subscribe<sensor_msgs::CameraInfo>(req.camera_ns+"/camera_info", 10, boost::bind(in_cb1, _1,  s1 ));
00115   }
00116   catch(ros::InvalidNameException& e)
00117   {
00118     ROS_WARN("failed to add subscriber to topic [%s/camera_info],  because it's an invalid name: %s", req.camera_ns.c_str(), e.what());
00119     return false;
00120   }
00121 
00122   try
00123   {//sub_2
00124    sp.sub2  = g_node->subscribe<sensor_msgs::Image>(req.camera_ns+"/image_rect", 10, boost::bind(in_cb2, _1,  s2 ));
00125   }
00126   catch(ros::InvalidNameException& e)
00127   {
00128     ROS_WARN("failed to add subscriber to topic [%s/image_rect],  because it's an invalid name: %s", req.camera_ns.c_str(), e.what());
00129     return false;
00130   }
00131   
00132  
00133   //sp.sub1 = sub_1;
00134   //sp.sub2 = sub_2;
00135 
00136   if (save_bandwidth == true)
00137   {
00138     if (g_sub_pairs.size() > 0) // for topics that are not relayed, unsubsribe.
00139     {  for ( std::list<sub_pair>::iterator it = g_sub_pairs.begin(); it != g_sub_pairs.end(); ++it)
00140        {
00141          it->sub1.shutdown(); 
00142          it->sub2.shutdown();
00143        }
00144     }
00145     g_sub_pairs.clear();
00146   }
00147   
00148 
00149   g_sub_pairs.push_back( sp );
00150   printf("%d\n", int(g_sub_pairs.size()));
00151   g_selected = g_sub_pairs.end();
00152   g_selected --; //important
00153  
00154   std_msgs::String t;
00155   t.data = req.camera_ns;
00156   g_pub_selected.publish(t);
00157 
00158   printf("now listening to [%s] and [%s]\n", g_selected->sub1.getTopic().c_str(), g_selected->sub2.getTopic().c_str());
00159   //printf("now listening to [%s] and [%s]\n", sp.sub1.getTopic().c_str(), sp.sub2.getTopic().c_str());
00160 
00161   return true;
00162 }
00163 
00164 
00165 
00166 
00167 
00168 int main(int argc, char **argv)
00169 {
00170   //std::vector<std::string> args;
00171   //ros::removeROSArgs(argc, (const char**)argv, args);
00172   //printf("%s", args[1].c_str());
00173   //if (args.size() < 1)
00174   //{
00175   //  printf("\nusage: camera_dispatcher OUTPUT_NS\n\n");
00176   //  return 1;
00177   //}
00178   //std::string output_ns;
00179   //output_ns = args[1];
00180 
00181     
00182   ros::init(argc, argv, std::string("camera_dispatcher"), ros::init_options::AnonymousName);
00183   //Anonymize the node name. Adds a random number to the end of your node's name, to make it unique. 
00184   
00185   printf("%d\n", argc);
00186   printf("%s\n", argv[0]);
00187   printf("%s\n", argv[1]);
00188   
00189   if (argc < 2)
00190   {
00191     printf("\nusage: rosrun camera_pose_toolkits camera_dispatcher_node OUTPUT_NS\n\n");
00192     return 1;
00193   }
00194   
00195   ros::NodeHandle n;
00196   g_node = &n;
00197   
00198   std::string output_ns;
00199   output_ns = argv[1];
00200   // 
00201   g_output_topic1 = output_ns + "/camera_info";
00202   g_output_topic2 = output_ns + "/image_rect";
00203 
00204 
00205   // Put our API into the "camera_mux" namespace 
00206   ros::NodeHandle d_nh(argv[1]);//"camera_dispatcher"
00207   // Latched publisher for selected input topic name
00208   g_pub_selected = d_nh.advertise<std_msgs::String>(std::string("selected"), 1, true); // latched, slow changing
00209   //When a connection is latched, the last message published is saved and automatically sent to any future subscribers that connect.
00210   // New service
00211   ros::ServiceServer ss_switch = d_nh.advertiseService(std::string("switch"), switch_srv_cb);
00212 
00213   ros::spin();
00214 
00215   for (std::list<sub_pair>::iterator it = g_sub_pairs.begin(); it != g_sub_pairs.end(); ++it)
00216   {
00217     it->sub1.shutdown();
00218     it->sub2.shutdown();
00219   }
00220 
00221   g_sub_pairs.clear();
00222   return 0;
00223 }
00224 
00225 
00226 
00227 
00228 
00229 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_toolkits
Author(s): Yiping Liu
autogenerated on Thu Aug 15 2013 12:02:52