pretty printing added

facontidavide · May 23, 2024 · b789cbb · b789cbb
1 parent 4cd89b2
commit b789cbb
Show file tree

Hide file tree

Showing 3 changed files with 56 additions and 18 deletions.
diff --git a/include/rosx_introspection/ros_parser.hpp b/include/rosx_introspection/ros_parser.hpp
@@ -143,7 +143,8 @@ class Parser
                    Deserializer* deserializer) const;
 
   bool deserializeIntoJson(Span<const uint8_t> buffer, std::string* json_txt,
-                           Deserializer* deserializer, bool ignore_constants) const;
+                           Deserializer* deserializer, int indent = 0,
+                           bool ignore_constants = true) const;
 
   typedef std::function<void(const ROSType&, Span<uint8_t>&)> VisitingCallback;
 

diff --git a/src/ros_parser.cpp b/src/ros_parser.cpp
@@ -29,6 +29,9 @@
 #include "rapidjson/document.h"
 #include "rapidjson/stringbuffer.h"
 #include "rapidjson/writer.h"
+#include "rapidjson/reader.h"
+#include "rapidjson/prettywriter.h"
+#include "rapidjson/memorystream.h"
 
 namespace RosMsgParser
 {
@@ -248,7 +251,8 @@ bool Parser::deserialize(Span<const uint8_t> buffer, FlatMessage* flat_container
 }
 
 bool Parser::deserializeIntoJson(Span<const uint8_t> buffer, std::string* json_txt,
-                                 Deserializer* deserializer, bool ignore_constants) const
+                                 Deserializer* deserializer, int indent,
+                                 bool ignore_constants) const
 {
   deserializer->init(buffer);
 
@@ -418,16 +422,27 @@ bool Parser::deserializeIntoJson(Span<const uint8_t> buffer, std::string* json_t
   json_document.AddMember("topic", topic_name, alloc);
   json_document.AddMember("msg", json_node, alloc);
 
-  rapidjson::StringBuffer json_buffer;
-  json_buffer.Reserve(2048);
+  static rapidjson::StringBuffer json_buffer;
+  json_buffer.Reserve(65536);
+  json_buffer.Clear();
 
-  rapidjson::Writer<rapidjson::StringBuffer, rapidjson::UTF8<>, rapidjson::UTF8<>,
-                    rapidjson::CrtAllocator,
-                    rapidjson::kWriteDefaultFlags | rapidjson::kWriteNanAndInfFlag>
-      json_writer(json_buffer);
-  json_document.Accept(json_writer);
+  if (indent == 0)
+  {
+    rapidjson::Writer<rapidjson::StringBuffer, rapidjson::UTF8<>, rapidjson::UTF8<>,
+                      rapidjson::CrtAllocator,
+                      rapidjson::kWriteDefaultFlags | rapidjson::kWriteNanAndInfFlag>
+        json_writer(json_buffer);
+    json_document.Accept(json_writer);
 
-  *json_txt = json_buffer.GetString();
+    *json_txt = json_buffer.GetString();
+  }
+  else
+  {
+    rapidjson::PrettyWriter<rapidjson::StringBuffer> json_writer(json_buffer);
+    json_writer.SetIndent(' ', indent);
+    json_document.Accept(json_writer);
+    *json_txt = json_buffer.GetString();
+  }
 
   return true;
 }

diff --git a/test/test_ros2.cpp b/test/test_ros2.cpp
@@ -8,15 +8,8 @@
 
 using namespace RosMsgParser;
 
-TEST_CASE("Parse ROS2 [JointState]")
+sensor_msgs::msg::JointState BuildSampleJointState()
 {
-  ParsersCollection<ROS2_Deserializer> parser;
-
-  const std::string topic_type = "sensor_msgs/JointState";
-
-  parser.registerParser("joint_state", ROSType(topic_type),
-                        GetMessageDefinition(topic_type));
-
   sensor_msgs::msg::JointState joint_state;
 
   joint_state.header.stamp.sec = 1234;
@@ -40,6 +33,18 @@ TEST_CASE("Parse ROS2 [JointState]")
     joint_state.velocity[i] = 30 + i;
     joint_state.effort[i] = 50 + i;
   }
+  return joint_state;
+}
+
+TEST_CASE("Parse ROS2 [JointState]")
+{
+  ParsersCollection<ROS2_Deserializer> parser;
+  const std::string topic_type = "sensor_msgs/JointState";
+
+  parser.registerParser("joint_state", ROSType(topic_type),
+                        GetMessageDefinition(topic_type));
+
+  auto joint_state = BuildSampleJointState();
 
   std::vector<uint8_t> buffer = BuildMessageBuffer(joint_state, topic_type);
 
@@ -96,3 +101,20 @@ TEST_CASE("Parse ROS2 [JointState]")
   CHECK(flat_container->name[3].first.toStdString() == ("joint_state/name[2]"));
   CHECK(flat_container->name[3].second == ("bye"));
 }
+
+TEST_CASE("Parse ROS2 [JointState_JSON]")
+{
+  const std::string topic_type = "sensor_msgs/JointState";
+
+  Parser parser("joint_state", ROSType(topic_type), GetMessageDefinition(topic_type));
+  ROS2_Deserializer deserializer;
+
+  auto joint_state = BuildSampleJointState();
+
+  std::vector<uint8_t> buffer = BuildMessageBuffer(joint_state, topic_type);
+
+  std::string json;
+  parser.deserializeIntoJson(buffer, &json, &deserializer, 2);
+
+  std::cout << "\n JSON encoding:\n" << json << std::endl;
+}