reconfigured for building large dataset

frank-Regal · Apr 30, 2024 · d51e334 · d51e334
1 parent 1323a5c
commit d51e334
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 12 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,6 +1,10 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(bag_utility)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   rosbag

diff --git a/launch/save_bags.launch b/launch/save_bags.launch
@@ -2,6 +2,7 @@
 
     <!-- input args -->
     <arg name="connect_rf_client" default='false'/>
+    <arg name="use_param_server" default='false'/>
 
     <!-- robofleet client group -->
     <group if="$(arg connect_rf_client)" ns="bag_utility_rf_client"> 
@@ -28,6 +29,7 @@
         <!-- set params -->
         <param name="output_directory" value="$(arg path)" />
         <param name="postfix" value="$(arg name)" />
+        <param name="use_param_server" value="$(arg use_param_server)"/>
 
         <!--launch save_bags node -->
         <node name="save_bags" pkg="bag_utility" type="save_bags" output="screen"/>

diff --git a/src/save_bags.cpp b/src/save_bags.cpp
@@ -2,6 +2,7 @@
 #include <chrono>
 #include <vector>
 #include <functional>
+#include <filesystem>
 
 #include "rosbag/bag.h"
 #include <ros/ros.h>
@@ -21,11 +22,7 @@ class SaveBags {
      */
     SaveBags(
         ros::NodeHandle& Nh):
-        nh_(Nh),
-        file_timestamp_("new"),
-        post_fix_("data"),
-        output_directory_("~/"),
-        is_recording_(false) {
+        SaveBags(Nh, std::string("~/"), std::string("data"), false) {
     }
 
     /**
@@ -37,13 +34,18 @@ class SaveBags {
      */
     SaveBags(
         ros::NodeHandle& Nh,
-        std::string& OutputDirectory,
-        std::string& PostFix):
+        std::string OutputDirectory,
+        std::string PostFix,
+        bool UseParamServer):
         nh_(Nh),
         file_timestamp_("new"),
         post_fix_(PostFix),
         output_directory_(OutputDirectory),
+        use_param_server_(UseParamServer),
         is_recording_(false) {
+
+        node_ns_ = nh_.getNamespace();
+        std::filesystem::create_directories(output_directory_);
     }
 
     /**
@@ -149,13 +151,20 @@ class SaveBags {
     {
         if(is_recording_){ForceCloseBag();}
 
-        GetTimeStamp(file_timestamp_);
-        bag_name_ = file_timestamp_ + post_fix_ + ".bag";
+        if(!use_param_server_){
+            GetTimeStamp(file_timestamp_);
+            bag_name_ = file_timestamp_ + post_fix_ + ".bag";
+        } else {
+            GetParamServerFilename(filename_);
+            bag_name_ = filename_ + ".bag";
+        }
+
         std::string out_path = output_directory_ + "/" + bag_name_;
         bag_.open(out_path, rosbag::bagmode::Write);
         is_recording_ = true;
 
-        ROS_INFO("'%s' opened.\n\nWriting ...\n", bag_name_.c_str());
+        //ROS_INFO("[bag_utility] opened: '%s' Writing ...", out_path.c_str());
+        std::cout << "[bag_utility] opened: '" << out_path << "' Writing ..." << std::endl;
 
         WriteToBag<std_msgs::Empty>(Msg, TopicName);
     }
@@ -182,12 +191,15 @@ class SaveBags {
     // init
     std::vector<ros::Subscriber> subscribers_;
     std::string file_timestamp_;
+    std::string filename_;
     std::string output_directory_;
     std::string post_fix_;
     std::string bag_name_;
+    std::string node_ns_;
     ros::NodeHandle nh_;
     rosbag::Bag bag_;
     bool is_recording_;
+    bool use_param_server_;
 
     /**
      * @brief Get current timestamp
@@ -203,6 +215,12 @@ class SaveBags {
         TimeStamp = ss.str();
     }
 
+    void GetParamServerFilename(std::string& Filename)
+    {
+        std::string filename_param {node_ns_ + "/filename"};
+        if (!nh_.getParam(filename_param, Filename)) {ROS_ERROR("Filename not set");}
+    }
+
     /**
      * @brief Overloaded template function to create a subscriber
      * 
@@ -290,14 +308,15 @@ int main(int argc, char** argv) {
 
     // load file output directory and postfix
     std::string output_directory, post_fix;
-    if(nh.getParam("output_directory", output_directory) && nh.getParam("postfix", post_fix)) {
+    bool use_param_server;
+    if(nh.getParam("output_directory", output_directory) && nh.getParam("postfix", post_fix) && nh.getParam("use_param_server", use_param_server)) {
         ROS_INFO("\n\tSaving bags to: '%s'\n\tFiles with be post fixed with: '%s'", output_directory.c_str(), post_fix.c_str()); 
     } else {
         ROS_ERROR("Faild to load 'output_directory' and 'post_fixed' params.");
     }
 
     // init class
-    SaveBags save_bags(nh, output_directory, post_fix);
+    SaveBags save_bags(nh, output_directory, post_fix, use_param_server);
 
     // init topic subscribers
     save_bags.SubscribeToTopic<std_msgs::Empty>("start_capture_topicname_", 0, &SaveBags::StartRecordingBag); // topic starts bag recording