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 "macros.h"
00055 
00056 namespace rosbag {
00057 
00058 struct ROSBAG_DECL PlayerOptions
00059 {
00060     PlayerOptions();
00061 
00062     void check();
00063 
00064     bool     quiet;
00065     bool     start_paused;
00066     bool     at_once;
00067     bool     bag_time;
00068     double   bag_time_frequency;
00069     double   time_scale;
00070     int      queue_size;
00071     ros::WallDuration advertise_sleep;
00072     bool     try_future;
00073     bool     has_time;
00074     bool     loop;
00075     float    time;
00076     bool     has_duration;
00077     float    duration;
00078     bool     keep_alive;
00079     ros::Duration skip_empty;
00080 
00081     std::vector<std::string> bags;
00082     std::vector<std::string> topics;
00083 };
00084 
00085 
00087 class ROSBAG_DECL TimePublisher {
00088 public:
00092     TimePublisher();
00093 
00094     void setPublishFrequency(double publish_frequency);
00095     
00096     void setTimeScale(double time_scale);
00097 
00099     void setHorizon(const ros::Time& horizon);
00100 
00102     void setWCHorizon(const ros::WallTime& horizon);
00103 
00105     void setTime(const ros::Time& time);
00106 
00108     ros::Time const& getTime() const;
00109 
00114     void runClock(const ros::WallDuration& duration);
00115 
00117     void runStalledClock(const ros::WallDuration& duration);
00118 
00120     void stepClock();
00121 
00122     bool horizonReached();
00123 
00124 private:
00125     bool do_publish_;
00126     
00127     double publish_frequency_;
00128     double time_scale_;
00129     
00130     ros::NodeHandle node_handle_;
00131     ros::Publisher time_pub_;
00132     
00133     ros::WallDuration wall_step_;
00134     
00135     ros::WallTime next_pub_;
00136 
00137     ros::WallTime wc_horizon_;
00138     ros::Time horizon_;
00139     ros::Time current_;
00140 };
00141 
00142 
00144 
00148 class ROSBAG_DECL Player
00149 {
00150 public:
00151     Player(PlayerOptions const& options);
00152     ~Player();
00153 
00154     void publish();
00155 
00156 private:
00157     int readCharFromStdin();
00158     void setupTerminal();
00159     void restoreTerminal();
00160 
00161     void doPublish(rosbag::MessageInstance const& m);
00162 
00163     void doKeepAlive();
00164 
00165     void printTime();
00166 
00167 
00168 private:
00169 
00170     PlayerOptions options_;
00171 
00172     ros::NodeHandle node_handle_;
00173 
00174     bool paused_;
00175 
00176     ros::WallTime paused_time_;
00177 
00178     std::vector<boost::shared_ptr<Bag> >  bags_;
00179     std::map<std::string, ros::Publisher> publishers_;
00180 
00181     // Terminal
00182     bool    terminal_modified_;
00183 #if defined(_MSC_VER)
00184     HANDLE input_handle;
00185     DWORD stdin_set;
00186 #else
00187     termios orig_flags_;
00188     fd_set  stdin_fdset_;
00189 #endif
00190     int     maxfd_;
00191 
00192     TimeTranslator time_translator_;
00193     TimePublisher time_publisher_;
00194 
00195     ros::Time start_time_;
00196     ros::Duration bag_length_;
00197 };
00198 
00199 
00200 } // namespace rosbag
00201 
00202 #endif


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