Skip to content

Commit

Permalink
Add ServerFactory for better decoupling from server implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
achim-k committed Feb 14, 2023
1 parent bf6ca9d commit 427675d
Show file tree
Hide file tree
Showing 10 changed files with 100 additions and 76 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ add_library(foxglove_bridge_base SHARED
foxglove_bridge_base/src/foxglove_bridge.cpp
foxglove_bridge_base/src/parameter.cpp
foxglove_bridge_base/src/serialization.cpp
foxglove_bridge_base/src/server_factory.cpp
foxglove_bridge_base/src/test/test_client.cpp
)
target_include_directories(foxglove_bridge_base
Expand Down
8 changes: 8 additions & 0 deletions foxglove_bridge_base/include/foxglove_bridge/common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,14 @@ enum class ClientBinaryOpcode : uint8_t {
SERVICE_CALL_REQUEST = 2,
};

enum class WebSocketLogLevel {
Debug,
Info,
Warn,
Error,
Critical,
};

struct ChannelWithoutId {
std::string topic;
std::string encoding;
Expand Down
21 changes: 21 additions & 0 deletions foxglove_bridge_base/include/foxglove_bridge/server_factory.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
#pragma once

#include <memory>
#include <string>

#include <websocketpp/common/connection_hdl.hpp>

#include "common.hpp"
#include "server_interface.hpp"

namespace foxglove {

class ServerFactory {
public:
template <typename ConnectionHandle>
static std::unique_ptr<ServerInterface<ConnectionHandle>> createServer(
const std::string& name, const std::function<void(WebSocketLogLevel, char const*)>& logHandler,
const ServerOptions& options);
};

} // namespace foxglove
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ struct ServerOptions {
std::vector<std::string> supportedEncodings;
std::unordered_map<std::string, std::string> metadata;
size_t sendBufferLimitBytes = DEFAULT_SEND_BUFFER_LIMIT_BYTES;
bool useTls = false;
std::string certfile = "";
std::string keyfile = "";
std::string sessionId;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,15 +5,9 @@
#include <asio/ip/address.hpp>
#include <websocketpp/logger/levels.hpp>

namespace foxglove {
#include "common.hpp"

enum class WebSocketLogLevel {
Debug,
Info,
Warn,
Error,
Critical,
};
namespace foxglove {

using LogCallback = std::function<void(WebSocketLogLevel, char const*)>;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1109,7 +1109,4 @@ inline void Server<WebSocketTls>::setupTlsHandler() {
});
}

extern template class Server<WebSocketTls>;
extern template class Server<WebSocketNoTls>;

} // namespace foxglove
9 changes: 1 addition & 8 deletions foxglove_bridge_base/src/foxglove_bridge.cpp
Original file line number Diff line number Diff line change
@@ -1,18 +1,11 @@
#include "foxglove_bridge/foxglove_bridge.hpp"

#define ASIO_STANDALONE
#include "foxglove_bridge/websocket_notls.hpp"
#include "foxglove_bridge/websocket_server.hpp"
#include "foxglove_bridge/websocket_tls.hpp"
#include "websocketpp/version.hpp"

namespace foxglove {

const char* WebSocketUserAgent() {
return websocketpp::user_agent;
}

// Explicit template instantiation for common server configurations
template class Server<WebSocketTls>;
template class Server<WebSocketNoTls>;

} // namespace foxglove
21 changes: 21 additions & 0 deletions foxglove_bridge_base/src/server_factory.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
#include <websocketpp/common/connection_hdl.hpp>

#include <foxglove_bridge/server_factory.hpp>

#define ASIO_STANDALONE
#include <foxglove_bridge/websocket_server.hpp>

namespace foxglove {

template <>
std::unique_ptr<ServerInterface<websocketpp::connection_hdl>> ServerFactory::createServer(
const std::string& name, const std::function<void(WebSocketLogLevel, char const*)>& logHandler,
const ServerOptions& options) {
if (options.useTls) {
return std::make_unique<foxglove::Server<foxglove::WebSocketTls>>(name, logHandler, options);
} else {
return std::make_unique<foxglove::Server<foxglove::WebSocketNoTls>>(name, logHandler, options);
}
}

} // namespace foxglove
51 changes: 23 additions & 28 deletions ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,3 @@
#define ASIO_STANDALONE

#include <functional>
#include <memory>
#include <mutex>
Expand All @@ -16,10 +14,12 @@
#include <ros_babel_fish/babel_fish_message.h>
#include <ros_babel_fish/generation/providers/integrated_description_provider.h>
#include <rosgraph_msgs/Clock.h>
#include <websocketpp/common/connection_hdl.hpp>

#include <foxglove_bridge/foxglove_bridge.hpp>
#include <foxglove_bridge/generic_service.hpp>
#include <foxglove_bridge/param_utils.hpp>
#include <foxglove_bridge/server_factory.hpp>
#include <foxglove_bridge/service_utils.hpp>
#include <foxglove_bridge/websocket_server.hpp>

Expand All @@ -34,10 +34,11 @@ constexpr double MIN_UPDATE_PERIOD_MS = 100.0;
constexpr uint32_t PUBLICATION_QUEUE_LENGTH = 10;
constexpr int SERVICE_TYPE_RETRIEVAL_TIMEOUT_MS = 250;

using ConnectionHandle = websocketpp::connection_hdl;
using TopicAndDatatype = std::pair<std::string, std::string>;
using SubscriptionsByClient = std::map<foxglove::ConnHandle, ros::Subscriber, std::owner_less<>>;
using SubscriptionsByClient = std::map<ConnectionHandle, ros::Subscriber, std::owner_less<>>;
using ClientPublications = std::unordered_map<foxglove::ClientChannelId, ros::Publisher>;
using PublicationsByClient = std::map<foxglove::ConnHandle, ClientPublications, std::owner_less<>>;
using PublicationsByClient = std::map<ConnectionHandle, ClientPublications, std::owner_less<>>;

class FoxgloveBridge : public nodelet::Nodelet {
public:
Expand Down Expand Up @@ -92,21 +93,18 @@ class FoxgloveBridge : public nodelet::Nodelet {
serverOptions.metadata = {{"ROS_DISTRO", std::getenv("ROS_DISTRO")}};
serverOptions.sendBufferLimitBytes = send_buffer_limit;
serverOptions.sessionId = sessionId;
serverOptions.useTls = useTLS;
serverOptions.certfile = certfile;
serverOptions.keyfile = keyfile;
serverOptions.useCompression = useCompression;

const auto logHandler =
std::bind(&FoxgloveBridge::logHandler, this, std::placeholders::_1, std::placeholders::_2);
if (useTLS) {
serverOptions.certfile = certfile;
serverOptions.keyfile = keyfile;
_server = std::make_unique<foxglove::Server<foxglove::WebSocketTls>>(
"foxglove_bridge", std::move(logHandler), serverOptions);
} else {
_server = std::make_unique<foxglove::Server<foxglove::WebSocketNoTls>>(
"foxglove_bridge", std::move(logHandler), serverOptions);
}

foxglove::ServerHandlers<foxglove::ConnHandle> hdlrs;
_server = foxglove::ServerFactory::createServer<ConnectionHandle>("foxglove_bridge",
logHandler, serverOptions);

foxglove::ServerHandlers<ConnectionHandle> hdlrs;
hdlrs.subscribeHandler = std::bind(&FoxgloveBridge::subscribeHandler, this,
std::placeholders::_1, std::placeholders::_2);
hdlrs.unsubscribeHandler = std::bind(&FoxgloveBridge::unsubscribeHandler, this,
Expand Down Expand Up @@ -165,7 +163,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}
};

void subscribeHandler(foxglove::ChannelId channelId, foxglove::ConnHandle clientHandle) {
void subscribeHandler(foxglove::ChannelId channelId, ConnectionHandle clientHandle) {
std::lock_guard<std::mutex> lock(_subscriptionsMutex);

auto it = _channelToTopicAndDatatype.find(channelId);
Expand Down Expand Up @@ -215,7 +213,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}
}

void unsubscribeHandler(foxglove::ChannelId channelId, foxglove::ConnHandle clientHandle) {
void unsubscribeHandler(foxglove::ChannelId channelId, ConnectionHandle clientHandle) {
std::lock_guard<std::mutex> lock(_subscriptionsMutex);

auto it = _channelToTopicAndDatatype.find(channelId);
Expand Down Expand Up @@ -258,7 +256,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}

void clientAdvertiseHandler(const foxglove::ClientAdvertisement& channel,
foxglove::ConnHandle clientHandle) {
ConnectionHandle clientHandle) {
if (channel.encoding != ROS1_CHANNEL_ENCODING) {
ROS_ERROR("Unsupported encoding. Only '%s' encoding is supported at the moment.",
ROS1_CHANNEL_ENCODING);
Expand Down Expand Up @@ -309,7 +307,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}

void clientUnadvertiseHandler(foxglove::ClientChannelId channelId,
foxglove::ConnHandle clientHandle) {
ConnectionHandle clientHandle) {
std::unique_lock<std::shared_mutex> lock(_publicationsMutex);

auto clientPublicationsIt = _clientAdvertisedTopics.find(clientHandle);
Expand Down Expand Up @@ -344,7 +342,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}

void clientMessageHandler(const foxglove::ClientMessage& clientMsg,
foxglove::ConnHandle clientHandle) {
ConnectionHandle clientHandle) {
ros_babel_fish::BabelFishMessage::Ptr msg(new ros_babel_fish::BabelFishMessage);
msg->read(clientMsg);

Expand Down Expand Up @@ -574,8 +572,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}

void parameterRequestHandler(const std::vector<std::string>& parameters,
const std::optional<std::string>& requestId,
foxglove::ConnHandle hdl) {
const std::optional<std::string>& requestId, ConnectionHandle hdl) {
std::vector<std::string> parameterNames = parameters;
if (parameterNames.empty()) {
getMTNodeHandle().getParamNames(parameterNames);
Expand All @@ -601,8 +598,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}

void parameterChangeHandler(const std::vector<foxglove::Parameter>& parameters,
const std::optional<std::string>& requestId,
foxglove::ConnHandle hdl) {
const std::optional<std::string>& requestId, ConnectionHandle hdl) {
using foxglove::ParameterType;
auto nh = this->getMTNodeHandle();
for (const auto& param : parameters) {
Expand Down Expand Up @@ -647,8 +643,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}

void parameterSubscriptionHandler(const std::vector<std::string>& parameters,
foxglove::ParameterSubscriptionOperation op,
foxglove::ConnHandle) {
foxglove::ParameterSubscriptionOperation op, ConnectionHandle) {
for (const auto& paramName : parameters) {
if (!isWhitelisted(paramName, _paramWhitelistPatterns)) {
ROS_WARN("Parameter '%s' is not whitelisted", paramName.c_str());
Expand Down Expand Up @@ -714,15 +709,15 @@ class FoxgloveBridge : public nodelet::Nodelet {
}

void rosMessageHandler(
const foxglove::Channel& channel, foxglove::ConnHandle clientHandle,
const foxglove::Channel& channel, ConnectionHandle clientHandle,
const ros::MessageEvent<ros_babel_fish::BabelFishMessage const>& msgEvent) {
const auto& msg = msgEvent.getConstMessage();
const auto receiptTimeNs = msgEvent.getReceiptTime().toNSec();
_server->sendMessage(clientHandle, channel.id, receiptTimeNs, msg->buffer(), msg->size());
}

void serviceRequestHandler(const foxglove::ServiceRequest& request,
foxglove::ConnHandle clientHandle) {
ConnectionHandle clientHandle) {
std::shared_lock<std::shared_mutex> lock(_servicesMutex);
const auto serviceIt = _advertisedServices.find(request.serviceId);
if (serviceIt == _advertisedServices.end()) {
Expand Down Expand Up @@ -763,7 +758,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
}
}

std::unique_ptr<foxglove::ServerInterface<foxglove::ConnHandle>> _server;
std::unique_ptr<foxglove::ServerInterface<ConnectionHandle>> _server;
ros_babel_fish::IntegratedDescriptionProvider _rosTypeInfoProvider;
std::vector<std::regex> _topicWhitelistPatterns;
std::vector<std::regex> _paramWhitelistPatterns;
Expand Down
Loading

0 comments on commit 427675d

Please sign in to comment.