Fixed command-line parsing to handle ROS args, #1862
This commit is contained in:
parent
f7c808e322
commit
d98854e381
|
@ -66,18 +66,24 @@ void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
|
||||||
s_count = 0;
|
s_count = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define USAGE "\nusage: drop IN_TOPIC X Y [OUT_TOPIC]\n\n" \
|
||||||
|
" This program will drop X out of every Y messages from IN_TOPIC,\n" \
|
||||||
|
" forwarding the rest to OUT_TOPIC if given, else to a topic \n" \
|
||||||
|
" named IN_TOPIC_drop\n\n"
|
||||||
int main(int argc, char **argv)
|
int main(int argc, char **argv)
|
||||||
{
|
{
|
||||||
if ((argc != 4 && argc != 5) || atoi(argv[2]) < 0 || atoi(argv[3]) < 1)
|
if(argc < 2)
|
||||||
{
|
{
|
||||||
printf("\nusage: drop IN_TOPIC X Y [OUT_TOPIC]\n\n"
|
puts(USAGE);
|
||||||
" This program will drop X out of every Y messages from IN_TOPIC,\n"
|
|
||||||
" forwarding the rest to OUT_TOPIC if given, else to a topic \n"
|
|
||||||
" named IN_TOPIC_drop\n\n");
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
ros::init(argc, argv, string(argv[1]) + string("_drop"),
|
ros::init(argc, argv, string(argv[1]) + string("_drop"),
|
||||||
ros::init_options::AnonymousName);
|
ros::init_options::AnonymousName);
|
||||||
|
if ((argc != 4 && argc != 5) || atoi(argv[2]) < 0 || atoi(argv[3]) < 1)
|
||||||
|
{
|
||||||
|
puts(USAGE);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
if (argc == 4)
|
if (argc == 4)
|
||||||
g_output_topic = string(argv[1]) + string("_drop");
|
g_output_topic = string(argv[1]) + string("_drop");
|
||||||
else // argc == 5
|
else // argc == 5
|
||||||
|
|
|
@ -125,6 +125,11 @@ int main(int argc, char **argv)
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
string intopic = string(argv[2]);
|
||||||
|
|
||||||
|
ros::init(argc, argv, intopic + string("_throttle"),
|
||||||
|
ros::init_options::AnonymousName);
|
||||||
|
|
||||||
if(!strcmp(argv[1], "messages"))
|
if(!strcmp(argv[1], "messages"))
|
||||||
g_use_messages = true;
|
g_use_messages = true;
|
||||||
else if(!strcmp(argv[1], "bytes"))
|
else if(!strcmp(argv[1], "bytes"))
|
||||||
|
@ -134,8 +139,6 @@ int main(int argc, char **argv)
|
||||||
puts(USAGE);
|
puts(USAGE);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
string intopic = string(argv[2]);
|
|
||||||
|
|
||||||
if(g_use_messages && argc == 5)
|
if(g_use_messages && argc == 5)
|
||||||
g_output_topic = string(argv[4]);
|
g_output_topic = string(argv[4]);
|
||||||
|
@ -159,8 +162,6 @@ int main(int argc, char **argv)
|
||||||
g_window = atof(argv[4]);
|
g_window = atof(argv[4]);
|
||||||
}
|
}
|
||||||
|
|
||||||
ros::init(argc, argv, intopic + string("_throttle"),
|
|
||||||
ros::init_options::AnonymousName);
|
|
||||||
ros::NodeHandle n;
|
ros::NodeHandle n;
|
||||||
g_node = &n;
|
g_node = &n;
|
||||||
ros::Subscriber sub = n.subscribe<ShapeShifter>(intopic, 10, &in_cb);
|
ros::Subscriber sub = n.subscribe<ShapeShifter>(intopic, 10, &in_cb);
|
||||||
|
|
Loading…
Reference in New Issue