diff --git a/lib/message_parser.cc b/lib/message_parser.cc index 3dfba7f..164a960 100644 --- a/lib/message_parser.cc +++ b/lib/message_parser.cc @@ -9,7 +9,7 @@ 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. @@ -17,7 +17,7 @@ const RosValue::RosValuePointer MessageParser::parse() { 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(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(); } } diff --git a/lib/message_parser.h b/lib/message_parser.h index d2f09ac..f332b92 100644 --- a/lib/message_parser.h +++ b/lib/message_parser.h @@ -24,7 +24,7 @@ class MessageParser { { }; - const RosValue::RosValuePointer parse(); + const RosValue::Pointer parse(); private: static std::unordered_map primitive_size_map_; diff --git a/lib/ros_message.h b/lib/ros_message.h index 00cffe2..d91166a 100644 --- a/lib/ros_message.h +++ b/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 msg_def_; void hydrate() { diff --git a/lib/ros_msg_types.h b/lib/ros_msg_types.h index 61be3d8..486a029 100644 --- a/lib/ros_msg_types.h +++ b/lib/ros_msg_types.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include "ros_value.h" diff --git a/lib/ros_value.cc b/lib/ros_value.cc index 450f109..1dde0c8 100644 --- a/lib/ros_value.cc +++ b/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 +const T &RosValue::getValue(const std::string &key) const { + return get(key)->as(); +} + +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() 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 RosValue::getObjects() const { + if (type_ != Type::object) { + throw std::runtime_error("Cannot getObjects of a non-object RosValue"); + } + + std::unordered_map 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::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 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(), + 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::pair RosValue::const_iterator, std::unordered_map::const_iterator>::operator*() const { +const std::pair RosValue::const_iterator, std::unordered_map::const_iterator>::operator*() const { return std::make_pair(index_->first, value_.object_info_.children.at(index_->second)); } diff --git a/lib/ros_value.h b/lib/ros_value.h index 4c0136d..71372df 100644 --- a/lib/ros_value.h +++ b/lib/ros_value.h @@ -1,8 +1,9 @@ #pragma once -#include +#include #include #include +#include #include #include #include @@ -15,43 +16,14 @@ namespace Embag { class RosValue { public: - class RosValuePointer : public VectorItemPointer { - public: - RosValuePointer() - { - } - - RosValuePointer(const std::weak_ptr>& base) - : RosValuePointer(base, 0) - { - } - - RosValuePointer(const std::weak_ptr>& base, size_t index) - : VectorItemPointer(base.lock(), index) - { - } - - const RosValuePointer operator()(const std::string &key) const { - return (**this)(key); - } - - const RosValuePointer operator[](const std::string &key) const { - return (**this)[key]; - } - - const RosValuePointer operator[](const size_t idx) const { - return (**this)[idx]; - } - }; + class Pointer; struct ros_value_list_t { std::weak_ptr> base; size_t offset; size_t length; - const RosValuePointer at(size_t index) const { - return RosValuePointer(base, offset + index); - } + const Pointer at(size_t index) const; }; enum class Type { @@ -73,6 +45,7 @@ class RosValue { // Custom types object, array, + primitive_array, }; static size_t primitiveTypeToSize(const Type type); static std::string primitiveTypeToFormat(const Type type); @@ -164,13 +137,13 @@ class RosValue { const_iterator(const RosValue& value, size_t index) : const_iterator_base>(value, index) { - if (value.type_ != Type::object && value.type_ != Type::array) { - throw std::runtime_error("Cannot iterate a RosValue that is not an object or array"); + if (value.type_ != Type::object && value.type_ != Type::array && value.type_ != Type::primitive_array) { + throw std::runtime_error("Cannot iterate the values of a non-object or non-array RosValue"); } } const ReturnType operator*() const { - return this->value_.getChildren().at(this->index_); + return this->value_.at(this->index_); } }; @@ -181,7 +154,7 @@ class RosValue { : const_iterator_base::const_iterator>(value, index) { if (value.type_ != Type::object) { - throw std::runtime_error("Cannot only iterate the keys or key/value pairs of an object"); + throw std::runtime_error("Cannot iterate the keys or key/value pairs of an non-object RosValue"); } } @@ -194,7 +167,7 @@ class RosValue { } template const_iterator endValues() const { - return RosValue::const_iterator(*this, getChildren().length); + return RosValue::const_iterator(*this, this->size()); } template const_iterator::const_iterator> beginItems() const { @@ -216,11 +189,19 @@ class RosValue { private: struct _array_identifier {}; public: + RosValue(const Type type, const std::shared_ptr>& message_buffer, const size_t offset) + : type_(type) + , primitive_info_({ offset, message_buffer }) + { + if (type_ == Type::object || type_ == Type::array || type_ == Type::primitive_array) { + throw std::runtime_error("Cannot create an object or array with this constructor"); + } + } RosValue(const Type type) : type_(type) - , primitive_info_() + , primitive_info_({ 0, nullptr }) { - if (type_ == Type::object || type_ == Type::array) { + if (type_ == Type::object || type_ == Type::array || type_ == Type::primitive_array) { throw std::runtime_error("Cannot create an object or array with this constructor"); } } @@ -235,40 +216,68 @@ class RosValue { , array_info_() { } + RosValue(const Type element_type, const std::shared_ptr>& message_buffer) + : type_(Type::primitive_array) + , primitive_array_info_(element_type, message_buffer) + { + } - // Define custom copy constructor and destructor because of the union for the infos + // Define custom copy constructor, destructor, and assignment operator because of the union for the infos RosValue(const RosValue &other): type_(other.type_) { if (type_ == Type::object) { new (&object_info_) auto(other.object_info_); } else if (type_ == Type::array) { new (&array_info_) auto(other.array_info_); + } else if (type_ == Type::primitive_array) { + new (&primitive_array_info_) auto(other.primitive_array_info_); } else { new (&primitive_info_) auto(other.primitive_info_); } } + ~RosValue() { + destroy_object_info(); + } + + RosValue operator=(const RosValue& other) { + if (type_ != other.type_) { + destroy_object_info(); + } + + type_ = other.type_; + if (type_ == Type::object) { + object_info_ = other.object_info_; + } else if (type_ == Type::array) { + array_info_ = other.array_info_; + } else if (type_ == Type::primitive_array) { + primitive_array_info_ = other.primitive_array_info_; + } else { + primitive_info_ = other.primitive_info_; + } + } + + void destroy_object_info() { if (type_ == Type::object) { object_info_.~object_info_t(); } else if (type_ == Type::array) { array_info_.~array_info_t(); + } else if (type_ == Type::primitive_array) { + primitive_array_info_.~primitive_array_info_t(); } else { primitive_info_.~primitive_info_t(); } } - // Convenience accessors - const RosValuePointer operator()(const std::string &key) const; - const RosValuePointer operator[](const std::string &key) const; - const RosValuePointer operator[](const size_t idx) const; - const RosValuePointer get(const std::string &key) const; - const RosValuePointer at(size_t idx) const; - const RosValuePointer at(const std::string &key) const; + const Pointer operator()(const std::string &key) const; + const Pointer operator[](const std::string &key) const; + const Pointer operator[](const size_t idx) const; + const Pointer get(const std::string &key) const; + const Pointer at(size_t idx) const; + const Pointer at(const std::string &key) const; template - const T &getValue(const std::string &key) const { - return get(key)->as(); - } + const T &getValue(const std::string &key) const; template const T as() const { @@ -289,65 +298,23 @@ class RosValue { } size_t size() const { - if (type_ != Type::array && type_ != Type::object) { + if (type_ == Type::array || type_ == Type::object) { + return getChildren().length; + } else if (type_ == Type::primitive_array) { + return primitive_array_info_.length; + } else { throw std::runtime_error("Value is not an array or an object"); } - - return getChildren().length; - } - - std::unordered_map getObjects() const { - if (type_ != Type::object) { - throw std::runtime_error("Cannot getObjects of a non-object RosValue"); - } - - std::unordered_map objects; - objects.reserve(object_info_.children.length); - for (const auto& field : *object_info_.field_indexes) { - objects.emplace(field.first, RosValuePointer(object_info_.children.base, object_info_.children.offset + field.second)); - } - return objects; } - std::vector getValues() const { - if (type_ != Type::object && type_ != Type::array) { - throw std::runtime_error("Cannot getValues of a non object or array RosValue"); - } - - const ros_value_list_t& children = getChildren(); - std::vector values; - values.reserve(children.length); - for (size_t i = 0; i < children.length; ++i) { - values.emplace_back(children.base, children.offset + i); - } - return values; - } + std::unordered_map getObjects() const; + std::vector getValues() const; // This interface is used to provide a buffer_info interface to python bindings. // The buffer_info object essentially provides the python runtime with a way // to directly access the underlying memory that an object contains, and thus // operate on it in a much more optimized fashion. - pybind11::buffer_info getPrimitiveArrayBufferInfo() { - if (getType() != Embag::RosValue::Type::array) { - throw std::runtime_error("Only arrays can be represented as buffers!"); - } - - const Embag::RosValue::Type type_of_elements = at(0)->getType(); - if (type_of_elements == Embag::RosValue::Type::object || type_of_elements == Embag::RosValue::Type::string) { - throw std::runtime_error("In order to be represented as a buffer, an array's elements must not be objects or strings!"); - } - - const size_t size_of_elements = Embag::RosValue::primitiveTypeToSize(type_of_elements); - return pybind11::buffer_info( - (void*) &at(0)->getPrimitive(), - size_of_elements, - Embag::RosValue::primitiveTypeToFormat(type_of_elements), - 1, - { size() }, - { size_of_elements }, - true - ); - } + pybind11::buffer_info getPrimitiveArrayBufferInfo(); std::string toString(const std::string &path = "") const; void print(const std::string &path = "") const; @@ -358,8 +325,21 @@ class RosValue { private: struct primitive_info_t { - size_t offset = 0; - std::shared_ptr> message_buffer = nullptr; + size_t offset; + std::shared_ptr> message_buffer; + }; + + struct primitive_array_info_t { + primitive_array_info_t(const Type element_type, const std::shared_ptr>& message_buffer) + : element_type(element_type) + , message_buffer(message_buffer) + { + } + + Type element_type; + size_t offset; + size_t length; + std::shared_ptr> message_buffer; }; struct array_info_t { @@ -374,6 +354,7 @@ class RosValue { Type type_; union { primitive_info_t primitive_info_; + primitive_array_info_t primitive_array_info_; array_info_t array_info_; object_info_t object_info_; }; @@ -397,6 +378,69 @@ class RosValue { friend class MessageParser; }; +class RosValue::Pointer { + private: + struct vector_based_value_info_t { + std::shared_ptr> base; + size_t index; + }; + boost::variant info_; + + public: + Pointer() + : info_(vector_based_value_info_t({nullptr, 0})) + { + } + + Pointer(const std::weak_ptr>& base) + : Pointer(base, 0) + { + } + + Pointer(const std::weak_ptr>& base, size_t index) + : Pointer(base.lock(), index) + { + } + + Pointer(const std::shared_ptr>& base, size_t index) + : info_(vector_based_value_info_t({base, index})) + { + } + + Pointer(const RosValue::Type type, const std::shared_ptr>& message_buffer, const size_t offset) + : info_(RosValue(type, message_buffer, offset)) + { + } + + const Pointer operator()(const std::string &key) const { + return (**this)(key); + } + + const Pointer operator[](const std::string &key) const { + return (**this)[key]; + } + + const Pointer operator[](const size_t idx) const { + return (**this)[idx]; + } + + const RosValue* operator->() const { + return &**this; + } + + private: + friend PyBindPointerWrapper; + + const RosValue& operator*() const { + if (info_.which() == 0) { + return boost::get(info_); + } else { + vector_based_value_info_t info = boost::get(info_); + return info.base->at(info.index); + } + } +}; + template<> const std::string RosValue::as() const; diff --git a/lib/util.h b/lib/util.h index 3329f33..f614665 100644 --- a/lib/util.h +++ b/lib/util.h @@ -12,8 +12,8 @@ std::unique_ptr make_unique(Args &&... args) { using message_stream = boost::iostreams::stream; -template -class VectorItemPointer; +template +class PyBindPointerWrapper; } @@ -21,59 +21,49 @@ namespace pybind11 { namespace detail { template struct holder_helper; - template - struct holder_helper> { - static const T* get(const Embag::VectorItemPointer &vip) { - return vip.get(); + template + struct holder_helper> { + static const ValueType* get(const Embag::PyBindPointerWrapper &pointer_wrapper) { + return pointer_wrapper.get(); } }; }} namespace Embag { -template -class VectorItemPointer { - std::shared_ptr> base; - size_t index; - +template +class PyBindPointerWrapper { public: - VectorItemPointer(const std::shared_ptr>& base, size_t index) - : base(base) - , index(index) + PyBindPointerWrapper() { } - VectorItemPointer(const std::weak_ptr>& base, size_t index) - : VectorItemPointer(base.lock(), index) - { - } - - VectorItemPointer(T*& ref) - : VectorItemPointer() + PyBindPointerWrapper(ValueType*& ref) { throw std::runtime_error("This should never be called"); } - VectorItemPointer() - : index(0) + PyBindPointerWrapper(const PointerType& pointer_to_wrap) + : wrapped_pointer_(pointer_to_wrap) { } - const T* operator->() const { + const ValueType* operator->() const { return get(); } - protected: - // We don't want any public interface that exposes the underlying item as without - // a shared_ptr to the vector that contains the item, the memory may be freed. - friend const T* pybind11::detail::holder_helper>::get(const VectorItemPointer& vip); + private: + PointerType wrapped_pointer_; + + // We don't want any public interface that exposes the underlying item's pointer as it may be improperly used. + friend const ValueType* pybind11::detail::holder_helper>::get(const PyBindPointerWrapper& pointer_wrapper); - const T* get() const { + const ValueType* get() const { return &**this; } - const T& operator*() const { - return base->at(index); + const ValueType& operator*() const { + return *wrapped_pointer_; } }; diff --git a/python/adapters.h b/python/adapters.h index 91fcfef..2dfb654 100644 --- a/python/adapters.h +++ b/python/adapters.h @@ -6,13 +6,13 @@ namespace py = pybind11; -py::dict rosValueToDict(const Embag::VectorItemPointer &ros_value); -py::list rosValueToList(const Embag::VectorItemPointer &ros_value); +py::dict rosValueToDict(const Embag::PyBindPointerWrapper &ros_value); +py::list rosValueToList(const Embag::PyBindPointerWrapper &ros_value); -py::list rosValueToList(const Embag::VectorItemPointer &ros_value) { +py::list rosValueToList(const Embag::PyBindPointerWrapper &ros_value) { using Type = Embag::RosValue::Type; - if (ros_value->getType() != Type::array) { + if (ros_value->getType() != Type::array && ros_value->getType() != Type::primitive_array) { throw std::runtime_error("Provided RosValue is not an array"); } @@ -81,7 +81,9 @@ py::list rosValueToList(const Embag::VectorItemPointer &ros_val list.append(rosValueToDict(value)); break; } - case Type::array: { + case Type::array: + case Type::primitive_array: + { list.append(rosValueToList(value)); break; } @@ -94,7 +96,7 @@ py::list rosValueToList(const Embag::VectorItemPointer &ros_val return list; } -py::dict rosValueToDict(const Embag::VectorItemPointer &ros_value) { +py::dict rosValueToDict(const Embag::PyBindPointerWrapper &ros_value) { using Type = Embag::RosValue::Type; if (ros_value->getType() != Type::object) { @@ -170,7 +172,9 @@ py::dict rosValueToDict(const Embag::VectorItemPointer &ros_val dict[key] = rosValueToDict(value); break; } - case Type::array: { + case Type::array: + case Type::primitive_array: + { dict[key] = rosValueToList(value); break; } @@ -183,10 +187,11 @@ py::dict rosValueToDict(const Embag::VectorItemPointer &ros_val return dict; } -py::object castValue(const Embag::VectorItemPointer& value) { +py::object castValue(const Embag::PyBindPointerWrapper& value) { switch (value->getType()) { case Embag::RosValue::Type::object: case Embag::RosValue::Type::array: + case Embag::RosValue::Type::primitive_array: return py::cast(value); case Embag::RosValue::Type::ros_bool: return py::cast(value->as()); @@ -222,7 +227,7 @@ py::object castValue(const Embag::VectorItemPointer& value) { } } -py::object getField(Embag::VectorItemPointer &v, const std::string field_name) { +py::object getField(Embag::PyBindPointerWrapper &v, const std::string field_name) { if (v->getType() != Embag::RosValue::Type::object) { throw std::runtime_error("Can only getField on an object"); } @@ -230,7 +235,7 @@ py::object getField(Embag::VectorItemPointer &v, const std::str return castValue(v->get(field_name)); } -py::object getIndex(Embag::VectorItemPointer &v, const size_t index) { +py::object getIndex(Embag::PyBindPointerWrapper &v, const size_t index) { if (v->getType() != Embag::RosValue::Type::array) { throw std::runtime_error("Can only getIndex on an array"); } @@ -241,6 +246,6 @@ py::object getIndex(Embag::VectorItemPointer &v, const size_t i namespace Embag { template<> const py::object RosValue::const_iterator::operator*() const { - return castValue(value_.getChildren().at(index_)); + return castValue(value_.at(index_)); } } diff --git a/python/embag.cc b/python/embag.cc index c31b072..ad6f418 100644 --- a/python/embag.cc +++ b/python/embag.cc @@ -10,7 +10,9 @@ namespace py = pybind11; -PYBIND11_DECLARE_HOLDER_TYPE(T, Embag::VectorItemPointer); +template +using PyBindHolderType = Embag::PyBindPointerWrapper; +PYBIND11_DECLARE_HOLDER_TYPE(ValueType, PyBindHolderType); PYBIND11_MODULE(libembag, m) { m.doc() = "Python bindings for Embag"; @@ -56,8 +58,8 @@ PYBIND11_MODULE(libembag, m) { .def("__str__", [](std::shared_ptr &m) { return encodeStrLatin1(m->toString()); }) - .def("data", [](std::shared_ptr &m) { - return (Embag::VectorItemPointer&) m->data(); + .def("data", [](std::shared_ptr &m) -> Embag::PyBindPointerWrapper& { + return (Embag::PyBindPointerWrapper&) m->data(); }) .def("dict", [](std::shared_ptr &m) { if (m->data()->getType() != Embag::RosValue::Type::object) { @@ -71,31 +73,33 @@ PYBIND11_MODULE(libembag, m) { .def_readonly("md5", &Embag::RosMessage::md5) .def_readonly("raw_data_len", &Embag::RosMessage::raw_data_len); - auto ros_value = py::class_>(m, "RosValue", py::dynamic_attr(), py::buffer_protocol()) + auto ros_value = py::class_>(m, "RosValue", py::dynamic_attr(), py::buffer_protocol()) .def_buffer(&Embag::RosValue::getPrimitiveArrayBufferInfo) .def("get", &Embag::RosValue::get) .def("getType", &Embag::RosValue::getType) .def("__len__", &Embag::RosValue::size) - .def("__str__", [](Embag::VectorItemPointer &v, const std::string &path) { + .def("__str__", [](Embag::PyBindPointerWrapper &v, const std::string &path) { return encodeStrLatin1(v->toString()); }, py::arg("path") = "") - .def("__iter__", [](Embag::VectorItemPointer &v) { + .def("__iter__", [](Embag::PyBindPointerWrapper &v) { switch (v->getType()) { // TODO: Allow object iteration - case Embag::RosValue::Type::array: { + case Embag::RosValue::Type::array: + case Embag::RosValue::Type::primitive_array: + { return py::make_iterator(v->beginValues(), v->endValues()); } default: throw std::runtime_error("Can only iterate array RosValues"); } }, py::keep_alive<0, 1>() /* Essential: keep object alive while iterator exists */) - .def("__getattr__", [](Embag::VectorItemPointer &v, const std::string &attr) { + .def("__getattr__", [](Embag::PyBindPointerWrapper &v, const std::string &attr) { return getField(v, attr); }) - .def("__getitem__", [](Embag::VectorItemPointer &v, const std::string &key) { + .def("__getitem__", [](Embag::PyBindPointerWrapper &v, const std::string &key) { return getField(v, key); }) - .def("__getitem__", [](Embag::VectorItemPointer &v, const size_t index) { + .def("__getitem__", [](Embag::PyBindPointerWrapper &v, const size_t index) { return getIndex(v, index); }); diff --git a/python/ros_compat.h b/python/ros_compat.h index 8933db2..79dff68 100644 --- a/python/ros_compat.h +++ b/python/ros_compat.h @@ -14,7 +14,7 @@ struct IteratorCompat { const auto msg = *iterator_; return py::make_tuple( msg->topic, - (Embag::VectorItemPointer&) msg->data(), + (Embag::PyBindPointerWrapper&) msg->data(), // TODO: Change the timestamp to a rostime - they have more precision! msg->timestamp.to_sec() ); diff --git a/test/BUILD b/test/BUILD index 4ba0778..17e9eb8 100644 --- a/test/BUILD +++ b/test/BUILD @@ -2,7 +2,7 @@ cc_test( name = "embag_test", srcs = ["embag_test.cc"], copts = ["-Iexternal/gtest/include"], - data = ["test.bag", "test_2.bag"], + data = ["test.bag", "test_2.bag", "array_test.bag"], deps = [ "//lib:embag", "@gtest", diff --git a/test/array_test.bag b/test/array_test.bag new file mode 100644 index 0000000..872de2e Binary files /dev/null and b/test/array_test.bag differ diff --git a/test/embag_test.cc b/test/embag_test.cc index 7acdabb..33c326c 100644 --- a/test/embag_test.cc +++ b/test/embag_test.cc @@ -178,7 +178,7 @@ TEST_F(ViewTest, AllMessages) { ASSERT_EQ(message->data()["scan_time"]->as(), 0.0); // Arrays are exposed at blobs - ASSERT_EQ(message->data()["ranges"]->getType(), Embag::RosValue::Type::array); + ASSERT_EQ(message->data()["ranges"]->getType(), Embag::RosValue::Type::primitive_array); const auto array = message->data()["ranges"]; ASSERT_EQ(array[0]->getType(), Embag::RosValue::Type::float32); ASSERT_EQ(array->size(), 90); @@ -196,7 +196,7 @@ TEST_F(ViewTest, AllMessages) { ASSERT_EQ(message->data()["header"]["frame_id"]->as(), "odom"); ASSERT_NE(message->data()["pose"]["pose"]["position"]["x"]->as(), 0.0); - ASSERT_EQ(message->data()["pose"]["covariance"]->getType(), Embag::RosValue::Type::array); + ASSERT_EQ(message->data()["pose"]["covariance"]->getType(), Embag::RosValue::Type::primitive_array); const auto array = message->data()["pose"]["covariance"]; for (size_t i = 0; i < array->size(); i++) { @@ -249,4 +249,65 @@ TEST_F(StreamTest, BagFromStream) { bag->close(); } +class ArraysTest : public ::testing::Test { + protected: + Embag::View view_{"test/array_test.bag"}; +}; + +TEST_F(ArraysTest, ArrayReading) { + uint32_t index = 0; + uint32_t inner_index = 0; + for (const auto &message : view_.getMessages("/array_test")) { + ASSERT_EQ(message->topic, "/array_test"); + ASSERT_EQ(message->timestamp.secs, index); + ASSERT_EQ(message->timestamp.nsecs, 0); + + ASSERT_EQ(message->data()["index"]->as(), index); + // ASSERT_EQ(message->data()["index_as_text"], std::format("{}", index)); + + // For each array type, confirm that both iteration and index access work + // Tests a dynamically sized array of boolean primitives + inner_index = 0; + const auto dynamic_bool_array = message->data()["index_as_dynamic_bool_array"]; + for (auto item = dynamic_bool_array->beginValues(); item != dynamic_bool_array->endValues(); item++) { + ASSERT_EQ((*item)->as(), index == inner_index++); + } + for (inner_index = 0; inner_index < 20; inner_index++) { + ASSERT_EQ(dynamic_bool_array[inner_index]->as(), index == inner_index); + } + + // Tests a statically sized array of boolean primitives + inner_index = 0; + const auto static_bool_array = message->data()["index_as_static_bool_array"]; + for (auto item = static_bool_array->beginValues(); item != static_bool_array->endValues(); item++) { + ASSERT_EQ((*item)->as(), index == inner_index++); + } + for (inner_index = 0; inner_index < 20; inner_index++) { + ASSERT_EQ(static_bool_array[inner_index]->as(), index == inner_index); + } + + // Tests an array of strings + inner_index = 0; + const auto string_array = message->data()["index_as_string_array"]; + for (auto item = string_array->beginValues(); item != string_array->endValues(); item++) { + ASSERT_EQ((*item)->as(), index == inner_index++ ? "true" : "false"); + } + for (inner_index = 0; inner_index < 20; inner_index++) { + ASSERT_EQ(string_array[inner_index]->as(), index == inner_index ? "true" : "false"); + } + + // Tests an array of std/Bool objects + inner_index = 0; + const auto bool_object_array = message->data()["index_as_bool_object_array"]; + for (auto item = bool_object_array->beginValues(); item != bool_object_array->endValues(); item++) { + ASSERT_EQ((*item)["data"]->as(), index == inner_index++); + } + for (inner_index = 0; inner_index < 20; inner_index++) { + ASSERT_EQ(bool_object_array[inner_index]["data"]->as(), index == inner_index); + } + + index++; + } +} + // TODO: test multi-bag message sorting