play.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage, Inc. nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 ********************************************************************/
00034 
00035 #include "rosbag/player.h"
00036 #include "boost/program_options.hpp"
00037 
00038 namespace po = boost::program_options;
00039 
00040 rosbag::PlayerOptions parseOptions(int argc, char** argv) {
00041     rosbag::PlayerOptions opts;
00042 
00043     po::options_description desc("Allowed options");
00044 
00045     desc.add_options()
00046       ("help,h", "produce help message")
00047       ("quiet,q", "suppress console output")
00048       ("immediate,i", "play back all messages without waiting")
00049       ("pause", "start in paused mode")
00050       ("queue", po::value<int>()->default_value(100), "use an outgoing queue of size SIZE")
00051       ("clock", "publish the clock time")
00052       ("hz", po::value<float>()->default_value(100.0f), "use a frequency of HZ when publishing clock time")
00053       ("delay,d", po::value<float>()->default_value(0.2f), "sleep SEC seconds after every advertise call (to allow subscribers to connect)")
00054       ("rate,r", po::value<float>()->default_value(1.0f), "multiply the publish rate by FACTOR")
00055       ("start,s", po::value<float>()->default_value(0.0f), "start SEC seconds into the bag files")
00056       ("duration,u", po::value<float>(), "play only SEC seconds from the bag files")
00057       ("skip-empty", po::value<float>(), "skip regions in the bag with no messages for more than SEC seconds")
00058       ("loop,l", "loop playback")
00059       ("keep-alive,k", "keep alive past end of bag (useful for publishing latched topics)")
00060       ("try-future-version", "still try to open a bag file, even if the version is not known to the player")
00061       ("topics", po::value< std::vector<std::string> >()->multitoken(), "topics to play back")
00062       ("bags", po::value< std::vector<std::string> >(), "bag files to play back from");
00063     
00064     po::positional_options_description p;
00065     p.add("bags", -1);
00066     
00067     po::variables_map vm;
00068     
00069     try 
00070     {
00071       po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
00072     } catch (boost::program_options::invalid_command_line_syntax& e)
00073     {
00074       throw ros::Exception(e.what());
00075     }  catch (boost::program_options::unknown_option& e)
00076     {
00077       throw ros::Exception(e.what());
00078     }
00079 
00080     if (vm.count("help")) {
00081       std::cout << desc << std::endl;
00082       exit(0);
00083     }
00084 
00085     if (vm.count("quiet"))
00086       opts.quiet = true;
00087     if (vm.count("immediate"))
00088       opts.at_once = true;
00089     if (vm.count("pause"))
00090       opts.start_paused = true;
00091     if (vm.count("queue"))
00092       opts.queue_size = vm["queue"].as<int>();
00093     if (vm.count("hz"))
00094       opts.bag_time_frequency = vm["hz"].as<float>();
00095     if (vm.count("clock"))
00096       opts.bag_time = true;
00097     if (vm.count("delay"))
00098       opts.advertise_sleep = ros::WallDuration(vm["delay"].as<float>());
00099     if (vm.count("rate"))
00100       opts.time_scale = vm["rate"].as<float>();
00101     if (vm.count("start"))
00102     {
00103       opts.time = vm["start"].as<float>();
00104       opts.has_time = true;
00105     }
00106     if (vm.count("duration"))
00107     {
00108       opts.duration = vm["duration"].as<float>();
00109       opts.has_duration = true;
00110     }
00111     if (vm.count("skip-empty"))
00112       opts.skip_empty = ros::Duration(vm["skip-empty"].as<float>());
00113     if (vm.count("loop"))
00114       opts.loop = true;
00115     if (vm.count("keep-alive"))
00116       opts.keep_alive = true;
00117 
00118     if (vm.count("topics"))
00119     {
00120       std::vector<std::string> topics = vm["topics"].as< std::vector<std::string> >();
00121       for (std::vector<std::string>::iterator i = topics.begin();
00122            i != topics.end();
00123            i++)
00124         opts.topics.push_back(*i);
00125     }
00126 
00127     if (vm.count("bags"))
00128     {
00129       std::vector<std::string> bags = vm["bags"].as< std::vector<std::string> >();
00130       for (std::vector<std::string>::iterator i = bags.begin();
00131            i != bags.end();
00132            i++)
00133           opts.bags.push_back(*i);
00134     } else {
00135       if (vm.count("topics"))
00136         throw ros::Exception("When using --topics, --bags should be specified to list bags.");
00137       throw ros::Exception("You must specify at least one bag to play back.");
00138     }
00139             
00140     return opts;
00141 }
00142 
00143 int main(int argc, char** argv) {
00144     ros::init(argc, argv, "play", ros::init_options::AnonymousName);
00145 
00146     // Parse the command-line options
00147     rosbag::PlayerOptions opts;
00148     try {
00149         opts = parseOptions(argc, argv);
00150     }
00151     catch (ros::Exception const& ex) {
00152         ROS_ERROR("Error reading options: %s", ex.what());
00153         return 1;
00154     }
00155 
00156     rosbag::Player player(opts);
00157 
00158     try {
00159       player.publish();
00160     }
00161     catch (std::runtime_error& e) {
00162       ROS_FATAL("%s", e.what());
00163       return 1;
00164     }
00165     
00166     return 0;
00167 }


rosbag
Author(s): Tim Field, Jeremy Leibs, James Bowman
autogenerated on Mon Oct 6 2014 11:47:09