File switchboard.hpp
File List > illixr > switchboard.hpp
Go to the documentation of this file
#pragma once
#if defined(_WIN32) || defined(_WIN64)
# include
#include (this_event.get_ro())) << " " <<
// this_event.use_count() << " v\n";
callback_(std::move(this_event), dequeued_);
if (cb_log_) {
cb_log_.log(record{_switchboard_callback_header,
{
{plugin_id_},
{topic_name_},
{dequeued_},
{cb_start_cpu_time},
{thread_cpu_time()},
{cb_start_wall_time},
{std::chrono::high_resolution_clock::now()},
}});
}
} else {
// Nothing to do.
idle_cycles_++;
}
}
void thread_on_stop() {
// Drain queue
std::size_t unprocessed = enqueued_ - dequeued_;
{
ptr<const event> this_event;
for (std::size_t i = 0; i < unprocessed; ++i) {
[[maybe_unused]] bool ret = queue_.try_dequeue(token_, this_event);
assert(ret);
// std::cerr << "deq (stopping) " << ptr_to_str(reinterpret_cast(this_event.get_ro())) << " "
// << this_event.use_count() << " v\n";
this_event.reset();
}
}
// Log stats
if (record_logger_) {
record_logger_->log(record{_switchboard_topic_stop_header,
{
{plugin_id_},
{topic_name_},
{dequeued_},
{unprocessed},
{idle_cycles_},
}});
}
}
const std::string& topic_name_;
plugin_id_t plugin_id_;
std::function<void(ptr<const event>&&, std::size_t)> callback_;
const std::shared_ptr<record_logger> record_logger_;
record_coalescer cb_log_;
moodycamel::BlockingConcurrentQueue<ptr<const event>> queue_{8 /*max size estimate*/};
moodycamel::ConsumerToken token_{queue_};
static constexpr std::chrono::milliseconds queue_timeout_{100};
std::size_t enqueued_{0};
std::size_t dequeued_{0};
std::size_t idle_cycles_{0};
// This needs to be last,
// so it is destructed before the data it uses.
managed_thread thread_;
};
class topic_buffer {
public:
topic_buffer() {
#ifndef NDEBUG
spdlog::get("illixr")->info("[switchboard] topic buffer created");
#endif
}
void enqueue(ptr<const event>&& this_event) {
queue_size_++;
[[maybe_unused]] bool ret = queue_.enqueue(std::move(this_event));
assert(ret);
}
[[nodiscard]] size_t size() const {
return queue_size_;
}
ptr<const event> dequeue() {
ptr<const event> obj;
queue_size_--;
queue_.wait_dequeue(token_, obj);
return obj;
}
bool try_dequeue(ptr<const event>& obj) {
if (!queue_.try_dequeue(token_, obj)) {
return false;
}
queue_size_--;
return true;
}
private:
moodycamel::BlockingConcurrentQueue<ptr<const event>> queue_{8 /*max size estimate*/};
moodycamel::ConsumerToken token_{queue_};
std::atomic<size_t> queue_size_{0};
};
class topic {
public:
topic(std::string name, const std::type_info& ty, std::shared_ptr<record_logger> record_logger_)
: name_{std::move(name)}
, type_info_{ty}
, record_logger_{std::move(record_logger_)}
, latest_index_{0} { }
const std::string& name() {
return name_;
}
const std::type_info& ty() {
return type_info_;
}
[[nodiscard]] ptr<const event> get() const {
size_t idx = latest_index_.load() % latest_buffer_size_;
ptr<const event> this_event = latest_buffer_[idx];
// if (this_event) {
// std::cerr << "get " << ptr_to_str(reinterpret_cast(this_event.get())) << " " <<
// this_event.use_count() << "v \n";
// }
return this_event;
}
void put(ptr<const event>&& this_event) {
assert(this_event != nullptr);
assert(this_event.use_count() <= 2);
/* The pointer that this gets exchanged with needs to get dropped. */
size_t index = (latest_index_.load() + 1) % latest_buffer_size_;
latest_buffer_[index] = this_event;
latest_index_++;
// Read/write on subscriptions_.
// Must acquire shared state on subscriptions_lock_
std::unique_lock lock{subscriptions_lock_};
for (topic_subscription& ts : subscriptions_) {
// std::cerr << "enq " << ptr_to_str(reinterpret_cast(this_event->get())) << " " <<
// this_event->use_count() << " <\n";
ptr<const event> event_ptr_copy{this_event};
ts.enqueue(std::move(event_ptr_copy));
}
for (topic_buffer& ts : buffers_) {
// std::cerr << "enq " << ptr_to_str(reinterpret_cast(this_event->get())) << " " <<
// this_event->use_count() << " <\n";
ptr<const event> event_ptr_copy{this_event};
ts.enqueue(std::move(event_ptr_copy));
}
// std::cerr << "put done " << ptr_to_str(reinterpret_cast(this_event->get())) << " " <<
// this_event->use_count() << " (= 1 + len(sub)) \n";
}
[[maybe_unused]] void deserialize_and_put(std::vector<char>& buffer, network::topic_config& config) {
if (config.serialization_method == network::topic_config::SerializationMethod::BOOST) {
// TODO: Need to differentiate and support protobuf deserialization
try {
boost::iostreams::stream<boost::iostreams::array_source> stream{buffer.data(), buffer.size()};
// Use no_header for cross-platform compatibility (sizeof(long) differs between Windows and Linux)
boost::archive::binary_iarchive ia{stream, boost::archive::no_header};
ptr<event> this_event;
ia >> this_event;
put(std::move(this_event));
} catch (const std::exception& e) {
std::cerr << "[switchboard] dropping undeserializable message on topic '" << name() << "' ("
<< buffer.size() << " bytes): " << e.what() << std::endl;
} catch (...) {
std::cerr << "[switchboard] dropping undeserializable message on topic '" << name() << "' ("
<< buffer.size() << " bytes): unknown exception" << std::endl;
}
} else {
ptr<event> message = std::make_shared<event_wrapper<std::string>>((std::string(buffer.begin(), buffer.end())));
put(std::move(message));
}
}
void schedule(plugin_id_t plugin_id, const std::function<void(ptr<const event>&&, std::size_t)>& callback) {
// Write on subscriptions_.
// Must acquire unique state on subscriptions_lock_
const std::unique_lock lock{subscriptions_lock_};
subscriptions_.emplace_back(name_, plugin_id, callback, record_logger_);
}
topic_buffer& get_buffer() {
const std::unique_lock lock{subscriptions_lock_};
buffers_.emplace_back();
return buffers_.back();
}
void stop() {
// Write on subscriptions_.
// Must acquire unique state on subscriptions_lock_
const std::unique_lock lock{subscriptions_lock_};
subscriptions_.clear();
}
private:
static constexpr std::size_t latest_buffer_size_ = 256;
const std::string name_;
const std::type_info& type_info_;
const std::shared_ptr<record_logger> record_logger_;
std::atomic<size_t> latest_index_;
std::array<ptr<const event>, latest_buffer_size_> latest_buffer_;
std::list<topic_subscription> subscriptions_;
std::list<topic_buffer> buffers_;
std::shared_mutex subscriptions_lock_;
};
public:
template<typename Specific_event>
class reader {
public:
explicit reader(topic& topic)
: topic_{topic} {
#ifndef NDEBUG
if (typeid(Specific_event) != topic_.ty()) {
spdlog::get("illixr")->error("[switchboard] topic '{}' holds type {}, but caller used type {}", topic_.name(),
topic_.ty().name(), typeid(Specific_event).name());
abort();
}
#endif
}
ptr<const Specific_event> get_ro_nullable() const noexcept {
ptr<const event> this_event = topic_.get();
ptr<const Specific_event> this_specific_event = std::dynamic_pointer_cast<const Specific_event>(this_event);
if (this_event != nullptr) {
assert(this_specific_event /* Otherwise, dynamic cast failed; dynamic type information could be wrong*/);
return this_specific_event;
} else {
return ptr<const Specific_event>{nullptr};
}
}
ptr<const Specific_event> get_ro() const {
ptr<const Specific_event> this_specific_event = get_ro_nullable();
if (this_specific_event != nullptr) {
return this_specific_event;
} else {
throw std::runtime_error("No event on topic");
}
}
[[maybe_unused]] ptr<Specific_event> get_rw() const {
/*
This method is currently not more efficient than calling get_ro() and making a copy,
but in the future it could be.
*/
ptr<const Specific_event> this_specific_event = get_ro();
return std::make_shared<Specific_event>(*this_specific_event);
}
private:
topic& topic_;
};
template<typename Specific_event>
class buffered_reader {
public:
explicit buffered_reader(topic& topic)
: topic_{topic}
, topic_buffer_{topic_.get_buffer()} { }
[[nodiscard]] size_t size() const {
return topic_buffer_.size();
}
virtual ptr<const Specific_event> dequeue() {
// CPU_TIMER_TIME_EVENT_INFO(true, false, "callback", cpu_timer::make_type_eraser("", topic_.name(),
// serial_no_));
serial_no_++;
ptr<const event> this_event = topic_buffer_.dequeue();
ptr<const Specific_event> this_specific_event = std::dynamic_pointer_cast<const Specific_event>(this_event);
return this_specific_event;
}
ptr<const Specific_event> try_dequeue() {
ptr<const event> this_event;
if (!topic_buffer_.try_dequeue(this_event)) {
return nullptr;
}
serial_no_++;
return std::dynamic_pointer_cast<const Specific_event>(this_event);
}
private:
topic& topic_;
size_t serial_no_ = 0;
topic_buffer& topic_buffer_;
};
template<typename Specific_event>
class writer {
public:
explicit writer(topic& topic)
: topic_{topic} { }
template<class... Args>
ptr<Specific_event> allocate(Args&&... args) {
return std::make_shared<Specific_event>(std::forward<Args>(args)...);
}
virtual void put(ptr<Specific_event>&& this_specific_event) {
assert(typeid(Specific_event) == topic_.ty());
assert(this_specific_event != nullptr);
assert(this_specific_event.use_count() == 1);
ptr<const event> this_event =
std::const_pointer_cast<const event>(std::static_pointer_cast<event>(std::move(this_specific_event)));
assert(this_event.use_count() <= 2);
topic_.put(std::move(this_event));
}
protected:
// Reference to the underlying topic
topic& topic_;
};
template<typename Serializable_event>
class network_writer final : public writer<Serializable_event> {
public:
explicit network_writer(topic& topic, ptr<network::network_backend> backend = nullptr,
const network::topic_config& config = {})
: writer<Serializable_event>{topic}
, backend_{std::move(backend)}
, config_{config} { }
void put(ptr<Serializable_event>&& this_specific_event) override {
if (backend_->is_topic_networked(this->topic_.name())) {
if (config_.serialization_method == network::topic_config::SerializationMethod::BOOST) {
auto base_event = std::dynamic_pointer_cast<event>(std::move(this_specific_event));
assert(base_event && "Event is not derived from switchboard::event");
// Default serialization method - Boost
std::vector<char> buffer;
boost::iostreams::back_insert_device<std::vector<char>> inserter{buffer};
boost::iostreams::stream_buffer<boost::iostreams::back_insert_device<std::vector<char>>> stream{inserter};
// Use no_header for cross-platform compatibility (sizeof(long) differs between Windows and Linux)
boost::archive::binary_oarchive oa{stream, boost::archive::no_header};
oa << base_event;
// flush
stream.pubsync();
backend_->topic_send(this->topic_.name(), std::move(std::string(buffer.begin(), buffer.end())));
} else {
// PROTOBUF - this_specific_event will be a string
auto message_ptr = std::dynamic_pointer_cast<event_wrapper<std::string>>(this_specific_event);
std::string message = **message_ptr;
backend_->topic_send(this->topic_.name(), std::move(message));
}
} else {
writer<Serializable_event>::put(std::move(this_specific_event));
}
}
private:
ptr<network::network_backend> backend_;
network::topic_config config_;
};
public:
explicit switchboard(const phonebook* pb)
: phonebook_{pb}
, record_logger_{pb ? pb->lookup_impl<record_logger>() : nullptr} {
for (const auto& item : ENV_VARS) {
char* value = getenv(item.c_str());
if (value) {
env_vars_[item] = value;
} else {
env_vars_[item] = "";
}
}
}
[[maybe_unused]] bool topic_exists(const std::string& topic_name) {
const std::shared_lock lock{registry_lock_};
auto found = registry_.find(topic_name);
return found != registry_.end();
}
[[maybe_unused]] topic& get_topic(const std::string& topic_name) {
const std::shared_lock lock{registry_lock_};
auto found = registry_.find(topic_name);
if (found != registry_.end()) {
return found->second;
} else {
throw std::runtime_error("Topic not found");
}
}
void set_env(const std::string& var, const std::string& val) {
env_vars_[var] = val;
setenv(var.c_str(), val.c_str(), 1);
}
std::vector<std::string> env_names() const {
std::vector<std::string> keys(env_vars_.size());
std::transform(env_vars_.begin(), env_vars_.end(), keys.begin(), [](auto pair) {
return pair.first;
});
return keys;
}
std::string get_env(const std::string& var, std::string _default = "") {
try {
if (!env_vars_.at(var).empty())
return env_vars_.at(var);
env_vars_.at(var) = _default;
return _default;
} catch (std::out_of_range&) {
char* val = std::getenv(var.c_str());
if (val) {
std::string temp(val);
set_env(var, val); // store it locally for faster retrieval
return temp;
}
return _default;
}
}
bool get_env_bool(const std::string& var, const std::string& def = "false") {
std::string val = get_env(var, def);
// see if we are dealing with an int value
try {
const int i_val = std::stoi(val);
if (i_val <= 0)
return false;
return true;
} catch (...) { }
const std::vector<std::string> affirmative{"yes", "y", "true", "on"};
for (auto s : affirmative) {
if (std::equal(val.begin(), val.end(), s.begin(), s.end(), [](char a, char b) {
return std::tolower(a) == std::tolower(b);
}))
return true;
}
return false;
}
const char* get_env_char(const std::string& var, const std::string _default = "") {
std::string val = get_env(var, _default);
if (val.empty())
return nullptr;
return strdup(val.c_str());
}
[[maybe_unused]] int get_env_int(const std::string& var, const int _default = 0) {
std::string val = get_env(var, std::to_string(_default));
try {
int res = std::stoi(val);
return res;
} catch (...) { }
return _default;
}
[[maybe_unused]] long get_env_long(const std::string& var, const long _default = 0) {
std::string val = get_env(var, std::to_string(_default));
try {
long res = std::stol(val);
return res;
} catch (...) { }
return _default;
}
[[maybe_unused]] unsigned long get_env_ulong(const std::string& var, const unsigned long _default = 0) {
std::string val = get_env(var, std::to_string(_default));
try {
unsigned long res = std::stoul(val);
return res;
} catch (...) { }
return _default;
}
[[maybe_unused]] double get_env_double(const std::string& var, const double _default = 0.) {
std::string val = get_env(var, std::to_string(_default));
try {
double res = std::stod(val);
return res;
} catch (...) { }
return _default;
}
[[maybe_unused]] float get_env_float(const std::string& var, const double _default = 0.f) {
std::string val = get_env(var, std::to_string(_default));
try {
int res = std::stof(val);
return res;
} catch (...) { }
return _default;
}
template<typename Specific_event>
void schedule(plugin_id_t plugin_id, std::string topic_name,
std::function<void(ptr<const Specific_event>&&, std::size_t)> fn) {
try_register_topic<Specific_event>(topic_name)
.schedule(plugin_id, [=](ptr<const event>&& this_event, std::size_t it_no) {
assert(this_event);
ptr<const Specific_event> this_specific_event =
std::dynamic_pointer_cast<const Specific_event>(std::move(this_event));
assert(this_specific_event);
fn(std::move(this_specific_event), it_no);
});
}
template<typename Specific_event>
writer<Specific_event> get_writer(const std::string& topic_name) {
return writer<Specific_event>{try_register_topic<Specific_event>(topic_name)};
}
template<typename Specific_event>
network_writer<Specific_event> get_network_writer(const std::string& topic_name, network::topic_config config = {}) {
ptr<network::network_backend> backend;
switch (config.transport_method) {
case network::topic_config::TransportMethod::UDP:
backend = phonebook_->lookup_impl<network::udp_backend>();
if (!backend) {
throw std::runtime_error("[switchboard] UDP transport requested for topic '" + topic_name +
"' but no UDP network backend is registered in the phonebook. "
"Ensure the udp_network_backend plugin is loaded before this writer is constructed.");
}
break;
case network::topic_config::TransportMethod::TCP:
default:
backend = phonebook_->lookup_impl<network::tcp_backend>();
if (!backend) {
throw std::runtime_error("[switchboard] TCP transport requested for topic '" + topic_name +
"' but no TCP network backend is registered in the phonebook.");
}
break;
}
if (registry_.find(topic_name) == registry_.end())
backend->topic_create(topic_name, config);
return network_writer<Specific_event>{try_register_topic<Specific_event>(topic_name), backend, config};
}
template<typename Specific_event>
reader<Specific_event> get_reader(const std::string& topic_name) {
return reader<Specific_event>{try_register_topic<Specific_event>(topic_name)};
}
template<typename Specific_event>
buffered_reader<Specific_event> get_buffered_reader(const std::string& topic_name) {
return buffered_reader<Specific_event>{try_register_topic<Specific_event>(topic_name)};
}
void stop() {
const std::shared_lock lock{registry_lock_};
for (auto& pair : registry_) {
pair.second.stop();
}
}
#ifdef __ANDROID__
void set_android_app(android_app* app) {
if (app_) {
spdlog::get("illixr")->error("Android app already set");
return;
}
app_ = app;
}
android_app* get_android_app() {
return app_;
}
#endif
private:
const phonebook* phonebook_;
std::unordered_map<std::string, topic> registry_;
std::shared_mutex registry_lock_;
std::shared_ptr<record_logger> record_logger_;
std::unordered_map<std::string, std::string> env_vars_;
#ifdef __ANDROID__
android_app* app_ = nullptr;
#endif
template<typename Specific_event>
topic& try_register_topic(const std::string& topic_name) {
{
const std::shared_lock lock{registry_lock_};
auto found = registry_.find(topic_name);
if (found != registry_.end()) {
topic& _topic = found->second;
#ifndef NDEBUG
if (typeid(Specific_event) != _topic.ty()) {
spdlog::get("illixr")->error("[switchboard] topic '{}' holds type {}, but caller used type {}", topic_name,
_topic.ty().name(), typeid(Specific_event).name());
abort();
}
#endif
return _topic;
}
}
#ifndef NDEBUG
spdlog::get("illixr")->debug("[switchboard] Creating: {} for {}", topic_name, typeid(Specific_event).name());
#endif
// Topic not found. Need to create it here.
const std::unique_lock lock{registry_lock_};
return registry_.try_emplace(topic_name, topic_name, typeid(Specific_event), record_logger_).first->second;
}
class coordinate_system {
private:
Eigen::Vector3f position_;
Eigen::Quaternionf orientation_;
public:
coordinate_system()
: position_{0., 0., 0.}
, orientation_{1., 0., 0., 0.} {
const char* ini_pose = getenv("WCS_ORIGIN");
// =
// if (!ini_pose.empty()) {
if (ini_pose) {
std::string ini_pose_str(ini_pose);
std::stringstream iss(ini_pose_str);
std::string token;
std::vector<float> ip;
while (!iss.eof() && std::getline(iss, token, ',')) {
ip.emplace_back(std::stof(token));
}
if (ip.size() == 3) {
position_.x() = ip[0];
position_.y() = ip[1];
position_.z() = ip[2];
} else if (ip.size() == 4) {
orientation_.w() = ip[0];
orientation_.x() = ip[1];
orientation_.y() = ip[2];
orientation_.z() = ip[3];
} else if (ip.size() == 7) {
position_.x() = ip[0];
position_.y() = ip[1];
position_.z() = ip[2];
orientation_.w() = ip[3];
orientation_.x() = ip[4];
orientation_.y() = ip[5];
orientation_.z() = ip[6];
}
}
}
[[nodiscard]] const Eigen::Vector3f& position() const {
return position_;
}
[[nodiscard]] const Eigen::Quaternionf& orientation() const {
return orientation_;
}
};
public:
coordinate_system root_coordinates;
};
} // namespace ILLIXR