Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00032
00033
00034 #include <cstdio>
00035 #include "topic_tools/shape_shifter.h"
00036 #include "topic_tools/parse.h"
00037
00038 using std::string;
00039 using std::vector;
00040 using namespace topic_tools;
00041
00042 ros::NodeHandle *g_node = NULL;
00043 bool g_advertised = false;
00044 string g_input_topic;
00045 string g_output_topic;
00046 ros::Publisher g_pub;
00047 ros::Subscriber* g_sub;
00048 bool g_lazy;
00049 ros::TransportHints g_th;
00050
00051 void conn_cb(const ros::SingleSubscriberPublisher&);
00052 void in_cb(const boost::shared_ptr<ShapeShifter const>& msg);
00053
00054 void conn_cb(const ros::SingleSubscriberPublisher&)
00055 {
00056
00057
00058 if(g_lazy && !g_sub)
00059 {
00060 ROS_DEBUG("lazy mode; resubscribing");
00061 g_sub = new ros::Subscriber(g_node->subscribe<ShapeShifter>(g_input_topic, 10, &in_cb, g_th));
00062 }
00063 }
00064
00065 void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
00066 {
00067 if (!g_advertised)
00068 {
00069
00070 bool latch = false;
00071 ros::M_string::iterator it = msg->__connection_header->find("latching");
00072 if((it != msg->__connection_header->end()) && (it->second == "1"))
00073 {
00074 ROS_DEBUG("input topic is latched; latching output topic to match");
00075 latch = true;
00076 }
00077 g_pub = msg->advertise(*g_node, g_output_topic, 10, latch, conn_cb);
00078 g_advertised = true;
00079 ROS_INFO("advertised as %s\n", g_output_topic.c_str());
00080 }
00081
00082
00083 if(g_lazy && !g_pub.getNumSubscribers())
00084 {
00085 ROS_DEBUG("lazy mode; unsubscribing");
00086 delete g_sub;
00087 g_sub = NULL;
00088 }
00089 else
00090 g_pub.publish(msg);
00091 }
00092
00093 int main(int argc, char **argv)
00094 {
00095 if (argc < 2)
00096 {
00097 printf("\nusage: relay IN_TOPIC [OUT_TOPIC]\n\n");
00098 return 1;
00099 }
00100 std::string topic_name;
00101 if(!getBaseName(string(argv[1]), topic_name))
00102 return 1;
00103 ros::init(argc, argv, topic_name + string("_relay"),
00104 ros::init_options::AnonymousName);
00105 if (argc == 2)
00106 g_output_topic = string(argv[1]) + string("_relay");
00107 else
00108 g_output_topic = string(argv[2]);
00109 g_input_topic = string(argv[1]);
00110 ros::NodeHandle n;
00111 g_node = &n;
00112
00113 ros::NodeHandle pnh("~");
00114 bool unreliable = false;
00115 pnh.getParam("unreliable", unreliable);
00116 pnh.getParam("lazy", g_lazy);
00117 if (unreliable)
00118 g_th.unreliable().reliable();
00119
00120 g_sub = new ros::Subscriber(n.subscribe<ShapeShifter>(g_input_topic, 10, &in_cb, g_th));
00121 ros::spin();
00122 return 0;
00123 }
00124