diff --git a/foxglove_bridge_base/include/foxglove_bridge/server_interface.hpp b/foxglove_bridge_base/include/foxglove_bridge/server_interface.hpp index 4c02caa..61cf636 100644 --- a/foxglove_bridge_base/include/foxglove_bridge/server_interface.hpp +++ b/foxglove_bridge_base/include/foxglove_bridge/server_interface.hpp @@ -99,6 +99,8 @@ class ServerInterface { virtual void broadcastTime(uint64_t timestamp) = 0; virtual void sendServiceResponse(ConnectionHandle clientHandle, const ServiceResponse& response) = 0; + virtual void sendServiceFailure(ConnectionHandle clientHandle, ServiceId serviceId, + uint32_t callId, const std::string& message) = 0; virtual void updateConnectionGraph(const MapOfSets& publishedTopics, const MapOfSets& subscribedTopics, const MapOfSets& advertisedServices) = 0; diff --git a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp index a642dee..99bceb5 100644 --- a/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp +++ b/foxglove_bridge_base/include/foxglove_bridge/websocket_server.hpp @@ -145,6 +145,8 @@ class Server final : public ServerInterface { const uint8_t* payload, size_t payloadSize) override; void broadcastTime(uint64_t timestamp) override; void sendServiceResponse(ConnHandle clientHandle, const ServiceResponse& response) override; + void sendServiceFailure(ConnHandle clientHandle, ServiceId serviceId, uint32_t callId, + const std::string& message) override; void updateConnectionGraph(const MapOfSets& publishedTopics, const MapOfSets& subscribedTopics, const MapOfSets& advertisedServices) override; void sendFetchAssetResponse(ConnHandle clientHandle, const FetchAssetResponse& response) override; @@ -768,8 +770,10 @@ inline void Server::handleBinaryMessage(ConnHandle hdl, Mes case ClientBinaryOpcode::SERVICE_CALL_REQUEST: { ServiceRequest request; if (length < request.size()) { - sendStatusAndLogMsg(hdl, StatusLevel::Error, - "Invalid service call request length " + std::to_string(length)); + const std::string errMessage = + "Invalid service call request length " + std::to_string(length); + sendServiceFailure(hdl, request.serviceId, request.callId, errMessage); + _server.get_elog().write(RECOVERABLE, errMessage); return; } @@ -778,15 +782,23 @@ inline void Server::handleBinaryMessage(ConnHandle hdl, Mes { std::shared_lock lock(_servicesMutex); if (_services.find(request.serviceId) == _services.end()) { - sendStatusAndLogMsg( - hdl, StatusLevel::Error, - "Service " + std::to_string(request.serviceId) + " is not advertised"); + const std::string errMessage = + "Service " + std::to_string(request.serviceId) + " is not advertised"; + sendServiceFailure(hdl, request.serviceId, request.callId, errMessage); + _server.get_elog().write(RECOVERABLE, errMessage); return; } } - if (_handlers.serviceRequestHandler) { + try { + if (!_handlers.serviceRequestHandler) { + throw foxglove::ServiceError(request.serviceId, "No service handler"); + } + _handlers.serviceRequestHandler(request, hdl); + } catch (const std::exception& e) { + sendServiceFailure(hdl, request.serviceId, request.callId, e.what()); + _server.get_elog().write(RECOVERABLE, e.what()); } } break; default: { @@ -1024,6 +1036,16 @@ inline uint16_t Server::getPort() { return endpoint.port(); } +template +inline void Server::sendServiceFailure(ConnHandle clientHandle, + ServiceId serviceId, uint32_t callId, + const std::string& message) { + sendJson(clientHandle, json{{"op", "serviceCallFailure"}, + {"serviceId", serviceId}, + {"callId", callId}, + {"message", message}}); +}; + template inline void Server::updateConnectionGraph( const MapOfSets& publishedTopics, const MapOfSets& subscribedTopics, diff --git a/ros2_foxglove_bridge/tests/smoke_test.cpp b/ros2_foxglove_bridge/tests/smoke_test.cpp index be5522b..0d8f83f 100644 --- a/ros2_foxglove_bridge/tests/smoke_test.cpp +++ b/ros2_foxglove_bridge/tests/smoke_test.cpp @@ -604,6 +604,32 @@ TEST_F(ServiceTest, testCallServiceParallel) { } } +TEST_F(ServiceTest, testCallNonexistentService) { + auto client = std::make_shared>(); + ASSERT_EQ(std::future_status::ready, client->connect(URI).wait_for(std::chrono::seconds(5))); + + std::promise promise; + auto serviceFailureFuture = promise.get_future(); + client->setTextMessageHandler([&promise](const std::string& payload) mutable { + const auto msg = nlohmann::json::parse(payload); + if (msg["op"].get() == "serviceCallFailure") { + promise.set_value(msg); + } + }); + + foxglove::ServiceRequest request; + request.serviceId = 99u; + request.callId = 123u; + request.encoding = ""; + request.data = {}; + client->sendServiceRequest(request); + + ASSERT_EQ(std::future_status::ready, serviceFailureFuture.wait_for(std::chrono::seconds(5))); + const auto failureMsg = serviceFailureFuture.get(); + EXPECT_EQ(failureMsg["serviceId"].get(), request.serviceId); + EXPECT_EQ(failureMsg["callId"].get(), request.callId); +} + TEST(SmokeTest, receiveMessagesOfMultipleTransientLocalPublishers) { const std::string topicName = "/latched"; auto node = rclcpp::Node::make_shared("node");