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;
+}

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

@@ -0,0 +1,863 @@
+/*********************************************************************
+* 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 "rosbag/message_instance.h"
+#include "rosbag/view.h"
+
+#if !defined(_MSC_VER)
+  #include <sys/select.h>
+#endif
+
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
+
+#include "rosgraph_msgs/Clock.h"
+
+#define foreach BOOST_FOREACH
+
+using std::map;
+using std::pair;
+using std::string;
+using std::vector;
+using boost::shared_ptr;
+using ros::Exception;
+
+namespace rosbag {
+
+ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size, const std::string& prefix) {
+    ros::AdvertiseOptions opts(prefix + c->topic, queue_size, c->md5sum, c->datatype, c->msg_def);
+    ros::M_string::const_iterator header_iter = c->header->find("latching");
+    opts.latch = (header_iter != c->header->end() && header_iter->second == "1");
+    return opts;
+}
+
+
+ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& m, uint32_t queue_size, const std::string& prefix) {
+    return ros::AdvertiseOptions(prefix + m.getTopic(), queue_size, m.getMD5Sum(), m.getDataType(), m.getMessageDefinition());
+}
+
+// PlayerOptions
+
+PlayerOptions::PlayerOptions() :
+    prefix(""),
+    quiet(false),
+    start_paused(false),
+    at_once(false),
+    bag_time(false),
+    bag_time_frequency(0.0),
+    time_scale(1.0),
+    queue_size(0),
+    advertise_sleep(0.2),
+    try_future(false),
+    has_time(false),
+    loop(false),
+    time(0.0f),
+    has_duration(false),
+    duration(0.0f),
+    keep_alive(false),
+    wait_for_subscribers(false),
+    rate_control_topic(""),
+    rate_control_max_delay(1.0f),
+    skip_empty(ros::DURATION_MAX)
+{
+}
+
+void PlayerOptions::check() {
+    if (bags.size() == 0)
+        throw Exception("You must specify at least one bag file to play from");
+    if (has_duration && duration <= 0.0)
+        throw Exception("Invalid duration, must be > 0.0");
+}
+
+// Player
+
+Player::Player(PlayerOptions const& options) :
+    options_(options),
+    paused_(false),
+    // If we were given a list of topics to pause on, then go into that mode
+    // by default (it can be toggled later via 't' from the keyboard).
+    pause_for_topics_(options_.pause_topics.size() > 0),
+    pause_change_requested_(false),
+    requested_pause_state_(false),
+    terminal_modified_(false)
+{
+  ros::NodeHandle private_node_handle("~");
+  pause_service_ = private_node_handle.advertiseService("pause_playback", &Player::pauseCallback, this);
+}
+
+Player::~Player() {
+    foreach(shared_ptr<Bag> bag, bags_)
+        bag->close();
+
+    restoreTerminal();
+}
+
+void Player::publish() {
+    options_.check();
+
+    // Open all the bag files
+    foreach(string const& filename, options_.bags) {
+        ROS_INFO("Opening %s", filename.c_str());
+
+        try
+        {
+            shared_ptr<Bag> bag(boost::make_shared<Bag>());
+            bag->open(filename, bagmode::Read);
+            bags_.push_back(bag);
+        }
+        catch (BagUnindexedException ex) {
+            std::cerr << "Bag file " << filename << " is unindexed.  Run rosbag reindex." << std::endl;
+            return;
+        }
+    }
+
+    setupTerminal();
+
+    if (!node_handle_.ok())
+      return;
+
+    if (!options_.prefix.empty())
+    {
+      ROS_INFO_STREAM("Using prefix '" << options_.prefix << "'' for topics ");
+    }
+
+    if (!options_.quiet)
+      puts("");
+    
+    // Publish all messages in the bags
+    View full_view;
+    foreach(shared_ptr<Bag> bag, bags_)
+        full_view.addQuery(*bag);
+
+    ros::Time initial_time = full_view.getBeginTime();
+
+    initial_time += ros::Duration(options_.time);
+
+    ros::Time finish_time = ros::TIME_MAX;
+    if (options_.has_duration)
+    {
+      finish_time = initial_time + ros::Duration(options_.duration);
+    }
+
+    View view;
+    TopicQuery topics(options_.topics);
+
+    if (options_.topics.empty())
+    {
+      foreach(shared_ptr<Bag> bag, bags_)
+        view.addQuery(*bag, initial_time, finish_time);
+    } else {
+      foreach(shared_ptr<Bag> bag, bags_)
+        view.addQuery(*bag, topics, initial_time, finish_time);
+    }
+
+    if (view.size() == 0)
+    {
+      std::cerr << "No messages to play on specified topics.  Exiting." << std::endl;
+      ros::shutdown();
+      return;
+    }
+
+    // Advertise all of our messages
+    foreach(const ConnectionInfo* c, view.getConnections())
+    {
+        ros::M_string::const_iterator header_iter = c->header->find("callerid");
+        std::string callerid = (header_iter != c->header->end() ? header_iter->second : string(""));
+
+        string callerid_topic = callerid + c->topic;
+
+        map<string, ros::Publisher>::iterator pub_iter = publishers_.find(callerid_topic);
+        if (pub_iter == publishers_.end()) {
+
+            ros::AdvertiseOptions opts = createAdvertiseOptions(c, options_.queue_size, options_.prefix);
+
+            ros::Publisher pub = node_handle_.advertise(opts);
+            publishers_.insert(publishers_.begin(), pair<string, ros::Publisher>(callerid_topic, pub));
+
+            pub_iter = publishers_.find(callerid_topic);
+        }
+    }
+
+    if (options_.rate_control_topic != "")
+    {
+        std::cout << "Creating rate control topic subscriber..." << std::flush;
+
+        boost::shared_ptr<ros::Subscriber> sub(boost::make_shared<ros::Subscriber>());
+        ros::SubscribeOptions ops;
+        ops.topic = options_.rate_control_topic;
+        ops.queue_size = 10;
+        ops.md5sum = ros::message_traits::md5sum<topic_tools::ShapeShifter>();
+        ops.datatype = ros::message_traits::datatype<topic_tools::ShapeShifter>();
+        ops.helper = boost::make_shared<ros::SubscriptionCallbackHelperT<
+            const ros::MessageEvent<topic_tools::ShapeShifter const> &> >(
+                boost::bind(&Player::updateRateTopicTime, this, _1));
+
+        rate_control_sub_ = node_handle_.subscribe(ops);
+
+        std::cout << " done." << std::endl;
+    }
+
+
+    std::cout << "Waiting " << options_.advertise_sleep.toSec() << " seconds after advertising topics..." << std::flush;
+    options_.advertise_sleep.sleep();
+    std::cout << " done." << std::endl;
+
+    std::cout << std::endl << "Hit space to toggle paused, or 's' to step." << std::endl;
+
+    paused_ = options_.start_paused;
+
+    if (options_.wait_for_subscribers)
+    {
+        waitForSubscribers();
+    }
+
+    while (true) {
+        // Set up our time_translator and publishers
+
+        time_translator_.setTimeScale(options_.time_scale);
+
+        start_time_ = view.begin()->getTime();
+        time_translator_.setRealStartTime(start_time_);
+        bag_length_ = view.getEndTime() - view.getBeginTime();
+
+        // Set the last rate control to now, so the program doesn't start delayed.
+        last_rate_control_ = start_time_;
+
+        time_publisher_.setTime(start_time_);
+
+        ros::WallTime now_wt = ros::WallTime::now();
+        time_translator_.setTranslatedStartTime(ros::Time(now_wt.sec, now_wt.nsec));
+
+
+        time_publisher_.setTimeScale(options_.time_scale);
+        if (options_.bag_time)
+            time_publisher_.setPublishFrequency(options_.bag_time_frequency);
+        else
+            time_publisher_.setPublishFrequency(-1.0);
+
+        paused_time_ = now_wt;
+
+        // Call do-publish for each message
+        foreach(MessageInstance m, view) {
+            if (!node_handle_.ok())
+                break;
+
+            doPublish(m);
+        }
+
+        if (options_.keep_alive)
+            while (node_handle_.ok())
+                doKeepAlive();
+
+        if (!node_handle_.ok()) {
+            std::cout << std::endl;
+            break;
+        }
+        if (!options_.loop) {
+            std::cout << std::endl << "Done." << std::endl;
+            break;
+        }
+    }
+
+    ros::shutdown();
+}
+
+void Player::updateRateTopicTime(const ros::MessageEvent<topic_tools::ShapeShifter const>& msg_event)
+{
+    boost::shared_ptr<topic_tools::ShapeShifter const> const &ssmsg = msg_event.getConstMessage();
+    std::string def = ssmsg->getMessageDefinition();
+    size_t length = ros::serialization::serializationLength(*ssmsg);
+    
+    // Check the message definition.
+    std::istringstream f(def);
+    std::string s;
+    bool flag = false;
+    while(std::getline(f, s, '\n')) {
+        if (!s.empty() && s.find("#") != 0) {
+            // Does not start with #, is not a comment.
+            if (s.find("Header ") == 0) {
+                flag = true;
+            }
+            break;
+        }
+    }
+    // If the header is not the first element in the message according to the definition, throw an error.
+    if (!flag) {
+        std::cout << std::endl << "WARNING: Rate control topic is bad, header is not first. MSG may be malformed." << std::endl;
+        return;
+    }
+
+    std::vector<uint8_t> buffer(length);
+    ros::serialization::OStream ostream(&buffer[0], length);
+    ros::serialization::Serializer<topic_tools::ShapeShifter>::write(ostream, *ssmsg);
+
+    // Assuming that the header is the first several bytes of the message.
+    //uint32_t header_sequence_id   = buffer[0] | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2] << 16 | (uint32_t)buffer[3] << 24;
+    int32_t header_timestamp_sec  = buffer[4] | (uint32_t)buffer[5] << 8 | (uint32_t)buffer[6] << 16 | (uint32_t)buffer[7] << 24;
+    int32_t header_timestamp_nsec = buffer[8] | (uint32_t)buffer[9] << 8 | (uint32_t)buffer[10] << 16 | (uint32_t)buffer[11] << 24;
+
+    last_rate_control_ = ros::Time(header_timestamp_sec, header_timestamp_nsec);
+}
+
+void Player::printTime()
+{
+    if (!options_.quiet) {
+
+        ros::Time current_time = time_publisher_.getTime();
+        ros::Duration d = current_time - start_time_;
+
+
+        if (paused_)
+        {
+            printf("\r [PAUSED ]  Bag Time: %13.6f   Duration: %.6f / %.6f               \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec());
+        }
+        else if (delayed_)
+        {
+            ros::Duration time_since_rate = std::max(ros::Time::now() - last_rate_control_, ros::Duration(0));
+            printf("\r [DELAYED]  Bag Time: %13.6f   Duration: %.6f / %.6f   Delay: %.2f \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec(), time_since_rate.toSec());
+        }
+        else
+        {
+            printf("\r [RUNNING]  Bag Time: %13.6f   Duration: %.6f / %.6f               \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec());
+        }
+        fflush(stdout);
+    }
+}
+
+bool Player::pauseCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
+{
+  pause_change_requested_ = (req.data != paused_);
+  requested_pause_state_ = req.data;
+
+  res.success = pause_change_requested_;
+
+  if (res.success)
+  {
+    res.message = std::string("Playback is now ") + (requested_pause_state_ ? "paused" : "resumed");
+  }
+  else
+  {
+    res.message = std::string("Bag is already ") + (requested_pause_state_ ? "paused." : "running.");
+  }
+
+  return true;
+}
+
+void Player::processPause(const bool paused, ros::WallTime &horizon)
+{
+  paused_ = paused;
+
+  if (paused_)
+  {
+    paused_time_ = ros::WallTime::now();
+  }
+  else
+  {
+    // Make sure time doesn't shift after leaving pause.
+    ros::WallDuration shift = ros::WallTime::now() - paused_time_;
+    paused_time_ = ros::WallTime::now();
+
+    time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
+
+    horizon += shift;
+    time_publisher_.setWCHorizon(horizon);
+  }
+}
+
+void Player::waitForSubscribers() const
+{
+    bool all_topics_subscribed = false;
+    std::cout << "Waiting for subscribers." << std::endl;
+    while (!all_topics_subscribed) {
+        all_topics_subscribed = true;
+        foreach(const PublisherMap::value_type& pub, publishers_) {
+            all_topics_subscribed &= pub.second.getNumSubscribers() > 0;
+        }
+        ros::Duration(0.1).sleep();
+    }
+    std::cout << "Finished waiting for subscribers." << std::endl;
+}
+
+void Player::doPublish(MessageInstance const& m) {
+    string const& topic   = m.getTopic();
+    ros::Time const& time = m.getTime();
+    string callerid       = m.getCallerId();
+    
+    ros::Time translated = time_translator_.translate(time);
+    ros::WallTime horizon = ros::WallTime(translated.sec, translated.nsec);
+
+    time_publisher_.setHorizon(time);
+    time_publisher_.setWCHorizon(horizon);
+
+    string callerid_topic = callerid + topic;
+
+    map<string, ros::Publisher>::iterator pub_iter = publishers_.find(callerid_topic);
+    ROS_ASSERT(pub_iter != publishers_.end());
+
+    // Update subscribers.
+    ros::spinOnce();
+
+    // If immediate specified, play immediately
+    if (options_.at_once) {
+        time_publisher_.stepClock();
+        pub_iter->second.publish(m);
+        printTime();
+        return;
+    }
+
+    // If skip_empty is specified, skip this region and shift.
+    if (time - time_publisher_.getTime() > options_.skip_empty)
+    {
+      time_publisher_.stepClock();
+
+      ros::WallDuration shift = ros::WallTime::now() - horizon ;
+      time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
+      horizon += shift;
+      time_publisher_.setWCHorizon(horizon);
+      (pub_iter->second).publish(m);
+      printTime();
+      return;
+    }
+
+    if (pause_for_topics_)
+    {
+        for (std::vector<std::string>::iterator i = options_.pause_topics.begin();
+             i != options_.pause_topics.end();
+             ++i)
+        {
+            if (topic == *i)
+            {
+                paused_ = true;
+                paused_time_ = ros::WallTime::now();
+            }
+        }
+    }
+
+    // Check if the rate control topic has posted recently enough to continue, or if a delay is needed.
+    // Delayed is separated from paused to allow more verbose printing.
+    if (rate_control_sub_ != NULL) {
+        if ((time_publisher_.getTime() - last_rate_control_).toSec() > options_.rate_control_max_delay) {
+            delayed_ = true;
+            paused_time_ = ros::WallTime::now();
+        }
+    }
+
+    while ((paused_ || delayed_ || !time_publisher_.horizonReached()) && node_handle_.ok())
+    {
+        bool charsleftorpaused = true;
+        while (charsleftorpaused && node_handle_.ok())
+        {
+            ros::spinOnce();
+
+            if (pause_change_requested_)
+            {
+              processPause(requested_pause_state_, horizon);
+              pause_change_requested_ = false;
+            }
+
+            switch (readCharFromStdin()){
+            case ' ':
+                processPause(!paused_, horizon);
+                break;
+            case 's':
+                if (paused_) {
+                    time_publisher_.stepClock();
+
+                    ros::WallDuration shift = ros::WallTime::now() - horizon ;
+                    paused_time_ = ros::WallTime::now();
+
+                    time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
+
+                    horizon += shift;
+                    time_publisher_.setWCHorizon(horizon);
+            
+                    (pub_iter->second).publish(m);
+
+                    printTime();
+                    return;
+                }
+                break;
+            case 't':
+                pause_for_topics_ = !pause_for_topics_;
+                break;
+            case EOF:
+                if (paused_)
+                {
+                    printTime();
+                    time_publisher_.runStalledClock(ros::WallDuration(.1));
+                    ros::spinOnce();
+                }
+                else if (delayed_)
+                {
+                    printTime();
+                    time_publisher_.runStalledClock(ros::WallDuration(.1));
+                    ros::spinOnce();
+                    // You need to check the rate here too.
+                    if(rate_control_sub_ == NULL || (time_publisher_.getTime() - last_rate_control_).toSec() <= options_.rate_control_max_delay) {
+                        delayed_ = false;
+                        // Make sure time doesn't shift after leaving delay.
+                        ros::WallDuration shift = ros::WallTime::now() - paused_time_;
+                        paused_time_ = ros::WallTime::now();
+         
+                        time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
+
+                        horizon += shift;
+                        time_publisher_.setWCHorizon(horizon);
+                    }
+                }
+                else
+                    charsleftorpaused = false;
+            }
+        }
+
+        printTime();
+        time_publisher_.runClock(ros::WallDuration(.1));
+        ros::spinOnce();
+    }
+
+    pub_iter->second.publish(m);
+}
+
+
+void Player::doKeepAlive() {
+    //Keep pushing ourself out in 10-sec increments (avoids fancy math dealing with the end of time)
+    ros::Time const& time = time_publisher_.getTime() + ros::Duration(10.0);
+
+    ros::Time translated = time_translator_.translate(time);
+    ros::WallTime horizon = ros::WallTime(translated.sec, translated.nsec);
+
+    time_publisher_.setHorizon(time);
+    time_publisher_.setWCHorizon(horizon);
+
+    if (options_.at_once) {
+        return;
+    }
+
+    // If we're done and just staying alive, don't watch the rate control topic. We aren't publishing anyway.
+    delayed_ = false;
+
+    while ((paused_ || !time_publisher_.horizonReached()) && node_handle_.ok())
+    {
+        bool charsleftorpaused = true;
+        while (charsleftorpaused && node_handle_.ok())
+        {
+            switch (readCharFromStdin()){
+            case ' ':
+                paused_ = !paused_;
+                if (paused_) {
+                    paused_time_ = ros::WallTime::now();
+                }
+                else
+                {
+                    // Make sure time doesn't shift after leaving pause.
+                    ros::WallDuration shift = ros::WallTime::now() - paused_time_;
+                    paused_time_ = ros::WallTime::now();
+         
+                    time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
+
+                    horizon += shift;
+                    time_publisher_.setWCHorizon(horizon);
+                }
+                break;
+            case EOF:
+                if (paused_)
+                {
+                    printTime();
+                    time_publisher_.runStalledClock(ros::WallDuration(.1));
+                    ros::spinOnce();
+                }
+                else
+                    charsleftorpaused = false;
+            }
+        }
+
+        printTime();
+        time_publisher_.runClock(ros::WallDuration(.1));
+        ros::spinOnce();
+    }
+}
+
+
+
+void Player::setupTerminal() {
+    if (terminal_modified_)
+        return;
+
+#if defined(_MSC_VER)
+    input_handle = GetStdHandle(STD_INPUT_HANDLE);
+    if (input_handle == INVALID_HANDLE_VALUE)
+    {
+        std::cout << "Failed to set up standard input handle." << std::endl;
+        return;
+    }
+    if (! GetConsoleMode(input_handle, &stdin_set) )
+    {
+        std::cout << "Failed to save the console mode." << std::endl;
+        return;
+    }
+    // don't actually need anything but the default, alternatively try this
+    //DWORD event_mode = ENABLE_WINDOW_INPUT | ENABLE_MOUSE_INPUT;
+    //if (! SetConsoleMode(input_handle, event_mode) )
+    //{
+    // std::cout << "Failed to set the console mode." << std::endl;
+    // return;
+    //}
+    terminal_modified_ = true;
+#else
+    const int fd = fileno(stdin);
+    termios flags;
+    tcgetattr(fd, &orig_flags_);
+    flags = orig_flags_;
+    flags.c_lflag &= ~ICANON;      // set raw (unset canonical modes)
+    flags.c_cc[VMIN]  = 0;         // i.e. min 1 char for blocking, 0 chars for non-blocking
+    flags.c_cc[VTIME] = 0;         // block if waiting for char
+    tcsetattr(fd, TCSANOW, &flags);
+
+    FD_ZERO(&stdin_fdset_);
+    FD_SET(fd, &stdin_fdset_);
+    maxfd_ = fd + 1;
+    terminal_modified_ = true;
+#endif
+}
+
+void Player::restoreTerminal() {
+	if (!terminal_modified_)
+		return;
+
+#if defined(_MSC_VER)
+    SetConsoleMode(input_handle, stdin_set);
+#else
+    const int fd = fileno(stdin);
+    tcsetattr(fd, TCSANOW, &orig_flags_);
+#endif
+    terminal_modified_ = false;
+}
+
+int Player::readCharFromStdin() {
+#ifdef __APPLE__
+    fd_set testfd;
+    FD_COPY(&stdin_fdset_, &testfd);
+#elif !defined(_MSC_VER)
+    fd_set testfd = stdin_fdset_;
+#endif
+
+#if defined(_MSC_VER)
+    DWORD events = 0;
+    INPUT_RECORD input_record[1];
+    DWORD input_size = 1;
+    BOOL b = GetNumberOfConsoleInputEvents(input_handle, &events);
+    if (b && events > 0)
+    {
+        b = ReadConsoleInput(input_handle, input_record, input_size, &events);
+        if (b)
+        {
+            for (unsigned int i = 0; i < events; ++i)
+            {
+                if (input_record[i].EventType & KEY_EVENT & input_record[i].Event.KeyEvent.bKeyDown)
+                {
+                    CHAR ch = input_record[i].Event.KeyEvent.uChar.AsciiChar;
+                    return ch;
+                }
+            }
+        }
+    }
+    return EOF;
+#else
+    timeval tv;
+    tv.tv_sec  = 0;
+    tv.tv_usec = 0;
+    if (select(maxfd_, &testfd, NULL, NULL, &tv) <= 0)
+        return EOF;
+    return getc(stdin);
+#endif
+}
+
+TimePublisher::TimePublisher() : time_scale_(1.0)
+{
+  setPublishFrequency(-1.0);
+  time_pub_ = node_handle_.advertise<rosgraph_msgs::Clock>("clock",1);
+}
+
+void TimePublisher::setPublishFrequency(double publish_frequency)
+{
+  publish_frequency_ = publish_frequency;
+  
+  do_publish_ = (publish_frequency > 0.0);
+
+  wall_step_.fromSec(1.0 / publish_frequency);
+}
+
+void TimePublisher::setTimeScale(double time_scale)
+{
+    time_scale_ = time_scale;
+}
+
+void TimePublisher::setHorizon(const ros::Time& horizon)
+{
+    horizon_ = horizon;
+}
+
+void TimePublisher::setWCHorizon(const ros::WallTime& horizon)
+{
+  wc_horizon_ = horizon;
+}
+
+void TimePublisher::setTime(const ros::Time& time)
+{
+    current_ = time;
+}
+
+ros::Time const& TimePublisher::getTime() const
+{
+    return current_;
+}
+
+void TimePublisher::runClock(const ros::WallDuration& duration)
+{
+    if (do_publish_)
+    {
+        rosgraph_msgs::Clock pub_msg;
+
+        ros::WallTime t = ros::WallTime::now();
+        ros::WallTime done = t + duration;
+
+        while (t < done && t < wc_horizon_)
+        {
+            ros::WallDuration leftHorizonWC = wc_horizon_ - t;
+
+            ros::Duration d(leftHorizonWC.sec, leftHorizonWC.nsec);
+            d *= time_scale_;
+
+            current_ = horizon_ - d;
+
+            if (current_ >= horizon_)
+              current_ = horizon_;
+
+            if (t >= next_pub_)
+            {
+                pub_msg.clock = current_;
+                time_pub_.publish(pub_msg);
+                next_pub_ = t + wall_step_;
+            }
+
+            ros::WallTime target = done;
+            if (target > wc_horizon_)
+              target = wc_horizon_;
+            if (target > next_pub_)
+              target = next_pub_;
+
+            ros::WallTime::sleepUntil(target);
+
+            t = ros::WallTime::now();
+        }
+    } else {
+
+        ros::WallTime t = ros::WallTime::now();
+
+        ros::WallDuration leftHorizonWC = wc_horizon_ - t;
+
+        ros::Duration d(leftHorizonWC.sec, leftHorizonWC.nsec);
+        d *= time_scale_;
+
+        current_ = horizon_ - d;
+        
+        if (current_ >= horizon_)
+            current_ = horizon_;
+
+        ros::WallTime target = ros::WallTime::now() + duration;
+
+        if (target > wc_horizon_)
+            target = wc_horizon_;
+
+        ros::WallTime::sleepUntil(target);
+    }
+}
+
+void TimePublisher::stepClock()
+{
+    if (do_publish_)
+    {
+        current_ = horizon_;
+
+        rosgraph_msgs::Clock pub_msg;
+
+        pub_msg.clock = current_;
+        time_pub_.publish(pub_msg);
+
+        ros::WallTime t = ros::WallTime::now();
+        next_pub_ = t + wall_step_;
+    } else {
+        current_ = horizon_;
+    }
+}
+
+void TimePublisher::runStalledClock(const ros::WallDuration& duration)
+{
+    if (do_publish_)
+    {
+        rosgraph_msgs::Clock pub_msg;
+
+        ros::WallTime t = ros::WallTime::now();
+        ros::WallTime done = t + duration;
+
+        while ( t < done )
+        {
+            if (t > next_pub_)
+            {
+                pub_msg.clock = current_;
+                time_pub_.publish(pub_msg);
+                next_pub_ = t + wall_step_;
+            }
+
+            ros::WallTime target = done;
+
+            if (target > next_pub_)
+              target = next_pub_;
+
+            ros::WallTime::sleepUntil(target);
+
+            t = ros::WallTime::now();
+        }
+    } else {
+        duration.sleep();
+    }
+}
+
+bool TimePublisher::horizonReached()
+{
+  return ros::WallTime::now() > wc_horizon_;
+}
+
+} // namespace rosbag

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

@@ -0,0 +1,298 @@
+/*********************************************************************
+* 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/recorder.h"
+#include "rosbag/exceptions.h"
+
+#include "boost/program_options.hpp"
+#include <string>
+#include <sstream>
+
+namespace po = boost::program_options;
+
+//! Parse the command-line arguments for recorder options
+rosbag::RecorderOptions parseOptions(int argc, char** argv) {
+    rosbag::RecorderOptions opts;
+
+    po::options_description desc("Allowed options");
+
+    desc.add_options()
+      ("help,h", "produce help message")
+      ("all,a", "record all topics")
+      ("regex,e", "match topics using regular expressions")
+      ("exclude,x", po::value<std::string>(), "exclude topics matching regular expressions")
+      ("quiet,q", "suppress console output")
+      ("output-prefix,o", po::value<std::string>(), "prepend PREFIX to beginning of bag name")
+      ("output-name,O", po::value<std::string>(), "record bagnamed NAME.bag")
+      ("buffsize,b", po::value<int>()->default_value(256), "Use an internal buffer of SIZE MB (Default: 256)")
+      ("chunksize", po::value<int>()->default_value(768), "Set chunk size of message data, in KB (Default: 768. Advanced)")
+      ("limit,l", po::value<int>()->default_value(0), "Only record NUM messages on each topic")
+      ("min-space,L", po::value<std::string>()->default_value("1G"), "Minimum allowed space on recording device (use G,M,k multipliers)")
+      ("bz2,j", "use BZ2 compression")
+      ("lz4", "use LZ4 compression")
+      ("split", po::value<int>()->implicit_value(0), "Split the bag file and continue recording when maximum size or maximum duration reached.")
+      ("max-splits", po::value<int>(), "Keep a maximum of N bag files, when reaching the maximum erase the oldest one to keep a constant number of files.")
+      ("topic", po::value< std::vector<std::string> >(), "topic to record")
+      ("size", po::value<uint64_t>(), "The maximum size of the bag to record in MB.")
+      ("duration", po::value<std::string>(), "Record a bag of maximum duration in seconds, unless 'm', or 'h' is appended.")
+      ("node", po::value<std::string>(), "Record all topics subscribed to by a specific node.")
+      ("tcpnodelay", "Use the TCP_NODELAY transport hint when subscribing to topics.")
+      ("udp", "Use the UDP transport hint when subscribing to topics.");
+
+  
+    po::positional_options_description p;
+    p.add("topic", -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("all"))
+      opts.record_all = true;
+    if (vm.count("regex"))
+      opts.regex = true;
+    if (vm.count("exclude"))
+    {
+      opts.do_exclude = true;
+      opts.exclude_regex = vm["exclude"].as<std::string>();
+    }
+    if (vm.count("quiet"))
+      opts.quiet = true;
+    if (vm.count("output-prefix"))
+    {
+      opts.prefix = vm["output-prefix"].as<std::string>();
+      opts.append_date = true;
+    }
+    if (vm.count("output-name"))
+    {
+      opts.prefix = vm["output-name"].as<std::string>();
+      opts.append_date = false;
+    }
+    if (vm.count("split"))
+    {
+      opts.split = true;
+
+      int S = vm["split"].as<int>();
+      if (S != 0)
+      {
+        ROS_WARN("Use of \"--split <MAX_SIZE>\" has been deprecated.  Please use --split --size <MAX_SIZE> or --split --duration <MAX_DURATION>");
+        if (S < 0)
+          throw ros::Exception("Split size must be 0 or positive");
+        opts.max_size = 1048576 * S;
+      }
+    }
+    if(vm.count("max-splits"))
+    {
+        if(!opts.split)
+        {
+            ROS_WARN("--max-splits is ignored without --split");
+        }
+        else
+        {
+            opts.max_splits = vm["max-splits"].as<int>();
+        }
+    }
+    if (vm.count("buffsize"))
+    {
+      int m = vm["buffsize"].as<int>();
+      if (m < 0)
+        throw ros::Exception("Buffer size must be 0 or positive");
+      opts.buffer_size = 1048576 * m;
+    }
+    if (vm.count("chunksize"))
+    {
+      int chnk_sz = vm["chunksize"].as<int>();
+      if (chnk_sz < 0)
+        throw ros::Exception("Chunk size must be 0 or positive");
+      opts.chunk_size = 1024 * chnk_sz;
+    }
+    if (vm.count("limit"))
+    {
+      opts.limit = vm["limit"].as<int>();
+    }
+    if (vm.count("min-space"))
+    {
+        std::string ms = vm["min-space"].as<std::string>();
+        long long int value = 1073741824ull;
+        char mul = 0;
+        // Sane default values, just in case
+        opts.min_space_str = "1G";
+        opts.min_space = value;
+        if (sscanf(ms.c_str(), " %lld%c", &value, &mul) > 0) {
+            opts.min_space_str = ms;
+            switch (mul) {
+                case 'G':
+                case 'g':
+                    opts.min_space = value * 1073741824ull;
+                    break;
+                case 'M':
+                case 'm':
+                    opts.min_space = value * 1048576ull;
+                    break;
+                case 'K':
+                case 'k':
+                    opts.min_space = value * 1024ull;
+                    break;
+                default:
+                    opts.min_space = value;
+                    break;
+            }
+        }
+        ROS_DEBUG("Rosbag using minimum space of %lld bytes, or %s", opts.min_space, opts.min_space_str.c_str());
+    }
+    if (vm.count("bz2") && vm.count("lz4"))
+    {
+      throw ros::Exception("Can only use one type of compression");
+    }
+    if (vm.count("bz2"))
+    {
+      opts.compression = rosbag::compression::BZ2;
+    }
+    if (vm.count("lz4"))
+    {
+      opts.compression = rosbag::compression::LZ4;
+    }
+    if (vm.count("duration"))
+    {
+      std::string duration_str = vm["duration"].as<std::string>();
+
+      double duration;
+      double multiplier = 1.0;
+      std::string unit("");
+
+      std::istringstream iss(duration_str);
+      if ((iss >> duration).fail())
+        throw ros::Exception("Duration must start with a floating point number.");
+
+      if ( (!iss.eof() && ((iss >> unit).fail())) )
+      {
+        throw ros::Exception("Duration unit must be s, m, or h");
+      }
+      if (unit == std::string(""))
+        multiplier = 1.0;
+      else if (unit == std::string("s"))
+        multiplier = 1.0;
+      else if (unit == std::string("m"))
+        multiplier = 60.0;
+      else if (unit == std::string("h"))
+        multiplier = 3600.0;
+      else
+        throw ros::Exception("Duration unit must be s, m, or h");
+
+      
+      opts.max_duration = ros::Duration(duration * multiplier);
+      if (opts.max_duration <= ros::Duration(0))
+        throw ros::Exception("Duration must be positive.");
+    }
+    if (vm.count("size"))
+    {
+      opts.max_size = vm["size"].as<uint64_t>() * 1048576;
+      if (opts.max_size <= 0)
+        throw ros::Exception("Split size must be 0 or positive");
+    }
+    if (vm.count("node"))
+    {
+      opts.node = vm["node"].as<std::string>();
+      std::cout << "Recording from: " << opts.node << std::endl;
+    }
+    if (vm.count("tcpnodelay"))
+    {
+      opts.transport_hints.tcpNoDelay();
+    }
+    if (vm.count("udp"))
+    {
+      opts.transport_hints.udp();
+    }
+
+    // Every non-option argument is assumed to be a topic
+    if (vm.count("topic"))
+    {
+      std::vector<std::string> bags = vm["topic"].as< std::vector<std::string> >();
+      std::sort(bags.begin(), bags.end());
+      bags.erase(std::unique(bags.begin(), bags.end()), bags.end());
+      for (std::vector<std::string>::iterator i = bags.begin();
+           i != bags.end();
+           i++)
+        opts.topics.push_back(*i);
+    }
+
+
+    // check that argument combinations make sense
+    if(opts.exclude_regex.size() > 0 &&
+            !(opts.record_all || opts.regex)) {
+        fprintf(stderr, "Warning: Exclusion regex given, but no topics to subscribe to.\n"
+                "Adding implicit 'record all'.");
+        opts.record_all = true;
+    }
+
+    return opts;
+}
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "record", ros::init_options::AnonymousName);
+
+    // Parse the command-line options
+    rosbag::RecorderOptions opts;
+    try {
+        opts = parseOptions(argc, argv);
+    }
+    catch (ros::Exception const& ex) {
+        ROS_ERROR("Error reading options: %s", ex.what());
+        return 1;
+    }
+    catch(boost::regex_error const& ex) {
+        ROS_ERROR("Error reading options: %s\n", ex.what());
+        return 1;
+    }
+
+    // Run the recorder
+    rosbag::Recorder recorder(opts);
+    int result = recorder.run();
+    
+    return result;
+}

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

@@ -0,0 +1,706 @@
+/*********************************************************************
+* 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/recorder.h"
+
+#include <sys/stat.h>
+#include <boost/filesystem.hpp>
+// Boost filesystem v3 is default in 1.46.0 and above
+// Fallback to original posix code (*nix only) if this is not true
+#if BOOST_FILESYSTEM_VERSION < 3
+  #include <sys/statvfs.h>
+#endif
+#include <time.h>
+
+#include <queue>
+#include <set>
+#include <sstream>
+#include <string>
+
+#include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
+#include <boost/regex.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/xtime.hpp>
+#include <boost/date_time/local_time/local_time.hpp>
+
+#include <ros/ros.h>
+#include <topic_tools/shape_shifter.h>
+
+#include "ros/network.h"
+#include "ros/xmlrpc_manager.h"
+#include "xmlrpcpp/XmlRpc.h"
+
+#define foreach BOOST_FOREACH
+
+using std::cout;
+using std::endl;
+using std::set;
+using std::string;
+using std::vector;
+using boost::shared_ptr;
+using ros::Time;
+
+namespace rosbag {
+
+// OutgoingMessage
+
+OutgoingMessage::OutgoingMessage(string const& _topic, topic_tools::ShapeShifter::ConstPtr _msg, boost::shared_ptr<ros::M_string> _connection_header, Time _time) :
+    topic(_topic), msg(_msg), connection_header(_connection_header), time(_time)
+{
+}
+
+// OutgoingQueue
+
+OutgoingQueue::OutgoingQueue(string const& _filename, std::queue<OutgoingMessage>* _queue, Time _time) :
+    filename(_filename), queue(_queue), time(_time)
+{
+}
+
+// RecorderOptions
+
+RecorderOptions::RecorderOptions() :
+    trigger(false),
+    record_all(false),
+    regex(false),
+    do_exclude(false),
+    quiet(false),
+    append_date(true),
+    snapshot(false),
+    verbose(false),
+    compression(compression::Uncompressed),
+    prefix(""),
+    name(""),
+    exclude_regex(),
+    buffer_size(1048576 * 256),
+    chunk_size(1024 * 768),
+    limit(0),
+    split(false),
+    max_size(0),
+    max_splits(0),
+    max_duration(-1.0),
+    node(""),
+    min_space(1024 * 1024 * 1024),
+    min_space_str("1G")
+{
+}
+
+// Recorder
+
+Recorder::Recorder(RecorderOptions const& options) :
+    options_(options),
+    num_subscribers_(0),
+    exit_code_(0),
+    queue_size_(0),
+    split_count_(0),
+    writing_enabled_(true)
+{
+}
+
+int Recorder::run() {
+    if (options_.trigger) {
+        doTrigger();
+        return 0;
+    }
+
+    if (options_.topics.size() == 0) {
+        // Make sure limit is not specified with automatic topic subscription
+        if (options_.limit > 0) {
+            fprintf(stderr, "Specifing a count is not valid with automatic topic subscription.\n");
+            return 1;
+        }
+
+        // Make sure topics are specified
+        if (!options_.record_all && (options_.node == std::string(""))) {
+            fprintf(stderr, "No topics specified.\n");
+            return 1;
+        }
+    }
+
+    ros::NodeHandle nh;
+    if (!nh.ok())
+        return 0;
+
+    last_buffer_warn_ = Time();
+    queue_ = new std::queue<OutgoingMessage>;
+
+    // Subscribe to each topic
+    if (!options_.regex) {
+    	foreach(string const& topic, options_.topics)
+			subscribe(topic);
+    }
+
+    if (!ros::Time::waitForValid(ros::WallDuration(2.0)))
+      ROS_WARN("/use_sim_time set to true and no clock published.  Still waiting for valid time...");
+
+    ros::Time::waitForValid();
+
+    start_time_ = ros::Time::now();
+
+    // Don't bother doing anything if we never got a valid time
+    if (!nh.ok())
+        return 0;
+
+    ros::Subscriber trigger_sub;
+
+    // Spin up a thread for writing to the file
+    boost::thread record_thread;
+    if (options_.snapshot)
+    {
+        record_thread = boost::thread(boost::bind(&Recorder::doRecordSnapshotter, this));
+
+        // Subscribe to the snapshot trigger
+        trigger_sub = nh.subscribe<std_msgs::Empty>("snapshot_trigger", 100, boost::bind(&Recorder::snapshotTrigger, this, _1));
+    }
+    else
+        record_thread = boost::thread(boost::bind(&Recorder::doRecord, this));
+
+
+
+    ros::Timer check_master_timer;
+    if (options_.record_all || options_.regex || (options_.node != std::string("")))
+    {
+        // check for master first
+        doCheckMaster(ros::TimerEvent(), nh);
+        check_master_timer = nh.createTimer(ros::Duration(1.0), boost::bind(&Recorder::doCheckMaster, this, _1, boost::ref(nh)));
+    }
+
+    ros::MultiThreadedSpinner s(10);
+    ros::spin(s);
+
+    queue_condition_.notify_all();
+
+    record_thread.join();
+
+    delete queue_;
+
+    return exit_code_;
+}
+
+shared_ptr<ros::Subscriber> Recorder::subscribe(string const& topic) {
+    ROS_INFO("Subscribing to %s", topic.c_str());
+
+    ros::NodeHandle nh;
+    shared_ptr<int> count(boost::make_shared<int>(options_.limit));
+    shared_ptr<ros::Subscriber> sub(boost::make_shared<ros::Subscriber>());
+
+    ros::SubscribeOptions ops;
+    ops.topic = topic;
+    ops.queue_size = 100;
+    ops.md5sum = ros::message_traits::md5sum<topic_tools::ShapeShifter>();
+    ops.datatype = ros::message_traits::datatype<topic_tools::ShapeShifter>();
+    ops.helper = boost::make_shared<ros::SubscriptionCallbackHelperT<
+        const ros::MessageEvent<topic_tools::ShapeShifter const> &> >(
+            boost::bind(&Recorder::doQueue, this, _1, topic, sub, count));
+    ops.transport_hints = options_.transport_hints;
+    *sub = nh.subscribe(ops);
+
+    currently_recording_.insert(topic);
+    num_subscribers_++;
+
+    return sub;
+}
+
+bool Recorder::isSubscribed(string const& topic) const {
+    return currently_recording_.find(topic) != currently_recording_.end();
+}
+
+bool Recorder::shouldSubscribeToTopic(std::string const& topic, bool from_node) {
+    // ignore already known topics
+    if (isSubscribed(topic)) {
+        return false;
+    }
+
+    // subtract exclusion regex, if any
+    if(options_.do_exclude && boost::regex_match(topic, options_.exclude_regex)) {
+        return false;
+    }
+
+    if(options_.record_all || from_node) {
+        return true;
+    }
+    
+    if (options_.regex) {
+        // Treat the topics as regular expressions
+        foreach(string const& regex_str, options_.topics) {
+            boost::regex e(regex_str);
+            boost::smatch what;
+            if (boost::regex_match(topic, what, e, boost::match_extra))
+                return true;
+        }
+    }
+    else {
+        foreach(string const& t, options_.topics)
+            if (t == topic)
+                return true;
+    }
+    
+    return false;
+}
+
+template<class T>
+std::string Recorder::timeToStr(T ros_t)
+{
+    (void)ros_t;
+    std::stringstream msg;
+    const boost::posix_time::ptime now=
+        boost::posix_time::second_clock::local_time();
+    boost::posix_time::time_facet *const f=
+        new boost::posix_time::time_facet("%Y-%m-%d-%H-%M-%S");
+    msg.imbue(std::locale(msg.getloc(),f));
+    msg << now;
+    return msg.str();
+}
+
+//! Callback to be invoked to save messages into a queue
+void Recorder::doQueue(const ros::MessageEvent<topic_tools::ShapeShifter const>& msg_event, string const& topic, shared_ptr<ros::Subscriber> subscriber, shared_ptr<int> count) {
+    //void Recorder::doQueue(topic_tools::ShapeShifter::ConstPtr msg, string const& topic, shared_ptr<ros::Subscriber> subscriber, shared_ptr<int> count) {
+    Time rectime = Time::now();
+    
+    if (options_.verbose)
+        cout << "Received message on topic " << subscriber->getTopic() << endl;
+
+    OutgoingMessage out(topic, msg_event.getMessage(), msg_event.getConnectionHeaderPtr(), rectime);
+    
+    {
+        boost::mutex::scoped_lock lock(queue_mutex_);
+
+        queue_->push(out);
+        queue_size_ += out.msg->size();
+        
+        // Check to see if buffer has been exceeded
+        while (options_.buffer_size > 0 && queue_size_ > options_.buffer_size) {
+            OutgoingMessage drop = queue_->front();
+            queue_->pop();
+            queue_size_ -= drop.msg->size();
+
+            if (!options_.snapshot) {
+                Time now = Time::now();
+                if (now > last_buffer_warn_ + ros::Duration(5.0)) {
+                    ROS_WARN("rosbag record buffer exceeded.  Dropping oldest queued message.");
+                    last_buffer_warn_ = now;
+                }
+            }
+        }
+    }
+  
+    if (!options_.snapshot)
+        queue_condition_.notify_all();
+
+    // If we are book-keeping count, decrement and possibly shutdown
+    if ((*count) > 0) {
+        (*count)--;
+        if ((*count) == 0) {
+            subscriber->shutdown();
+
+            num_subscribers_--;
+
+            if (num_subscribers_ == 0)
+                ros::shutdown();
+        }
+    }
+}
+
+void Recorder::updateFilenames() {
+    vector<string> parts;
+
+    std::string prefix = options_.prefix;
+    size_t ind = prefix.rfind(".bag");
+
+    if (ind != std::string::npos && ind == prefix.size() - 4)
+    {
+      prefix.erase(ind);
+    }
+
+    if (prefix.length() > 0)
+        parts.push_back(prefix);
+    if (options_.append_date)
+        parts.push_back(timeToStr(ros::WallTime::now()));
+    if (options_.split)
+        parts.push_back(boost::lexical_cast<string>(split_count_));
+
+    if (parts.size() == 0)
+    {
+      throw BagException("Bag filename is empty (neither of these was specified: prefix, append_date, split)");
+    }
+
+    target_filename_ = parts[0];
+    for (unsigned int i = 1; i < parts.size(); i++)
+        target_filename_ += string("_") + parts[i];
+
+    target_filename_ += string(".bag");
+    write_filename_ = target_filename_ + string(".active");
+}
+
+//! Callback to be invoked to actually do the recording
+void Recorder::snapshotTrigger(std_msgs::Empty::ConstPtr trigger) {
+    (void)trigger;
+    updateFilenames();
+    
+    ROS_INFO("Triggered snapshot recording with name %s.", target_filename_.c_str());
+    
+    {
+        boost::mutex::scoped_lock lock(queue_mutex_);
+        queue_queue_.push(OutgoingQueue(target_filename_, queue_, Time::now()));
+        queue_      = new std::queue<OutgoingMessage>;
+        queue_size_ = 0;
+    }
+
+    queue_condition_.notify_all();
+}
+
+void Recorder::startWriting() {
+    bag_.setCompression(options_.compression);
+    bag_.setChunkThreshold(options_.chunk_size);
+
+    updateFilenames();
+    try {
+        bag_.open(write_filename_, bagmode::Write);
+    }
+    catch (rosbag::BagException e) {
+        ROS_ERROR("Error writing: %s", e.what());
+        exit_code_ = 1;
+        ros::shutdown();
+    }
+    ROS_INFO("Recording to %s.", target_filename_.c_str());
+}
+
+void Recorder::stopWriting() {
+    ROS_INFO("Closing %s.", target_filename_.c_str());
+    bag_.close();
+    rename(write_filename_.c_str(), target_filename_.c_str());
+}
+
+void Recorder::checkNumSplits()
+{
+    if(options_.max_splits>0)
+    {
+        current_files_.push_back(target_filename_);
+        if(current_files_.size()>options_.max_splits)
+        {
+            int err = unlink(current_files_.front().c_str());
+            if(err != 0)
+            {
+                ROS_ERROR("Unable to remove %s: %s", current_files_.front().c_str(), strerror(errno));
+            }
+            current_files_.pop_front();
+        }
+    }
+}
+
+bool Recorder::checkSize()
+{
+    if (options_.max_size > 0)
+    {
+        if (bag_.getSize() > options_.max_size)
+        {
+            if (options_.split)
+            {
+                stopWriting();
+                split_count_++;
+                checkNumSplits();
+                startWriting();
+            } else {
+                ros::shutdown();
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+bool Recorder::checkDuration(const ros::Time& t)
+{
+    if (options_.max_duration > ros::Duration(0))
+    {
+        if (t - start_time_ > options_.max_duration)
+        {
+            if (options_.split)
+            {
+                while (start_time_ + options_.max_duration < t)
+                {
+                    stopWriting();
+                    split_count_++;
+                    checkNumSplits();
+                    start_time_ += options_.max_duration;
+                    startWriting();
+                }
+            } else {
+                ros::shutdown();
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+
+//! Thread that actually does writing to file.
+void Recorder::doRecord() {
+    // Open bag file for writing
+    startWriting();
+
+    // Schedule the disk space check
+    warn_next_ = ros::WallTime();
+    checkDisk();
+    check_disk_next_ = ros::WallTime::now() + ros::WallDuration().fromSec(20.0);
+
+    // Technically the queue_mutex_ should be locked while checking empty.
+    // Except it should only get checked if the node is not ok, and thus
+    // it shouldn't be in contention.
+    ros::NodeHandle nh;
+    while (nh.ok() || !queue_->empty()) {
+        boost::unique_lock<boost::mutex> lock(queue_mutex_);
+
+        bool finished = false;
+        while (queue_->empty()) {
+            if (!nh.ok()) {
+                lock.release()->unlock();
+                finished = true;
+                break;
+            }
+            boost::xtime xt;
+#if BOOST_VERSION >= 105000
+            boost::xtime_get(&xt, boost::TIME_UTC_);
+#else
+            boost::xtime_get(&xt, boost::TIME_UTC);
+#endif
+            xt.nsec += 250000000;
+            queue_condition_.timed_wait(lock, xt);
+            if (checkDuration(ros::Time::now()))
+            {
+                finished = true;
+                break;
+            }
+        }
+        if (finished)
+            break;
+
+        OutgoingMessage out = queue_->front();
+        queue_->pop();
+        queue_size_ -= out.msg->size();
+        
+        lock.release()->unlock();
+        
+        if (checkSize())
+            break;
+
+        if (checkDuration(out.time))
+            break;
+
+        if (scheduledCheckDisk() && checkLogging())
+            bag_.write(out.topic, out.time, *out.msg, out.connection_header);
+    }
+
+    stopWriting();
+}
+
+void Recorder::doRecordSnapshotter() {
+    ros::NodeHandle nh;
+  
+    while (nh.ok() || !queue_queue_.empty()) {
+        boost::unique_lock<boost::mutex> lock(queue_mutex_);
+        while (queue_queue_.empty()) {
+            if (!nh.ok())
+                return;
+            queue_condition_.wait(lock);
+        }
+        
+        OutgoingQueue out_queue = queue_queue_.front();
+        queue_queue_.pop();
+        
+        lock.release()->unlock();
+        
+        string target_filename = out_queue.filename;
+        string write_filename  = target_filename + string(".active");
+        
+        try {
+            bag_.open(write_filename, bagmode::Write);
+        }
+        catch (rosbag::BagException ex) {
+            ROS_ERROR("Error writing: %s", ex.what());
+            return;
+        }
+
+        while (!out_queue.queue->empty()) {
+            OutgoingMessage out = out_queue.queue->front();
+            out_queue.queue->pop();
+
+            bag_.write(out.topic, out.time, *out.msg);
+        }
+
+        stopWriting();
+    }
+}
+
+void Recorder::doCheckMaster(ros::TimerEvent const& e, ros::NodeHandle& node_handle) {
+    (void)e;
+    (void)node_handle;
+    ros::master::V_TopicInfo topics;
+    if (ros::master::getTopics(topics)) {
+		foreach(ros::master::TopicInfo const& t, topics) {
+			if (shouldSubscribeToTopic(t.name))
+				subscribe(t.name);
+		}
+    }
+    
+    if (options_.node != std::string(""))
+    {
+
+      XmlRpc::XmlRpcValue req;
+      req[0] = ros::this_node::getName();
+      req[1] = options_.node;
+      XmlRpc::XmlRpcValue resp;
+      XmlRpc::XmlRpcValue payload;
+
+      if (ros::master::execute("lookupNode", req, resp, payload, true))
+      {
+        std::string peer_host;
+        uint32_t peer_port;
+
+        if (!ros::network::splitURI(static_cast<std::string>(resp[2]), peer_host, peer_port))
+        {
+          ROS_ERROR("Bad xml-rpc URI trying to inspect node at: [%s]", static_cast<std::string>(resp[2]).c_str());
+        } else {
+
+          XmlRpc::XmlRpcClient c(peer_host.c_str(), peer_port, "/");
+          XmlRpc::XmlRpcValue req2;
+          XmlRpc::XmlRpcValue resp2;
+          req2[0] = ros::this_node::getName();
+          c.execute("getSubscriptions", req2, resp2);
+          
+          if (!c.isFault() && resp2.valid() && resp2.size() > 0 && static_cast<int>(resp2[0]) == 1)
+          {
+            for(int i = 0; i < resp2[2].size(); i++)
+            {
+              if (shouldSubscribeToTopic(resp2[2][i][0], true))
+                subscribe(resp2[2][i][0]);
+            }
+          } else {
+            ROS_ERROR("Node at: [%s] failed to return subscriptions.", static_cast<std::string>(resp[2]).c_str());
+          }
+        }
+      }
+    }
+}
+
+void Recorder::doTrigger() {
+    ros::NodeHandle nh;
+    ros::Publisher pub = nh.advertise<std_msgs::Empty>("snapshot_trigger", 1, true);
+    pub.publish(std_msgs::Empty());
+
+    ros::Timer terminate_timer = nh.createTimer(ros::Duration(1.0), boost::bind(&ros::shutdown));
+    ros::spin();
+}
+
+bool Recorder::scheduledCheckDisk() {
+    boost::mutex::scoped_lock lock(check_disk_mutex_);
+
+    if (ros::WallTime::now() < check_disk_next_)
+        return true;
+
+    check_disk_next_ += ros::WallDuration().fromSec(20.0);
+    return checkDisk();
+}
+
+bool Recorder::checkDisk() {
+#if BOOST_FILESYSTEM_VERSION < 3
+    struct statvfs fiData;
+    if ((statvfs(bag_.getFileName().c_str(), &fiData)) < 0)
+    {
+        ROS_WARN("Failed to check filesystem stats.");
+        return true;
+    }
+    unsigned long long free_space = 0;
+    free_space = (unsigned long long) (fiData.f_bsize) * (unsigned long long) (fiData.f_bavail);
+    if (free_space < options_.min_space)
+    {
+        ROS_ERROR("Less than %s of space free on disk with %s.  Disabling recording.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
+        writing_enabled_ = false;
+        return false;
+    }
+    else if (free_space < 5 * options_.min_space)
+    {
+        ROS_WARN("Less than 5 x %s of space free on disk with %s.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
+    }
+    else
+    {
+        writing_enabled_ = true;
+    }
+#else
+    boost::filesystem::path p(boost::filesystem::system_complete(bag_.getFileName().c_str()));
+    p = p.parent_path();
+    boost::filesystem::space_info info;
+    try
+    {
+        info = boost::filesystem::space(p);
+    }
+    catch (boost::filesystem::filesystem_error &e) 
+    { 
+        ROS_WARN("Failed to check filesystem stats [%s].", e.what());
+        writing_enabled_ = false;
+        return false;
+    }
+    if ( info.available < options_.min_space)
+    {
+        ROS_ERROR("Less than %s of space free on disk with %s.  Disabling recording.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
+        writing_enabled_ = false;
+        return false;
+    }
+    else if (info.available < 5 * options_.min_space)
+    {
+        ROS_WARN("Less than 5 x %s of space free on disk with %s.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
+        writing_enabled_ = true;
+    }
+    else
+    {
+        writing_enabled_ = true;
+    }
+#endif
+    return true;
+}
+
+bool Recorder::checkLogging() {
+    if (writing_enabled_)
+        return true;
+
+    ros::WallTime now = ros::WallTime::now();
+    if (now >= warn_next_) {
+        warn_next_ = now + ros::WallDuration().fromSec(5.0);
+        ROS_WARN("Not logging message because logging disabled.  Most likely cause is a full disk.");
+    }
+    return false;
+}
+
+} // namespace rosbag

thirdparty/ros/ros_comm/tools/rosbag/src/rosbag/__init__.py

@@ -0,0 +1,37 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, 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.
+
+from .bag import Bag, Compression, ROSBagException, ROSBagFormatException, ROSBagUnindexedException
+
+# Import rosbag main to be used by the rosbag executable
+from .rosbag_main import rosbagmain
+

thirdparty/ros/ros_comm/tools/rosbag/src/rosbag/rosbag_main.py

@@ -0,0 +1,900 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010, 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.
+
+from __future__ import print_function
+
+import optparse
+import os
+import shutil
+import signal
+import subprocess
+import sys
+import time
+try:
+    from UserDict import UserDict  # Python 2.x
+except ImportError:
+    from collections import UserDict  # Python 3.x
+
+import roslib.message
+import roslib.packages
+
+from .bag import Bag, Compression, ROSBagException, ROSBagFormatException, ROSBagUnindexedException
+from .migration import MessageMigrator, fixbag2, checkbag
+
+def print_trans(old, new, indent):
+    from_txt = '%s [%s]' % (old._type, old._md5sum)
+    if new is not None:
+        to_txt= '%s [%s]' % (new._type, new._md5sum)
+    else:
+        to_txt = 'Unknown'
+    print('    ' * indent + ' * From: %s' % from_txt)
+    print('    ' * indent + '   To:   %s' % to_txt)
+
+def handle_split(option, opt_str, value, parser):
+    parser.values.split = True
+    if len(parser.rargs) > 0 and parser.rargs[0].isdigit():
+        print("Use of \"--split <MAX_SIZE>\" has been deprecated.  Please use --split --size <MAX_SIZE> or --split --duration <MAX_DURATION>", file=sys.stderr)
+        parser.values.size = int(parser.rargs.pop(0))
+
+
+def _stop_process(signum, frame, old_handler, process):
+    process.terminate()
+    if old_handler:
+        old_handler(signum, frame)
+
+
+def record_cmd(argv):
+    parser = optparse.OptionParser(usage="rosbag record TOPIC1 [TOPIC2 TOPIC3 ...]",
+                                   description="Record a bag file with the contents of specified topics.",
+                                   formatter=optparse.IndentedHelpFormatter())
+
+    parser.add_option("-a", "--all",           dest="all",           default=False, action="store_true",          help="record all topics")
+    parser.add_option("-e", "--regex",         dest="regex",         default=False, action="store_true",          help="match topics using regular expressions")
+    parser.add_option("-x", "--exclude",       dest="exclude_regex", default="",    action="store",               help="exclude topics matching the follow regular expression (subtracts from -a or regex)")
+    parser.add_option("-q", "--quiet",         dest="quiet",         default=False, action="store_true",          help="suppress console output")
+    parser.add_option("-o", "--output-prefix", dest="prefix",        default=None,  action="store",               help="prepend PREFIX to beginning of bag name (name will always end with date stamp)")
+    parser.add_option("-O", "--output-name",   dest="name",          default=None,  action="store",               help="record to bag with name NAME.bag")
+    parser.add_option(      "--split",         dest="split",         default=False, callback=handle_split, action="callback",    help="split the bag when maximum size or duration is reached")
+    parser.add_option(      "--max-splits",    dest="max_splits",                   type='int',   action="store", help="Keep a maximum of N bag files, when reaching the maximum erase the oldest one to keep a constant number of files.", metavar="MAX_SPLITS")
+    parser.add_option(      "--size",          dest="size",                         type='int',   action="store", help="record a bag of maximum size SIZE MB. (Default: infinite)", metavar="SIZE")
+    parser.add_option(      "--duration",      dest="duration",                     type='string',action="store", help="record a bag of maximum duration DURATION in seconds, unless 'm', or 'h' is appended.", metavar="DURATION")
+    parser.add_option("-b", "--buffsize",      dest="buffsize",      default=256,   type='int',   action="store", help="use an internal buffer of SIZE MB (Default: %default, 0 = infinite)", metavar="SIZE")
+    parser.add_option("--chunksize",           dest="chunksize",     default=768,   type='int',   action="store", help="Advanced. Record to chunks of SIZE KB (Default: %default)", metavar="SIZE")
+    parser.add_option("-l", "--limit",         dest="num",           default=0,     type='int',   action="store", help="only record NUM messages on each topic")
+    parser.add_option(      "--node",          dest="node",          default=None,  type='string',action="store", help="record all topics subscribed to by a specific node")
+    parser.add_option("-j", "--bz2",           dest="compression",   default=None,  action="store_const", const='bz2', help="use BZ2 compression")
+    parser.add_option("--lz4",                 dest="compression",                  action="store_const", const='lz4', help="use LZ4 compression")
+    parser.add_option("--tcpnodelay",          dest="tcpnodelay",                   action="store_true",          help="Use the TCP_NODELAY transport hint when subscribing to topics.")
+    parser.add_option("--udp",                 dest="udp",                          action="store_true",          help="Use the UDP transport hint when subscribing to topics.")
+
+    (options, args) = parser.parse_args(argv)
+
+    if len(args) == 0 and not options.all and not options.node:
+        parser.error("You must specify a topic name or else use the '-a' option.")
+
+    if options.prefix is not None and options.name is not None:
+        parser.error("Can't set both prefix and name.")
+
+    recordpath = roslib.packages.find_node('rosbag', 'record')
+    if not recordpath:
+        parser.error("Cannot find rosbag/record executable")
+    cmd = [recordpath[0]]
+
+    cmd.extend(['--buffsize',  str(options.buffsize)])
+    cmd.extend(['--chunksize', str(options.chunksize)])
+
+    if options.num != 0:      cmd.extend(['--limit', str(options.num)])
+    if options.quiet:         cmd.extend(["--quiet"])
+    if options.prefix:        cmd.extend(["-o", options.prefix])
+    if options.name:          cmd.extend(["-O", options.name])
+    if options.exclude_regex: cmd.extend(["--exclude", options.exclude_regex])
+    if options.all:           cmd.extend(["--all"])
+    if options.regex:         cmd.extend(["--regex"])
+    if options.compression:   cmd.extend(["--%s" % options.compression])
+    if options.split:
+        if not options.duration and not options.size:
+            parser.error("Split specified without giving a maximum duration or size")
+        cmd.extend(["--split"])
+        if options.max_splits:
+            cmd.extend(["--max-splits", str(options.max_splits)])
+    if options.duration:    cmd.extend(["--duration", options.duration])
+    if options.size:        cmd.extend(["--size", str(options.size)])
+    if options.node:
+        cmd.extend(["--node", options.node])
+    if options.tcpnodelay:  cmd.extend(["--tcpnodelay"])
+    if options.udp:         cmd.extend(["--udp"])
+
+    cmd.extend(args)
+
+    old_handler = signal.signal(
+        signal.SIGTERM,
+        lambda signum, frame: _stop_process(signum, frame, old_handler, process)
+    )
+    # Better way of handling it than os.execv
+    # This makes sure stdin handles are passed to the process.
+    process = subprocess.Popen(cmd)
+    process.wait()
+
+
+def info_cmd(argv):
+    parser = optparse.OptionParser(usage='rosbag info [options] BAGFILE1 [BAGFILE2 BAGFILE3 ...]',
+                                   description='Summarize the contents of one or more bag files.')
+    parser.add_option('-y', '--yaml', dest='yaml', default=False, action='store_true', help='print information in YAML format')
+    parser.add_option('-k', '--key',  dest='key',  default=None,  action='store',      help='print information on the given key')
+    parser.add_option(      '--freq', dest='freq', default=False, action='store_true', help='display topic message frequency statistics')
+    (options, args) = parser.parse_args(argv)
+
+    if len(args) == 0:
+        parser.error('You must specify at least 1 bag file.')
+    if options.key and not options.yaml:
+        parser.error('You can only specify key when printing in YAML format.')
+
+    for i, arg in enumerate(args):
+        try:
+            b = Bag(arg, 'r', skip_index=not options.freq)
+            if options.yaml:
+                info = b._get_yaml_info(key=options.key)
+                if info is not None:
+                    print(info)
+            else:
+                print(b)
+            b.close()
+            if i < len(args) - 1:
+                print('---')
+        
+        except ROSBagUnindexedException as ex:
+            print('ERROR bag unindexed: %s.  Run rosbag reindex.' % arg,
+                  file=sys.stderr)
+            sys.exit(1)
+        except ROSBagException as ex:
+            print('ERROR reading %s: %s' % (arg, str(ex)), file=sys.stderr)
+            sys.exit(1)
+        except IOError as ex:
+            print('ERROR reading %s: %s' % (arg, str(ex)), file=sys.stderr)
+            sys.exit(1)
+
+
+def handle_topics(option, opt_str, value, parser):
+    topics = []
+    for arg in parser.rargs:
+        if arg[:2] == "--" and len(arg) > 2:
+            break
+        if arg[:1] == "-" and len(arg) > 1:
+            break
+        topics.append(arg)
+    parser.values.topics.extend(topics)
+    del parser.rargs[:len(topics)]
+
+
+def handle_pause_topics(option, opt_str, value, parser):
+    pause_topics = []
+    for arg in parser.rargs:
+        if arg[:2] == "--" and len(arg) > 2:
+            break
+        if arg[:1] == "-" and len(arg) > 1:
+            break
+        pause_topics.append(arg)
+    parser.values.pause_topics.extend(pause_topics)
+    del parser.rargs[:len(pause_topics)]
+
+
+def play_cmd(argv):
+    parser = optparse.OptionParser(usage="rosbag play BAGFILE1 [BAGFILE2 BAGFILE3 ...]",
+                                   description="Play back the contents of one or more bag files in a time-synchronized fashion.")
+    parser.add_option("-p", "--prefix",       dest="prefix",     default='',    type='str',          help="prefix all output topics")
+    parser.add_option("-q", "--quiet",        dest="quiet",      default=False, action="store_true", help="suppress console output")
+    parser.add_option("-i", "--immediate",    dest="immediate",  default=False, action="store_true", help="play back all messages without waiting")
+    parser.add_option("--pause",              dest="pause",      default=False, action="store_true", help="start in paused mode")
+    parser.add_option("--queue",              dest="queue",      default=100,     type='int', action="store", help="use an outgoing queue of size SIZE (defaults to %default)", metavar="SIZE")
+    parser.add_option("--clock",              dest="clock",      default=False, action="store_true", help="publish the clock time")
+    parser.add_option("--hz",                 dest="freq",       default=100,   type='float', action="store", help="use a frequency of HZ when publishing clock time (default: %default)", metavar="HZ")
+    parser.add_option("-d", "--delay",        dest="delay",      default=0.2,   type='float', action="store", help="sleep SEC seconds after every advertise call (to allow subscribers to connect)", metavar="SEC")
+    parser.add_option("-r", "--rate",         dest="rate",       default=1.0,   type='float', action="store", help="multiply the publish rate by FACTOR", metavar="FACTOR")
+    parser.add_option("-s", "--start",        dest="start",      default=0.0,   type='float', action="store", help="start SEC seconds into the bag files", metavar="SEC")
+    parser.add_option("-u", "--duration",     dest="duration",   default=None,  type='float', action="store", help="play only SEC seconds from the bag files", metavar="SEC")
+    parser.add_option("--skip-empty",         dest="skip_empty", default=None,  type='float', action="store", help="skip regions in the bag with no messages for more than SEC seconds", metavar="SEC")
+    parser.add_option("-l", "--loop",         dest="loop",       default=False, action="store_true", help="loop playback")
+    parser.add_option("-k", "--keep-alive",   dest="keep_alive", default=False, action="store_true", help="keep alive past end of bag (useful for publishing latched topics)")
+    parser.add_option("--try-future-version", dest="try_future", default=False, action="store_true", help="still try to open a bag file, even if the version number is not known to the player")
+    parser.add_option("--topics", dest="topics", default=[],  callback=handle_topics, action="callback", help="topics to play back")
+    parser.add_option("--pause-topics", dest="pause_topics", default=[],  callback=handle_pause_topics, action="callback", help="topics to pause on during playback")
+    parser.add_option("--bags",  help="bags files to play back from")
+    parser.add_option("--wait-for-subscribers",  dest="wait_for_subscribers", default=False, action="store_true", help="wait for at least one subscriber on each topic before publishing")
+
+    (options, args) = parser.parse_args(argv)
+
+    if options.bags:
+        args.append(options.bags)
+
+    if len(args) == 0:
+        parser.error('You must specify at least 1 bag file to play back.')
+
+    playpath = roslib.packages.find_node('rosbag', 'play')
+    if not playpath:
+        parser.error("Cannot find rosbag/play executable")
+    cmd = [playpath[0]]
+
+    if options.prefix:
+        cmd.extend(["--prefix", str(options.prefix)])
+
+    if options.quiet:      cmd.extend(["--quiet"])
+    if options.pause:      cmd.extend(["--pause"])
+    if options.immediate:  cmd.extend(["--immediate"])
+    if options.loop:       cmd.extend(["--loop"])
+    if options.keep_alive: cmd.extend(["--keep-alive"])
+    if options.try_future: cmd.extend(["--try-future-version"])
+    if options.wait_for_subscribers: cmd.extend(["--wait-for-subscribers"])
+
+    if options.clock:
+        cmd.extend(["--clock", "--hz", str(options.freq)])
+
+    cmd.extend(['--queue', str(options.queue)])
+    cmd.extend(['--rate', str(options.rate)])
+    cmd.extend(['--delay', str(options.delay)])
+    cmd.extend(['--start', str(options.start)])
+    if options.duration:
+        cmd.extend(['--duration', str(options.duration)])
+    if options.skip_empty:
+        cmd.extend(['--skip-empty', str(options.skip_empty)])
+
+    if options.topics:
+        cmd.extend(['--topics'] + options.topics)
+
+    if options.pause_topics:
+        cmd.extend(['--pause-topics'] + options.pause_topics)
+
+    # prevent bag files to be passed as --topics or --pause-topics
+    if options.topics or options.pause_topics:
+        cmd.extend(['--bags'])
+
+    cmd.extend(args)
+
+    old_handler = signal.signal(
+        signal.SIGTERM,
+        lambda signum, frame: _stop_process(signum, frame, old_handler, process)
+    )
+    # Better way of handling it than os.execv
+    # This makes sure stdin handles are passed to the process.
+    process = subprocess.Popen(cmd)
+    process.wait()
+
+
+def filter_cmd(argv):
+    def expr_eval(expr):
+        def eval_fn(topic, m, t):
+            return eval(expr)
+        return eval_fn
+
+    parser = optparse.OptionParser(usage="""rosbag filter [options] INBAG OUTBAG EXPRESSION
+
+EXPRESSION can be any Python-legal expression.
+
+The following variables are available:
+ * topic: name of topic
+ * m: message
+ * t: time of message (t.secs, t.nsecs)""",
+                                   description='Filter the contents of the bag.')
+    parser.add_option('-p', '--print', action='store', dest='verbose_pattern', default=None, metavar='PRINT-EXPRESSION', help='Python expression to print for verbose debugging. Uses same variables as filter-expression')
+
+    options, args = parser.parse_args(argv)
+    if len(args) == 0:
+        parser.error('You must specify an in bag, an out bag, and an expression.')
+    if len(args) == 1:
+        parser.error('You must specify an out bag and an expression.')
+    if len(args) == 2:
+        parser.error("You must specify an expression.")
+    if len(args) > 3:
+        parser.error("Too many arguments.")
+
+    inbag_filename, outbag_filename, expr = args
+
+    if not os.path.isfile(inbag_filename):
+        print('Cannot locate input bag file [%s]' % inbag_filename, file=sys.stderr)
+        sys.exit(2)
+
+    if os.path.realpath(inbag_filename) == os.path.realpath(outbag_filename):
+        print('Cannot use same file as input and output [%s]' % inbag_filename, file=sys.stderr)
+        sys.exit(3)
+
+    filter_fn = expr_eval(expr)
+
+    outbag = Bag(outbag_filename, 'w')
+    
+    try:
+        inbag = Bag(inbag_filename)
+    except ROSBagUnindexedException as ex:
+        print('ERROR bag unindexed: %s.  Run rosbag reindex.' % inbag_filename, file=sys.stderr)
+        sys.exit(1)
+
+    try:
+        meter = ProgressMeter(outbag_filename, inbag._uncompressed_size)
+        total_bytes = 0
+    
+        if options.verbose_pattern:
+            verbose_pattern = expr_eval(options.verbose_pattern)
+    
+            for topic, raw_msg, t in inbag.read_messages(raw=True):
+                msg_type, serialized_bytes, md5sum, pos, pytype = raw_msg
+                msg = pytype()
+                msg.deserialize(serialized_bytes)
+
+                if filter_fn(topic, msg, t):
+                    print('MATCH', verbose_pattern(topic, msg, t))
+                    outbag.write(topic, msg, t)
+                else:
+                    print('NO MATCH', verbose_pattern(topic, msg, t))          
+
+                total_bytes += len(serialized_bytes) 
+                meter.step(total_bytes)
+        else:
+            for topic, raw_msg, t in inbag.read_messages(raw=True):
+                msg_type, serialized_bytes, md5sum, pos, pytype = raw_msg
+                msg = pytype()
+                msg.deserialize(serialized_bytes)
+
+                if filter_fn(topic, msg, t):
+                    outbag.write(topic, msg, t)
+
+                total_bytes += len(serialized_bytes)
+                meter.step(total_bytes)
+        
+        meter.finish()
+
+    finally:
+        inbag.close()
+        outbag.close()
+
+def fix_cmd(argv):
+    parser = optparse.OptionParser(usage='rosbag fix INBAG OUTBAG [EXTRARULES1 EXTRARULES2 ...]', description='Repair the messages in a bag file so that it can be played in the current system.')
+    parser.add_option('-n', '--noplugins', action='store_true', dest='noplugins', help='do not load rulefiles via plugins')
+    parser.add_option('--force', action='store_true', dest='force', help='proceed with migrations, even if not all rules defined')
+
+    (options, args) = parser.parse_args(argv)
+
+    if len(args) < 1:
+        parser.error('You must pass input and output bag files.')
+    if len(args) < 2:
+        parser.error('You must pass an output bag file.')
+
+    inbag_filename  = args[0]
+    outbag_filename = args[1]
+    rules           = args[2:]   
+
+    ext = os.path.splitext(outbag_filename)[1]
+    if ext == '.bmr':
+        parser.error('Input file should be a bag file, not a rule file.')
+    if ext != '.bag':
+        parser.error('Output file must be a bag file.')
+
+    outname = outbag_filename + '.tmp'
+
+    if os.path.exists(outbag_filename):
+        if not os.access(outbag_filename, os.W_OK):
+            print('Don\'t have permissions to access %s' % outbag_filename, file=sys.stderr)
+            sys.exit(1)
+    else:
+        try:
+            file = open(outbag_filename, 'w')
+            file.close()
+        except IOError as e:
+            print('Cannot open %s for writing' % outbag_filename, file=sys.stderr)
+            sys.exit(1)
+
+    if os.path.exists(outname):
+        if not os.access(outname, os.W_OK):
+            print('Don\'t have permissions to access %s' % outname, file=sys.stderr)
+            sys.exit(1)
+    else:
+        try:
+            file = open(outname, 'w')
+            file.close()
+        except IOError as e:
+            print('Cannot open %s for writing' % outname, file=sys.stderr)
+            sys.exit(1)
+
+    if options.noplugins is None:
+        options.noplugins = False
+
+    migrator = MessageMigrator(rules, plugins=not options.noplugins)
+    
+    try:
+        migrations = fixbag2(migrator, inbag_filename, outname, options.force)
+    except ROSBagUnindexedException as ex:
+        print('ERROR bag unindexed: %s.  Run rosbag reindex.' % inbag_filename,
+              file=sys.stderr)
+        sys.exit(1)
+
+    if len(migrations) == 0:
+        os.rename(outname, outbag_filename)
+        print('Bag migrated successfully.')
+    else:
+        print('Bag could not be migrated.  The following migrations could not be performed:')
+        for m in migrations:
+            print_trans(m[0][0].old_class, m[0][-1].new_class, 0)
+            
+            if len(m[1]) > 0:
+                print('    %d rules missing:' % len(m[1]))
+                for r in m[1]:
+                    print_trans(r.old_class, r.new_class,1)
+                    
+        print('Try running \'rosbag check\' to create the necessary rule files or run \'rosbag fix\' with the \'--force\' option.')
+        os.remove(outname)
+        sys.exit(1)
+
+def check_cmd(argv):
+    parser = optparse.OptionParser(usage='rosbag check BAG [-g RULEFILE] [EXTRARULES1 EXTRARULES2 ...]', description='Determine whether a bag is playable in the current system, or if it can be migrated.')
+    parser.add_option('-g', '--genrules',  action='store',      dest='rulefile', default=None, help='generate a rulefile named RULEFILE')
+    parser.add_option('-a', '--append',    action='store_true', dest='append',                 help='append to the end of an existing rulefile after loading it')
+    parser.add_option('-n', '--noplugins', action='store_true', dest='noplugins',              help='do not load rulefiles via plugins')
+    (options, args) = parser.parse_args(argv)
+
+    if len(args) == 0:
+        parser.error('You must specify a bag file to check.')
+    if options.append and options.rulefile is None:
+        parser.error('Cannot specify -a without also specifying -g.')
+    if options.rulefile is not None:
+        rulefile_exists = os.path.isfile(options.rulefile)
+        if rulefile_exists and not options.append:
+            parser.error('The file %s already exists.  Include -a if you intend to append.' % options.rulefile)
+        if not rulefile_exists and options.append:
+            parser.error('The file %s does not exist, and so -a is invalid.' % options.rulefile)
+    
+    if options.append:
+        append_rule = [options.rulefile]
+    else:
+        append_rule = []
+
+    # First check that the bag is not unindexed 
+    try:
+        Bag(args[0])
+    except ROSBagUnindexedException as ex:
+        print('ERROR bag unindexed: %s.  Run rosbag reindex.' % args[0], file=sys.stderr)
+        sys.exit(1)
+
+    mm = MessageMigrator(args[1:] + append_rule, not options.noplugins)
+
+    migrations = checkbag(mm, args[0])
+       
+    if len(migrations) == 0:
+        print('Bag file does not need any migrations.')
+        exit(0)
+        
+    print('The following migrations need to occur:')
+    all_rules = []
+    for m in migrations:
+        all_rules.extend(m[1])
+
+        print_trans(m[0][0].old_class, m[0][-1].new_class, 0)
+        if len(m[1]) > 0:
+            print("    %d rules missing:" % len(m[1]))
+            for r in m[1]:
+                print_trans(r.old_class, r.new_class, 1)
+
+    if options.rulefile is None:
+        if all_rules == []:
+            print("\nAll rules defined.  Bag is ready to be migrated")
+        else:
+            print("\nTo generate rules, please run with -g <rulefile>")
+        exit(0)
+
+    output = ''
+    rules_left = mm.filter_rules_unique(all_rules)
+
+    if rules_left == []:
+        print("\nNo additional rule files needed to be generated.  %s not created."%(options.rulefile))
+        exit(0)
+
+    while len(rules_left) > 0:
+        extra_rules = []
+        for r in rules_left:
+            if r.new_class is None:
+                print('The message type %s appears to have moved.  Please enter the type to migrate it to.' % r.old_class._type)
+                new_type = raw_input('>')
+                new_class = roslib.message.get_message_class(new_type)
+                while new_class is None:
+                    print("\'%s\' could not be found in your system.  Please make sure it is built." % new_type)
+                    new_type = raw_input('>')
+                    new_class = roslib.message.get_message_class(new_type)
+                new_rule = mm.make_update_rule(r.old_class, new_class)
+                R = new_rule(mm, 'GENERATED.' + new_rule.__name__)
+                R.find_sub_paths()
+                new_rules = [r for r in mm.expand_rules(R.sub_rules) if r.valid == False]
+                extra_rules.extend(new_rules)
+                print('Creating the migration rule for %s requires additional missing rules:' % new_type)
+                for nr in new_rules:
+                    print_trans(nr.old_class, nr.new_class,1)
+                output += R.get_class_def()
+            else:
+                output += r.get_class_def()
+        rules_left = mm.filter_rules_unique(extra_rules)
+    f = open(options.rulefile, 'a')
+    f.write(output)
+    f.close()
+
+    print('\nThe necessary rule files have been written to: %s' % options.rulefile)
+
+def compress_cmd(argv):
+    parser = optparse.OptionParser(usage='rosbag compress [options] BAGFILE1 [BAGFILE2 ...]',
+                                   description='Compress one or more bag files.')
+    parser.add_option(      '--output-dir', action='store',       dest='output_dir',  help='write to directory DIR', metavar='DIR')
+    parser.add_option('-f', '--force',      action='store_true',  dest='force',       help='force overwriting of backup file if it exists')
+    parser.add_option('-q', '--quiet',      action='store_true',  dest='quiet',       help='suppress noncritical messages')
+    parser.add_option('-j', '--bz2',        action='store_const', dest='compression', help='use BZ2 compression', const=Compression.BZ2, default=Compression.BZ2)
+    parser.add_option(      '--lz4',        action='store_const', dest='compression', help='use lz4 compression', const=Compression.LZ4)
+    (options, args) = parser.parse_args(argv)
+
+    if len(args) < 1:
+        parser.error('You must specify at least one bag file.')
+
+    op = lambda inbag, outbag, quiet: change_compression_op(inbag, outbag, options.compression, options.quiet)
+
+    bag_op(args, False, lambda b: False, op, options.output_dir, options.force, options.quiet)
+
+def decompress_cmd(argv):
+    parser = optparse.OptionParser(usage='rosbag decompress [options] BAGFILE1 [BAGFILE2 ...]',
+                                   description='Decompress one or more bag files.')
+    parser.add_option(      '--output-dir', action='store',      dest='output_dir', help='write to directory DIR', metavar='DIR')
+    parser.add_option('-f', '--force',      action='store_true', dest='force',      help='force overwriting of backup file if it exists')
+    parser.add_option('-q', '--quiet',      action='store_true', dest='quiet',      help='suppress noncritical messages')
+
+    (options, args) = parser.parse_args(argv)
+
+    if len(args) < 1:
+        parser.error('You must specify at least one bag file.')
+    
+    op = lambda inbag, outbag, quiet: change_compression_op(inbag, outbag, Compression.NONE, options.quiet)
+    
+    bag_op(args, False, lambda b: False, op, options.output_dir, options.force, options.quiet)
+
+def reindex_cmd(argv):
+    parser = optparse.OptionParser(usage='rosbag reindex [options] BAGFILE1 [BAGFILE2 ...]',
+                                   description='Reindexes one or more bag files.')
+    parser.add_option(      '--output-dir', action='store',      dest='output_dir', help='write to directory DIR', metavar='DIR')
+    parser.add_option('-f', '--force',      action='store_true', dest='force',      help='force overwriting of backup file if it exists')
+    parser.add_option('-q', '--quiet',      action='store_true', dest='quiet',      help='suppress noncritical messages')
+
+    (options, args) = parser.parse_args(argv)
+
+    if len(args) < 1:
+        parser.error('You must specify at least one bag file.')
+    
+    op = lambda inbag, outbag, quiet: reindex_op(inbag, outbag, options.quiet)
+
+    bag_op(args, True, lambda b: b.version > 102, op, options.output_dir, options.force, options.quiet)
+
+def bag_op(inbag_filenames, allow_unindexed, copy_fn, op, output_dir=None, force=False, quiet=False):
+    for inbag_filename in inbag_filenames:
+        # Check we can read the file
+        try:
+            inbag = Bag(inbag_filename, 'r', allow_unindexed=allow_unindexed)
+        except ROSBagUnindexedException:
+            print('ERROR bag unindexed: %s.  Run rosbag reindex.' % inbag_filename, file=sys.stderr)
+            continue
+        except (ROSBagException, IOError) as ex:
+            print('ERROR reading %s: %s' % (inbag_filename, str(ex)), file=sys.stderr)
+            continue
+
+        # Determine whether we should copy the bag    
+        copy = copy_fn(inbag)
+        
+        inbag.close()
+
+        # Determine filename for output bag
+        if output_dir is None:
+            outbag_filename = inbag_filename
+        else:
+            outbag_filename = os.path.join(output_dir, os.path.split(inbag_filename)[1])
+
+        backup_filename = None
+        if outbag_filename == inbag_filename:
+            # Rename the input bag to ###.orig.###, and open for reading
+            backup_filename = '%s.orig%s' % os.path.splitext(inbag_filename)
+            
+            if not force and os.path.exists(backup_filename):
+                if not quiet:
+                    print('Skipping %s. Backup path %s already exists.' % (inbag_filename, backup_filename), file=sys.stderr)
+                continue
+            
+            try:
+                if copy:
+                    shutil.copy(inbag_filename, backup_filename)
+                else:
+                    os.rename(inbag_filename, backup_filename)
+            except OSError as ex:
+                print('ERROR %s %s to %s: %s' % ('copying' if copy else 'moving', inbag_filename, backup_filename, str(ex)), file=sys.stderr)
+                continue
+            
+            source_filename = backup_filename
+        else:
+            if copy:
+                shutil.copy(inbag_filename, outbag_filename)
+                source_filename = outbag_filename
+            else:
+                source_filename = inbag_filename
+
+        try:
+            inbag = Bag(source_filename, 'r', allow_unindexed=allow_unindexed)
+
+            # Open the output bag file for writing
+            try:
+                if copy:
+                    outbag = Bag(outbag_filename, 'a', allow_unindexed=allow_unindexed)
+                else:
+                    outbag = Bag(outbag_filename, 'w')
+            except (ROSBagException, IOError) as ex:
+                print('ERROR writing to %s: %s' % (outbag_filename, str(ex)), file=sys.stderr)
+                inbag.close()
+                continue
+
+            # Perform the operation
+            try:
+                op(inbag, outbag, quiet=quiet)
+            except ROSBagException as ex:
+                print('\nERROR operating on %s: %s' % (source_filename, str(ex)), file=sys.stderr)
+                inbag.close()
+                outbag.close()
+                continue
+                
+            outbag.close()
+            inbag.close()
+
+        except KeyboardInterrupt:
+            if backup_filename is not None:
+                try:
+                    if copy:
+                        os.remove(backup_filename)
+                    else:
+                        os.rename(backup_filename, inbag_filename)
+                except OSError as ex:
+                    print('ERROR %s %s to %s: %s', ('removing' if copy else 'moving', backup_filename, inbag_filename, str(ex)), file=sys.stderr)
+                    break
+    
+        except (ROSBagException, IOError) as ex:
+            print('ERROR operating on %s: %s' % (inbag_filename, str(ex)), file=sys.stderr)
+
+def change_compression_op(inbag, outbag, compression, quiet):
+    outbag.compression = compression
+
+    if quiet:
+        for topic, msg, t in inbag.read_messages(raw=True):
+            outbag.write(topic, msg, t, raw=True)
+    else:
+        meter = ProgressMeter(outbag.filename, inbag._uncompressed_size)
+
+        total_bytes = 0
+        for topic, msg, t in inbag.read_messages(raw=True):
+            msg_type, serialized_bytes, md5sum, pos, pytype = msg
+    
+            outbag.write(topic, msg, t, raw=True)
+            
+            total_bytes += len(serialized_bytes) 
+            meter.step(total_bytes)
+        
+        meter.finish()
+
+def reindex_op(inbag, outbag, quiet):
+    if inbag.version == 102:
+        if quiet:
+            try:
+                for offset in inbag.reindex():
+                    pass
+            except:
+                pass
+
+            for (topic, msg, t) in inbag.read_messages():
+                outbag.write(topic, msg, t)
+        else:
+            meter = ProgressMeter(outbag.filename, inbag.size)
+            try:
+                for offset in inbag.reindex():
+                    meter.step(offset)
+            except:
+                pass
+            meter.finish()
+
+            meter = ProgressMeter(outbag.filename, inbag.size)
+            for (topic, msg, t) in inbag.read_messages():
+                outbag.write(topic, msg, t)
+                meter.step(inbag._file.tell())
+            meter.finish()
+    else:
+        if quiet:
+            try:
+                for offset in outbag.reindex():
+                    pass
+            except:
+                pass
+        else:
+            meter = ProgressMeter(outbag.filename, outbag.size)
+            try:
+                for offset in outbag.reindex():
+                    meter.step(offset)
+            except:
+                pass
+            meter.finish()
+
+class RosbagCmds(UserDict):
+    def __init__(self):
+        UserDict.__init__(self)
+        self._description = {}
+        self['help'] = self.help_cmd
+
+    def add_cmd(self, name, function, description):
+        self[name] = function
+        self._description[name] = description
+        
+    def get_valid_cmds(self):
+        str = "Available subcommands:\n"
+        for k in sorted(self.keys()):
+            str += "   %s  " % k
+            if k in self._description.keys():
+                str +="\t%s" % self._description[k]
+            str += "\n"
+        return str
+
+    def help_cmd(self,argv):
+        argv = [a for a in argv if a != '-h' and a != '--help']
+
+        if len(argv) == 0:
+            print('Usage: rosbag <subcommand> [options] [args]')
+            print()
+            print("A bag is a file format in ROS for storing ROS message data. The rosbag command can record, replay and manipulate bags.")
+            print()
+            print(self.get_valid_cmds())
+            print('For additional information, see http://wiki.ros.org/rosbag')
+            print()
+            return
+
+        cmd = argv[0]
+        if cmd in self:
+            self[cmd](['-h'])
+        else:
+            print("Unknown command: '%s'" % cmd, file=sys.stderr)
+            print(self.get_valid_cmds(), file=sys.stderr)
+
+class ProgressMeter(object):
+    def __init__(self, path, bytes_total, refresh_rate=1.0):
+        self.path           = path
+        self.bytes_total    = bytes_total
+        self.refresh_rate   = refresh_rate
+        
+        self.elapsed        = 0.0
+        self.update_elapsed = 0.0
+        self.bytes_read     = 0.0
+
+        self.start_time     = time.time()
+
+        self._update_progress()
+
+    def step(self, bytes_read, force_update=False):
+        self.bytes_read = bytes_read
+        self.elapsed    = time.time() - self.start_time
+        
+        if force_update or self.elapsed - self.update_elapsed > self.refresh_rate:
+            self._update_progress()
+            self.update_elapsed = self.elapsed
+
+    def _update_progress(self):
+        max_path_len = self.terminal_width() - 37
+        path = self.path
+        if len(path) > max_path_len:
+            path = '...' + self.path[-max_path_len + 3:]
+
+        bytes_read_str  = self._human_readable_size(float(self.bytes_read))
+        bytes_total_str = self._human_readable_size(float(self.bytes_total))
+        
+        if self.bytes_read < self.bytes_total:
+            complete_fraction = float(self.bytes_read) / self.bytes_total
+            pct_complete      = int(100.0 * complete_fraction)
+
+            if complete_fraction > 0.0:
+                eta = self.elapsed * (1.0 / complete_fraction - 1.0)
+                eta_min, eta_sec = eta / 60, eta % 60
+                if eta_min > 99:
+                    eta_str = '--:--'
+                else:
+                    eta_str = '%02d:%02d' % (eta_min, eta_sec)
+            else:
+                eta_str = '--:--'
+
+            progress = '%-*s %3d%% %8s / %8s %s ETA' % (max_path_len, path, pct_complete, bytes_read_str, bytes_total_str, eta_str)
+        else:
+            progress = '%-*s 100%% %19s %02d:%02d    ' % (max_path_len, path, bytes_total_str, self.elapsed / 60, self.elapsed % 60)
+
+        print('\r', progress, end='')
+        sys.stdout.flush()
+        
+    def _human_readable_size(self, size):
+        multiple = 1024.0
+        for suffix in ['KB', 'MB', 'GB', 'TB', 'PB', 'EB', 'ZB', 'YB']:
+            size /= multiple
+            if size < multiple:
+                return '%.1f %s' % (size, suffix)
+    
+        raise ValueError('number too large')
+
+    def finish(self):
+        self.step(self.bytes_total, force_update=True)
+        print()
+
+    @staticmethod
+    def terminal_width():
+        """Estimate the width of the terminal"""
+        width = 0
+        try:
+            import struct, fcntl, termios
+            s     = struct.pack('HHHH', 0, 0, 0, 0)
+            x     = fcntl.ioctl(1, termios.TIOCGWINSZ, s)
+            width = struct.unpack('HHHH', x)[1]
+        except (IOError, ImportError):
+            pass
+        if width <= 0:
+            try:
+                width = int(os.environ['COLUMNS'])
+            except:
+                pass
+        if width <= 0:
+            width = 80
+    
+        return width
+
+def rosbagmain(argv=None):
+    cmds = RosbagCmds()
+    cmds.add_cmd('record', record_cmd, "Record a bag file with the contents of specified topics.")
+    cmds.add_cmd('info', info_cmd, 'Summarize the contents of one or more bag files.')
+    cmds.add_cmd('play', play_cmd, "Play back the contents of one or more bag files in a time-synchronized fashion.")
+    cmds.add_cmd('check', check_cmd, 'Determine whether a bag is playable in the current system, or if it can be migrated.')
+    cmds.add_cmd('fix', fix_cmd, 'Repair the messages in a bag file so that it can be played in the current system.')
+    cmds.add_cmd('filter', filter_cmd, 'Filter the contents of the bag.')
+    cmds.add_cmd('compress', compress_cmd, 'Compress one or more bag files.')
+    cmds.add_cmd('decompress', decompress_cmd, 'Decompress one or more bag files.')
+    cmds.add_cmd('reindex', reindex_cmd, 'Reindexes one or more bag files.')
+
+    if argv is None:
+        argv = sys.argv
+
+    if '-h' in argv or '--help' in argv:
+        argv = [a for a in argv if a != '-h' and a != '--help']
+        argv.insert(1, 'help')
+
+    if len(argv) > 1:
+        cmd = argv[1]
+    else:
+        cmd = 'help'
+
+    try:
+        if cmd in cmds:
+            cmds[cmd](argv[2:])
+        else:
+            cmds['help']([cmd])
+    except KeyboardInterrupt:
+        pass

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

@@ -0,0 +1,64 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2010, 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/time_translator.h"
+
+namespace rosbag {
+
+TimeTranslator::TimeTranslator()
+    : time_scale_(1.0), real_start_(ros::TIME_MIN), translated_start_(ros::TIME_MIN)
+{
+}
+
+void TimeTranslator::setTimeScale(double const& s) {
+    time_scale_ = s;
+}
+
+void TimeTranslator::setRealStartTime(ros::Time const& t) {
+    real_start_ = t;
+}
+
+void TimeTranslator::setTranslatedStartTime(ros::Time const& t) {
+    translated_start_ = t;
+}
+
+void TimeTranslator::shift(ros::Duration const& d) {
+    translated_start_ += d;
+}
+
+ros::Time TimeTranslator::translate(ros::Time const& t) {
+    return translated_start_ + (t - real_start_) * (1.0 / time_scale_);
+}
+
+} // namespace rosbag