rosbag: pause and status support

[ablasdel]

This patch provides a client server interface for pausing recording and querying the recording status of rosbag record

http://ros-users.122217.n3.nabble.com/rosbag-patch-for-pausing-recording-and-status-information-td1000250.html#a1000250

rosbag.diff

Index: include/rosbag/recorder.h
===================================================================
--- include/rosbag/recorder.h   (revision 10468)
+++ include/rosbag/recorder.h   (working copy)
@@ -53,6 +53,8 @@

 #include <std_msgs/Empty.h>
 #include <topic_tools/shape_shifter.h>
+#include "rosbag/GetStatus.h"
+#include "rosbag/SetStatus.h"

 #include "rosbag/bag.h"
 #include "rosbag/stream.h"
@@ -114,6 +116,10 @@

     boost::shared_ptr<ros::Subscriber> subscribe(std::string const& topic);

+    bool getStatus(rosbag::GetStatus::Request  &req,
+         rosbag::GetStatus::Response &res );
+    bool setStatus(rosbag::SetStatus::Request  &req,
+         rosbag::SetStatus::Response &res );
     int run();

 private:
@@ -146,7 +152,9 @@

     std::string                   target_filename_;
     std::string                   write_filename_;
+    boost::mutex                  update_filename_mutex_;

+    boost::mutex                  currently_recording_mutex_;
     std::set<std::string>         currently_recording_;  //!< set of currenly recording topics
     int                           num_subscribers_;      //!< used for book-keeping of our number of subscribers

@@ -164,6 +172,7 @@

     ros::Time                     last_buffer_warn_;

+    boost::mutex                  writing_enabled_mutex_;
     bool          writing_enabled_;
     boost::mutex  check_disk_mutex_;
     ros::WallTime check_disk_next_;
Index: src/recorder.cpp
===================================================================
--- src/recorder.cpp    (revision 10468)
+++ src/recorder.cpp    (working copy)
@@ -50,6 +50,8 @@

 #include <ros/ros.h>
 #include <topic_tools/shape_shifter.h>
+#include "rosbag/GetStatus.h"
+#include "rosbag/SetStatus.h"

 #define foreach BOOST_FOREACH

@@ -111,6 +113,38 @@
 {
 }

+bool Recorder::setStatus(rosbag::SetStatus::Request  &req,
+         rosbag::SetStatus::Response &res )
+{
+    boost::mutex::scoped_lock status_lock(update_filename_mutex_);
+    boost::mutex::scoped_lock enable_lock(writing_enabled_mutex_);
+    writing_enabled_ = req.writing_enabled;
+    if (writing_enabled_) {
+        ROS_INFO("enabling message logging");
+    } else {
+        ROS_INFO("disabling message logging");
+    }
+
+    res.success = true;
+    return true;
+}
+
+bool Recorder::getStatus(rosbag::GetStatus::Request  &req,
+         rosbag::GetStatus::Response &res )
+{
+    boost::mutex::scoped_lock status_lock(update_filename_mutex_);
+    boost::mutex::scoped_lock enable_lock(writing_enabled_mutex_);
+    boost::mutex::scoped_lock topics_lock(currently_recording_mutex_);
+    std::set<std::string>::iterator topic;
+    res.target_filename = target_filename_;
+    res.write_filename = write_filename_;
+    res.writing_enabled = writing_enabled_;
+    for( topic = currently_recording_.begin(); topic != currently_recording_.end(); topic++ ) {
+        res.topics.push_back(*topic);
+    }
+    return true;
+}
+
 int Recorder::run() {
     if (options_.trigger) {
         doTrigger();
@@ -155,6 +189,9 @@

     ros::Time::waitForValid();

+    ros::NodeHandle n("~");
+    ros::ServiceServer get_service = n.advertiseService("get_status", &Recorder::getStatus, this);
+    ros::ServiceServer set_service = n.advertiseService("set_status", &Recorder::setStatus, this);

     // Subscribe to each topic
     if (!options_.regex) {
@@ -182,6 +219,7 @@
    ROS_INFO("Subscribing to %s", topic.c_str());

     ros::NodeHandle nh;
+    boost::mutex::scoped_lock lock(currently_recording_mutex_);
     shared_ptr<int> count(new int(options_.limit));
     shared_ptr<ros::Subscriber> sub(new ros::Subscriber);
     *sub = nh.subscribe<topic_tools::ShapeShifter>(topic, 100, boost::bind(&Recorder::doQueue, this, _1, topic, sub, count));
@@ -300,6 +338,7 @@
     for (unsigned int i = 1; i < parts.size(); i++)
         target_filename_ += string("_") + parts[i];

+    boost::mutex::scoped_lock lock(update_filename_mutex_);
     target_filename_ += string(".bag");
     write_filename_ = target_filename_ + string(".active");
 }
@@ -457,6 +496,7 @@
 }

 bool Recorder::checkDisk() {
+    boost::mutex::scoped_lock lock(writing_enabled_mutex_);
     struct statvfs fiData;
     if ((statvfs(bag_.getFileName().c_str(), &fiData)) < 0) {
         ROS_WARN("Failed to check filesystem stats.");
Index: CMakeLists.txt
===================================================================
--- CMakeLists.txt  (revision 10468)
+++ CMakeLists.txt  (working copy)
@@ -4,6 +4,7 @@
 set(ROS_BUILD_TYPE Debug)

 rosbuild_init()
+rosbuild_gensrv()

 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

GetStatus.srv

---
string target_filename
string write_filename
bool writing_enabled
string[] topics

SetStatus.srv

bool writing_enabled

---
bool success

[pirobot]

+1 This would be very useful.