diff --git a/CMakeLists.txt b/CMakeLists.txt index b23a4de..4910c67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,6 +186,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") ) add_library(foxglove_bridge_component SHARED + ros2_foxglove_bridge/src/json_to_ros.cpp ros2_foxglove_bridge/src/message_definition_cache.cpp ros2_foxglove_bridge/src/param_utils.cpp ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -269,6 +270,11 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake") target_link_libraries(base64_test foxglove_bridge_base) enable_strict_compiler_warnings(base64_test) + ament_add_gtest(json_to_ros_test ros2_foxglove_bridge/tests/json_to_ros_test.cpp) + ament_target_dependencies(json_to_ros_test rclcpp rclcpp_components std_msgs) + target_link_libraries(json_to_ros_test foxglove_bridge_component) + enable_strict_compiler_warnings(json_to_ros_test) + ament_add_gtest(smoke_test ros2_foxglove_bridge/tests/smoke_test.cpp) ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs std_srvs) target_link_libraries(smoke_test foxglove_bridge_base) diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp new file mode 100644 index 0000000..09a3fd6 --- /dev/null +++ b/ros2_foxglove_bridge/include/foxglove_bridge/json_to_ros.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include + +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 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 diff --git a/ros2_foxglove_bridge/src/json_to_ros.cpp b/ros2_foxglove_bridge/src/json_to_ros.cpp new file mode 100644 index 0000000..07988ee --- /dev/null +++ b/ros2_foxglove_bridge/src/json_to_ros.cpp @@ -0,0 +1,135 @@ +#include "foxglove_bridge/json_to_ros.hpp" + +#include + +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(); + break; + case MessageType::Double: + field = value.get(); + break; + case MessageType::LongDouble: + field = value.get(); + break; + case MessageType::Char: + field = value.get(); + break; + case MessageType::WChar: + field = value.get(); + break; + case MessageType::Bool: + field = value.get(); + break; + case MessageType::Octet: + case MessageType::UInt8: + field = value.get(); + break; + case MessageType::Int8: + field = value.get(); + break; + case MessageType::UInt16: + field = value.get(); + break; + case MessageType::Int16: + field = value.get(); + break; + case MessageType::UInt32: + field = value.get(); + break; + case MessageType::Int32: + field = value.get(); + break; + case MessageType::UInt64: + field = value.get(); + break; + case MessageType::Int64: + field = value.get(); + break; + case MessageType::String: + field = value.get(); + break; + case MessageType::WString: + field = value.get(); + break; + case MessageType::Compound: { + // Recursively convert compound messages + auto& compound = field.as(); + 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(); + 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 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 diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 7e010a1..d645699 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -1,8 +1,8 @@ #include -#include #include +#include #include namespace foxglove_bridge { @@ -13,80 +13,6 @@ inline bool isHiddenTopicOrService(const std::string& name) { } return name.front() == '_' || name.find("/_") != std::string::npos; } - -inline 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(); - break; - case MessageType::Double: - field = value.get(); - break; - case MessageType::LongDouble: - field = value.get(); - break; - case MessageType::Char: - field = value.get(); - break; - case MessageType::WChar: - field = value.get(); - break; - case MessageType::Bool: - field = value.get(); - break; - case MessageType::Octet: - case MessageType::UInt8: - field = value.get(); - break; - case MessageType::Int8: - field = value.get(); - break; - case MessageType::UInt16: - field = value.get(); - break; - case MessageType::Int16: - field = value.get(); - break; - case MessageType::UInt32: - field = value.get(); - break; - case MessageType::Int32: - field = value.get(); - break; - case MessageType::UInt64: - field = value.get(); - break; - case MessageType::Int64: - field = value.get(); - break; - case MessageType::String: - field = value.get(); - break; - case MessageType::WString: - field = value.get(); - break; - case MessageType::Compound: { - // Recursively convert compound messages - auto& compound = field.as(); - for (const auto& key : compound.keys()) { - assignJsonToMessageField(compound[key], value[key]); - } - break; - } - case MessageType::Array: { - // FIXME: Handle variable length arrays, fixed length arrays, and bounded arrays - break; - } - } -} } // namespace using namespace std::chrono_literals; @@ -788,49 +714,22 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne // Publish the message publisher->publish(serializedMessage); } else if (message.advertisement.encoding == "json") { - // Decode the JSON message - nlohmann::json json; - try { - const std::string_view messageDataView{reinterpret_cast(message.data.data()), - message.data.size()}; - json = nlohmann::json::parse(messageDataView); - } catch (const nlohmann::json::parse_error& ex) { + const auto jsonMessage = + std::string_view{reinterpret_cast(message.getData()), message.getLength()}; + ros2_babel_fish::CompoundMessage::SharedPtr rosMsg; + const auto maybeErr = + jsonMessageToRos(jsonMessage, message.advertisement.schemaName, _babelFish, rosMsg); + if (maybeErr) { throw foxglove::ClientChannelError(message.advertisement.channelId, - "Dropping client message from " + + std::string{"Dropping client message from "} + _server->remoteEndpointString(hdl) + - " with invalid JSON: " + ex.what()); - } - - // Convert the JSON message to a ROS message using ros2_babel_fish - ros2_babel_fish::CompoundMessage::SharedPtr rosMsgPtr; - try { - rosMsgPtr = _babelFish->create_message_shared(message.advertisement.schemaName); - } catch (const std::exception& ex) { - throw foxglove::ClientChannelError( - message.advertisement.channelId, - "Dropping client message from " + _server->remoteEndpointString(hdl) + - " with unknown schema \"" + message.advertisement.schemaName + "\": " + ex.what()); - } - auto& rosMsg = *rosMsgPtr; - for (const auto& key : rosMsg.keys()) { - if (!json.contains(key)) { - continue; - } - - try { - assignJsonToMessageField(rosMsg[key], json[key]); - } catch (const std::exception& ex) { - throw foxglove::ClientChannelError(message.advertisement.channelId, - "Dropping client message from " + - _server->remoteEndpointString(hdl) + - " with invalid JSON: " + ex.what()); - } + " with encoding \"json\": " + maybeErr.value().what()); } // Publish the assembled ROS message auto publisherHandle = publisher->get_publisher_handle(); const auto status = - rcl_publish(publisherHandle.get(), rosMsg.type_erased_message().get(), nullptr); + rcl_publish(publisherHandle.get(), rosMsg->type_erased_message().get(), nullptr); if (RCL_RET_OK != status) { rclcpp::exceptions::throw_from_rcl_error(status, "Failed to publish message"); } diff --git a/ros2_foxglove_bridge/tests/json_to_ros_test.cpp b/ros2_foxglove_bridge/tests/json_to_ros_test.cpp new file mode 100644 index 0000000..692c51f --- /dev/null +++ b/ros2_foxglove_bridge/tests/json_to_ros_test.cpp @@ -0,0 +1,42 @@ +#include + +#include + +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(), ""); + 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(), "Hello, World!"); + EXPECT_TRUE(output->type_erased_message()); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}