Ros2gams shield schema (#50)

* adaptions in ros2gams to be able to handle list renaming and ignore members * fixed bugs caused by changes to ReferenceFrame in ros2gams * performance improvements * added more comments to rosparser for ros2gams * fixed problems with capnproto enums and wrong agent prefix in ros2gams * added progressbar to ros2gams * metadata in checkpoints optional for ros2gams and added PointCloudXYZ * PointCloud PCL support for ros2gams * improved manifest file handling and contents for ros2gams * Fixed bug in Datatypes.h for new PointCloud types
jredmondson · Oct 2, 2018 · 4d4de85 · 4d4de85
1 parent c2f870e
commit 4d4de85
Show file tree

Hide file tree

Showing 14 changed files with 437 additions and 110 deletions.
diff --git a/scripts/linux/base_build.sh b/scripts/linux/base_build.sh
@@ -530,7 +530,7 @@ if [ $PREREQS -eq 1 ] && [ $MAC -eq 0 ]; then
   fi #android condition ends
 
   if [ $ROS -eq 1 ]; then
-    sudo apt-get install -y ros-kinetic-desktop-full python-rosinstall ros-kinetic-ros-type-introspection ros-kinetic-move-base-msgs ros-kinetic-navigation libactionlib-dev libactionlib-msgs-dev libmove-base-msgs-dev
+    sudo apt-get install -y ros-kinetic-desktop-full python-rosinstall ros-kinetic-ros-type-introspection ros-kinetic-move-base-msgs ros-kinetic-navigation libactionlib-dev libactionlib-msgs-dev libmove-base-msgs-dev ros-kinetic-pcl-conversions ros-kinetic-pcl-ros libpcl-dev
 
     if [ $ROS_FIRST_SETUP -eq 1 ]; then
       sudo rosdep init

diff --git a/scripts/linux/zip_dep_bin.sh b/scripts/linux/zip_dep_bin.sh
@@ -22,7 +22,7 @@ if [ $PRINT_USAGE -eq 0 ] ; then
   SCRIPT=${BIN}.sh
   ZIP=${BIN}.zip
 
-  DEPS=$(ldd $BIN | cut -d' ' -f3 | egrep "(/opt|/home)")
+  DEPS=$(ldd $BIN | cut -d' ' -f3 | egrep "(/opt|/home|pcl)")
 
   mkdir -p $DIR
 

diff --git a/src/gams/programs/ros2gams.cpp b/src/gams/programs/ros2gams.cpp
@@ -65,7 +65,6 @@
 #pragma GCC diagnostic pop
 #endif
 
-
 namespace logger = madara::logger;
 namespace knowledge = madara::knowledge;
 namespace containers = knowledge::containers;
@@ -80,22 +79,28 @@ std::string rosbag_path = "";
 //std::string rosbag_robot_prefix = "robot_";
 
 // prefix for the madara checkoints
-std::string checkpoint_prefix = "checkpoint_";
+std::string checkpoint_prefix = "checkpoint";
 
 // path to the filter definition file
 std::string map_file = "";
 
 //path to the checkpoint stream file
 std::string stream_file = "";
 
+//path to the manifest file
+std::string manifest_file = "";
+
 // save as a karl or binary file
 bool save_as_karl = false;
 bool save_as_json = false;
 bool save_as_binary = false;
 bool save_as_stream = false;
 
+//Metadata in the checkpoints
+bool store_metadata = false;
+
 //Buffer size
-int write_buffer_size = 10240000;
+int write_buffer_size = 1024*1024*100;
 
 // the world and the base frame of the robot
 std::string base_frame = "";
@@ -137,6 +142,10 @@ void handle_arguments (int argc, char ** argv)
       checkpoint_prefix = argv[i + 1];
       i++;
     }
+    else if (arg1 == "-meta")
+    {
+      store_metadata = true;
+    }
     else if (arg1 == "-m" || arg1 == "--map-file")
     {
       if (i + 1 < argc)
@@ -154,6 +163,14 @@ void handle_arguments (int argc, char ** argv)
       }
       i++;
     }
+    else if (arg1 == "-mp" || arg1 == "--manifest-path")
+    {
+      if (i + 1 < argc)
+      {
+        manifest_file = argv[i + 1];
+      }
+      i++;
+    }
     else if (arg1 == "-rp" || arg1 == "--ros-robot-prefix")
     {
       //rosbag_robot_prefix = argv[i + 1];
@@ -215,6 +232,8 @@ void handle_arguments (int argc, char ** argv)
       "                                       base as a karl checkpoint"\
                                               "(default)\n" \
       "  [-m|--map-file file]                 File with filter information\n" \
+      "  [-meta]                              stores metadata in the checkpoints\n" \
+      "  [-mp|--manifest-path file]           path to the manifest file\n" \
       "  [-y|--frequency hz]                  Checkpoint frequency\n" \
       "                                       (default:checkpoint with each\n" \
       "                                        message in the bagfile)\n" \
@@ -384,10 +403,6 @@ int main (int argc, char ** argv)
       RosIntrospection::ROSType (datatype), definition);
   }
 
-  /*std::vector<RosIntrospection::SubstitutionRule> rules;
-  rules.push_back( RosIntrospection::SubstitutionRule("angle_min", "angle_min", "aaa@") );
-  parser.registerRenamingRules( RosIntrospection::ROSType ("sensor_msgs/LaserScan"), rules);*/
-  // Name substitution
   parser.register_rename_rules(name_substitution_map);
 
   // Load the capnproto schemas
@@ -417,10 +432,12 @@ int main (int argc, char ** argv)
   }
 
   // Attach streaming
+
+  madara::knowledge::CheckpointSettings stream_settings;
   if (save_as_stream)
   {
-    madara::knowledge::CheckpointSettings stream_settings;
     stream_settings.filename = stream_file + ".stk";
+    stream_settings.buffer_size = write_buffer_size;
     delete_existing_file(stream_settings.filename, delete_existing);
 
     //kb.attach_streamer(std::move(stream_settings), kb, 100);
@@ -431,6 +448,9 @@ int main (int argc, char ** argv)
 
   // Iterate through all topics in the bagfile
   std::cout << "Converting...\n";
+  float progress;
+  int message_index = 0;
+  int progress_bar_width = 70;
   for (const rosbag::MessageInstance m: view)
   {
     std::string topic = m.getTopic ();
@@ -502,18 +522,55 @@ int main (int argc, char ** argv)
     }
     settings.last_lamport_clock += 1;
 
+    // Printing the progress bar
+    // See https://stackoverflow.com/questions/14539867/how-to-display-a-progress-indicator-in-pure-c-c-cout-printf
+    message_index++;
+    progress = message_index / float(view.size ());
+    std::cout << "[";
+    int pos = progress_bar_width * progress;
+    for (int i = 0; i < progress_bar_width; ++i) {
+        if (i < pos)
+        {
+          std::cout << "=";
+        }
+        else if (i == pos)
+        {
+          std::cout << ">";
+        }
+        else
+        {
+          std::cout << " ";
+        }
+    }
+    std::cout << "] " << int(progress * 100.0) << " % " << message_index << "/"
+      << view.size() << "\r";
+    std::cout.flush();
   }
+  std::cout << std::endl << "done" << std::endl;
 
-  // Storing stats in the manifest knowledge
-  manifest.set ("last_timestamp", (Integer) settings.last_timestamp);
-  manifest.set ("initial_timestamp", (Integer) settings.initial_timestamp);
-  manifest.set ("duration_ns", (Integer)
-    ( settings.last_timestamp - settings.initial_timestamp));
-  manifest.set ("last_lamport_clock", (Integer) settings.last_lamport_clock-1);
-  manifest.set ("initial_lamport_clock",
-    (Integer) settings.initial_lamport_clock);
-  manifest.set ("last_checkpoint_id", checkpoint_id-1);
-  manifest.save_as_karl(checkpoint_prefix + "_manifest.kb");
+  if (manifest_file != "")
+  {
+    // Storing stats in the manifest knowledge
+    manifest.set ("last_timestamp", (Integer) settings.last_timestamp);
+    manifest.set ("initial_timestamp", (Integer) settings.initial_timestamp);
+    manifest.set ("last_lamport_clock", (Integer) settings.last_lamport_clock-1);
+    manifest.set ("initial_lamport_clock",
+      (Integer) settings.initial_lamport_clock);
+    manifest.set ("last_checkpoint_id", checkpoint_id-1);
+
+    // ROSBAG infos
+    manifest.set ("name", bag.getFileName());
+    ros::Time begin_time = view.getBeginTime ();
+    ros::Time end_time = view.getEndTime ();
+    manifest.set ("date", begin_time.toNSec ());
+    manifest.set ("duration", (end_time - begin_time).toNSec ());
+    if (save_as_stream)
+    {
+      manifest.set ("size", 
+        boost::filesystem::file_size(stream_settings.filename));
+    }
+    manifest.save_as_karl (manifest_file + ".mf");
+  }
 }
 #endif
 
@@ -526,20 +583,17 @@ int main (int argc, char ** argv)
 int save_checkpoint (knowledge::KnowledgeBase * knowledge,
     knowledge::CheckpointSettings * settings, std::string meta_prefix)
 {
-  /*knowledge->set (meta_prefix + ".originator",
-        settings->originator);
-  knowledge->set (meta_prefix + ".version",
-  settings->version);
-  knowledge->set (meta_prefix + ".current_timestamp",
-    (Integer) time (NULL));*/
-  knowledge->set (meta_prefix + ".last_timestamp",
-    (Integer) settings->last_timestamp);
-  knowledge->set (meta_prefix + ".initial_timestamp",
-    (Integer) settings->initial_timestamp);
-  knowledge->set (meta_prefix + ".last_lamport_clock",
-    (Integer) settings->last_lamport_clock);
-  knowledge->set (meta_prefix + ".initial_lamport_clock",
-    (Integer) settings->initial_lamport_clock);
+  if ( store_metadata )
+  {
+    knowledge->set (meta_prefix + ".last_timestamp",
+      (Integer) settings->last_timestamp);
+    knowledge->set (meta_prefix + ".initial_timestamp",
+      (Integer) settings->initial_timestamp);
+    knowledge->set (meta_prefix + ".last_lamport_clock",
+      (Integer) settings->last_lamport_clock);
+    knowledge->set (meta_prefix + ".initial_lamport_clock",
+      (Integer) settings->initial_lamport_clock);
+  }
 
   int ret = 0;
   std::string filename = settings->filename;

diff --git a/src/gams/types/Datatypes.h b/src/gams/types/Datatypes.h
@@ -26,13 +26,15 @@
 #include "Joy.capnp.h"
 #include "LaserScan.capnp.h"
 #include "MagneticField.capnp.h"
-#include "PointCloud.capnp.h"
-#include "PointCloud2.capnp.h"
+#include "PointCloudXYZ.capnp.h"
+#include "PointCloudXYZI.capnp.h"
 #include "PointField.capnp.h"
 #include "Temperature.capnp.h"
 #include "TimeReference.capnp.h"
 #include "RegionOfInterest.capnp.h"
 #include "Point.capnp.h"
+#include "PointXYZ.capnp.h"
+#include "PointXYZI.capnp.h"
 #include "MapMetaData.capnp.h"
 #include "Pose.capnp.h"
 #include "Quaternion.capnp.h"
@@ -75,13 +77,15 @@ namespace gams
       mk::Any::register_type<mk::CapnObject<Joy> >("Joy");
       mk::Any::register_type<mk::CapnObject<LaserScan> >("LaserScan");
       mk::Any::register_type<mk::CapnObject<MagneticField> >("MagneticField");
-      mk::Any::register_type<mk::CapnObject<PointCloud> >("PointCloud");
-      mk::Any::register_type<mk::CapnObject<PointCloud2> >("PointCloud2");
+      mk::Any::register_type<mk::CapnObject<PointCloudXYZ> >("PointCloudXYZ");
+      mk::Any::register_type<mk::CapnObject<PointCloudXYZI> >("PointCloudXYZI");
       mk::Any::register_type<mk::CapnObject<PointField> >("PointField");
       mk::Any::register_type<mk::CapnObject<Temperature> >("Temperature");
       mk::Any::register_type<mk::CapnObject<TimeReference> >("TimeReference");
       mk::Any::register_type<mk::CapnObject<RegionOfInterest> >("RegionOfInterest");
       mk::Any::register_type<mk::CapnObject<Point> >("Point");
+      mk::Any::register_type<mk::CapnObject<PointXYZ> >("PointXYZ");
+      mk::Any::register_type<mk::CapnObject<PointXYZI> >("PointXYZI");
       mk::Any::register_type<mk::CapnObject<MapMetaData> >("MapMetaData");
       mk::Any::register_type<mk::CapnObject<Pose> >("Pose");
       mk::Any::register_type<mk::CapnObject<Quaternion> >("Quaternion");

diff --git a/src/gams/types/PointCloud.capnp b/src/gams/types/PointCloud.capnp
diff --git a/src/gams/types/PointCloud2.capnp b/src/gams/types/PointCloud2.capnp
diff --git a/src/gams/types/PointCloudXYZ.capnp b/src/gams/types/PointCloudXYZ.capnp
@@ -0,0 +1,20 @@
+@0xcf7419e96f91ec54;
+
+# Namespace setup
+using Cxx = import "/capnp/c++.capnp";
+$Cxx.namespace("gams::types");
+
+using import "PointXYZ.capnp".PointXYZ;
+
+# TODO use capnproto generics (Templates) to unduplicate this and PointCloudXYZI
+
+# Type definition
+struct PointCloudXYZ {
+  tov @0 :UInt64;
+  frameId @1 :Text;
+  width @2 :UInt32;
+  height @3 :UInt32;
+  isDense @4 :Bool;
+  points @5 :List(PointXYZ);
+
+}
diff --git a/src/gams/types/PointCloudXYZI.capnp b/src/gams/types/PointCloudXYZI.capnp
@@ -0,0 +1,21 @@
+@0xa62f4365c6b2a67c;
+
+# Namespace setup
+using Cxx = import "/capnp/c++.capnp";
+$Cxx.namespace("gams::types");
+
+using import "PointXYZI.capnp".PointXYZI;
+
+# TODO use capnproto generics (Templates) to unduplicate this and PointCloudXYZ
+
+# Type definition
+struct PointCloudXYZI {
+  tov @0 :UInt64;
+  frameId @1 :Text;
+  width @2 :UInt32;
+  height @3 :UInt32;
+  isDense @4 :Bool;
+  points @5 :List(PointXYZI);
+
+}
+
diff --git a/src/gams/types/PointXYZ.capnp b/src/gams/types/PointXYZ.capnp
@@ -0,0 +1,15 @@
+@0xb8c9b16fb51cc7cc;
+
+# Namespace setup
+using Cxx = import "/capnp/c++.capnp";
+$Cxx.namespace("gams::types");
+
+# Capnfile Imports
+
+# Type definition
+struct PointXYZ {
+   y @0: Float32;
+   x @1: Float32;
+   z @2: Float32;
+
+}
diff --git a/src/gams/types/PointXYZI.capnp b/src/gams/types/PointXYZI.capnp
@@ -0,0 +1,14 @@
+@0xc94eebdb0ddcfeb7;
+
+# Namespace setup
+using Cxx = import "/capnp/c++.capnp";
+$Cxx.namespace("gams::types");
+
+# Type definition
+struct PointXYZI {
+   y @0 :Float32;
+   x @1 :Float32;
+   z @2 :Float32;
+   intensity @3 :Float32;
+
+}