Fix #131: Portable binary read/write of Pointcloud and ScanGraph

- uint32_t for size, move inttypes.h include into octomap_types.h
OctoMap · Jan 13, 2017 · 1b2ef18 · 1b2ef18
1 parent a0915f6
commit 1b2ef18
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 7 deletions.
diff --git a/octomap/include/octomap/OcTreeKey.h b/octomap/include/octomap/OcTreeKey.h
@@ -40,7 +40,6 @@
 #include <ciso646>
 
 #include <assert.h>
-#include <inttypes.h>
 
 /* Libc++ does not implement the TR1 namespace, all c++11 related functionality
  * is instead implemented in the std namespace.

diff --git a/octomap/include/octomap/octomap_types.h b/octomap/include/octomap/octomap_types.h
@@ -37,6 +37,7 @@
 #include <stdio.h>
 #include <vector>
 #include <list>
+#include <inttypes.h>
 
 #include <octomap/math/Vector3.h>
 #include <octomap/math/Pose6D.h>

diff --git a/octomap/src/Pointcloud.cpp b/octomap/src/Pointcloud.cpp
@@ -43,6 +43,8 @@
 #endif
 #include <fstream>
 #include <math.h>
+#include <assert.h>
+#include <limits>
 
 #include <octomap/Pointcloud.h>
 
@@ -284,14 +286,14 @@ namespace octomap {
 
   std::istream& Pointcloud::readBinary(std::istream &s) {
 
-    unsigned int pc_size = 0;
+    uint32_t pc_size = 0;
     s.read((char*)&pc_size, sizeof(pc_size));
     OCTOMAP_DEBUG("Reading %d points from binary file...", pc_size);
 
     if (pc_size > 0) {
       this->points.reserve(pc_size);
       point3d p;
-      for (unsigned int i=0; i<pc_size; i++) {
+      for (uint32_t i=0; i<pc_size; i++) {
         p.readBinary(s);
         if (!s.fail()) {
           this->push_back(p);
@@ -302,6 +304,8 @@ namespace octomap {
         }
       }
     }
+    assert(pc_size == this->size());
+
     OCTOMAP_DEBUG("done.\n");
 
     return s;
@@ -310,8 +314,15 @@ namespace octomap {
 
   std::ostream& Pointcloud::writeBinary(std::ostream &s) const {
 
-    size_t pc_size = this->size();
-    OCTOMAP_DEBUG("Writing %lu points to binary file...", (unsigned long)pc_size);
+    // check if written unsigned int can hold size
+    size_t orig_size = this->size();
+    if (orig_size > std::numeric_limits<uint32_t>::max()){
+      OCTOMAP_ERROR("Pointcloud::writeBinary ERROR: Point cloud too large to be written");
+      return s;
+    }
+
+    uint32_t pc_size = static_cast<uint32_t>(this->size());
+    OCTOMAP_DEBUG("Writing %u points to binary file...", pc_size);
     s.write((char*)&pc_size, sizeof(pc_size));
 
     for (Pointcloud::const_iterator it = this->begin(); it != this->end(); it++) {

diff --git a/octomap/src/ScanGraph.cpp b/octomap/src/ScanGraph.cpp
@@ -55,7 +55,9 @@ namespace octomap {
 
     scan->writeBinary(s);
     pose.writeBinary(s);
-    s.write((char*)&id, sizeof(id));
+
+    uint32_t uintId = static_cast<uint32_t>(id);
+    s.write((char*)&uintId, sizeof(uintId));
 
     return s;
   }
@@ -67,7 +69,9 @@ namespace octomap {
 
     this->pose.readBinary(s);
 
-    s.read((char*)&this->id, sizeof(this->id));
+    uint32_t uintId;
+    s.read((char*)&uintId, sizeof(uintId));
+    this->id = uintId;
 
     return s;
   }