diff --git a/CMakeLists.txt b/CMakeLists.txt index f2d3c32..c61c977 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2db79a0..3d98a82 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,6 +17,8 @@ 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} ) @@ -24,6 +26,7 @@ target_link_libraries( commonROS PUBLIC Qt5::Widgets Qt5::Xml + nlohmann_json::nlohmann_json rclcpp::rclcpp rcpputils::rcpputils rosbag2_transport::rosbag2_transport diff --git a/src/ros_parsers/json_string_parser.cpp b/src/ros_parsers/json_string_parser.cpp new file mode 100644 index 0000000..32739e4 --- /dev/null +++ b/src/ros_parsers/json_string_parser.cpp @@ -0,0 +1,168 @@ +#include "json_string_parser.h" + +#include + +#include +#include + +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(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(value.get())); + return; + } + + if (value.is_number_unsigned()) + { + pushNumeric(prefix, timestamp, static_cast(value.get())); + return; + } + + if (value.is_number_float()) + { + pushNumeric(prefix, timestamp, value.get()); + 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; +} diff --git a/src/ros_parsers/json_string_parser.h b/src/ros_parsers/json_string_parser.h new file mode 100644 index 0000000..b5cfae0 --- /dev/null +++ b/src/ros_parsers/json_string_parser.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include + +#include +#include + +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 _known_series; +}; diff --git a/src/ros_parsers/ros2_parser.cpp b/src/ros_parsers/ros2_parser.cpp index 5c05427..3112bb1 100644 --- a/src/ros_parsers/ros2_parser.cpp +++ b/src/ros_parsers/ros2_parser.cpp @@ -1,4 +1,5 @@ #include "ros2_parser.h" +#include "json_string_parser.h" #include @@ -162,5 +163,9 @@ TopicInfo CreateTopicInfo(const std::string& topic_name, const std::string& type std::shared_ptr 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(topic_name, data); + } return factories.at("ros2msg")->createParser(topic_name, type_name, CreateSchema(type_name), data); }