From 311a8c6d24a967dffc807c406ca62578becf5259 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 16 Jun 2024 16:13:27 +0200 Subject: [PATCH 1/9] add rosx_introspection to convert JSON encoding to cdr --- CMakeLists.txt | 10 +-- package.xml | 4 +- .../foxglove_bridge/ros2_foxglove_bridge.hpp | 3 + .../src/ros2_foxglove_bridge.cpp | 67 ++++++++++++++++--- 4 files changed, 65 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f0adae8..e8cefa5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,7 +24,6 @@ macro(enable_strict_compiler_warnings target) endif() endmacro() -find_package(nlohmann_json QUIET) find_package(OpenSSL REQUIRED) find_package(Threads REQUIRED) find_package(websocketpp REQUIRED) @@ -87,11 +86,7 @@ target_link_libraries(foxglove_bridge_base ZLIB::ZLIB ${CMAKE_THREAD_LIBS_INIT} ) -if(nlohmann_json_FOUND) - target_link_libraries(foxglove_bridge_base nlohmann_json::nlohmann_json) -else() - message(STATUS "nlohmann_json not found, will search at compile time") -endif() + enable_strict_compiler_warnings(foxglove_bridge_base) message(STATUS "ROS_VERSION: " $ENV{ROS_VERSION}) @@ -146,6 +141,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(resource_retriever REQUIRED) + find_package(rosx_introspection REQUIRED) add_library(foxglove_bridge_component SHARED ros2_foxglove_bridge/src/message_definition_cache.cpp @@ -168,7 +164,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") $ $ ) - ament_target_dependencies(foxglove_bridge_component ament_index_cpp rclcpp rclcpp_components resource_retriever rosgraph_msgs) + ament_target_dependencies(foxglove_bridge_component ament_index_cpp rclcpp rclcpp_components resource_retriever rosgraph_msgs rosx_introspection) target_link_libraries(foxglove_bridge_component foxglove_bridge_base) rclcpp_components_register_nodes(foxglove_bridge_component "foxglove_bridge::FoxgloveBridge") enable_strict_compiler_warnings(foxglove_bridge_component) diff --git a/package.xml b/package.xml index b22928d..418a00e 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,9 @@ rclcpp rclcpp_components + rosx_introspection + + gtest rostest @@ -36,7 +39,6 @@ asio libssl-dev libwebsocketpp-dev - nlohmann-json-dev ros_environment zlib diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp index 4a09ab6..143c800 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp @@ -20,6 +20,8 @@ #include #include +#include + namespace foxglove_bridge { using ConnectionHandle = websocketpp::connection_hdl; @@ -82,6 +84,7 @@ class FoxgloveBridge : public rclcpp::Node { std::atomic _subscribeGraphUpdates = false; bool _includeHidden = false; std::unique_ptr _fetchAssetQueue; + std::unordered_map> _jsonParsers; void subscribeConnectionGraph(bool subscribe); diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 19b2c7d..4d970fe 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -61,7 +61,7 @@ FoxgloveBridge::FoxgloveBridge(const rclcpp::NodeOptions& options) if (_useSimTime) { serverOptions.capabilities.push_back(foxglove::CAPABILITY_TIME); } - serverOptions.supportedEncodings = {"cdr"}; + serverOptions.supportedEncodings = {"cdr", "json"}; serverOptions.metadata = {{"ROS_DISTRO", rosDistro}}; serverOptions.sendBufferLimitBytes = send_buffer_limit; serverOptions.sessionId = std::to_string(std::time(nullptr)); @@ -264,7 +264,14 @@ void FoxgloveBridge::updateAdvertisedTopics( topicAndDatatype.first.c_str(), topicAndDatatype.second.c_str(), err.what()); continue; } - + // register the JSON parser for this scheName + auto parserIt = _jsonParsers.find(newChannel.topic); + if (parserIt == _jsonParsers.end()) { + auto parser = std::make_shared(newChannel.topic, + RosMsgParser::ROSType(newChannel.schemaName), + newChannel.schema); + _jsonParsers.insert({newChannel.topic, parser}); + } channelsToAdd.push_back(newChannel); } @@ -627,9 +634,10 @@ void FoxgloveBridge::clientAdvertise(const foxglove::ClientAdvertisement& advert publisherOptions.callback_group = _clientPublishCallbackGroup; auto publisher = this->create_generic_publisher(topicName, topicType, qos, publisherOptions); - RCLCPP_INFO(this->get_logger(), "Client %s is advertising \"%s\" (%s) on channel %d", + RCLCPP_INFO(this->get_logger(), + "Client %s is advertising \"%s\" (%s) on channel %d with encoding \"%s\"", _server->remoteEndpointString(hdl).c_str(), topicName.c_str(), topicType.c_str(), - advertisement.channelId); + advertisement.channelId, advertisement.encoding.c_str()); // Store the new topic advertisement clientPublications.emplace(advertisement.channelId, std::move(publisher)); @@ -702,14 +710,51 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne publisher = it2->second; } - // Copy the message payload into a SerializedMessage object - rclcpp::SerializedMessage serializedMessage{message.getLength()}; - auto& rclSerializedMsg = serializedMessage.get_rcl_serialized_message(); - std::memcpy(rclSerializedMsg.buffer, message.getData(), message.getLength()); - rclSerializedMsg.buffer_length = message.getLength(); + auto PublishMessage = [this, publisher](const void* data, size_t size) { + // Copy the message payload into a SerializedMessage object + rclcpp::SerializedMessage serializedMessage{size}; + auto& rclSerializedMsg = serializedMessage.get_rcl_serialized_message(); + std::memcpy(rclSerializedMsg.buffer, data, size); + rclSerializedMsg.buffer_length = size; + // Publish the message + publisher->publish(serializedMessage); + }; - // Publish the message - publisher->publish(serializedMessage); + if (message.advertisement.encoding == "cdr") { + PublishMessage(message.getData(), message.getLength()); + } else if (message.advertisement.encoding == "json") { + // get the specific parser for this schemaName + std::shared_ptr parser; + { + std::lock_guard lock(_subscriptionsMutex); + auto parserIt = _jsonParsers.find(message.advertisement.topic); + if (parserIt != _jsonParsers.end()) { + parser = parserIt->second; + } + } + if (!parser) { + RCLCPP_WARN(get_logger(), "No parser found for %s", message.advertisement.topic.c_str()); + } else { + thread_local RosMsgParser::ROS2_Serializer serializer; + serializer.reset(); + std::string_view jsonMessage(reinterpret_cast(message.getData()), message.getLength()); + try{ + parser->serializeFromJson(jsonMessage, &serializer); + PublishMessage(serializer.getBufferData(), serializer.getBufferSize()); + } + catch (const std::exception& ex) { + throw foxglove::ClientChannelError(message.advertisement.channelId, + std::string{"Dropping client message from "} + + _server->remoteEndpointString(hdl) + + " with encoding \"json\": " + ex.what()); + } + } + } else { + throw foxglove::ClientChannelError( + message.advertisement.channelId, + "Dropping client message from " + _server->remoteEndpointString(hdl) + + " with unknown encoding \"" + message.advertisement.encoding + "\""); + } } void FoxgloveBridge::setParameters(const std::vector& parameters, From d512c99266ac2c8e54c9b81a4a6b1927b2f1f663 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 16 Jun 2024 17:18:30 +0200 Subject: [PATCH 2/9] revert nlohmann-json changes --- CMakeLists.txt | 7 ++++++- package.xml | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e8cefa5..359f353 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ macro(enable_strict_compiler_warnings target) endif() endmacro() +find_package(nlohmann_json QUIET) find_package(OpenSSL REQUIRED) find_package(Threads REQUIRED) find_package(websocketpp REQUIRED) @@ -86,7 +87,11 @@ target_link_libraries(foxglove_bridge_base ZLIB::ZLIB ${CMAKE_THREAD_LIBS_INIT} ) - +if(nlohmann_json_FOUND) + target_link_libraries(foxglove_bridge_base nlohmann_json::nlohmann_json) +else() + message(STATUS "nlohmann_json not found, will search at compile time") +endif() enable_strict_compiler_warnings(foxglove_bridge_base) message(STATUS "ROS_VERSION: " $ENV{ROS_VERSION}) diff --git a/package.xml b/package.xml index 418a00e..105fe79 100644 --- a/package.xml +++ b/package.xml @@ -39,6 +39,7 @@ asio libssl-dev libwebsocketpp-dev + nlohmann-json-dev ros_environment zlib From c6faef48469a1a948d91487dd6ae8245c4703b16 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 17 Jun 2024 18:27:02 +0200 Subject: [PATCH 3/9] Update ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp Co-authored-by: Hans-Joachim Krauch --- ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 4d970fe..536c662 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -710,7 +710,7 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne publisher = it2->second; } - auto PublishMessage = [this, publisher](const void* data, size_t size) { + auto PublishMessage = [publisher](const void* data, size_t size) { // Copy the message payload into a SerializedMessage object rclcpp::SerializedMessage serializedMessage{size}; auto& rclSerializedMsg = serializedMessage.get_rcl_serialized_message(); From 0d5710f12afe194b0234d11bda14ed74b0e10bd1 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 22 Jul 2024 21:59:45 +0200 Subject: [PATCH 4/9] fix compilation --- ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 8551c80..7fc0a45 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -712,7 +712,7 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne publisher = it2->second; } - auto PublishMessage = [publisher](const void* data, size_t size) { + auto PublishMessage = [publisher, this](const void* data, size_t size) { // Copy the message payload into a SerializedMessage object rclcpp::SerializedMessage serializedMessage{size}; auto& rclSerializedMsg = serializedMessage.get_rcl_serialized_message(); From 345ffa2de616f92c55c191d64e24051b51b5903a Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 22 Jul 2024 22:05:38 +0200 Subject: [PATCH 5/9] apply clang-format --- .../foxglove_bridge/ros2_foxglove_bridge.hpp | 3 +-- .../src/ros2_foxglove_bridge.cpp | 17 ++++++++--------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp index a46ae5f..dcbd1a2 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -20,8 +21,6 @@ #include #include -#include - namespace foxglove_bridge { using ConnectionHandle = websocketpp::connection_hdl; diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 7fc0a45..041e6af 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -269,9 +269,8 @@ void FoxgloveBridge::updateAdvertisedTopics( // register the JSON parser for this scheName auto parserIt = _jsonParsers.find(newChannel.topic); if (parserIt == _jsonParsers.end()) { - auto parser = std::make_shared(newChannel.topic, - RosMsgParser::ROSType(newChannel.schemaName), - newChannel.schema); + auto parser = std::make_shared( + newChannel.topic, RosMsgParser::ROSType(newChannel.schemaName), newChannel.schema); _jsonParsers.insert({newChannel.topic, parser}); } channelsToAdd.push_back(newChannel); @@ -743,16 +742,16 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne } else { thread_local RosMsgParser::ROS2_Serializer serializer; serializer.reset(); - std::string_view jsonMessage(reinterpret_cast(message.getData()), message.getLength()); - try{ + std::string_view jsonMessage(reinterpret_cast(message.getData()), + message.getLength()); + try { parser->serializeFromJson(jsonMessage, &serializer); PublishMessage(serializer.getBufferData(), serializer.getBufferSize()); - } - catch (const std::exception& ex) { + } catch (const std::exception& ex) { throw foxglove::ClientChannelError(message.advertisement.channelId, std::string{"Dropping client message from "} + - _server->remoteEndpointString(hdl) + - " with encoding \"json\": " + ex.what()); + _server->remoteEndpointString(hdl) + + " with encoding \"json\": " + ex.what()); } } } else { From fb00ebcbb950b67798f4c11aebccd97d00571730 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 28 Jul 2024 16:47:31 +0200 Subject: [PATCH 6/9] more error messages and correct potential bug --- .../src/ros2_foxglove_bridge.cpp | 36 +++++++++++-------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 041e6af..9ef5153 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -266,12 +266,17 @@ void FoxgloveBridge::updateAdvertisedTopics( topicAndDatatype.first.c_str(), topicAndDatatype.second.c_str(), err.what()); continue; } - // register the JSON parser for this scheName - auto parserIt = _jsonParsers.find(newChannel.topic); - if (parserIt == _jsonParsers.end()) { - auto parser = std::make_shared( - newChannel.topic, RosMsgParser::ROSType(newChannel.schemaName), newChannel.schema); - _jsonParsers.insert({newChannel.topic, parser}); + // register the JSON parser for this schemaName + if(newChannel.schema.empty() && newChannel.encoding == "json") { + RCLCPP_ERROR(this->get_logger(), "Schema is empty for topic \"%s\" (%s)", + newChannel.topic.c_str(), newChannel.schemaName.c_str()); + } else { + auto parserIt = _jsonParsers.find(newChannel.schemaName); + if (parserIt == _jsonParsers.end()) { + auto parser = std::make_shared( + newChannel.topic, RosMsgParser::ROSType(newChannel.schemaName), newChannel.schema); + _jsonParsers.insert({newChannel.schemaName, parser}); + } } channelsToAdd.push_back(newChannel); } @@ -732,33 +737,36 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne std::shared_ptr parser; { std::lock_guard lock(_subscriptionsMutex); - auto parserIt = _jsonParsers.find(message.advertisement.topic); + auto parserIt = _jsonParsers.find(message.advertisement.schemaName); if (parserIt != _jsonParsers.end()) { parser = parserIt->second; } } if (!parser) { - RCLCPP_WARN(get_logger(), "No parser found for %s", message.advertisement.topic.c_str()); + throw foxglove::ClientChannelError(message.advertisement.channelId, + "Dropping client message from " + + _server->remoteEndpointString(hdl) + + " with encoding \"json\": no parser found"); } else { thread_local RosMsgParser::ROS2_Serializer serializer; serializer.reset(); - std::string_view jsonMessage(reinterpret_cast(message.getData()), - message.getLength()); + const std::string jsonMessage(reinterpret_cast(message.getData()), + message.getLength()); try { parser->serializeFromJson(jsonMessage, &serializer); PublishMessage(serializer.getBufferData(), serializer.getBufferSize()); } catch (const std::exception& ex) { throw foxglove::ClientChannelError(message.advertisement.channelId, - std::string{"Dropping client message from "} + - _server->remoteEndpointString(hdl) + - " with encoding \"json\": " + ex.what()); + "Dropping client message from " + + _server->remoteEndpointString(hdl) + + " with encoding \"json\": " + ex.what()); } } } else { throw foxglove::ClientChannelError( message.advertisement.channelId, "Dropping client message from " + _server->remoteEndpointString(hdl) + - " with unknown encoding \"" + message.advertisement.encoding + "\""); + " with unknown encoding \"" + message.advertisement.encoding + "\""); } } From 7cee1d7095b32c609f3e7b8f39a82a3d64718748 Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Mon, 29 Jul 2024 11:34:42 +0000 Subject: [PATCH 7/9] create json parser only when client channel is advertised --- .../src/ros2_foxglove_bridge.cpp | 66 +++++++++++++------ 1 file changed, 45 insertions(+), 21 deletions(-) diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 9ef5153..1e37963 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -266,18 +266,6 @@ void FoxgloveBridge::updateAdvertisedTopics( topicAndDatatype.first.c_str(), topicAndDatatype.second.c_str(), err.what()); continue; } - // register the JSON parser for this schemaName - if(newChannel.schema.empty() && newChannel.encoding == "json") { - RCLCPP_ERROR(this->get_logger(), "Schema is empty for topic \"%s\" (%s)", - newChannel.topic.c_str(), newChannel.schemaName.c_str()); - } else { - auto parserIt = _jsonParsers.find(newChannel.schemaName); - if (parserIt == _jsonParsers.end()) { - auto parser = std::make_shared( - newChannel.topic, RosMsgParser::ROSType(newChannel.schemaName), newChannel.schema); - _jsonParsers.insert({newChannel.schemaName, parser}); - } - } channelsToAdd.push_back(newChannel); } @@ -613,6 +601,42 @@ void FoxgloveBridge::clientAdvertise(const foxglove::ClientAdvertisement& advert std::to_string(advertisement.channelId) + " it had already advertised"); } + if (advertisement.schemaName.empty()) { + throw foxglove::ClientChannelError( + advertisement.channelId, + "Received client advertisement from " + _server->remoteEndpointString(hdl) + " for channel " + + std::to_string(advertisement.channelId) + " with empty schema name"); + } + + if (advertisement.encoding == "json") { + // register the JSON parser for this schemaName + auto parserIt = _jsonParsers.find(advertisement.schemaName); + if (parserIt == _jsonParsers.end()) { + const auto& schemaName = advertisement.schemaName; + std::string schema = ""; + + if (!advertisement.schema.empty()) { + // Schema is given by the advertisement + schema = std::string(reinterpret_cast(advertisement.schema.data()), + advertisement.schema.size()); + } else { + // Schema not given, look it up. + auto [format, msgDefinition] = _messageDefinitionCache.get_full_text(schemaName); + if (format != foxglove::MessageDefinitionFormat::MSG) { + throw foxglove::ClientChannelError( + advertisement.channelId, + "Message definition (.msg) for schema " + schemaName + " not found."); + } + + schema = msgDefinition; + } + + auto parser = std::make_shared( + advertisement.topic, RosMsgParser::ROSType(schemaName), schema); + _jsonParsers.insert({schemaName, parser}); + } + } + try { // Create a new topic advertisement const auto& topicName = advertisement.topic; @@ -716,7 +740,7 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne publisher = it2->second; } - auto PublishMessage = [publisher, this](const void* data, size_t size) { + auto publishMessage = [publisher, this](const void* data, size_t size) { // Copy the message payload into a SerializedMessage object rclcpp::SerializedMessage serializedMessage{size}; auto& rclSerializedMsg = serializedMessage.get_rcl_serialized_message(); @@ -731,12 +755,12 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne }; if (message.advertisement.encoding == "cdr") { - PublishMessage(message.getData(), message.getLength()); + publishMessage(message.getData(), message.getLength()); } else if (message.advertisement.encoding == "json") { // get the specific parser for this schemaName std::shared_ptr parser; { - std::lock_guard lock(_subscriptionsMutex); + std::lock_guard lock(_clientAdvertisementsMutex); auto parserIt = _jsonParsers.find(message.advertisement.schemaName); if (parserIt != _jsonParsers.end()) { parser = parserIt->second; @@ -745,8 +769,8 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne if (!parser) { throw foxglove::ClientChannelError(message.advertisement.channelId, "Dropping client message from " + - _server->remoteEndpointString(hdl) + - " with encoding \"json\": no parser found"); + _server->remoteEndpointString(hdl) + + " with encoding \"json\": no parser found"); } else { thread_local RosMsgParser::ROS2_Serializer serializer; serializer.reset(); @@ -754,19 +778,19 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne message.getLength()); try { parser->serializeFromJson(jsonMessage, &serializer); - PublishMessage(serializer.getBufferData(), serializer.getBufferSize()); + publishMessage(serializer.getBufferData(), serializer.getBufferSize()); } catch (const std::exception& ex) { throw foxglove::ClientChannelError(message.advertisement.channelId, "Dropping client message from " + - _server->remoteEndpointString(hdl) + - " with encoding \"json\": " + ex.what()); + _server->remoteEndpointString(hdl) + + " with encoding \"json\": " + ex.what()); } } } else { throw foxglove::ClientChannelError( message.advertisement.channelId, "Dropping client message from " + _server->remoteEndpointString(hdl) + - " with unknown encoding \"" + message.advertisement.encoding + "\""); + " with unknown encoding \"" + message.advertisement.encoding + "\""); } } From 44abb8d6eaae72502067d106d6a01555d566224e Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Mon, 29 Jul 2024 11:35:10 +0000 Subject: [PATCH 8/9] fix wrong exception being caught --- .../include/foxglove_bridge/websocket_server.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp index 0bccdb0..2dc4057 100644 --- a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp +++ b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp @@ -761,10 +761,10 @@ inline void Server::handleBinaryMessage(ConnHandle hdl, Mes length, data}; _handlers.clientMessageHandler(clientMessage, hdl); - } catch (const ServiceError& e) { + } catch (const ClientChannelError& e) { sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what()); } catch (...) { - sendStatusAndLogMsg(hdl, StatusLevel::Error, "callService: Failed to execute handler"); + sendStatusAndLogMsg(hdl, StatusLevel::Error, "clientMessage: Failed to execute handler"); } } break; case ClientBinaryOpcode::SERVICE_CALL_REQUEST: { From db0d47c6dcb8ab0ed4c602df9dd57e573b0d2036 Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Mon, 29 Jul 2024 11:35:21 +0000 Subject: [PATCH 9/9] add smoke tests for json parsing --- ros2_foxglove_bridge/tests/smoke_test.cpp | 63 +++++++++++++++++------ 1 file changed, 48 insertions(+), 15 deletions(-) diff --git a/ros2_foxglove_bridge/tests/smoke_test.cpp b/ros2_foxglove_bridge/tests/smoke_test.cpp index 8112e78..306b6ca 100644 --- a/ros2_foxglove_bridge/tests/smoke_test.cpp +++ b/ros2_foxglove_bridge/tests/smoke_test.cpp @@ -15,8 +15,10 @@ constexpr char URI[] = "ws://localhost:8765"; // Binary representation of std_msgs/msg/String for "hello world" -constexpr uint8_t HELLO_WORLD_BINARY[] = {0, 1, 0, 0, 12, 0, 0, 0, 104, 101, - 108, 108, 111, 32, 119, 111, 114, 108, 100, 0}; +constexpr uint8_t HELLO_WORLD_CDR[] = {0, 1, 0, 0, 12, 0, 0, 0, 104, 101, + 108, 108, 111, 32, 119, 111, 114, 108, 100, 0}; +constexpr char HELLO_WORLD_JSON[] = "{\"data\": \"hello world\"}"; +constexpr char STD_MSGS_STRING_SCHEMA[] = "data string"; constexpr auto ONE_SECOND = std::chrono::seconds(1); constexpr auto DEFAULT_TIMEOUT = std::chrono::seconds(10); @@ -124,7 +126,11 @@ class ServiceTest : public TestWithExecutor { std::shared_ptr> _wsClient; }; -class ExistingPublisherTest : public TestWithExecutor { +class PublisherTest + : public TestWithExecutor, + public testing::WithParamInterface>> {}; + +class ExistingPublisherTest : public PublisherTest { public: inline static const std::string TOPIC_NAME = "/some_topic"; @@ -198,8 +204,8 @@ TEST(SmokeTest, testSubscription) { client->subscribe({{subscriptionId, channel.id}}); ASSERT_EQ(std::future_status::ready, msgFuture.wait_for(ONE_SECOND)); const auto msgData = msgFuture.get(); - ASSERT_EQ(sizeof(HELLO_WORLD_BINARY), msgData.size()); - EXPECT_EQ(0, std::memcmp(HELLO_WORLD_BINARY, msgData.data(), msgData.size())); + ASSERT_EQ(sizeof(HELLO_WORLD_CDR), msgData.size()); + EXPECT_EQ(0, std::memcmp(HELLO_WORLD_CDR, msgData.data(), msgData.size())); // Unsubscribe from the channel again. client->unsubscribe({subscriptionId}); @@ -243,8 +249,8 @@ TEST(SmokeTest, testSubscriptionParallel) { for (auto& future : futures) { ASSERT_EQ(std::future_status::ready, future.wait_for(DEFAULT_TIMEOUT)); auto msgData = future.get(); - ASSERT_EQ(sizeof(HELLO_WORLD_BINARY), msgData.size()); - EXPECT_EQ(0, std::memcmp(HELLO_WORLD_BINARY, msgData.data(), msgData.size())); + ASSERT_EQ(sizeof(HELLO_WORLD_CDR), msgData.size()); + EXPECT_EQ(0, std::memcmp(HELLO_WORLD_CDR, msgData.data(), msgData.size())); } for (auto client : clients) { @@ -252,12 +258,16 @@ TEST(SmokeTest, testSubscriptionParallel) { } } -TEST_F(TestWithExecutor, testPublishing) { +TEST_P(PublisherTest, testPublishing) { + const auto& [encoding, message] = GetParam(); + foxglove::ClientAdvertisement advertisement; advertisement.channelId = 1; advertisement.topic = "/foo"; - advertisement.encoding = "cdr"; - advertisement.schemaName = "std_msgs/String"; + advertisement.encoding = encoding; + advertisement.schemaName = "std_msgs/msg/String"; + advertisement.schema = + std::vector(STD_MSGS_STRING_SCHEMA, std::end(STD_MSGS_STRING_SCHEMA)); // Set up a ROS node with a subscriber std::promise msgPromise; @@ -279,7 +289,7 @@ TEST_F(TestWithExecutor, testPublishing) { ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(ONE_SECOND)); // Publish the message and unadvertise again - client->publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY)); + client->publish(advertisement.channelId, message.data(), message.size()); client->unadvertise({advertisement.channelId}); // Ensure that we have received the correct message via our ROS subscriber @@ -288,12 +298,25 @@ TEST_F(TestWithExecutor, testPublishing) { EXPECT_EQ("hello world", msgFuture.get()); } -TEST_F(ExistingPublisherTest, testPublishingWithExistingPublisher) { +INSTANTIATE_TEST_SUITE_P( + TestPublishingCDR, PublisherTest, + testing::Values(std::make_pair("cdr", std::vector(HELLO_WORLD_CDR, + std::end(HELLO_WORLD_CDR))))); + +INSTANTIATE_TEST_SUITE_P( + TestPublishingJSON, PublisherTest, + testing::Values(std::make_pair("json", std::vector(HELLO_WORLD_JSON, + std::end(HELLO_WORLD_JSON))))); + +TEST_P(ExistingPublisherTest, testPublishingWithExistingPublisher) { + const auto& [encoding, message] = GetParam(); + foxglove::ClientAdvertisement advertisement; advertisement.channelId = 1; advertisement.topic = TOPIC_NAME; - advertisement.encoding = "cdr"; - advertisement.schemaName = "std_msgs/String"; + advertisement.encoding = encoding; + advertisement.schemaName = "std_msgs/msg/String"; + advertisement.schema = {}; // Schema intentionally left empty. // Set up a ROS node with a subscriber std::promise msgPromise; @@ -316,7 +339,7 @@ TEST_F(ExistingPublisherTest, testPublishingWithExistingPublisher) { ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(ONE_SECOND)); // Publish the message and unadvertise again - client->publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY)); + client->publish(advertisement.channelId, message.data(), message.size()); client->unadvertise({advertisement.channelId}); // Ensure that we have received the correct message via our ROS subscriber @@ -325,6 +348,16 @@ TEST_F(ExistingPublisherTest, testPublishingWithExistingPublisher) { EXPECT_EQ("hello world", msgFuture.get()); } +INSTANTIATE_TEST_SUITE_P( + ExistingPublisherTestCDR, ExistingPublisherTest, + testing::Values(std::make_pair("cdr", std::vector(HELLO_WORLD_CDR, + std::end(HELLO_WORLD_CDR))))); + +INSTANTIATE_TEST_SUITE_P( + ExistingPublisherTestJSON, ExistingPublisherTest, + testing::Values(std::make_pair("json", std::vector(HELLO_WORLD_JSON, + std::end(HELLO_WORLD_JSON))))); + TEST_F(ParameterTest, testGetAllParams) { const std::string requestId = "req-testGetAllParams"; auto future = foxglove::waitForParameters(_wsClient, requestId);