Merge pull request #708 from osrf/better_bridge_bagger

Better bridge bagger
osrf · Nov 24, 2020 · a92da26 · a92da26
2 parents aab915a + 753292e
commit a92da26
Show file tree

Hide file tree

Showing 4 changed files with 39 additions and 5 deletions.
diff --git a/subt_ros/CMakeLists.txt b/subt_ros/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   topic_tools
   subt_msgs
   subt_communication_broker
+  rosbag
 )
 
 add_message_files(

diff --git a/subt_ros/launch/competition_init.launch b/subt_ros/launch/competition_init.launch
@@ -40,11 +40,6 @@
       name="set_pose_relay"/>
   </group>
 
-  <!-- rosbag a /robot_data topic which can be used by a team to record
-       custom data -->
-  <node pkg="rosbag" type="record" name="rosbag_robot_data"
-        args="record --split --size=1000 --max-splits=2 -O $(env HOME)/.ros/robot_data.bag -e '/robot_data(.*)'"/>
-
   <node pkg="tf2_ros" type="static_transform_publisher" name="tf_world_static" args="0 0 0 0 0 0 world $(arg world_name)"/>
 
 </launch>
diff --git a/subt_ros/package.xml b/subt_ros/package.xml
@@ -14,6 +14,7 @@
   <depend>ignition-math6</depend>
   <depend>ignition-msgs4</depend>
   <depend>ignition-transport7</depend>
+  <depend>rosbag</depend>
   <depend>theora_image_transport</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

diff --git a/subt_ros/src/SubtRosRelay.cc b/subt_ros/src/SubtRosRelay.cc
@@ -31,6 +31,7 @@
 #include <subt_communication_broker/protobuf/datagram.pb.h>
 #include <subt_communication_broker/common_types.h>
 #include <subt_ros/CompetitionClock.h>
+#include <rosbag/recorder.h>
 
 #include <ignition/transport/Node.hh>
 
@@ -166,6 +167,12 @@ class SubtRosRelay
 
   /// \brief Name of the robot this relay is associated with.
   public: std::vector<std::string> robotNames;
+
+  /// \brief Thread on which the ROS bag recorder runs.
+  public: std::unique_ptr<std::thread> bagThread = nullptr;
+
+  /// \brief Pointer to the ROS bag recorder.
+  public: std::unique_ptr<rosbag::Recorder> rosRecorder;
 };
 
 //////////////////////////////////////////////////
@@ -222,6 +229,26 @@ SubtRosRelay::SubtRosRelay()
     this->rosnode->advertise<std_msgs::Int32>("score", 1000);
   this->rosCompetitionClockPub =
     this->rosnode->advertise<subt_ros::CompetitionClock>("run_clock", 1000);
+
+  // Setup a ros bag recorder.
+  rosbag::RecorderOptions recorderOptions;
+  recorderOptions.append_date=false;
+  recorderOptions.split=true;
+  recorderOptions.max_splits=1;
+
+  // This equation is sourced from line 133 in
+  // http://docs.ros.org/en/noetic/api/rosbag/html/c++/record_8cpp_source.html
+  recorderOptions.max_size=1048576 * 1000;
+
+  recorderOptions.prefix="robot_data";
+  recorderOptions.regex=true;
+  recorderOptions.topics.push_back("/robot_data(.*)");
+
+  // Spawn thread for recording /subt/ data to rosbag.
+  this->rosRecorder.reset(new rosbag::Recorder(recorderOptions));
+  this->bagThread.reset(new std::thread([&](){
+        this->rosRecorder->run();
+  }));
 }
 
 //////////////////////////////////////////////////
@@ -255,6 +282,16 @@ void SubtRosRelay::OnCompetitionClock(const ignition::msgs::Clock &_msg)
   clockMsg.data.sec = _msg.sim().sec();
   clockMsg.data.nsec = _msg.sim().nsec();
   this->rosCompetitionClockPub.publish(clockMsg);
+
+  // Shutdown when the phase == finished. This makes sure that the rosbag
+  // ends cleanly.
+  if (clockMsg.phase == "finished" &&
+      this->bagThread && this->bagThread->joinable())
+  {
+    // Shutdown ros. this makes the ROS bag recorder stop.
+    ros::shutdown();
+    this->bagThread->join();
+  }
 }
 
 /////////////////////////////////////////////////