added serialization to JSON

CMakeLists.txt

@@ -17,19 +17,19 @@ if (ament_cmake_FOUND)
     find_package(ament_index_cpp REQUIRED)
     find_package(sensor_msgs REQUIRED)
     find_package(rclcpp REQUIRED)
+    find_package(fastcdr REQUIRED)
 
     set(EXTRA_SRC
        src/ros_utils/message_definition_cache.cpp
        src/ros_utils/ros2_helpers.cpp)
 endif()
 
 find_package(RapidJSON REQUIRED)
-find_package(fastcdr QUIET)
 
-if(NOT fastcdr_FOUND )
-    message(STATUS "[FastCdr] not found, create shared libraries")
+if(NOT USING_ROS2 )
+    message(STATUS "[FastCdr] not found, create static libraries")
     # Override Fast-CDR option: compile as static lib
-    SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Create shared libraries by default")
+    SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Create static libraries by default")
     add_subdirectory(3rdparty/Fast-CDR)
     include_directories(3rdparty/Fast-CDR/include)
 else()
@@ -48,10 +48,11 @@ add_library(rosx_introspection STATIC
     src/ros_message.cpp
     src/ros_parser.cpp
     src/deserializer.cpp
+    src/serializer.cpp
     ${EXTRA_SRC}
     )
 
-target_link_libraries(rosx_introspection fastcdr)
+target_link_libraries(rosx_introspection )
 
 target_include_directories(rosx_introspection PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -61,7 +62,8 @@ if(USING_ROS2)
     ament_target_dependencies(rosx_introspection
         ament_index_cpp
         rclcpp
-        rosbag2_cpp)
+        rosbag2_cpp
+        fastcdr)
 endif()
 
 install(TARGETS rosx_introspection

include/rosx_introspection/ros_parser.hpp

@@ -26,6 +26,7 @@
 
 #include "rosx_introspection/stringtree_leaf.hpp"
 #include "rosx_introspection/deserializer.hpp"
+#include "rosx_introspection/serializer.hpp"
 
 namespace RosMsgParser
 {
@@ -144,7 +145,9 @@ class Parser
 
   bool deserializeIntoJson(Span<const uint8_t> buffer, std::string* json_txt,
                            Deserializer* deserializer, int indent = 0,
-                           bool ignore_constants = true) const;
+                           bool ignore_constants = false) const;
+
+  bool serializeFromJson(const std::string& json_string, Serializer* serializer) const;
 
   typedef std::function<void(const ROSType&, Span<uint8_t>&)> VisitingCallback;
 

include/rosx_introspection/ros_utils/ros2_helpers.hpp

@@ -71,4 +71,24 @@ inline std::vector<uint8_t> BuildMessageBuffer(const T& msg,
   return buffer;
 }
 
+template <typename T>
+inline T BufferToMessage(const std::vector<uint8_t>& buffer)
+{
+  // get the type name of the ROS message with traits
+  const std::string topic_type = rosidl_generator_traits::name<T>();
+  const auto& ts_identifier = rosidl_typesupport_cpp::typesupport_identifier;
+  auto ts_library = rosbag2_cpp::get_typesupport_library(topic_type, ts_identifier);
+  auto type_support =
+      rosbag2_cpp::get_typesupport_handle(topic_type, ts_identifier, ts_library);
+
+  rmw_serialized_message_t serialized_msg;
+  serialized_msg.buffer_capacity = buffer.size();
+  serialized_msg.buffer_length = buffer.size();
+  serialized_msg.buffer = const_cast<uint8_t*>(buffer.data());
+
+  T message;
+  auto res = rmw_deserialize(&serialized_msg, type_support, &message);
+  return message;
+}
+
 }  // namespace RosMsgParser

include/rosx_introspection/serializer.hpp

@@ -0,0 +1,118 @@
+#pragma once
+
+// API adapted to FastCDR
+
+#include <exception>
+
+#include "rosx_introspection/builtin_types.hpp"
+#include "rosx_introspection/variant.hpp"
+
+namespace eprosima::fastcdr
+{
+class FastBuffer;
+class Cdr;
+}  // namespace eprosima::fastcdr
+
+namespace RosMsgParser
+{
+
+class Serializer
+{
+public:
+  virtual ~Serializer() = default;
+
+  virtual bool isROS2() const = 0;
+
+  virtual void serialize(BuiltinType type, const Variant& val) = 0;
+
+  virtual void serializeString(const std::string& str) = 0;
+
+  virtual void serializeUInt32(uint32_t value) = 0;
+
+  virtual void reset() = 0;
+
+  virtual const char* getBufferData() const = 0;
+
+  virtual size_t getBufferSize() const = 0;
+
+  virtual void writeHeader() = 0;
+};
+
+//-----------------------------------------------------------------
+
+// Specialization od serializer that works with ROS1
+class ROS_Serializer : public Serializer
+{
+public:
+  bool isROS2() const override
+  {
+    return false;
+  }
+
+  void serialize(BuiltinType type, const Variant& val) override;
+
+  void serializeString(const std::string& str) override;
+
+  void serializeUInt32(uint32_t value) override;
+
+  void reset() override;
+
+  const char* getBufferData() const override;
+
+  size_t getBufferSize() const override;
+
+protected:
+  std::vector<uint8_t> _buffer;
+  size_t _current_size = 0;
+
+  template <typename T>
+  T serialize(const T& val)
+  {
+    T out;
+    if (_current_size + sizeof(T) > _buffer.size())
+    {
+      _buffer.resize((_current_size * 3) / 2);
+    }
+    auto* ptr = &(_buffer[_current_size]);
+    *(reinterpret_cast<const T*>(ptr)) = val;
+    _current_size += sizeof(T);
+    return out;
+  }
+};
+
+//-----------------------------------------------------------------
+
+// Specialization od deserializer that works with ROS2
+// wrapping FastCDR
+class FastCDR_Serializer : public Serializer
+{
+public:
+  FastCDR_Serializer();
+
+  bool isROS2() const override
+  {
+    return true;
+  }
+
+  void serialize(BuiltinType type, const Variant& val) override;
+
+  void serializeString(const std::string& str) override;
+
+  void serializeUInt32(uint32_t value) override;
+
+  void reset() override;
+
+  const char* getBufferData() const override;
+
+  size_t getBufferSize() const override;
+
+  void writeHeader() override;
+
+protected:
+  std::shared_ptr<eprosima::fastcdr::FastBuffer> _cdr_buffer;
+  std::shared_ptr<eprosima::fastcdr::Cdr> _cdr;
+};
+
+using ROS2_Serializer = FastCDR_Serializer;
+
+}  // namespace RosMsgParser

src/deserializer.cpp

@@ -5,8 +5,10 @@
 #if ((FASTCDR_VERSION_MAJOR < 2))
 #define get_buffer_pointer() getBufferPointer()
 #define get_current_position() getCurrentPosition()
+#define get_serialized_data_length() getSerializedDataLength()
 #define CdrVersion Cdr
 #endif
+
 namespace RosMsgParser
 {
 

src/ros_parser.cpp

@@ -447,4 +447,130 @@ bool Parser::deserializeIntoJson(Span<const uint8_t> buffer, std::string* json_t
   return true;
 }
 
+bool Parser::serializeFromJson(const std::string& json_string,
+                               Serializer* serializer) const
+{
+  rapidjson::Document json_document;
+  json_document.Parse(json_string.c_str());
+
+  serializer->writeHeader();
+
+  std::function<void(const ROSMessage*, rapidjson::Value&)> deserializeImpl;
+
+  deserializeImpl = [&](const ROSMessage* msg_node, rapidjson::Value& new_value) {
+    size_t index_s = 0;
+    size_t index_m = 0;
+
+    for (const ROSField& field : msg_node->fields())
+    {
+      const ROSType& field_type = field.type();
+      const auto field_name =
+          rapidjson::StringRef(field.name().data(), field.name().size());
+
+      uint32_t array_size = field.arraySize();
+
+      if (array_size == -1)
+      {
+        if (!new_value.HasMember(field_name.s))
+        {
+          std::cout << "missing member " << field_name.s << std::endl;
+          throw std::runtime_error("looks like it is a blob that wasn't serialized");
+        }
+
+        array_size = new_value[field_name.s].GetArray().Size();
+        serializer->serializeUInt32(array_size);
+      }
+
+      for (int i = 0; i < array_size; i++)
+      {
+        auto* value_field = &(new_value[field_name.s]);
+        const bool is_array = value_field->IsArray();
+        if (is_array)
+        {
+          value_field = &(value_field->GetArray()[i]);
+        }
+        const auto type_id = field_type.typeID();
+
+        switch (type_id)
+        {
+          case BOOL:
+            serializer->serialize(type_id, value_field->GetBool());
+            break;
+          case CHAR:
+            serializer->serialize(type_id, value_field->GetString()[0]);
+            break;
+
+          case BYTE:
+          case UINT8:
+          case UINT16:
+          case UINT32:
+            serializer->serialize(type_id, value_field->GetUint());
+            break;
+          case UINT64:
+            serializer->serialize(type_id, value_field->GetUint64());
+            break;
+
+            break;
+          case INT8:
+          case INT16:
+          case INT32:
+            serializer->serialize(type_id, value_field->GetInt());
+            break;
+          case INT64:
+            serializer->serialize(type_id, value_field->GetInt64());
+            break;
+
+          case FLOAT32:
+            serializer->serialize(type_id, value_field->GetFloat());
+            break;
+          case FLOAT64:
+            serializer->serialize(type_id, value_field->GetDouble());
+            break;
+
+          case DURATION:
+          case TIME: {
+            uint32_t secs = value_field->GetObject()["secs"].GetInt();
+            serializer->serializeUInt32(secs);
+
+            uint32_t nsecs = value_field->GetObject()["nsecs"].GetInt();
+            serializer->serializeUInt32(secs);
+          }
+          break;
+
+          case STRING: {
+            const char* str = value_field->GetString();
+            uint32_t len = value_field->GetStringLength();
+            serializer->serializeString(std::string(str, len));
+          }
+          break;
+          case OTHER: {
+            rapidjson::Value next_value = value_field->GetObject();
+            auto msg_node_child = field.getMessagePtr(_schema->msg_library);
+            deserializeImpl(msg_node_child.get(), next_value);
+          }
+          break;
+        }  // end switch
+
+      }  // end for array
+
+      if (field_type.typeID() == OTHER)
+      {
+        index_m++;
+      }
+      index_s++;
+    }  // end for fields
+  };   // end of lambda
+
+  FieldLeaf rootnode;
+  rootnode.node = _schema->field_tree.croot();
+  auto root_msg =
+      _schema->field_tree.croot()->value()->getMessagePtr(_schema->msg_library);
+
+  rapidjson::Value json_root = json_document.GetObject();
+  rapidjson::Value json_msg = json_root["msg"].GetObject();
+  deserializeImpl(root_msg.get(), json_msg);
+
+  return true;
+}
+
 }  // namespace RosMsgParser