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