Go to the documentation of this file.00001
00007
00008
00009
00010
00011 #include <cost_map_ros/image_bundles.hpp>
00012 #include <ecl/command_line.hpp>
00013 #include <ecl/console.hpp>
00014 #include <exception>
00015
00016 #include <ros/init.h>
00017 #include <ros/ros.h>
00018 #include <string>
00019
00020 #include "../../include/cost_map_ros/utilities.hpp"
00021
00022
00023
00024
00025
00026 int main(int argc, char **argv) {
00027 ecl::CmdLine cmd("Save a published cost map to an image bundle");
00028 ecl::UnlabeledValueArg<std::string> topicNameArg(
00029 "topic_name",
00030 "Topic name to listen to for costmaps (e.g. /image_bundle/cost_map)",
00031 true,
00032 "/image_bundle/cost_map",
00033 "string");
00034 ecl::UnlabeledValueArg<std::string> filenameArg(
00035 "filename",
00036 "Filename of the bundle's meta yaml file.",
00037 true,
00038 "foo.yaml",
00039 "string");
00040 cmd.add(topicNameArg);
00041 cmd.add(filenameArg);
00042
00043 std::vector<std::string> myargs;
00044 ros::removeROSArgs(argc, argv, myargs);
00045 char** myargv = new char*[myargs.size()];
00046 for(int i = 0, ie = myargs.size(); i < ie; ++i)
00047 {
00048 myargv[i] = const_cast<char*>(myargs[i].data());
00049 }
00050 cmd.parse(myargs.size(), myargv);
00051
00052 ros::init(argc, argv, "save_image_bundle");
00053 try {
00054 cost_map::SaveImageBundle dumper(topicNameArg.getValue(), filenameArg.getValue());
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064 ros::Rate rate(10);
00065 while ( ros::ok() ) {
00066 ros::spinOnce();
00067 if ( dumper.finished ) {
00068 break;
00069 } else {
00070 ROS_WARN_STREAM_DELAYED_THROTTLE(5, "SaveImageBundle : still waiting for a costmap to arrive [zzzz....]");
00071 rate.sleep();
00072 }
00073 }
00074 } catch (const std::exception& e) {
00075 std::cout << ecl::red << "[ERROR] " << e.what() << ecl::reset << std::endl;
00076 return EXIT_FAILURE;
00077 }
00078 return EXIT_SUCCESS;
00079 }