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 ros::MessageEvent<ShapeShifter>& msg_event);
00053
00054 void subscribe()
00055 {
00056 g_sub = new ros::Subscriber(g_node->subscribe(g_input_topic, 10, &in_cb, g_th));
00057 }
00058
00059 void conn_cb(const ros::SingleSubscriberPublisher&)
00060 {
00061
00062
00063 if(g_lazy && !g_sub)
00064 {
00065 ROS_DEBUG("lazy mode; resubscribing");
00066 subscribe();
00067 }
00068 }
00069
00070 void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event)
00071 {
00072 boost::shared_ptr<ShapeShifter const> const &msg = msg_event.getConstMessage();
00073 boost::shared_ptr<const ros::M_string> const& connection_header = msg_event.getConnectionHeaderPtr();
00074
00075 if (!g_advertised)
00076 {
00077
00078 bool latch = false;
00079 if (connection_header)
00080 {
00081 ros::M_string::const_iterator it = connection_header->find("latching");
00082 if((it != connection_header->end()) && (it->second == "1"))
00083 {
00084 ROS_DEBUG("input topic is latched; latching output topic to match");
00085 latch = true;
00086 }
00087 }
00088 g_pub = msg->advertise(*g_node, g_output_topic, 10, latch, conn_cb);
00089 g_advertised = true;
00090 ROS_INFO("advertised as %s\n", g_output_topic.c_str());
00091 }
00092
00093
00094 if(g_lazy && !g_pub.getNumSubscribers())
00095 {
00096 ROS_DEBUG("lazy mode; unsubscribing");
00097 delete g_sub;
00098 g_sub = NULL;
00099 }
00100 else
00101 g_pub.publish(msg);
00102 }
00103
00104 int main(int argc, char **argv)
00105 {
00106 if (argc < 2)
00107 {
00108 printf("\nusage: relay IN_TOPIC [OUT_TOPIC]\n\n");
00109 return 1;
00110 }
00111 std::string topic_name;
00112 if(!getBaseName(string(argv[1]), topic_name))
00113 return 1;
00114 ros::init(argc, argv, topic_name + string("_relay"),
00115 ros::init_options::AnonymousName);
00116 if (argc == 2)
00117 g_output_topic = string(argv[1]) + string("_relay");
00118 else
00119 g_output_topic = string(argv[2]);
00120 g_input_topic = string(argv[1]);
00121 ros::NodeHandle n;
00122 g_node = &n;
00123
00124 ros::NodeHandle pnh("~");
00125 bool unreliable = false;
00126 pnh.getParam("unreliable", unreliable);
00127 pnh.getParam("lazy", g_lazy);
00128 if (unreliable)
00129 g_th.unreliable().reliable();
00130
00131 subscribe();
00132 ros::spin();
00133 return 0;
00134 }
00135