Initial commit of primitive_array type

lib/message_parser.cc

@@ -9,15 +9,15 @@
 
 namespace Embag {
 
-const RosValue::RosValuePointer MessageParser::parse() {
+const RosValue::Pointer MessageParser::parse() {
   // The lowest number of RosValues occurs when we have a message with only doubles in a single type.
   // The number of RosValues in this case is the number of doubles that can fit in our buffer,
   // plus one for the RosValue object that will point to all the doubles.
   ros_values_->reserve(message_buffer_->size() / sizeof(double) + 1);
   ros_values_->emplace_back(msg_def_.fieldIndexes());
   ros_values_offset_ = 1;
   initObject(0, msg_def_);
-  return RosValue::RosValuePointer(ros_values_);
+  return RosValue::Pointer(ros_values_);
 }
 
 void MessageParser::initObject(size_t object_offset, const RosMsgTypes::BaseMsgDef &object_definition) {
@@ -42,7 +42,9 @@ void MessageParser::initObject(size_t object_offset, const RosMsgTypes::BaseMsgD
           initObject(child_offset, embedded_type);
           break;
         }
-        case RosValue::Type::array: {
+        case RosValue::Type::array:
+        case RosValue::Type::primitive_array:
+        {
           initArray(child_offset, field);
           break;
         }
@@ -63,8 +65,10 @@ void MessageParser::emplaceField(const RosMsgTypes::FieldDef &field) {
       auto& object_definition = field.typeDefinition();
       ros_values_->emplace_back(object_definition.fieldIndexes());
     }
-  } else {
+  } else if (field.type() == RosValue::Type::object || field.type() == RosValue::Type::string) {
     ros_values_->emplace_back(RosValue::_array_identifier());
+  } else {
+    ros_values_->emplace_back(field.type(), message_buffer_);
   }
 
   ++ros_values_offset_;
@@ -78,30 +82,38 @@ void MessageParser::initArray(size_t array_offset, const RosMsgTypes::FieldDef &
   } else {
     array_length = static_cast<uint32_t>(field.arraySize());
   }
-  const size_t children_offset = ros_values_offset_;
-  ros_values_offset_ += array_length;
 
-  ros_values_->at(array_offset).array_info_.children.length = array_length;
-  ros_values_->at(array_offset).array_info_.children.base = ros_values_;
-  ros_values_->at(array_offset).array_info_.children.offset = children_offset;
+  const RosValue::Type field_type = field.type();
+  if (field_type == RosValue::Type::object || field_type == RosValue::Type::string) {
+    const size_t children_offset = ros_values_offset_;
+    ros_values_offset_ += array_length;
 
-  if (field.type() != RosValue::Type::object) {
-    for (size_t i = 0; i < array_length; ++i) {
-      ros_values_->emplace_back(field.type());
-    }
+    ros_values_->at(array_offset).array_info_.children.length = array_length;
+    ros_values_->at(array_offset).array_info_.children.base = ros_values_;
+    ros_values_->at(array_offset).array_info_.children.offset = children_offset;
 
-    for (size_t i = 0; i < array_length; ++i) {
-      initPrimitive(children_offset + i, field);
-    }
-  } else {
-    auto& object_definition = field.typeDefinition();
-    for (size_t i = 0; i < array_length; ++i) {
-      ros_values_->emplace_back(object_definition.fieldIndexes());
-    }
+    if (field_type == RosValue::Type::string) {
+      for (size_t i = 0; i < array_length; ++i) {
+        ros_values_->emplace_back(field.type());
+      }
+
+      for (size_t i = 0; i < array_length; ++i) {
+        initPrimitive(children_offset + i, field);
+      }
+    } else {
+      auto& object_definition = field.typeDefinition();
+      for (size_t i = 0; i < array_length; ++i) {
+        ros_values_->emplace_back(object_definition.fieldIndexes());
+      }
 
-    for (size_t i = 0; i < array_length; ++i) {
-      initObject(children_offset + i, object_definition);
+      for (size_t i = 0; i < array_length; ++i) {
+        initObject(children_offset + i, object_definition);
+      }
     }
+  } else {
+    ros_values_->at(array_offset).primitive_array_info_.length = array_length;
+    ros_values_->at(array_offset).primitive_array_info_.offset = message_buffer_offset_;
+    message_buffer_offset_ += array_length * field.typeSize();
   }
 }
 

lib/message_parser.h

@@ -24,7 +24,7 @@ class MessageParser {
   {
   };
 
-  const RosValue::RosValuePointer parse();
+  const RosValue::Pointer parse();
 
  private:
   static std::unordered_map<std::string, size_t> primitive_size_map_;

lib/ros_message.h

@@ -24,7 +24,7 @@ class RosMessage {
   {
   }
 
-  const RosValue::RosValuePointer &data() {
+  const RosValue::Pointer &data() {
     if (!parsed_) {
       hydrate();
     }
@@ -58,7 +58,7 @@ class RosMessage {
 
  private:
   bool parsed_ = false;
-  RosValue::RosValuePointer data_;
+  RosValue::Pointer data_;
   std::shared_ptr<RosMsgTypes::MsgDef> msg_def_;
 
   void hydrate() {

lib/ros_msg_types.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include <unordered_map>
-#include <boost/variant.hpp>
 
 #include "ros_value.h"
 

lib/ros_value.cc

@@ -5,28 +5,41 @@
 
 namespace Embag {
 
-const RosValue::RosValuePointer RosValue::operator()(const std::string &key) const {
+const RosValue::Pointer RosValue::ros_value_list_t::at(size_t index) const {
+  if (index >= length) {
+    throw std::out_of_range("Provided index is out of range!");
+  }
+
+  return RosValue::Pointer(base, offset + index);
+}
+
+const RosValue::Pointer RosValue::operator()(const std::string &key) const {
   return get(key);
 }
 
-const RosValue::RosValuePointer RosValue::operator[](const std::string &key) const {
+const RosValue::Pointer RosValue::operator[](const std::string &key) const {
   return get(key);
 }
 
-const RosValue::RosValuePointer RosValue::operator[](const size_t idx) const {
+const RosValue::Pointer RosValue::operator[](const size_t idx) const {
   return at(idx);
 }
 
-const RosValue::RosValuePointer RosValue::at(const std::string &key) const {
+const RosValue::Pointer RosValue::at(const std::string &key) const {
   return get(key);
 }
 
-const RosValue::RosValuePointer RosValue::get(const std::string &key) const {
+template<typename T>
+const T &RosValue::getValue(const std::string &key) const {
+  return get(key)->as<T>();
+}
+
+const RosValue::Pointer RosValue::get(const std::string &key) const {
   if (type_ != Type::object) {
     throw std::runtime_error("Value is not an object");
   }
 
-  return object_info_.children.at(object_info_.field_indexes->at(key));
+  return at(object_info_.field_indexes->at(key));
 }
 
 template<>
@@ -40,12 +53,69 @@ const std::string RosValue::as<std::string>() const {
   return std::string(string_loc, string_loc + string_length);
 }
 
-const RosValue::RosValuePointer RosValue::at(const size_t idx) const {
-  if (type_ != Type::array) {
-    throw std::runtime_error("Value is not an array");
+const RosValue::Pointer RosValue::at(const size_t idx) const {
+  if (type_ == Type::object) {
+    return object_info_.children.at(idx);
+  } else if (type_ == Type::array) {
+    return array_info_.children.at(idx);
+  } else if (type_ == Type::primitive_array) {
+    return RosValue::Pointer(
+      primitive_array_info_.element_type,
+      primitive_array_info_.message_buffer,
+      primitive_array_info_.offset + idx
+    );
+  } else {
+    throw std::runtime_error("Value is not an array or object");
+  }
+}
+
+std::unordered_map<std::string, RosValue::Pointer> RosValue::getObjects() const {
+  if (type_ != Type::object) {
+    throw std::runtime_error("Cannot getObjects of a non-object RosValue");
+  }
+
+  std::unordered_map<std::string, RosValue::Pointer> objects;
+  objects.reserve(object_info_.children.length);
+  for (const auto& field : *object_info_.field_indexes) {
+    objects.emplace(field.first, at(field.second));
+  }
+  return objects;
+}
+
+std::vector<RosValue::Pointer> RosValue::getValues() const {
+  if (type_ != Type::object && type_ != Type::array && type_ != Type::primitive_array) {
+    throw std::runtime_error("Cannot getValues of a non object or array RosValue");
+  }
+
+  std::vector<RosValue::Pointer> ros_value_pointers;
+  const size_t size = this->size();
+  ros_value_pointers.reserve(size);
+  for (size_t i = 0; i < size; i++) {
+    ros_value_pointers.push_back(at(i));
+  }
+
+  return ros_value_pointers;
+}
+
+pybind11::buffer_info RosValue::getPrimitiveArrayBufferInfo() {
+  if (type_ != Embag::RosValue::Type::primitive_array) {
+    throw std::runtime_error("Only primitive arrays can be represented as buffers!");
+  }
+
+  if (primitive_array_info_.element_type == Embag::RosValue::Type::string) {
+    throw std::runtime_error("In order to be represented as a buffer, an array's elements must not be strings!");
   }
 
-  return array_info_.children.at(idx);
+  const size_t size_of_elements = Embag::RosValue::primitiveTypeToSize(primitive_array_info_.element_type);
+  return pybind11::buffer_info(
+    (void*) &at(0)->getPrimitive<uint8_t>(),
+    size_of_elements,
+    Embag::RosValue::primitiveTypeToFormat(primitive_array_info_.element_type),
+    1,
+    { size() },
+    { size_of_elements },
+    true
+  );
 }
 
 std::string RosValue::toString(const std::string &path) const {
@@ -214,7 +284,7 @@ const std::string& RosValue::const_iterator<const std::string&, std::unordered_m
 }
 
 template<>
-const std::pair<const std::string&, const RosValue::RosValuePointer> RosValue::const_iterator<const std::pair<const std::string&, const RosValue::RosValuePointer>, std::unordered_map<std::string, size_t>::const_iterator>::operator*() const {
+const std::pair<const std::string&, const RosValue::Pointer> RosValue::const_iterator<const std::pair<const std::string&, const RosValue::Pointer>, std::unordered_map<std::string, size_t>::const_iterator>::operator*() const {
   return std::make_pair(index_->first, value_.object_info_.children.at(index_->second));
 }