-
Notifications
You must be signed in to change notification settings - Fork 71
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Fix JSON message parsing, array support, refactor into separate file,…
… unit tests
- Loading branch information
Showing
5 changed files
with
214 additions
and
111 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
21 changes: 21 additions & 0 deletions
21
ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,21 @@ | ||
| #pragma once | ||
|
|
||
| #include <optional> | ||
|
|
||
| #include <ros2_babel_fish/babel_fish.hpp> | ||
|
|
||
| namespace foxglove_bridge { | ||
|
|
||
| /** | ||
| * Convert a JSON-serialized message with a given named schema to a ROS message | ||
| * using ros2_babel_fish. The output message is allocated as a shared pointer | ||
| * and assigned to the outputMessage argument. The return value is an optional | ||
| * exception, which if set indicates that an error occurred during the | ||
| * conversion and `outputMessage` is not valid. | ||
| */ | ||
| std::optional<std::exception> jsonMessageToRos( | ||
| const std::string_view jsonMessage, const std::string& schemaName, | ||
| ros2_babel_fish::BabelFish::SharedPtr babelFish, | ||
| ros2_babel_fish::CompoundMessage::SharedPtr& outputMessage); | ||
|
|
||
| } // namespace foxglove_bridge |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,135 @@ | ||
| #include "foxglove_bridge/json_to_ros.hpp" | ||
|
|
||
| #include <nlohmann/json.hpp> | ||
|
|
||
| namespace foxglove_bridge { | ||
|
|
||
| static void assignJsonToMessageField(ros2_babel_fish::Message& field, const nlohmann::json& value) { | ||
| using MessageType = ros2_babel_fish::MessageType; | ||
|
|
||
| if (value.is_null()) { | ||
| return; | ||
| } | ||
|
|
||
| switch (field.type()) { | ||
| case MessageType::None: | ||
| break; | ||
| case MessageType::Float: | ||
| field = value.get<float>(); | ||
| break; | ||
| case MessageType::Double: | ||
| field = value.get<double>(); | ||
| break; | ||
| case MessageType::LongDouble: | ||
| field = value.get<long double>(); | ||
| break; | ||
| case MessageType::Char: | ||
| field = value.get<char>(); | ||
| break; | ||
| case MessageType::WChar: | ||
| field = value.get<wchar_t>(); | ||
| break; | ||
| case MessageType::Bool: | ||
| field = value.get<bool>(); | ||
| break; | ||
| case MessageType::Octet: | ||
| case MessageType::UInt8: | ||
| field = value.get<uint8_t>(); | ||
| break; | ||
| case MessageType::Int8: | ||
| field = value.get<int8_t>(); | ||
| break; | ||
| case MessageType::UInt16: | ||
| field = value.get<uint16_t>(); | ||
| break; | ||
| case MessageType::Int16: | ||
| field = value.get<int16_t>(); | ||
| break; | ||
| case MessageType::UInt32: | ||
| field = value.get<uint32_t>(); | ||
| break; | ||
| case MessageType::Int32: | ||
| field = value.get<int32_t>(); | ||
| break; | ||
| case MessageType::UInt64: | ||
| field = value.get<uint64_t>(); | ||
| break; | ||
| case MessageType::Int64: | ||
| field = value.get<int64_t>(); | ||
| break; | ||
| case MessageType::String: | ||
| field = value.get<std::string>(); | ||
| break; | ||
| case MessageType::WString: | ||
| field = value.get<std::wstring>(); | ||
| break; | ||
| case MessageType::Compound: { | ||
| // Recursively convert compound messages | ||
| auto& compound = field.as<ros2_babel_fish::CompoundMessage>(); | ||
| for (const auto& key : compound.keys()) { | ||
| assignJsonToMessageField(compound[key], value[key]); | ||
| } | ||
| break; | ||
| } | ||
| case MessageType::Array: { | ||
| // Ensure the JSON value is an array | ||
| if (!value.is_array()) { | ||
| break; | ||
| } | ||
|
|
||
| auto& array = field.as<ros2_babel_fish::CompoundArrayMessage>(); | ||
| if (array.isFixedSize() || array.isBounded()) { | ||
| const size_t limit = std::min(array.maxSize(), value.size()); | ||
| for (size_t i = 0; i < limit; ++i) { | ||
| auto& arrayEntry = array.size() > i ? array[i] : array.appendEmpty(); | ||
| assignJsonToMessageField(arrayEntry, value[i]); | ||
| } | ||
| } else { | ||
| array.clear(); | ||
| for (const auto& jsonArrayEntry : value) { | ||
| auto& arrayEntry = array.appendEmpty(); | ||
| assignJsonToMessageField(arrayEntry, jsonArrayEntry); | ||
| } | ||
| } | ||
| break; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| std::optional<std::exception> jsonMessageToRos( | ||
| const std::string_view jsonMessage, const std::string& schemaName, | ||
| ros2_babel_fish::BabelFish::SharedPtr babelFish, | ||
| ros2_babel_fish::CompoundMessage::SharedPtr& outputMessage) { | ||
| // Decode the JSON message | ||
| nlohmann::json json; | ||
| try { | ||
| json = nlohmann::json::parse(jsonMessage); | ||
| } catch (const nlohmann::json::parse_error& e) { | ||
| return e; | ||
| } | ||
|
|
||
| // Convert the JSON message to a ROS message using ros2_babel_fish | ||
| ros2_babel_fish::CompoundMessage::SharedPtr rosMsgPtr; | ||
| try { | ||
| rosMsgPtr = babelFish->create_message_shared(schemaName); | ||
| } catch (const ros2_babel_fish::BabelFishException& e) { | ||
| return e; | ||
| } | ||
| auto& rosMsg = *rosMsgPtr; | ||
| for (const auto& key : rosMsg.keys()) { | ||
| if (!json.contains(key)) { | ||
| continue; | ||
| } | ||
|
|
||
| try { | ||
| assignJsonToMessageField(rosMsg[key], json[key]); | ||
| } catch (const std::exception& e) { | ||
| return e; | ||
| } | ||
| } | ||
|
|
||
| outputMessage = rosMsgPtr; | ||
| return std::nullopt; | ||
| } | ||
|
|
||
| } // namespace foxglove_bridge |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,42 @@ | ||
| #include <gtest/gtest.h> | ||
|
|
||
| #include <foxglove_bridge/json_to_ros.hpp> | ||
|
|
||
| TEST(JsonToRosTest, EmptyStringMsg) { | ||
| const std::string payload = "{}"; | ||
|
|
||
| auto babelFish = ros2_babel_fish::BabelFish::make_shared(); | ||
|
|
||
| ros2_babel_fish::CompoundMessage::SharedPtr output; | ||
| auto res = foxglove_bridge::jsonMessageToRos(payload, "std_msgs/msg/String", babelFish, output); | ||
| ASSERT_FALSE(res.has_value()) << "Error converting JSON to ROS: " << res.value().what(); | ||
| ASSERT_TRUE(output) << "Output message is null"; | ||
| EXPECT_EQ(output->datatype(), "std_msgs::msg::String"); | ||
| EXPECT_EQ(output->memberCount(), 1); | ||
| EXPECT_EQ((*output)["data"].type(), ros2_babel_fish::MessageType::String); | ||
| EXPECT_EQ((*output)["data"].value<std::string>(), ""); | ||
| EXPECT_TRUE(output->type_erased_message()); | ||
| } | ||
|
|
||
| TEST(JsonToRosTest, StringMsg) { | ||
| const std::string payload = R"( | ||
| { "data": "Hello, World!" } | ||
| )"; | ||
|
|
||
| auto babelFish = ros2_babel_fish::BabelFish::make_shared(); | ||
|
|
||
| ros2_babel_fish::CompoundMessage::SharedPtr output; | ||
| auto res = foxglove_bridge::jsonMessageToRos(payload, "std_msgs/msg/String", babelFish, output); | ||
| ASSERT_FALSE(res.has_value()) << "Error converting JSON to ROS: " << res.value().what(); | ||
| ASSERT_TRUE(output) << "Output message is null"; | ||
| EXPECT_EQ(output->datatype(), "std_msgs::msg::String"); | ||
| EXPECT_EQ(output->memberCount(), 1); | ||
| EXPECT_EQ((*output)["data"].type(), ros2_babel_fish::MessageType::String); | ||
| EXPECT_EQ((*output)["data"].value<std::string>(), "Hello, World!"); | ||
| EXPECT_TRUE(output->type_erased_message()); | ||
| } | ||
|
|
||
| int main(int argc, char** argv) { | ||
| testing::InitGoogleTest(&argc, argv); | ||
| return RUN_ALL_TESTS(); | ||
| } |