player.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage, Inc. nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 ********************************************************************/
00034 
00035 #ifndef ROSBAG_PLAYER_H
00036 #define ROSBAG_PLAYER_H
00037 
00038 #include <sys/stat.h>
00039 #if !defined(_MSC_VER)
00040   #include <termios.h>
00041   #include <unistd.h>
00042 #endif
00043 #include <time.h>
00044 
00045 #include <queue>
00046 #include <string>
00047 
00048 #include <ros/ros.h>
00049 #include <ros/time.h>
00050 
00051 #include "rosbag/bag.h"
00052 
00053 #include "rosbag/time_translator.h"
00054 #include "rosbag/macros.h"
00055 
00056 namespace rosbag {
00057 
00059 
00063 ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& msg, uint32_t queue_size);
00064 
00065 ROSBAG_DECL ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size);
00066 
00067 
00068 struct ROSBAG_DECL PlayerOptions
00069 {
00070     PlayerOptions();
00071 
00072     void check();
00073 
00074     bool     quiet;
00075     bool     start_paused;
00076     bool     at_once;
00077     bool     bag_time;
00078     double   bag_time_frequency;
00079     double   time_scale;
00080     int      queue_size;
00081     ros::WallDuration advertise_sleep;
00082     bool     try_future;
00083     bool     has_time;
00084     bool     loop;
00085     float    time;
00086     bool     has_duration;
00087     float    duration;
00088     bool     keep_alive;
00089     ros::Duration skip_empty;
00090 
00091     std::vector<std::string> bags;
00092     std::vector<std::string> topics;
00093 };
00094 
00095 
00097 class ROSBAG_DECL TimePublisher {
00098 public:
00102     TimePublisher();
00103 
00104     void setPublishFrequency(double publish_frequency);
00105     
00106     void setTimeScale(double time_scale);
00107 
00109     void setHorizon(const ros::Time& horizon);
00110 
00112     void setWCHorizon(const ros::WallTime& horizon);
00113 
00115     void setTime(const ros::Time& time);
00116 
00118     ros::Time const& getTime() const;
00119 
00124     void runClock(const ros::WallDuration& duration);
00125 
00127     void runStalledClock(const ros::WallDuration& duration);
00128 
00130     void stepClock();
00131 
00132     bool horizonReached();
00133 
00134 private:
00135     bool do_publish_;
00136     
00137     double publish_frequency_;
00138     double time_scale_;
00139     
00140     ros::NodeHandle node_handle_;
00141     ros::Publisher time_pub_;
00142     
00143     ros::WallDuration wall_step_;
00144     
00145     ros::WallTime next_pub_;
00146 
00147     ros::WallTime wc_horizon_;
00148     ros::Time horizon_;
00149     ros::Time current_;
00150 };
00151 
00152 
00154 
00158 class ROSBAG_DECL Player
00159 {
00160 public:
00161     Player(PlayerOptions const& options);
00162     ~Player();
00163 
00164     void publish();
00165 
00166 private:
00167     int readCharFromStdin();
00168     void setupTerminal();
00169     void restoreTerminal();
00170 
00171     void doPublish(rosbag::MessageInstance const& m);
00172 
00173     void doKeepAlive();
00174 
00175     void printTime();
00176 
00177 
00178 private:
00179 
00180     PlayerOptions options_;
00181 
00182     ros::NodeHandle node_handle_;
00183 
00184     bool paused_;
00185 
00186     ros::WallTime paused_time_;
00187 
00188     std::vector<boost::shared_ptr<Bag> >  bags_;
00189     std::map<std::string, ros::Publisher> publishers_;
00190 
00191     // Terminal
00192     bool    terminal_modified_;
00193 #if defined(_MSC_VER)
00194     HANDLE input_handle;
00195     DWORD stdin_set;
00196 #else
00197     termios orig_flags_;
00198     fd_set  stdin_fdset_;
00199 #endif
00200     int     maxfd_;
00201 
00202     TimeTranslator time_translator_;
00203     TimePublisher time_publisher_;
00204 
00205     ros::Time start_time_;
00206     ros::Duration bag_length_;
00207 };
00208 
00209 
00210 } // namespace rosbag
00211 
00212 #endif


rosbag
Author(s): Tim Field, Jeremy Leibs, James Bowman
autogenerated on Fri Aug 28 2015 12:33:52