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
00029
00030 #include <cstdlib>
00031 #include <cstdio>
00032 #include "topic_tools/shape_shifter.h"
00033 #include "topic_tools/parse.h"
00034
00035 using std::string;
00036 using std::vector;
00037 using namespace topic_tools;
00038
00039 static ros::NodeHandle *g_node = NULL;
00040 static int g_x = 0, g_y = 1;
00041 static bool g_advertised = false;
00042 static string g_output_topic;
00043 static ros::Publisher g_pub;
00044
00045 void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
00046 {
00047 static int s_count = 0;
00048 if (!g_advertised)
00049 {
00050 g_pub = msg->advertise(*g_node, g_output_topic, 10);
00051 g_advertised = true;
00052 printf("advertised as %s\n", g_output_topic.c_str());
00053 }
00054 if (s_count >= g_x)
00055 g_pub.publish(msg);
00056 ++s_count;
00057 if (s_count >= g_y)
00058 s_count = 0;
00059 }
00060
00061 #define USAGE "\nusage: drop IN_TOPIC X Y [OUT_TOPIC]\n\n" \
00062 " This program will drop X out of every Y messages from IN_TOPIC,\n" \
00063 " forwarding the rest to OUT_TOPIC if given, else to a topic \n" \
00064 " named IN_TOPIC_drop\n\n"
00065 int main(int argc, char **argv)
00066 {
00067 if(argc < 2)
00068 {
00069 puts(USAGE);
00070 return 1;
00071 }
00072 std::string topic_name;
00073 if(!getBaseName(string(argv[1]), topic_name))
00074 return 1;
00075 ros::init(argc, argv, topic_name + string("_drop"),
00076 ros::init_options::AnonymousName);
00077 if ((argc != 4 && argc != 5) || atoi(argv[2]) < 0 || atoi(argv[3]) < 1)
00078 {
00079 puts(USAGE);
00080 return 1;
00081 }
00082 if (argc == 4)
00083 g_output_topic = string(argv[1]) + string("_drop");
00084 else
00085 g_output_topic = string(argv[4]);
00086 ros::NodeHandle n;
00087 g_node = &n;
00088 g_x = atoi(argv[2]);
00089 g_y = atoi(argv[3]);
00090 ros::Subscriber sub = n.subscribe<ShapeShifter>(string(argv[1]), 10, in_cb);
00091 ros::spin();
00092 return 0;
00093 }
00094