Test rosbag c++

Signed-off-by: Nate Koenig <[email protected]>
petrlmat · Sep 30, 2020 · eff2b2c · eff2b2c
1 parent f6267e3
commit eff2b2c
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 2 deletions.
diff --git a/docker/cloudsim_sim/run_sim.bash b/docker/cloudsim_sim/run_sim.bash
@@ -13,6 +13,9 @@ bwm-ng -o csv -c 0 -t 1000 -T rate -I eth0 >> $FILE &
 # atop process monitoring
 atop -R -w /tmp/ign/logs/atop_log &
 
+# trap "rosnode kill /cloudsim_bagger" SIGINT
+# trap "rosnode kill /cloudsim_bagger" SIGTERM
+
 export ROS_LOG_DIR=/tmp/ign/logs/ros
-rosbag record --split --size=1000 -O /tmp/ign/logs/cloudsim.bag -e '/subt/.*' &
+# rosbag record --split --size=1000 -O /tmp/ign/logs/cloudsim.bag -e '/subt/.*' __name:=cloudsim_bagger &
 ign launch -v 4 $@
diff --git a/docker/json2docker.rb b/docker/json2docker.rb
@@ -209,7 +209,7 @@
   # In this example, two robots are started with the names X1 and X2.
   sim:
     image: #{options['sim_image']}
-    command: cloudsim_sim.ign headless:=#{submission['headless']} circuit:=#{submission['circuit']} worldName:=#{submission['world']} #{robotStr}
+    command: cloudsim_sim.ign headless:=#{submission['headless']} circuit:=#{submission['circuit']} ros:=true worldName:=#{submission['world']} #{robotStr}
     networks:
       sim_net:
         ipv4_address: 172.28.1.1

diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt
@@ -20,6 +20,7 @@ find_package(catkin REQUIRED
     subt_communication_model
     subt_communication_broker
     subt_ros
+    rosbag
 )
 
 find_package(ignition-gazebo2 2.1 QUIET REQUIRED)

diff --git a/subt_ign/package.xml b/subt_ign/package.xml
@@ -21,6 +21,7 @@
 
   <depend>libccd-dev</depend>
   <depend>libfcl-dev</depend>
+  <depend>rosbag</depend>
 
   <test_depend>rostest</test_depend>
 </package>
diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc
@@ -17,6 +17,7 @@
 
 #include <yaml-cpp/yaml.h>
 #include <ros/ros.h>
+#include <rosbag/recorder.h>
 
 #include <ignition/msgs/boolean.pb.h>
 #include <ignition/msgs/float.pb.h>
@@ -161,6 +162,7 @@ class subt::GameLogicPluginPrivate
   /// \brief Publish the score.
   /// \param[in] _event Unused.
   public: void PublishScore();
+  public: void RosBag();
 
   /// \brief Log robot pos data
   public: void LogRobotPosData();
@@ -314,6 +316,9 @@ class subt::GameLogicPluginPrivate
   /// \brief Thread on which scores are published
   public: std::unique_ptr<std::thread> publishThread = nullptr;
 
+  /// \brief Thread on which scores are published
+  public: std::unique_ptr<std::thread> bagThread = nullptr;
+
   /// \brief Whether the task has started.
   public: bool started = false;
 
@@ -556,6 +561,7 @@ class subt::GameLogicPluginPrivate
   public: ros::Publisher rosRegionEventPub;
   public: std::map<std::string, ros::Publisher> rosRobotPosePubs;
   public: std::map<std::string, ros::Publisher> rosRobotKinematicPubs;
+  public: rosbag::Recorder *rosRecorder;
   public: std::string prevPhase = "";
 
   /// \brief Counter to create unique id for events
@@ -578,6 +584,11 @@ GameLogicPlugin::~GameLogicPlugin()
   this->dataPtr->finished = true;
   if (this->dataPtr->publishThread)
     this->dataPtr->publishThread->join();
+
+  // Shutdown ros
+  ros::shutdown();
+  if (this->dataPtr->bagThread)
+    this->dataPtr->bagThread->join();
 }
 
 //////////////////////////////////////////////////
@@ -665,6 +676,17 @@ void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/,
       this->dataPtr->rosRegionEventPub =
         this->dataPtr->rosnode->advertise<subt_ros::RegionEvent>(
             "region_event", 100);
+
+      // Setup a ros bag recorder.
+      rosbag::RecorderOptions recorderOptions;
+      recorderOptions.append_date=false;
+      recorderOptions.prefix="/tmp/ign/logs/cloudsim";
+      recorderOptions.regex=true;
+      recorderOptions.topics.push_back("/subt/.*");
+
+      this->dataPtr->rosRecorder = new rosbag::Recorder(recorderOptions);
+      this->dataPtr->bagThread.reset(new std::thread(
+        &GameLogicPluginPrivate::RosBag, this->dataPtr.get()));
     }
   }
 
@@ -2117,6 +2139,14 @@ void GameLogicPluginPrivate::ParseArtifacts(
   }
 }
 
+/////////////////////////////////////////////////
+void GameLogicPluginPrivate::RosBag()
+{
+  this->rosRecorder->run();
+  delete this->rosRecorder;
+  this->rosRecorder = nullptr;
+}
+
 /////////////////////////////////////////////////
 void GameLogicPluginPrivate::PublishScore()
 {