Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ROSBAG_RECORDER_H
00036 #define ROSBAG_RECORDER_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 #include <vector>
00048
00049 #include <boost/thread/condition.hpp>
00050 #include <boost/thread/mutex.hpp>
00051 #include <boost/regex.hpp>
00052
00053 #include <ros/ros.h>
00054 #include <ros/time.h>
00055
00056 #include <std_msgs/Empty.h>
00057 #include <topic_tools/shape_shifter.h>
00058
00059 #include "rosbag/bag.h"
00060 #include "rosbag/stream.h"
00061 #include "rosbag/macros.h"
00062
00063 namespace rosbag {
00064
00065 class ROSBAG_DECL OutgoingMessage
00066 {
00067 public:
00068 OutgoingMessage(std::string const& _topic, topic_tools::ShapeShifter::ConstPtr _msg, boost::shared_ptr<ros::M_string> _connection_header, ros::Time _time);
00069
00070 std::string topic;
00071 topic_tools::ShapeShifter::ConstPtr msg;
00072 boost::shared_ptr<ros::M_string> connection_header;
00073 ros::Time time;
00074 };
00075
00076 class ROSBAG_DECL OutgoingQueue
00077 {
00078 public:
00079 OutgoingQueue(std::string const& _filename, std::queue<OutgoingMessage>* _queue, ros::Time _time);
00080
00081 std::string filename;
00082 std::queue<OutgoingMessage>* queue;
00083 ros::Time time;
00084 };
00085
00086 struct ROSBAG_DECL RecorderOptions
00087 {
00088 RecorderOptions();
00089
00090 bool trigger;
00091 bool record_all;
00092 bool regex;
00093 bool do_exclude;
00094 bool quiet;
00095 bool append_date;
00096 bool snapshot;
00097 bool verbose;
00098 CompressionType compression;
00099 std::string prefix;
00100 std::string name;
00101 boost::regex exclude_regex;
00102 uint32_t buffer_size;
00103 uint32_t chunk_size;
00104 uint32_t limit;
00105 bool split;
00106 uint32_t max_size;
00107 ros::Duration max_duration;
00108 std::string node;
00109
00110 std::vector<std::string> topics;
00111 };
00112
00113 class ROSBAG_DECL Recorder
00114 {
00115 public:
00116 Recorder(RecorderOptions const& options);
00117
00118 void doTrigger();
00119
00120 bool isSubscribed(std::string const& topic) const;
00121
00122 boost::shared_ptr<ros::Subscriber> subscribe(std::string const& topic);
00123
00124 int run();
00125
00126 private:
00127 void printUsage();
00128
00129 void updateFilenames();
00130 void startWriting();
00131 void stopWriting();
00132
00133 bool checkLogging();
00134 bool scheduledCheckDisk();
00135 bool checkDisk();
00136
00137 void snapshotTrigger(std_msgs::Empty::ConstPtr trigger);
00138
00139 void doQueue(ros::MessageEvent<topic_tools::ShapeShifter const> msg_event, std::string const& topic, boost::shared_ptr<ros::Subscriber> subscriber, boost::shared_ptr<int> count);
00140 void doRecord();
00141 bool checkSize();
00142 bool checkDuration(const ros::Time&);
00143 void doRecordSnapshotter();
00144 void doCheckMaster(ros::TimerEvent const& e, ros::NodeHandle& node_handle);
00145
00146 bool shouldSubscribeToTopic(std::string const& topic, bool from_node = false);
00147
00148 template<class T>
00149 static std::string timeToStr(T ros_t);
00150
00151 private:
00152 RecorderOptions options_;
00153
00154 Bag bag_;
00155
00156 std::string target_filename_;
00157 std::string write_filename_;
00158
00159 std::set<std::string> currently_recording_;
00160 int num_subscribers_;
00161
00162 int exit_code_;
00163
00164 boost::condition_variable_any queue_condition_;
00165 boost::mutex queue_mutex_;
00166 std::queue<OutgoingMessage>* queue_;
00167 uint64_t queue_size_;
00168 uint64_t max_queue_size_;
00169
00170 uint64_t split_count_;
00171
00172 std::queue<OutgoingQueue> queue_queue_;
00173
00174 ros::Time last_buffer_warn_;
00175
00176 ros::Time start_time_;
00177
00178 bool writing_enabled_;
00179 boost::mutex check_disk_mutex_;
00180 ros::WallTime check_disk_next_;
00181 ros::WallTime warn_next_;
00182 };
00183
00184 }
00185
00186 #endif