feat: parse JSON from std_msgs/String in ROS2 parser

Author: OpenClaw

CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(Boost REQUIRED)
 find_package(tf2_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(plotjuggler REQUIRED)
+find_package(nlohmann_json REQUIRED)
 
 
 cmake_policy (SET CMP0020 NEW)

src/CMakeLists.txt

@@ -17,13 +17,16 @@ add_library( commonROS STATIC
     parser_configuration.cpp
     parser_configuration.h
     ros_parsers/ros2_parser.cpp
+    ros_parsers/json_string_parser.cpp
+    ros_parsers/json_string_parser.h
     ${COMMON_UI_SRC}
 )
 
 target_link_libraries( commonROS
     PUBLIC
         Qt5::Widgets
         Qt5::Xml
+        nlohmann_json::nlohmann_json
         rclcpp::rclcpp
         rcpputils::rcpputils
         rosbag2_transport::rosbag2_transport

src/ros_parsers/json_string_parser.cpp

@@ -0,0 +1,168 @@
+#include "json_string_parser.h"
+
+#include <QDebug>
+
+#include <cstdint>
+#include <stdexcept>
+
+using namespace PJ;
+
+namespace
+{
+uint32_t ReadLe32(const uint8_t* ptr)
+{
+  return (uint32_t(ptr[0]) << 0) | (uint32_t(ptr[1]) << 8) | (uint32_t(ptr[2]) << 16) |
+         (uint32_t(ptr[3]) << 24);
+}
+}
+
+JsonStringParser::JsonStringParser(const std::string& topic_name, PJ::PlotDataMapRef& data)
+  : MessageParser(topic_name, data)
+{
+  qInfo().noquote() << QString("[JsonStringParser] created parser for topic=%1")
+                           .arg(QString::fromStdString(topic_name));
+}
+
+QString JsonStringParser::topicPrefix() const
+{
+  return QString::fromStdString(_topic_name);
+}
+
+bool JsonStringParser::parseRos2StringPayload(const PJ::MessageRef serialized_msg, std::string& text) const
+{
+  const uint8_t* data = serialized_msg.data();
+  const size_t size = serialized_msg.size();
+
+  if (size < 8)
+  {
+    qWarning().noquote() << QString("[%1] ROS2 String message too short to parse (%2 bytes)")
+                                .arg(topicPrefix())
+                                .arg(size);
+    return false;
+  }
+
+  const uint32_t cdr_header = ReadLe32(data);
+  if (cdr_header != 0x00010000 && cdr_header != 0x00000000)
+  {
+    qWarning().noquote() << QString("[%1] unexpected CDR encapsulation for std_msgs/String: 0x%2")
+                                .arg(topicPrefix())
+                                .arg(cdr_header, 8, 16, QLatin1Char('0'));
+  }
+
+  const uint32_t string_size = ReadLe32(data + 4);
+  const size_t payload_end = size_t(8) + size_t(string_size);
+  if (payload_end > size || string_size == 0)
+  {
+    qWarning().noquote() << QString("[%1] invalid std_msgs/String payload size: %2")
+                                .arg(topicPrefix())
+                                .arg(string_size);
+    return false;
+  }
+
+  const char* str_ptr = reinterpret_cast<const char*>(data + 8);
+  if (str_ptr[string_size - 1] != '\0')
+  {
+    qWarning().noquote() << QString("[%1] std_msgs/String payload is not null-terminated")
+                                .arg(topicPrefix());
+    return false;
+  }
+
+  text.assign(str_ptr, str_ptr + string_size - 1);
+  return true;
+}
+
+void JsonStringParser::pushNumeric(const std::string& key, double timestamp, double value)
+{
+  if (key.empty())
+  {
+    return;
+  }
+
+  const QString qkey = QString::fromStdString(key);
+  if (!_known_series.contains(qkey))
+  {
+    if (_known_series.size() >= qsizetype(_max_series))
+    {
+      qWarning().noquote() << QString("[%1] refusing to create additional JSON series beyond limit %2: %3")
+                                  .arg(topicPrefix())
+                                  .arg(_max_series)
+                                  .arg(qkey);
+      return;
+    }
+    _known_series.insert(qkey);
+  }
+  getSeries(key).pushBack({ timestamp, value });
+}
+
+void JsonStringParser::flattenJson(const nlohmann::json& value, const std::string& prefix,
+                                   double timestamp)
+{
+  if (value.is_object())
+  {
+    for (auto it = value.begin(); it != value.end(); ++it)
+    {
+      const std::string child_key = prefix.empty() ? it.key() : prefix + "." + it.key();
+      flattenJson(it.value(), child_key, timestamp);
+    }
+    return;
+  }
+
+  if (value.is_number_integer())
+  {
+    pushNumeric(prefix, timestamp, static_cast<double>(value.get<int64_t>()));
+    return;
+  }
+
+  if (value.is_number_unsigned())
+  {
+    pushNumeric(prefix, timestamp, static_cast<double>(value.get<uint64_t>()));
+    return;
+  }
+
+  if (value.is_number_float())
+  {
+    pushNumeric(prefix, timestamp, value.get<double>());
+    return;
+  }
+}
+
+bool JsonStringParser::parseMessage(const PJ::MessageRef serialized_msg, double& timestamp)
+{
+  qInfo().noquote() << QString("[JsonStringParser] parseMessage topic=%1 size=%2 timestamp=%3")
+                           .arg(topicPrefix())
+                           .arg(serialized_msg.size())
+                           .arg(timestamp, 0, 'g', 17);
+
+  std::string text;
+  if (!parseRos2StringPayload(serialized_msg, text))
+  {
+    return false;
+  }
+
+  nlohmann::json value;
+  try
+  {
+    value = nlohmann::json::parse(text);
+  }
+  catch (const std::exception& ex)
+  {
+    qWarning().noquote() << QString("[%1] failed to parse JSON from std_msgs/String: %2")
+                                .arg(topicPrefix())
+                                .arg(ex.what());
+    return false;
+  }
+
+  if (!value.is_object())
+  {
+    qWarning().noquote() << QString("[%1] expected top-level JSON object in std_msgs/String")
+                                .arg(topicPrefix());
+    return false;
+  }
+
+  qInfo().noquote() << QString("[JsonStringParser] parsed JSON object topic=%1 keys=%2")
+                           .arg(topicPrefix())
+                           .arg(int(value.size()));
+
+  flattenJson(value, "", timestamp);
+  return true;
+}

src/ros_parsers/json_string_parser.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <PlotJuggler/messageparser_base.h>
+#include <nlohmann/json.hpp>
+
+#include <QSet>
+#include <QString>
+
+class JsonStringParser : public PJ::MessageParser
+{
+public:
+  JsonStringParser(const std::string& topic_name, PJ::PlotDataMapRef& data);
+
+  bool parseMessage(const PJ::MessageRef serialized_msg, double& timestamp) override;
+
+private:
+  bool parseRos2StringPayload(const PJ::MessageRef serialized_msg, std::string& text) const;
+  void flattenJson(const nlohmann::json& value, const std::string& prefix, double timestamp);
+  void pushNumeric(const std::string& key, double timestamp, double value);
+  QString topicPrefix() const;
+
+  size_t _max_series = 200;
+  QSet<QString> _known_series;
+};

src/ros_parsers/ros2_parser.cpp

@@ -1,4 +1,5 @@
 #include "ros2_parser.h"
+#include "json_string_parser.h"
 
 #include <set>
 
@@ -162,5 +163,9 @@ TopicInfo CreateTopicInfo(const std::string& topic_name, const std::string& type
 std::shared_ptr<PJ::MessageParser> CreateParserROS2(const PJ::ParserFactories& factories, const std::string& topic_name,
                                                     const std::string& type_name, PJ::PlotDataMapRef& data)
 {
+  if (type_name == "std_msgs/msg/String" || type_name == "std_msgs/String")
+  {
+    return std::make_shared<JsonStringParser>(topic_name, data);
+  }
   return factories.at("ros2msg")->createParser(topic_name, type_name, CreateSchema(type_name), data);
 }