v01

thirdparty/ros/ros_comm/tools/rosbag/src/play.cpp

@@ -0,0 +1,193 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+********************************************************************/
+
+#include "rosbag/player.h"
+#include "boost/program_options.hpp"
+
+namespace po = boost::program_options;
+
+rosbag::PlayerOptions parseOptions(int argc, char** argv) {
+    rosbag::PlayerOptions opts;
+
+    po::options_description desc("Allowed options");
+
+    desc.add_options()
+      ("help,h", "produce help message")
+      ("prefix,p", po::value<std::string>()->default_value(""), "prefixes all output topics in replay")
+      ("quiet,q", "suppress console output")
+      ("immediate,i", "play back all messages without waiting")
+      ("pause", "start in paused mode")
+      ("queue", po::value<int>()->default_value(100), "use an outgoing queue of size SIZE")
+      ("clock", "publish the clock time")
+      ("hz", po::value<float>()->default_value(100.0f), "use a frequency of HZ when publishing clock time")
+      ("delay,d", po::value<float>()->default_value(0.2f), "sleep SEC seconds after every advertise call (to allow subscribers to connect)")
+      ("rate,r", po::value<float>()->default_value(1.0f), "multiply the publish rate by FACTOR")
+      ("start,s", po::value<float>()->default_value(0.0f), "start SEC seconds into the bag files")
+      ("duration,u", po::value<float>(), "play only SEC seconds from the bag files")
+      ("skip-empty", po::value<float>(), "skip regions in the bag with no messages for more than SEC seconds")
+      ("loop,l", "loop playback")
+      ("keep-alive,k", "keep alive past end of bag (useful for publishing latched topics)")
+      ("try-future-version", "still try to open a bag file, even if the version is not known to the player")
+      ("topics", po::value< std::vector<std::string> >()->multitoken(), "topics to play back")
+      ("pause-topics", po::value< std::vector<std::string> >()->multitoken(), "topics to pause playback on")
+      ("bags", po::value< std::vector<std::string> >(), "bag files to play back from")
+      ("wait-for-subscribers", "wait for at least one subscriber on each topic before publishing")
+      ("rate-control-topic", po::value<std::string>(), "watch the given topic, and if the last publish was more than <rate-control-max-delay> ago, wait until the topic publishes again to continue playback")
+      ("rate-control-max-delay", po::value<float>()->default_value(1.0f), "maximum time difference from <rate-control-topic> before pausing")
+      ;
+
+    po::positional_options_description p;
+    p.add("bags", -1);
+    
+    po::variables_map vm;
+    
+    try 
+    {
+      po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
+    } catch (boost::program_options::invalid_command_line_syntax& e)
+    {
+      throw ros::Exception(e.what());
+    }  catch (boost::program_options::unknown_option& e)
+    {
+      throw ros::Exception(e.what());
+    }
+
+    if (vm.count("help")) {
+      std::cout << desc << std::endl;
+      exit(0);
+    }
+
+    if (vm.count("prefix"))
+      opts.prefix = vm["prefix"].as<std::string>();
+    if (vm.count("quiet"))
+      opts.quiet = true;
+    if (vm.count("immediate"))
+      opts.at_once = true;
+    if (vm.count("pause"))
+      opts.start_paused = true;
+    if (vm.count("queue"))
+      opts.queue_size = vm["queue"].as<int>();
+    if (vm.count("hz"))
+      opts.bag_time_frequency = vm["hz"].as<float>();
+    if (vm.count("clock"))
+      opts.bag_time = true;
+    if (vm.count("delay"))
+      opts.advertise_sleep = ros::WallDuration(vm["delay"].as<float>());
+    if (vm.count("rate"))
+      opts.time_scale = vm["rate"].as<float>();
+    if (vm.count("start"))
+    {
+      opts.time = vm["start"].as<float>();
+      opts.has_time = true;
+    }
+    if (vm.count("duration"))
+    {
+      opts.duration = vm["duration"].as<float>();
+      opts.has_duration = true;
+    }
+    if (vm.count("skip-empty"))
+      opts.skip_empty = ros::Duration(vm["skip-empty"].as<float>());
+    if (vm.count("loop"))
+      opts.loop = true;
+    if (vm.count("keep-alive"))
+      opts.keep_alive = true;
+    if (vm.count("wait-for-subscribers"))
+      opts.wait_for_subscribers = true;
+
+    if (vm.count("topics"))
+    {
+      std::vector<std::string> topics = vm["topics"].as< std::vector<std::string> >();
+      for (std::vector<std::string>::iterator i = topics.begin();
+           i != topics.end();
+           i++)
+        opts.topics.push_back(*i);
+    }
+
+    if (vm.count("pause-topics"))
+    {
+      std::vector<std::string> pause_topics = vm["pause-topics"].as< std::vector<std::string> >();
+      for (std::vector<std::string>::iterator i = pause_topics.begin();
+           i != pause_topics.end();
+           i++)
+        opts.pause_topics.push_back(*i);
+    }
+
+    if (vm.count("rate-control-topic"))
+      opts.rate_control_topic = vm["rate-control-topic"].as<std::string>();
+
+    if (vm.count("rate-control-max-delay"))
+      opts.rate_control_max_delay = vm["rate-control-max-delay"].as<float>();
+
+    if (vm.count("bags"))
+    {
+      std::vector<std::string> bags = vm["bags"].as< std::vector<std::string> >();
+      for (std::vector<std::string>::iterator i = bags.begin();
+           i != bags.end();
+           i++)
+          opts.bags.push_back(*i);
+    } else {
+      if (vm.count("topics") || vm.count("pause-topics"))
+        throw ros::Exception("When using --topics or --pause-topics, --bags "
+          "should be specified to list bags.");
+      throw ros::Exception("You must specify at least one bag to play back.");
+    }
+            
+    return opts;
+}
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "play", ros::init_options::AnonymousName);
+
+    // Parse the command-line options
+    rosbag::PlayerOptions opts;
+    try {
+        opts = parseOptions(argc, argv);
+    }
+    catch (ros::Exception const& ex) {
+        ROS_ERROR("Error reading options: %s", ex.what());
+        return 1;
+    }
+
+    rosbag::Player player(opts);
+
+    try {
+      player.publish();
+    }
+    catch (std::runtime_error& e) {
+      ROS_FATAL("%s", e.what());
+      return 1;
+    }
+    
+    return 0;
+}