From 0c5fae43e2d89a6659a0310655d8120426d28066 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Mon, 6 Nov 2017 10:59:11 +0100 Subject: [PATCH 01/20] protoboard: Templated plugin to convert protobuf to blackboard This plugin still resides in the domain repository, but all domain-independent code is separated. Domain-specific protobuf-blackboard mappings are specified as template specializations. So in effect, this is not a plugin, but a plugin template wich can be moved to the core repo after the source layout has been refactored a bit. --- src/plugins/Makefile | 1 + src/plugins/protoboard/Makefile | 50 +++ src/plugins/protoboard/bb_msg_handlers.cpp | 287 ++++++++++++++++++ src/plugins/protoboard/blackboard_manager.cpp | 131 ++++++++ src/plugins/protoboard/blackboard_manager.h | 219 +++++++++++++ .../protoboard/interface_definitions.cpp | 54 ++++ .../protoboard/interface_definitions.h | 89 ++++++ src/plugins/protoboard/interfaces/Makefile | 24 ++ .../protoboard/interfaces/OrderInterface.xml | 48 +++ .../interfaces/PrepareMachineInterface.xml | 84 +++++ .../interfaces/ProtobufPeerInterface.xml | 35 +++ .../interfaces/RCLLGameStateInterface.xml | 33 ++ .../interfaces/RecvBeaconInterface.xml | 24 ++ .../interfaces/SendBeaconInterface.xml | 32 ++ src/plugins/protoboard/plugin.cpp | 14 + src/plugins/protoboard/plugin.h | 15 + src/plugins/protoboard/protobuf_thread.cpp | 265 ++++++++++++++++ src/plugins/protoboard/protobuf_thread.h | 124 ++++++++ 18 files changed, 1529 insertions(+) create mode 100644 src/plugins/protoboard/Makefile create mode 100644 src/plugins/protoboard/bb_msg_handlers.cpp create mode 100644 src/plugins/protoboard/blackboard_manager.cpp create mode 100644 src/plugins/protoboard/blackboard_manager.h create mode 100644 src/plugins/protoboard/interface_definitions.cpp create mode 100644 src/plugins/protoboard/interface_definitions.h create mode 100644 src/plugins/protoboard/interfaces/Makefile create mode 100644 src/plugins/protoboard/interfaces/OrderInterface.xml create mode 100644 src/plugins/protoboard/interfaces/PrepareMachineInterface.xml create mode 100644 src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml create mode 100644 src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml create mode 100644 src/plugins/protoboard/interfaces/RecvBeaconInterface.xml create mode 100644 src/plugins/protoboard/interfaces/SendBeaconInterface.xml create mode 100644 src/plugins/protoboard/plugin.cpp create mode 100644 src/plugins/protoboard/plugin.h create mode 100644 src/plugins/protoboard/protobuf_thread.cpp create mode 100644 src/plugins/protoboard/protobuf_thread.h diff --git a/src/plugins/Makefile b/src/plugins/Makefile index f896590e4e..7150b4c298 100644 --- a/src/plugins/Makefile +++ b/src/plugins/Makefile @@ -32,6 +32,7 @@ SUBDIRS = clips-motor-switch \ webtools-bridge \ skiller_motor_state \ asp-planner + protoboard \ include $(BUILDSYSDIR)/rules.mk diff --git a/src/plugins/protoboard/Makefile b/src/plugins/protoboard/Makefile new file mode 100644 index 0000000000..71dc8ce091 --- /dev/null +++ b/src/plugins/protoboard/Makefile @@ -0,0 +1,50 @@ +#***************************************************************************** +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +#***************************************************************************** + +BASEDIR = ../../.. +include $(BASEDIR)/etc/buildsys/config.mk +include $(BUILDSYSDIR)/protobuf.mk +include $(BUILDSYSDIR)/boost.mk + +CFLAGS += $(CFLAGS_CPP11) + +ifeq ($(CC),gcc) + ifneq ($(call gcc_atleast_version,4,8),1) + GCC_TOO_OLD=1 + endif +endif + +PRESUBDIRS = interfaces + +LIBS_protoboard = fawkescore fawkesaspects \ + fawkesblackboard fawkesinterface fawkesconfig \ + fawkes_protobuf_comm llsf_msgs \ + OrderInterface PrepareMachineInterface SendBeaconInterface ProtobufPeerInterface \ + RecvBeaconInterface RCLLGameStateInterface + +OBJS_protoboard = blackboard_manager.o protobuf_thread.o interface_definitions.o plugin.o + +ifneq ($(GCC_TOO_OLD),1) + PLUGINS_all = $(PLUGINDIR)/protoboard.$(SOEXT) +else + WARN_TARGETS += warning_old_gcc +endif + +OBJS_all = $(OBJS_protoboard) + +ifeq ($(OBJSSUBMAKE),1) + ifneq ($(WARN_TARGETS),) +all: $(WARN_TARGETS) + endif +.PHONY: warning_old_gcc +warning_old_gcc: + $(SILENT)echo -e "$(INDENT_PRINT)--> $(TRED)Can't build protoboard plugin$(TNORMAL) (GCC too old, have $(GCC_VERSION), required 4.8)" +endif + +include $(BUILDSYSDIR)/base.mk diff --git a/src/plugins/protoboard/bb_msg_handlers.cpp b/src/plugins/protoboard/bb_msg_handlers.cpp new file mode 100644 index 0000000000..b9f259db1c --- /dev/null +++ b/src/plugins/protoboard/bb_msg_handlers.cpp @@ -0,0 +1,287 @@ +#include "blackboard_manager.h" + +#include +#include +#include + +#include +#include +#include +#include + +namespace protoboard { + + +using namespace fawkes; +using namespace std; +using namespace boost::fusion; + + +std::unordered_map> make_receiving_interfaces_map() { + return { + make_pair("llsf_msgs.Order", make_shared>()), + make_pair("llsf_msgs.BeaconSignal", make_shared>()), + make_pair("llsf_msgs.GameState", make_shared>()), + }; +}; + + +/**************************************** + * OrderInterface * + ****************************************/ + +static const enum_map +base_color_enum_order { + {llsf_msgs::BaseColor::BASE_BLACK, OrderInterface::BASE_BLACK}, + {llsf_msgs::BaseColor::BASE_RED, OrderInterface::BASE_RED}, + {llsf_msgs::BaseColor::BASE_SILVER, OrderInterface::BASE_SILVER} +}; + +static const enum_map +ring_color_enum_order { + {llsf_msgs::RingColor::RING_BLUE, OrderInterface::RING_BLUE}, + {llsf_msgs::RingColor::RING_GREEN, OrderInterface::RING_GREEN}, + {llsf_msgs::RingColor::RING_ORANGE, OrderInterface::RING_ORANGE}, + {llsf_msgs::RingColor::RING_YELLOW, OrderInterface::RING_YELLOW}, +}; + +static const enum_map +cap_color_enum_order { + {llsf_msgs::CapColor::CAP_BLACK, OrderInterface::CAP_BLACK}, + {llsf_msgs::CapColor::CAP_GREY, OrderInterface::CAP_GREY}, +}; + +static const enum_map +complexity_enum_order { + {llsf_msgs::Order::Complexity::Order_Complexity_C0, OrderInterface::C0}, + {llsf_msgs::Order::Complexity::Order_Complexity_C1, OrderInterface::C1}, + {llsf_msgs::Order::Complexity::Order_Complexity_C2, OrderInterface::C2}, + {llsf_msgs::Order::Complexity::Order_Complexity_C3, OrderInterface::C3} +}; + + +template<> +void +pb_converter::handle( + const llsf_msgs::Order &msg, + OrderInterface *iface) +{ + iface->set_base_color(base_color_enum_order.of(msg.base_color())); + iface->set_cap_color(cap_color_enum_order.of(msg.cap_color())); + iface->set_complexity(complexity_enum_order.of(msg.complexity())); + for (int i = 0; i < msg.ring_colors_size(); ++i) + iface->set_ring_colors(static_cast(i), + ring_color_enum_order.of(msg.ring_colors(i))); + iface->set_order_id(msg.id()); + iface->set_delivery_period_begin(msg.delivery_period_begin()); + iface->set_delivery_period_end(msg.delivery_period_end()); + iface->set_delivery_gate(msg.delivery_gate()); +}; + + +/**************************************** + * RecvBeaconInterface * + ****************************************/ + +static const enum_map +team_enum_recv_beacon { + {llsf_msgs::Team::CYAN, RecvBeaconInterface::TEAM_COLOR::CYAN}, + {llsf_msgs::Team::MAGENTA, RecvBeaconInterface::TEAM_COLOR::MAGENTA} +}; + + +template<> +void +pb_converter::handle( + const llsf_msgs::BeaconSignal &msg, + RecvBeaconInterface *iface) +{ + iface->set_number(msg.number()); + iface->set_peer_name(msg.peer_name().substr(0, iface->maxlenof_peer_name()).c_str()); + iface->set_pose(0, msg.pose().x()); + iface->set_pose(1, msg.pose().y()); + iface->set_pose(2, msg.pose().ori()); + iface->set_team_color(team_enum_recv_beacon.of(msg.team_color())); + iface->set_team_name(msg.team_name().substr(0, iface->maxlenof_team_name()).c_str()); + iface->set_time_nsec(msg.time().nsec()); + iface->set_time_sec(msg.time().sec()); +} + + +template<> +void +BlackboardManager::handle_message( + SendBeaconInterface *iface, + SendBeaconInterface::SetPeerMessage *msg) +{ + iface->set_peer_id(msg->peer_id()); +} + + +/**************************************** + * GameStateInterface * + ****************************************/ + +static const enum_map +game_state_enum { + {llsf_msgs::GameState::State::GameState_State_INIT, RCLLGameStateInterface::GameState::INIT}, + {llsf_msgs::GameState::State::GameState_State_PAUSED, RCLLGameStateInterface::GameState::PAUSED}, + {llsf_msgs::GameState::State::GameState_State_RUNNING, RCLLGameStateInterface::GameState::RUNNING}, + {llsf_msgs::GameState::State::GameState_State_WAIT_START, RCLLGameStateInterface::GameState::WAIT_START} +}; + +static const enum_map +game_phase_enum { + {llsf_msgs::GameState::Phase::GameState_Phase_EXPLORATION, RCLLGameStateInterface::GamePhase::EXPLORATION}, + {llsf_msgs::GameState::Phase::GameState_Phase_POST_GAME, RCLLGameStateInterface::GamePhase::POST_GAME}, + {llsf_msgs::GameState::Phase::GameState_Phase_PRE_GAME, RCLLGameStateInterface::GamePhase::PRE_GAME}, + {llsf_msgs::GameState::Phase::GameState_Phase_PRODUCTION, RCLLGameStateInterface::GamePhase::PRODUCTION}, + {llsf_msgs::GameState::Phase::GameState_Phase_SETUP, RCLLGameStateInterface::GamePhase::SETUP} +}; + + +template<> +void +pb_converter::handle( + const llsf_msgs::GameState &msg, + RCLLGameStateInterface *iface) +{ + iface->set_game_time_nsec(msg.game_time().nsec()); + iface->set_game_time_sec(msg.game_time().sec()); + iface->set_phase(game_phase_enum.of(msg.phase())); + iface->set_points_cyan(msg.points_cyan()); + iface->set_points_magenta(msg.points_magenta()); + iface->set_state(game_state_enum.of(msg.state())); + iface->set_team_cyan(msg.team_cyan().substr(0, iface->maxlenof_team_cyan()).c_str()); + iface->set_team_magenta(msg.team_magenta().substr(0, iface->maxlenof_team_magenta()).c_str()); +} + + +/**************************************** + * SendBeaconMessage * + ****************************************/ + +static const enum_map +team_enum_beacon { + {llsf_msgs::CYAN, SendBeaconInterface::TEAM_COLOR::CYAN}, + {llsf_msgs::MAGENTA, SendBeaconInterface::TEAM_COLOR::MAGENTA} +}; + + +template<> +void +BlackboardManager::handle_message( + SendBeaconInterface *iface, + SendBeaconInterface::SendBeaconMessage *msg) +{ + std::shared_ptr m = std::make_shared(); + llsf_msgs::Pose2D *pose = new llsf_msgs::Pose2D(); + pose->set_x(msg->translation(0)); + pose->set_y(msg->translation(1)); + pose->set_ori(msg->orientation()); + m->set_allocated_pose(pose); + m->set_allocated_team_name(new std::string(msg->team_name())); + m->set_number(msg->number()); + m->set_allocated_peer_name(new std::string(msg->peer_name())); + m->set_team_color(team_enum_beacon.of(msg->team_color())); + + message_handler_->send(iface->peer_id(), m); +} + + +/**************************************** + * Prepare Messages * + ****************************************/ + +static const enum_map +team_enum_prepare { + {llsf_msgs::CYAN, PrepareMachineInterface::Team::CYAN}, + {llsf_msgs::MAGENTA, PrepareMachineInterface::Team::MAGENTA} +}; + +static const enum_map +base_color_enum { + {llsf_msgs::BaseColor::BASE_BLACK, PrepareMachineInterface::BASE_BLACK}, + {llsf_msgs::BaseColor::BASE_RED, PrepareMachineInterface::BASE_RED}, + {llsf_msgs::BaseColor::BASE_SILVER, PrepareMachineInterface::BASE_SILVER} +}; + +static const enum_map +machine_side_enum { + {llsf_msgs::MachineSide::INPUT, PrepareMachineInterface::MachineSide::INPUT}, + {llsf_msgs::MachineSide::OUTPUT, PrepareMachineInterface::MachineSide::OUTPUT} +}; + +static const enum_map +csop_enum { + {llsf_msgs::CSOp::MOUNT_CAP, PrepareMachineInterface::CSOp::MOUNT_CAP}, + {llsf_msgs::CSOp::RETRIEVE_CAP, PrepareMachineInterface::CSOp::RETRIEVE_CAP} +}; + + +template<> +void +BlackboardManager::handle_message( + PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareBSMessage *msg) +{ + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionBS *instr = new llsf_msgs::PrepareInstructionBS(); + instr->set_color(base_color_enum.of(msg->color())); + instr->set_side(machine_side_enum.of(msg->side())); + m->set_allocated_instruction_bs(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); +} + + +template<> +void +BlackboardManager::handle_message( + fawkes::PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareCSMessage *msg) +{ + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionCS *instr = new llsf_msgs::PrepareInstructionCS(); + instr->set_operation(csop_enum.of(msg->operation())); + m->set_allocated_instruction_cs(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); +} + +template<> +void +BlackboardManager::handle_message( + fawkes::PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareDSMessage *msg) +{} + +template<> +void +BlackboardManager::handle_message( + fawkes::PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareRSMessage *msg) +{} + +template<> +void +BlackboardManager::handle_message( + fawkes::PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareSSMessage *msg) +{} + +template<> +void +BlackboardManager::handle_message( + fawkes::PrepareMachineInterface *iface, + PrepareMachineInterface::SetPeerMessage *msg) +{} + + + +} // namespace protoboard + diff --git a/src/plugins/protoboard/blackboard_manager.cpp b/src/plugins/protoboard/blackboard_manager.cpp new file mode 100644 index 0000000000..a25c0c5b39 --- /dev/null +++ b/src/plugins/protoboard/blackboard_manager.cpp @@ -0,0 +1,131 @@ +#include "blackboard_manager.h" +#include "bb_msg_handlers.cpp" + + +namespace protoboard { + + +using namespace fawkes; + + +pb_convert::~pb_convert() +{} + + +BlackboardManager::BlackboardManager(ProtobufThead *msg_handler) + : Thread("ProtoboardBlackboardManager", Thread::OPMODE_WAITFORWAKEUP) + , message_handler_(msg_handler) + , bb_receiving_interfaces_(make_receiving_interfaces_map()) + , on_message_waker_(nullptr) + , next_peer_idx_(0) +{} + + +void BlackboardManager::init() +{ + peer_iface_ = blackboard->open_for_writing("/protoboard/peers"); + + for (pb_conversion_map::value_type &c : bb_receiving_interfaces_) + c.second->init(blackboard, logger); + + on_message_waker_ = new fawkes::BlackBoardOnMessageWaker(blackboard, peer_iface_, this); + boost::fusion::for_each(bb_sending_interfaces_, init_interface{this}); +} + + +void BlackboardManager::finalize() +{ + delete on_message_waker_; + blackboard->close(peer_iface_); +} + + +void BlackboardManager::loop() +{ + // Handle CreatePeer* messages + bool did_something = on_interface { peer_iface_, this } + .handle_msg_types < + ProtobufPeerInterface::CreatePeerMessage, + ProtobufPeerInterface::CreatePeerLocalMessage, + ProtobufPeerInterface::CreatePeerCryptoMessage, + ProtobufPeerInterface::CreatePeerLocalCryptoMessage + > (); + + did_something |= boost::fusion::any(bb_sending_interfaces_, handle_messages { this }); + + while (message_handler_->pb_queue_incoming()) { + ProtobufThead::incoming_message inc = message_handler_->pb_queue_pop(); + pb_conversion_map::iterator it; + + if ((it = bb_receiving_interfaces_.find(inc.msg->GetTypeName())) == bb_receiving_interfaces_.end()) + logger->log_error(name(), "Received message of unregistered type `%s'", inc.msg->GetTypeName().c_str()); + else + it->second->handle(inc.msg); + + did_something = true; + } + + if (!did_something) + logger->log_warn(name(), "Spurious wakeup. WTF?"); +} + + +void BlackboardManager::add_peer(ProtobufPeerInterface *iface, long peer_id) +{ + // TODO: Properly handle overflow. + iface->set_peers(next_peer_idx_++ % iface->maxlenof_peers(), peer_id); + iface->write(); +} + + +template<> void +BlackboardManager::handle_message( + ProtobufPeerInterface *iface, + ProtobufPeerInterface::CreatePeerMessage *msg) +{ add_peer(iface, message_handler_->peer_create(msg->address(), msg->port())); } + + +template<> void +BlackboardManager::handle_message( + ProtobufPeerInterface *iface, + ProtobufPeerInterface::CreatePeerLocalMessage *msg) +{ + add_peer(iface, + message_handler_->peer_create_local( + msg->address(), + msg->send_to_port(), + msg->recv_on_port())); +} + + +template<> void +BlackboardManager::handle_message( + ProtobufPeerInterface *iface, + ProtobufPeerInterface::CreatePeerCryptoMessage *msg) +{ + add_peer(iface, + message_handler_->peer_create_crypto( + msg->address(), + msg->port(), + msg->crypto_key(), + msg->cipher())); +} + + +template<> void +BlackboardManager::handle_message( + ProtobufPeerInterface *iface, + ProtobufPeerInterface::CreatePeerLocalCryptoMessage *msg) +{ + add_peer(iface, + message_handler_->peer_create_local_crypto( + msg->address(), + msg->send_to_port(), + msg->recv_on_port(), + msg->crypto_key(), + msg->cipher())); +} + + +} // namespace protoboard + diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h new file mode 100644 index 0000000000..e0fe4b7a8b --- /dev/null +++ b/src/plugins/protoboard/blackboard_manager.h @@ -0,0 +1,219 @@ +#ifndef BLACKBOARD_MANAGER_H +#define BLACKBOARD_MANAGER_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include "interface_definitions.h" +#include "protobuf_thread.h" + + + +namespace protoboard { + + +template +struct InterfaceMessageHandler; + + +class BlackboardManager; + + +template +struct enum_map { + typedef boost::bimap bimapT; + const std::vector list; + const boost::bimap map; + + constexpr enum_map(std::initializer_list init) + : list(init), + map(list.begin(), list.end()) + {} + + constexpr bbEnumT of(pbEnumT v) const + { return map.left.at(v); } + + constexpr pbEnumT of(bbEnumT v) const + { return map.right.at(v); } +}; + + +class pb_convert : public std::enable_shared_from_this { +public: + pb_convert() + : blackboard_(nullptr) + , pb_msg_index_(0) + {} + + virtual ~pb_convert(); + + virtual void handle(std::shared_ptr) + {} + + virtual void init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger) + { + blackboard_ = blackboard; + logger_ = logger; + } + +protected: + fawkes::BlackBoard *blackboard_; + fawkes::Logger *logger_; + size_t pb_msg_index_; +}; + + +template +class pb_converter + : public pb_convert { +public: + pb_converter(size_t max_ifaces = 8) + : interfaces_(max_ifaces, nullptr) + {} + + virtual ~pb_converter() {} + + virtual void + handle(std::shared_ptr msg) override + { + size_t if_idx = pb_msg_index_ % interfaces_.size(); + if (interfaces_[if_idx]) + blackboard_->close(interfaces_[if_idx]); + + interfaces_[if_idx] = blackboard_->open_for_writing( + iface_id_for_type(++pb_msg_index_).c_str()); + + handle(dynamic_cast(*msg), interfaces_[if_idx]); + interfaces_[if_idx]->write(); + } + + virtual void + handle(const ProtoT &msg, IfaceT *iface); + +private: + std::vector interfaces_; +}; + + +typedef std::unordered_map> pb_conversion_map; + + +pb_conversion_map make_receiving_interfaces_map(); + + +class BlackboardManager : + public fawkes::Thread, + public fawkes::LoggingAspect, + public fawkes::ConfigurableAspect, + public fawkes::BlackBoardAspect +{ +public: + BlackboardManager(ProtobufThead *msg_handler); + +protected: + virtual void init() override; + virtual void finalize() override; + virtual void loop() override; + +private: + void add_peer(fawkes::ProtobufPeerInterface *iface, long peer_id); + + template bool handle_message_type(InterfaceT *iface) + { + if (!iface->msgq_empty()) { + logger->log_info(name(), "%s", iface->uid()); + bool rv = false; + while (MessageT *msg = iface->msgq_first_safe(msg)) { + logger->log_info(name(), "%s", msg->type()); + handle_message(iface, msg); + iface->msgq_pop(); + rv = true; + } + return rv; + } else + return false; + } + + template void handle_message(InterfaceT *iface, MessageT *msg); + + + template + struct on_interface { + InterfaceT *iface; + BlackboardManager *manager; + + on_interface(InterfaceT *iface, BlackboardManager *manager) + : iface(iface), manager(manager) + {} + + template bool handle_msg_types () + { + return manager->handle_message_type(iface); + } + + // This template is disabled if MessageTs is {} to resolve ambiguity + template + typename std::enable_if<(sizeof...(MessageTs) > 0), bool>::type + handle_msg_types () + { return handle_msg_types() || handle_msg_types(); } + }; + + + struct init_interface { + BlackboardManager *manager; + template + void operator() (bb_iface_manager> &iface_mgr) const { + iface_mgr.init(manager->blackboard, manager); + } + }; + + + struct handle_messages { + BlackboardManager *manager; + + template + bool operator() (const bb_iface_manager> &pair) const + { return manager->handle_message_type(pair.interface()); } + + template + bool operator() (const bb_iface_manager> &iface_mgr) const + { + return on_interface{iface_mgr.interface(), manager}.template handle_msg_types() + || manager->handle_message_type(iface_mgr.interface()); + } + }; + + + + ProtobufThead *message_handler_; + fawkes::ProtobufPeerInterface *peer_iface_; + + sending_interfaces bb_sending_interfaces_; + + pb_conversion_map bb_receiving_interfaces_; + + fawkes::BlackBoardOnMessageWaker *on_message_waker_; + + unsigned int next_peer_idx_; + +}; + + +} // namespace protoboard + + +#endif // BLACKBOARD_MANAGER_H diff --git a/src/plugins/protoboard/interface_definitions.cpp b/src/plugins/protoboard/interface_definitions.cpp new file mode 100644 index 0000000000..30b3e75e10 --- /dev/null +++ b/src/plugins/protoboard/interface_definitions.cpp @@ -0,0 +1,54 @@ +#include + +#include "interface_definitions.h" + +#include +#include +#include + + + +namespace protoboard { + + +using namespace fawkes; +using namespace boost::fusion; + + +std::vector proto_dirs() +{ return { BASEDIR "/src/libs/llsf_msgs" }; } + + + +/* + * Sending interfaces: ID is used as-is + */ + +template<> +std::string iface_id_for_type() +{ return "/protoboard/send_beacon"; } + +template<> +std::string iface_id_for_type() +{ return "/protoboard/prepare_machine"; } + + + +/* + * Receiving interfaces: ID ends with slash before index is appended + */ + +template<> +std::string iface_id_for_type() +{ return "/protoboard/order/"; } + +template<> +std::string iface_id_for_type() +{ return "/protoboard/game_state/"; } + +template<> +std::string iface_id_for_type() +{ return "/protoboard/beacon/"; } + + +} // namespace protoboard diff --git a/src/plugins/protoboard/interface_definitions.h b/src/plugins/protoboard/interface_definitions.h new file mode 100644 index 0000000000..9756b21c09 --- /dev/null +++ b/src/plugins/protoboard/interface_definitions.h @@ -0,0 +1,89 @@ +#ifndef INTERFACE_DEFINITIONS_H +#define INTERFACE_DEFINITIONS_H + +#include + +#include +#include +#include + +#include +#include + + +namespace protoboard { + + +template std::string iface_id_for_type(); + +template std::string iface_id_for_type(size_t idx) +{ return iface_id_for_type() + std::to_string(idx); } + + +template +class bb_iface_manager { +public: + bb_iface_manager() + : interface_(nullptr) + , blackboard_(nullptr) + , waker_(nullptr) + {} + + void init(fawkes::BlackBoard *blackboard, fawkes::Thread *thread) + { + blackboard_ = blackboard; + interface_ = blackboard_->open_for_writing(iface_id_for_type().c_str()); + waker_ = new fawkes::BlackBoardOnMessageWaker(blackboard, interface_, thread); + } + + ~bb_iface_manager() + { + if (blackboard_ && interface_) + blackboard_->close(interface_); + if (waker_) + delete waker_; + } + + IfaceT *interface() const { return interface_; } + +private: + IfaceT *interface_; + fawkes::BlackBoard *blackboard_; + fawkes::BlackBoardOnMessageWaker *waker_; +}; + + +template +struct type_list +{}; + + +typedef std::tuple < + bb_iface_manager < + fawkes::SendBeaconInterface, + type_list < + fawkes::SendBeaconInterface::SendBeaconMessage, + fawkes::SendBeaconInterface::SetPeerMessage + > + >, + bb_iface_manager < + fawkes::PrepareMachineInterface, + type_list < + fawkes::PrepareMachineInterface::PrepareBSMessage, + fawkes::PrepareMachineInterface::PrepareCSMessage, + fawkes::PrepareMachineInterface::PrepareDSMessage, + fawkes::PrepareMachineInterface::PrepareRSMessage, + fawkes::PrepareMachineInterface::PrepareSSMessage, + fawkes::PrepareMachineInterface::SetPeerMessage + > + > +> sending_interfaces; + + +std::vector proto_dirs(); + + +} // namespace protoboard + + +#endif // INTERFACE_DEFINITIONS_H diff --git a/src/plugins/protoboard/interfaces/Makefile b/src/plugins/protoboard/interfaces/Makefile new file mode 100644 index 0000000000..8ba2a86f90 --- /dev/null +++ b/src/plugins/protoboard/interfaces/Makefile @@ -0,0 +1,24 @@ +#***************************************************************************** +# Makefile Build System for Fawkes: Robotino interfaces +# ------------------- +# Created on Fri Mar 26 17:25:00 2012 +# Copyright (C) 2006-2012 by Tim Niemueller, AllemaniACs RoboCup Team +# +#***************************************************************************** +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +#***************************************************************************** + +BASEDIR = ../../../.. + +include $(BASEDIR)/etc/buildsys/config.mk + +INTERFACES_all = $(notdir $(patsubst %.xml,%,$(wildcard $(SRCDIR)/*.xml))) +include $(BUILDSYSDIR)/interface.mk + +include $(BUILDSYSDIR)/base.mk + diff --git a/src/plugins/protoboard/interfaces/OrderInterface.xml b/src/plugins/protoboard/interfaces/OrderInterface.xml new file mode 100644 index 0000000000..372e451780 --- /dev/null +++ b/src/plugins/protoboard/interfaces/OrderInterface.xml @@ -0,0 +1,48 @@ + + + + + + What color to output + Red base + Black base + Silver base + + + Available ring colors + Blue ring + Green ring + Orange ring + Yellow ring + + + Available cap colors + Black cap + Grey cap + + + Team color + Cyan team + Magenta team + + + Product complexity + No ring + One ring + Two rings + Three rings + + + + Description of a single order + Order ID + Order complexity + Requested base color + Requested rings + Requested cap color + Delivery window start (seconds of game time) + Delivery window end (seconds of game time) + Which gate must be used at the DS + + + diff --git a/src/plugins/protoboard/interfaces/PrepareMachineInterface.xml b/src/plugins/protoboard/interfaces/PrepareMachineInterface.xml new file mode 100644 index 0000000000..2a8da2c3ed --- /dev/null +++ b/src/plugins/protoboard/interfaces/PrepareMachineInterface.xml @@ -0,0 +1,84 @@ + + + + + + Side of an MPS + Output side (even tag ID) + Input side (odd tag ID) + + + What color to output + Red base + Black base + Silver base + + + Available ring colors + Blue ring + Green ring + Orange ring + Yellow ring + + + Available cap colors + Black cap + Grey cap + + + Team color + Cyan team + Magenta team + + + Storage Station operation + Store something + Retrieve something + + + Cap Station operation + Retrieve cap + Mount cap + + + + + No data needed really, just store last msgid + Last sent message ID + Peer ID to send to + + + + Set peer ID that messages should be sent to + Peer ID to use + + + Make BS output specified base element at the specified side + The sending team's color + Machine name + Where the base should come out + What base color should come out + + + The sending team's color + Machine name + Delivery gate + + + The sending team's color + Machine name + Operation to perform + Where to store/retrieve + + + The sending team's color + Machine name + Ring color + + + The sending team's color + Machine name + Operation to perform + + + diff --git a/src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml b/src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml new file mode 100644 index 0000000000..dd93d5ac11 --- /dev/null +++ b/src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml @@ -0,0 +1,35 @@ + + + + + Current peers maintained by the protoboard plugin + Currently active peers + + + Create a peer without encryption + IP address or host name + Port to send to/receive on + + + Create a local peer without encryption (mainly for simulation) + IP address or host name + Port to send to + Port to receive on + + + Create a peer with encryption + IP address or host name + Port to send to/receive on + Crypto key + Cipher name + + + Create a local peer with encryption + IP address or host name + Port to send to + Port to receive on + Crypto key + Cipher name + + + diff --git a/src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml b/src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml new file mode 100644 index 0000000000..14f5b757ad --- /dev/null +++ b/src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml @@ -0,0 +1,33 @@ + + + + + + Game state + Just after startup of the refbox + Time not yet running + Game is running + Game was paused + + + Game phase + Game hasn't begun + Setup phase + Exploration phase + Production phase + Game has ended + + + + Full game state + Time in seconds since game start + Nanoseconds part + Current game state + Current game phase + Points awarded to cyan + Name of the cyan team + Points awarded to magenta + Name of the magenta team + + + diff --git a/src/plugins/protoboard/interfaces/RecvBeaconInterface.xml b/src/plugins/protoboard/interfaces/RecvBeaconInterface.xml new file mode 100644 index 0000000000..7cf629e644 --- /dev/null +++ b/src/plugins/protoboard/interfaces/RecvBeaconInterface.xml @@ -0,0 +1,24 @@ + + + + + + Team color + Cyan team + Magenta team + + + + + Data from last received BeaconSignal + Seconds since game start + Nanoseconds portion + Jersey number (dummy value if rcv'd from refbox) + Team name (dummy value if rcv'd from refbox) + Peer name + Team color + Pose (unused if rcv'd from refbox) + + + + diff --git a/src/plugins/protoboard/interfaces/SendBeaconInterface.xml b/src/plugins/protoboard/interfaces/SendBeaconInterface.xml new file mode 100644 index 0000000000..5d4ee75ff0 --- /dev/null +++ b/src/plugins/protoboard/interfaces/SendBeaconInterface.xml @@ -0,0 +1,32 @@ + + + + + + Team color + Cyan team + Magenta team + + + + + No data needed really, just store last msgid + Last sent message ID + Peer ID to send to + + + + Set peer ID that messages should be sent to + Peer ID to use + + + Send a beacon. Contents of the last message are copied into the relevant data fields. + Robot's number + Team name + The Robot's name + Team color + Robot's position on the field + Robot's orientation (yaw) on the field + + + diff --git a/src/plugins/protoboard/plugin.cpp b/src/plugins/protoboard/plugin.cpp new file mode 100644 index 0000000000..0ef1964b86 --- /dev/null +++ b/src/plugins/protoboard/plugin.cpp @@ -0,0 +1,14 @@ +#include "plugin.h" + +#include "blackboard_manager.h" +#include "protobuf_thread.h" + +ProtoboardPlugin::ProtoboardPlugin(fawkes::Configuration *config) + : Plugin(config) +{ + protoboard::ProtobufThead *msg_handler = new protoboard::ProtobufThead(); + protoboard::BlackboardManager *bb_mgr = new protoboard::BlackboardManager(msg_handler); + msg_handler->set_bb_manager(bb_mgr); + thread_list.push_back(bb_mgr); + thread_list.push_back(msg_handler); +} diff --git a/src/plugins/protoboard/plugin.h b/src/plugins/protoboard/plugin.h new file mode 100644 index 0000000000..d369429953 --- /dev/null +++ b/src/plugins/protoboard/plugin.h @@ -0,0 +1,15 @@ +#ifndef PROTOBOARD_PLUGIN_H +#define PROTOBOARD_PLUGIN_H + +#include + + +class ProtoboardPlugin : public fawkes::Plugin { +public: + ProtoboardPlugin(fawkes::Configuration *config); +}; + +PLUGIN_DESCRIPTION("Publish LLSF protobuf messages to blackboard") +EXPORT_PLUGIN(ProtoboardPlugin) + +#endif // PROTOBOARD_PLUGIN_H diff --git a/src/plugins/protoboard/protobuf_thread.cpp b/src/plugins/protoboard/protobuf_thread.cpp new file mode 100644 index 0000000000..92ccd163f2 --- /dev/null +++ b/src/plugins/protoboard/protobuf_thread.cpp @@ -0,0 +1,265 @@ +#include + +#include +#include +#include + +#include + +#include "protobuf_thread.h" +#include "blackboard_manager.h" + + +using namespace google::protobuf; +using namespace protobuf_comm; + + +namespace protoboard { + + +ProtobufThead::ProtobufThead() + : Thread("ProtoboardMessageHandler", Thread::OPMODE_CONTINUOUS) + , bb_manager_(nullptr) + , message_register_(nullptr) + , server_(nullptr) + , next_client_id_(0) +{} + + +ProtobufThead::~ProtobufThead() +{ + delete message_register_; + delete server_; +} + + +void ProtobufThead::init() +{ + message_register_ = new MessageRegister(proto_dirs()); + + if (!bb_manager_) + throw fawkes::Exception("BUG: %s's reference to blackboard manager thread hasn't been initialized", name()); +} + + + +bool ProtobufThead::pb_queue_incoming() +{ + fawkes::MutexLocker lock(&msgq_mutex_); + return !pb_queue_.empty(); +} + + +ProtobufThead::incoming_message ProtobufThead::pb_queue_pop() +{ + fawkes::MutexLocker lock(&msgq_mutex_); + incoming_message msg = pb_queue_.front(); + pb_queue_.pop(); + return msg; +} + + +/** Enable protobuf peer. + * @param address IP address to send messages to + * @param send_port UDP port to send messages to + * @param recv_port UDP port to receive messages on, 0 to use the same as the @p send_port + * @param crypto_key encryption key + * @param cipher cipher suite, see BufferEncryptor for supported types + * @return peer identifier + */ +long int +ProtobufThead::peer_create_local_crypto(const std::string &address, int send_to_port, int recv_on_port, + const std::string &crypto_key, const std::string &cipher) +{ + if (recv_on_port <= 0) recv_on_port = send_to_port; + + if (send_to_port > 0) { + protobuf_comm::ProtobufBroadcastPeer *peer = + new protobuf_comm::ProtobufBroadcastPeer( + address, send_to_port, recv_on_port, + message_register_, crypto_key, cipher); + + long int peer_id; + { + fawkes::MutexLocker lock(&map_mutex_); + peer_id = ++next_client_id_; + peers_[peer_id] = peer; + } + + peer->signal_received() + .connect(boost::bind(&ProtobufThead::handle_peer_msg, this, peer_id, _1, _2, _3, _4)); + peer->signal_recv_error() + .connect(boost::bind(&ProtobufThead::handle_peer_recv_error, this, peer_id, _1, _2)); + peer->signal_send_error() + .connect(boost::bind(&ProtobufThead::handle_peer_send_error, this, peer_id, _1)); + + return peer_id; + } else { + return 0; + } +} + + +/** Enable protobuf peer. + * @param address IP address to send messages to + * @param port UDP port to send and receive messages + * @param crypto_key encryption key + * @param cipher cipher suite, see BufferEncryptor for supported types + * @return peer identifier + */ +long int +ProtobufThead::peer_create_crypto(const std::string &address, int port, + const std::string &crypto_key, const std::string &cipher) +{ + return peer_create_local_crypto(address, port, port, crypto_key, cipher); +} + + +/** Enable protobuf peer. + * @param address IP address to send messages to + * @param port UDP port to send and receive messages + * @return peer identifier + */ +long int +ProtobufThead::peer_create(const std::string &address, int port) +{ + return peer_create_local_crypto(address, port, port); +} + +/** Enable protobuf peer. + * @param address IP address to send messages to + * @param send_port UDP port to send messages to + * @param recv_port UDP port to receive messages on, 0 to use the same as the @p send_port + * @return peer identifier + */ +long int +ProtobufThead::peer_create_local(const std::string &address, int send_to_port, + int recv_on_port) +{ + return peer_create_local_crypto(address, send_to_port, recv_on_port); +} + + +/** Disable peer. + * @param peer_id ID of the peer to destroy + */ +void +ProtobufThead::peer_destroy(long int peer_id) +{ + if (peers_.find(peer_id) != peers_.end()) { + delete peers_[peer_id]; + peers_.erase(peer_id); + } +} + + +/** Setup crypto for peer. + * @param peer_id ID of the peer to destroy + * @param crypto_key encryption key + * @param cipher cipher suite, see BufferEncryptor for supported types + */ +void +ProtobufThead::peer_setup_crypto(long int peer_id, + const std::string &crypto_key, + const std::string &cipher) +{ + if (peers_.find(peer_id) != peers_.end()) { + peers_[peer_id]->setup_crypto(crypto_key, cipher); + } +} + + +void +ProtobufThead::send(long int peer_id, + std::shared_ptr m) +{ + if (!m) { + if (logger) { + logger->log_warn(name(), "Cannot send broadcast: invalid message"); + } + return; + } + + fawkes::MutexLocker lock(&map_mutex_); + if (peers_.find(peer_id) == peers_.end()) return; + + //logger->log_info(name(), "Broadcasting %s", (*m)->GetTypeName().c_str()); + try { + peers_[peer_id]->send(m); + } catch (google::protobuf::FatalException &e) { + if (logger) { + logger->log_warn(name(), + "Failed to broadcast message of type %s: %s", + m->GetTypeName().c_str(), e.what()); + } + } catch (fawkes::Exception &e) { + if (logger) { + logger->log_warn(name(), + "Failed to broadcast message of type %s: %s", + m->GetTypeName().c_str(), e.what_no_backtrace()); + } + } catch (std::runtime_error &e) { + if (logger) { + logger->log_warn(name(), + "Failed to broadcast message of type %s: %s", + m->GetTypeName().c_str(), e.what()); + } + } +} + + +/** Handle message that came from a peer/robot + * @param endpoint the endpoint from which the message was received + * @param component_id component the message was addressed to + * @param msg_type type of the message + * @param msg the message + */ +void +ProtobufThead::handle_peer_msg(long int peer_id, + boost::asio::ip::udp::endpoint &endpoint, + uint16_t component_id, uint16_t msg_type, + std::shared_ptr msg) +{ + fawkes::MutexLocker lock(&msgq_mutex_); + pb_queue_.push({ + peer_id, + endpoint, + component_id, + msg_type, + std::move(msg) + }); + bb_manager_->wakeup(); +} + + +/** Handle error during peer message processing. + * @param endpoint endpoint of incoming message + * @param msg error message + */ +void +ProtobufThead::handle_peer_recv_error(long int peer_id, + boost::asio::ip::udp::endpoint &endpoint, std::string msg) +{ + if (logger) { + logger->log_warn(name(), + "Failed to receive peer message from %s:%u: %s", + endpoint.address().to_string().c_str(), endpoint.port(), + msg.c_str()); + } +} + + +/** Handle error during peer message processing. + * @param msg error message + */ +void +ProtobufThead::handle_peer_send_error(long int peer_id, std::string msg) +{ + if (logger) { + logger->log_warn(name(), + "Failed to send peer message: %s", msg.c_str()); + } +} + + +} // namespace protoboard; diff --git a/src/plugins/protoboard/protobuf_thread.h b/src/plugins/protoboard/protobuf_thread.h new file mode 100644 index 0000000000..0fdb616868 --- /dev/null +++ b/src/plugins/protoboard/protobuf_thread.h @@ -0,0 +1,124 @@ +#ifndef MESSAGE_HANDLER_H +#define MESSAGE_HANDLER_H + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#define CFG_PREFIX "/plugins/protoboard" + +namespace protobuf_comm { + class ProtobufStreamClient; + class ProtobufBroadcastPeer; +} + + +namespace protoboard { + + +class BlackboardManager; + + +class ProtobufThead : + public fawkes::Thread, + public fawkes::LoggingAspect, + public fawkes::ConfigurableAspect +{ +public: + ProtobufThead(); + virtual ~ProtobufThead() override; + + bool pb_queue_incoming(); + + struct incoming_message { + long int peer_id; + boost::asio::ip::udp::endpoint endpoint; + uint16_t component_id; + uint16_t msg_type; + std::shared_ptr msg; + }; + + incoming_message pb_queue_pop(); + + long int peer_create(const std::string &host, int port); + long int peer_create_local(const std::string &host, + int send_to_port, int recv_on_port); + long int peer_create_crypto(const std::string &host, int port, + const std::string &crypto_key = "", const std::string &cipher = ""); + long int peer_create_local_crypto(const std::string &host, + int send_to_port, int recv_on_port, + const std::string &crypto_key = "", const std::string &cipher = ""); + void peer_destroy(long int peer_id); + + void send(long int peer_id, std::shared_ptr msg); + + /** Get the communicator's message register. + * @return message register */ + protobuf_comm::MessageRegister & message_register() + { return *message_register_; } + + void set_bb_manager(BlackboardManager *bb_manager) + { bb_manager_ = bb_manager; } + +protected: + virtual void init() override; + +private: + /** Get protobuf_comm peers. + * @return protobuf_comm peer */ + const std::map & + peers() const + { return peers_; } + + BlackboardManager *bb_manager_; + + /** Signal invoked for a message that has been sent via broadcast. + * @return signal + */ + boost::signals2::signal)> & + signal_peer_sent() { return sig_peer_sent_; } + + + void peer_setup_crypto(long int peer_id, const std::string &crypto_key, const std::string &cipher); + + + void handle_peer_msg(long int peer_id, + boost::asio::ip::udp::endpoint &endpoint, + uint16_t component_id, uint16_t msg_type, + std::shared_ptr msg); + void wait_for_first_ga(long int peer_id, + boost::asio::ip::udp::endpoint &endpoint, + uint16_t component_id, uint16_t msg_type, + std::shared_ptr msg); + void handle_peer_recv_error(long int peer_id, boost::asio::ip::udp::endpoint &endpoint, std::string msg); + void handle_peer_send_error(long int peer_id, std::string msg); + + protobuf_comm::MessageRegister *message_register_; + protobuf_comm::ProtobufStreamServer *server_; + + boost::signals2::signal)> sig_peer_sent_; + + fawkes::Mutex map_mutex_; + fawkes::Mutex msgq_mutex_; + long int next_client_id_; + + std::map peers_; + + std::queue pb_queue_; + +}; + + +} // namespace protoboard + + +#endif // MESSAGE_HANDLER_H From d862d885f60ee5f7365f10f174ea313e36536e5a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Thu, 9 Nov 2017 11:20:07 +0100 Subject: [PATCH 02/20] protoboard: add dummy handler for RobotInfo & VersionInfo --- src/plugins/protoboard/bb_msg_handlers.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/plugins/protoboard/bb_msg_handlers.cpp b/src/plugins/protoboard/bb_msg_handlers.cpp index b9f259db1c..fb9767e6e0 100644 --- a/src/plugins/protoboard/bb_msg_handlers.cpp +++ b/src/plugins/protoboard/bb_msg_handlers.cpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include namespace protoboard { @@ -22,10 +24,16 @@ std::unordered_map> make_receiving_inte make_pair("llsf_msgs.Order", make_shared>()), make_pair("llsf_msgs.BeaconSignal", make_shared>()), make_pair("llsf_msgs.GameState", make_shared>()), + + // Dummy handler, i.e. discard message + make_pair("llsf_msgs.RobotInfo", make_shared()), + make_pair("llsf_msgs.VersionInfo", make_shared()) }; }; + + /**************************************** * OrderInterface * ****************************************/ From 04e57b108c7516650c3ef36be27175e199550923 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Thu, 9 Nov 2017 11:22:02 +0100 Subject: [PATCH 03/20] protoboard: auto. set time on BeaconSignal --- src/plugins/protoboard/bb_msg_handlers.cpp | 14 ++++++++++++++ src/plugins/protoboard/blackboard_manager.h | 4 +++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/plugins/protoboard/bb_msg_handlers.cpp b/src/plugins/protoboard/bb_msg_handlers.cpp index fb9767e6e0..481e748618 100644 --- a/src/plugins/protoboard/bb_msg_handlers.cpp +++ b/src/plugins/protoboard/bb_msg_handlers.cpp @@ -183,16 +183,30 @@ BlackboardManager::handle_message( SendBeaconInterface::SendBeaconMessage *msg) { std::shared_ptr m = std::make_shared(); + + // Set time here to simplify blackboard interface + llsf_msgs::Time *time = new llsf_msgs::Time(); + time->set_sec(clock->now().get_sec()); + time->set_nsec(clock->now().get_nsec()); + llsf_msgs::Pose2D *pose = new llsf_msgs::Pose2D(); pose->set_x(msg->translation(0)); pose->set_y(msg->translation(1)); pose->set_ori(msg->orientation()); + pose->set_allocated_timestamp(time); + m->set_allocated_pose(pose); m->set_allocated_team_name(new std::string(msg->team_name())); m->set_number(msg->number()); m->set_allocated_peer_name(new std::string(msg->peer_name())); m->set_team_color(team_enum_beacon.of(msg->team_color())); + m->set_allocated_time(new llsf_msgs::Time(*time)); + + // Same for sequence number, good idea to count it centrally, anyways + static google::protobuf::uint64 beacon_seq = 0; + m->set_seq(beacon_seq++); + message_handler_->send(iface->peer_id(), m); } diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h index e0fe4b7a8b..5ac495768c 100644 --- a/src/plugins/protoboard/blackboard_manager.h +++ b/src/plugins/protoboard/blackboard_manager.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -119,7 +120,8 @@ class BlackboardManager : public fawkes::Thread, public fawkes::LoggingAspect, public fawkes::ConfigurableAspect, - public fawkes::BlackBoardAspect + public fawkes::BlackBoardAspect, + public fawkes::ClockAspect { public: BlackboardManager(ProtobufThead *msg_handler); From cf07e255106db74197c1e4a7c9072cb36822ea59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Thu, 9 Nov 2017 11:23:13 +0100 Subject: [PATCH 04/20] protoboard: catch & log exceptions in handlers --- src/plugins/protoboard/blackboard_manager.cpp | 9 ++++++++- src/plugins/protoboard/blackboard_manager.h | 19 +++++++++++++------ 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/plugins/protoboard/blackboard_manager.cpp b/src/plugins/protoboard/blackboard_manager.cpp index a25c0c5b39..3575e48bce 100644 --- a/src/plugins/protoboard/blackboard_manager.cpp +++ b/src/plugins/protoboard/blackboard_manager.cpp @@ -51,21 +51,28 @@ void BlackboardManager::loop() ProtobufPeerInterface::CreatePeerLocalCryptoMessage > (); + // Handle sending blackboard interfaces did_something |= boost::fusion::any(bb_sending_interfaces_, handle_messages { this }); + // Handle receiving blackboard interfaces while (message_handler_->pb_queue_incoming()) { ProtobufThead::incoming_message inc = message_handler_->pb_queue_pop(); pb_conversion_map::iterator it; if ((it = bb_receiving_interfaces_.find(inc.msg->GetTypeName())) == bb_receiving_interfaces_.end()) logger->log_error(name(), "Received message of unregistered type `%s'", inc.msg->GetTypeName().c_str()); - else + else try { it->second->handle(inc.msg); + } catch (std::exception &e) { + logger->log_error(name(), "Exception while handling %s: %s", + inc.msg->GetTypeName().c_str(), e.what()); + } did_something = true; } if (!did_something) + // Thread woke up, but nothing was handled logger->log_warn(name(), "Spurious wakeup. WTF?"); } diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h index 5ac495768c..92306114f9 100644 --- a/src/plugins/protoboard/blackboard_manager.h +++ b/src/plugins/protoboard/blackboard_manager.h @@ -134,23 +134,29 @@ class BlackboardManager : private: void add_peer(fawkes::ProtobufPeerInterface *iface, long peer_id); - template bool handle_message_type(InterfaceT *iface) + template + bool handle_message_type(InterfaceT *iface) { if (!iface->msgq_empty()) { - logger->log_info(name(), "%s", iface->uid()); bool rv = false; while (MessageT *msg = iface->msgq_first_safe(msg)) { - logger->log_info(name(), "%s", msg->type()); - handle_message(iface, msg); + try { + handle_message(iface, msg); + } catch (std::exception &e) { + logger->log_error(name(), "Exception handling %s on %s: %s", + msg->type(), iface->uid(), e.what()); + } iface->msgq_pop(); rv = true; } + iface->write(); return rv; } else return false; } - template void handle_message(InterfaceT *iface, MessageT *msg); + template + void handle_message(InterfaceT *iface, MessageT *msg); template @@ -162,7 +168,8 @@ class BlackboardManager : : iface(iface), manager(manager) {} - template bool handle_msg_types () + template + bool handle_msg_types () { return manager->handle_message_type(iface); } From 583dfea5c01d4d5f715c0b51e6ce8d395c69ba74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Thu, 9 Nov 2017 16:17:29 +0100 Subject: [PATCH 05/20] protoboard: add pb_nesting_converter Required if a repeated protobuf field is mapped to multiple blackboard interfaces. --- src/plugins/protoboard/bb_msg_handlers.cpp | 24 +++++-- src/plugins/protoboard/blackboard_manager.h | 73 ++++++++++++++++----- 2 files changed, 78 insertions(+), 19 deletions(-) diff --git a/src/plugins/protoboard/bb_msg_handlers.cpp b/src/plugins/protoboard/bb_msg_handlers.cpp index 481e748618..fe634403fa 100644 --- a/src/plugins/protoboard/bb_msg_handlers.cpp +++ b/src/plugins/protoboard/bb_msg_handlers.cpp @@ -21,7 +21,14 @@ using namespace boost::fusion; std::unordered_map> make_receiving_interfaces_map() { return { - make_pair("llsf_msgs.Order", make_shared>()), + make_pair( + "llsf_msgs.OrderInfo", + make_shared< + pb_nesting_converter< + llsf_msgs::OrderInfo, + pb_buffered_converter + > + >()), make_pair("llsf_msgs.BeaconSignal", make_shared>()), make_pair("llsf_msgs.GameState", make_shared>()), @@ -32,8 +39,6 @@ std::unordered_map> make_receiving_inte }; - - /**************************************** * OrderInterface * ****************************************/ @@ -70,7 +75,18 @@ complexity_enum_order { template<> void -pb_converter::handle( +pb_nesting_converter>::handle( + const llsf_msgs::OrderInfo &msg, + pb_buffered_converter &order_to_iface) +{ + for (const llsf_msgs::Order &order : msg.orders()) + order_to_iface.handle(order); +} + + +template<> +void +pb_buffered_converter::handle( const llsf_msgs::Order &msg, OrderInterface *iface) { diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h index 92306114f9..d33fbd8643 100644 --- a/src/plugins/protoboard/blackboard_manager.h +++ b/src/plugins/protoboard/blackboard_manager.h @@ -57,39 +57,66 @@ class pb_convert : public std::enable_shared_from_this { public: pb_convert() : blackboard_(nullptr) - , pb_msg_index_(0) {} virtual ~pb_convert(); - virtual void handle(std::shared_ptr) - {} - - virtual void init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger) + virtual void + init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger) { blackboard_ = blackboard; logger_ = logger; } + virtual void handle(std::shared_ptr msg) + { handle(*msg); } + + virtual void handle(const google::protobuf::Message &) + {} + + protected: fawkes::BlackBoard *blackboard_; fawkes::Logger *logger_; - size_t pb_msg_index_; }; template -class pb_converter - : public pb_convert { +class pb_converter : public pb_convert { public: - pb_converter(size_t max_ifaces = 8) - : interfaces_(max_ifaces, nullptr) + pb_converter() + : interface_(nullptr) {} - virtual ~pb_converter() {} + virtual void + init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger) override + { + pb_convert::init(blackboard, logger); + interface_ = blackboard_->open_for_writing( + iface_id_for_type(0).c_str()); + } virtual void - handle(std::shared_ptr msg) override + handle(const google::protobuf::Message &msg) override + { handle(dynamic_cast(msg), interface_); } + + virtual void handle(const ProtoT &msg, IfaceT *iface); + +private: + IfaceT *interface_; +}; + + +template +class pb_buffered_converter : public pb_convert { +public: + pb_buffered_converter() + : interfaces_(num_ifaces, nullptr) + , pb_msg_index_(0) + {} + + virtual void + handle(const google::protobuf::Message &msg) override { size_t if_idx = pb_msg_index_ % interfaces_.size(); if (interfaces_[if_idx]) @@ -98,15 +125,31 @@ class pb_converter interfaces_[if_idx] = blackboard_->open_for_writing( iface_id_for_type(++pb_msg_index_).c_str()); - handle(dynamic_cast(*msg), interfaces_[if_idx]); + handle(dynamic_cast(msg), interfaces_[if_idx]); interfaces_[if_idx]->write(); } - virtual void - handle(const ProtoT &msg, IfaceT *iface); + virtual void handle(const ProtoT &msg, IfaceT *iface); private: std::vector interfaces_; + size_t pb_msg_index_; +}; + + +template +class pb_nesting_converter : public pb_convert { +public: + virtual void + handle(const google::protobuf::Message &msg) override + { + handle(dynamic_cast(msg), sub_converter_); + } + + virtual void handle(const ProtoT &msg, ConverterT &sub_converter); + +private: + ConverterT sub_converter_; }; From 55ce29cc1b8115691ed7bdf4fffd8846a3402f0f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Tue, 14 Nov 2017 17:06:50 +0100 Subject: [PATCH 06/20] protoboard: add quantity fields to OrderInterface --- src/plugins/protoboard/interfaces/OrderInterface.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/plugins/protoboard/interfaces/OrderInterface.xml b/src/plugins/protoboard/interfaces/OrderInterface.xml index 372e451780..62b1e3e9da 100644 --- a/src/plugins/protoboard/interfaces/OrderInterface.xml +++ b/src/plugins/protoboard/interfaces/OrderInterface.xml @@ -43,6 +43,9 @@ Delivery window start (seconds of game time) Delivery window end (seconds of game time) Which gate must be used at the DS + How many of this type must be delivered + How many were delivered by Cyan + How many were delivered by Magenta From f48ca2d4dea128b8066196f869f8004d84966f5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Tue, 14 Nov 2017 17:07:40 +0100 Subject: [PATCH 07/20] protoboard: fix pb->bb conversion for repeated fields So far, only a direct mapping from one or more incoming protobuf messages to one or more blackboard interfaces could be specified. This commit adds the ability to extract a repeated field from a message and map each element to a specific blackboard interface. Blackboard interfaces are opened dynamically for each repeated field, and closed again for each field that no longer exists (in relation to the previous message). --- src/plugins/protoboard/bb_msg_handlers.cpp | 52 +++++++----- src/plugins/protoboard/blackboard_manager.cpp | 6 +- src/plugins/protoboard/blackboard_manager.h | 82 +++++++++++-------- 3 files changed, 83 insertions(+), 57 deletions(-) diff --git a/src/plugins/protoboard/bb_msg_handlers.cpp b/src/plugins/protoboard/bb_msg_handlers.cpp index fe634403fa..97bc294ab7 100644 --- a/src/plugins/protoboard/bb_msg_handlers.cpp +++ b/src/plugins/protoboard/bb_msg_handlers.cpp @@ -24,9 +24,10 @@ std::unordered_map> make_receiving_inte make_pair( "llsf_msgs.OrderInfo", make_shared< - pb_nesting_converter< + pb_sequence_converter< llsf_msgs::OrderInfo, - pb_buffered_converter + pb_converter, + 10 > >()), make_pair("llsf_msgs.BeaconSignal", make_shared>()), @@ -74,32 +75,43 @@ complexity_enum_order { template<> -void -pb_nesting_converter>::handle( - const llsf_msgs::OrderInfo &msg, - pb_buffered_converter &order_to_iface) -{ - for (const llsf_msgs::Order &order : msg.orders()) - order_to_iface.handle(order); -} +const google::protobuf::RepeatedPtrField & +pb_sequence_converter, 10>::extract_sequence( + const llsf_msgs::OrderInfo &msg) +{ return msg.orders(); } template<> void -pb_buffered_converter::handle( +pb_converter::handle( const llsf_msgs::Order &msg, OrderInterface *iface) { - iface->set_base_color(base_color_enum_order.of(msg.base_color())); - iface->set_cap_color(cap_color_enum_order.of(msg.cap_color())); - iface->set_complexity(complexity_enum_order.of(msg.complexity())); + if (iface->base_color() != base_color_enum_order.of(msg.base_color())) + iface->set_base_color(base_color_enum_order.of(msg.base_color())); + if (iface->cap_color() != cap_color_enum_order.of(msg.cap_color())) + iface->set_cap_color(cap_color_enum_order.of(msg.cap_color())); + if (iface->complexity() != complexity_enum_order.of(msg.complexity())) + iface->set_complexity(complexity_enum_order.of(msg.complexity())); + for (int i = 0; i < msg.ring_colors_size(); ++i) - iface->set_ring_colors(static_cast(i), - ring_color_enum_order.of(msg.ring_colors(i))); - iface->set_order_id(msg.id()); - iface->set_delivery_period_begin(msg.delivery_period_begin()); - iface->set_delivery_period_end(msg.delivery_period_end()); - iface->set_delivery_gate(msg.delivery_gate()); + if (iface->ring_colors(static_cast(i)) != ring_color_enum_order.of(msg.ring_colors(i))) + iface->set_ring_colors(static_cast(i), ring_color_enum_order.of(msg.ring_colors(i))); + + if (iface->order_id() != msg.id()) + iface->set_order_id(msg.id()); + if (iface->delivery_period_begin() != msg.delivery_period_begin()) + iface->set_delivery_period_begin(msg.delivery_period_begin()); + if (iface->delivery_period_end() != msg.delivery_period_end()) + iface->set_delivery_period_end(msg.delivery_period_end()); + if (iface->delivery_gate() != msg.delivery_gate()) + iface->set_delivery_gate(msg.delivery_gate()); + if (iface->quantity_delivered_cyan() != msg.quantity_delivered_cyan()) + iface->set_quantity_delivered_cyan(msg.quantity_delivered_cyan()); + if (iface->quantity_delivered_magenta() != msg.quantity_delivered_magenta()) + iface->set_quantity_delivered_magenta(msg.quantity_delivered_magenta()); + if (iface->quantity_requested() != msg.quantity_requested()) + iface->set_quantity_requested(msg.quantity_requested()); }; diff --git a/src/plugins/protoboard/blackboard_manager.cpp b/src/plugins/protoboard/blackboard_manager.cpp index 3575e48bce..4dde23aa53 100644 --- a/src/plugins/protoboard/blackboard_manager.cpp +++ b/src/plugins/protoboard/blackboard_manager.cpp @@ -25,11 +25,11 @@ void BlackboardManager::init() { peer_iface_ = blackboard->open_for_writing("/protoboard/peers"); - for (pb_conversion_map::value_type &c : bb_receiving_interfaces_) - c.second->init(blackboard, logger); - on_message_waker_ = new fawkes::BlackBoardOnMessageWaker(blackboard, peer_iface_, this); boost::fusion::for_each(bb_sending_interfaces_, init_interface{this}); + + for (pb_conversion_map::value_type &c : bb_receiving_interfaces_) + c.second->init(blackboard, logger); } diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h index d33fbd8643..9855d0cb64 100644 --- a/src/plugins/protoboard/blackboard_manager.h +++ b/src/plugins/protoboard/blackboard_manager.h @@ -57,6 +57,7 @@ class pb_convert : public std::enable_shared_from_this { public: pb_convert() : blackboard_(nullptr) + , logger_(nullptr) {} virtual ~pb_convert(); @@ -74,7 +75,6 @@ class pb_convert : public std::enable_shared_from_this { virtual void handle(const google::protobuf::Message &) {} - protected: fawkes::BlackBoard *blackboard_; fawkes::Logger *logger_; @@ -84,72 +84,86 @@ class pb_convert : public std::enable_shared_from_this { template class pb_converter : public pb_convert { public: + typedef ProtoT input_type; + typedef IfaceT output_type; + pb_converter() : interface_(nullptr) {} + virtual ~pb_converter() + { blackboard_->close(interface_); } + virtual void init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger) override { pb_convert::init(blackboard, logger); interface_ = blackboard_->open_for_writing( - iface_id_for_type(0).c_str()); + iface_id_for_type(0).c_str()); } virtual void handle(const google::protobuf::Message &msg) override - { handle(dynamic_cast(msg), interface_); } + { + handle(dynamic_cast(msg), interface_); + interface_->write(); + } + + virtual void handle(const ProtoT &msg) + { handle(msg, interface_); } virtual void handle(const ProtoT &msg, IfaceT *iface); + virtual bool is_open() + { return interface_; } + + virtual void close() + { + if (is_open()) { + blackboard_->close(interface_); + interface_ = nullptr; + } + } + private: IfaceT *interface_; }; -template -class pb_buffered_converter : public pb_convert { +template +class pb_sequence_converter : public pb_convert { public: - pb_buffered_converter() - : interfaces_(num_ifaces, nullptr) - , pb_msg_index_(0) + typedef google::protobuf::RepeatedPtrField sequence_type; + + pb_sequence_converter() + : sub_converters_(seq_length) {} virtual void handle(const google::protobuf::Message &msg) override { - size_t if_idx = pb_msg_index_ % interfaces_.size(); - if (interfaces_[if_idx]) - blackboard_->close(interfaces_[if_idx]); - - interfaces_[if_idx] = blackboard_->open_for_writing( - iface_id_for_type(++pb_msg_index_).c_str()); - - handle(dynamic_cast(msg), interfaces_[if_idx]); - interfaces_[if_idx]->write(); - } - - virtual void handle(const ProtoT &msg, IfaceT *iface); - -private: - std::vector interfaces_; - size_t pb_msg_index_; -}; + typename std::vector::iterator out_it = sub_converters_.begin(); + sequence_type fields = extract_sequence(dynamic_cast(msg)); + typename sequence_type::const_iterator field_it = fields.begin(); + while (out_it != sub_converters_.end() && field_it != fields.end()) { + if (!out_it->is_open()) + out_it->init(blackboard_, logger_); + out_it->handle(*field_it); -template -class pb_nesting_converter : public pb_convert { -public: - virtual void - handle(const google::protobuf::Message &msg) override - { - handle(dynamic_cast(msg), sub_converter_); + ++out_it; + ++field_it; + } + for ( ; out_it != sub_converters_.end(); ++out_it) { + if (out_it->is_open()) + out_it->close(); + } } - virtual void handle(const ProtoT &msg, ConverterT &sub_converter); + virtual const sequence_type &extract_sequence(const ProtoT &msg); private: - ConverterT sub_converter_; + std::vector sub_converters_; }; From d93f1e5184dcf0d354d3eec744b3d5d37e6ef4ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Tue, 28 Nov 2017 18:09:18 +0100 Subject: [PATCH 08/20] protoboard: implement matching for repeated fields In the case of RCLL Orders, each repeated Order field in the OrderInfo message has a persisting identity specified by its order ID. Each order ID is either new, or it needs to be mapped to the specific blackboard interface holding that order. The appropriate blackboard interface should then only be changed if some of the other fields have changed. This commit changes the pb->bb conversion templates to require a method corresponds_to() that must return true if a certain pb message has the same identity as the respective blackboard interface. --- src/plugins/protoboard/bb_msg_handlers.cpp | 11 +- src/plugins/protoboard/blackboard_manager.cpp | 4 +- src/plugins/protoboard/blackboard_manager.h | 120 +++++++++++++----- 3 files changed, 96 insertions(+), 39 deletions(-) diff --git a/src/plugins/protoboard/bb_msg_handlers.cpp b/src/plugins/protoboard/bb_msg_handlers.cpp index 97bc294ab7..c1a491c142 100644 --- a/src/plugins/protoboard/bb_msg_handlers.cpp +++ b/src/plugins/protoboard/bb_msg_handlers.cpp @@ -26,8 +26,7 @@ std::unordered_map> make_receiving_inte make_shared< pb_sequence_converter< llsf_msgs::OrderInfo, - pb_converter, - 10 + pb_converter > >()), make_pair("llsf_msgs.BeaconSignal", make_shared>()), @@ -76,11 +75,17 @@ complexity_enum_order { template<> const google::protobuf::RepeatedPtrField & -pb_sequence_converter, 10>::extract_sequence( +pb_sequence_converter>::extract_sequence( const llsf_msgs::OrderInfo &msg) { return msg.orders(); } +template<> +bool +pb_converter::corresponds(const llsf_msgs::Order &msg, const OrderInterface *iface) +{ return iface->order_id() == msg.id(); } + + template<> void pb_converter::handle( diff --git a/src/plugins/protoboard/blackboard_manager.cpp b/src/plugins/protoboard/blackboard_manager.cpp index 4dde23aa53..a8056a544c 100644 --- a/src/plugins/protoboard/blackboard_manager.cpp +++ b/src/plugins/protoboard/blackboard_manager.cpp @@ -26,7 +26,9 @@ void BlackboardManager::init() peer_iface_ = blackboard->open_for_writing("/protoboard/peers"); on_message_waker_ = new fawkes::BlackBoardOnMessageWaker(blackboard, peer_iface_, this); - boost::fusion::for_each(bb_sending_interfaces_, init_interface{this}); + boost::fusion::for_each(bb_sending_interfaces_, [this] (auto &iface_mgr) { + iface_mgr.init(this->blackboard, this); + }); for (pb_conversion_map::value_type &c : bb_receiving_interfaces_) c.second->init(blackboard, logger); diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h index 9855d0cb64..81ae2ad380 100644 --- a/src/plugins/protoboard/blackboard_manager.h +++ b/src/plugins/protoboard/blackboard_manager.h @@ -13,6 +13,8 @@ #include #include +#include + #include #include @@ -35,11 +37,9 @@ class BlackboardManager; template -struct enum_map { +class enum_map { +public: typedef boost::bimap bimapT; - const std::vector list; - const boost::bimap map; - constexpr enum_map(std::initializer_list init) : list(init), map(list.begin(), list.end()) @@ -50,6 +50,10 @@ struct enum_map { constexpr pbEnumT of(bbEnumT v) const { return map.right.at(v); } + +private: + const std::vector list; + const bimapT map; }; @@ -63,7 +67,7 @@ class pb_convert : public std::enable_shared_from_this { virtual ~pb_convert(); virtual void - init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger) + init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger, size_t = 0) { blackboard_ = blackboard; logger_ = logger; @@ -88,32 +92,54 @@ class pb_converter : public pb_convert { typedef IfaceT output_type; pb_converter() - : interface_(nullptr) + : pb_convert() + , interface_(nullptr) {} + // Don't copy this + pb_converter(const pb_converter &) = delete; + pb_converter &operator = (const pb_converter &) = delete; + + // Only move! + pb_converter(pb_converter &&o) + : pb_convert(o) + , interface_(std::move(o.interface_)) + { o.interface_ = nullptr; } + + pb_converter &operator = (pb_converter &&o) + { + pb_convert::operator = (o); + this->interface_ = o.interface_; + o.interface_ = nullptr; + return *this; + } + + virtual ~pb_converter() - { blackboard_->close(interface_); } + { close(); } virtual void - init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger) override + init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger, size_t id = 0) override { pb_convert::init(blackboard, logger); interface_ = blackboard_->open_for_writing( - iface_id_for_type(0).c_str()); + iface_id_for_type(id).c_str()); + logger->log_info( + boost::core::demangle(typeid(*this).name()).c_str(), + "Initialized %s.", + iface_id_for_type(id).c_str()); } virtual void handle(const google::protobuf::Message &msg) override + { handle(dynamic_cast(msg)); } + + virtual void handle(const ProtoT &msg) { - handle(dynamic_cast(msg), interface_); + handle(msg, interface_); interface_->write(); } - virtual void handle(const ProtoT &msg) - { handle(msg, interface_); } - - virtual void handle(const ProtoT &msg, IfaceT *iface); - virtual bool is_open() { return interface_; } @@ -125,18 +151,43 @@ class pb_converter : public pb_convert { } } + IfaceT *interface() + { return interface_; } + + virtual bool corresponds_to(const ProtoT &msg) + { + if (is_open()) { + interface()->read(); + return corresponds(msg, interface()); + } + else + return true; + } + +protected: + virtual void handle(const ProtoT &msg, IfaceT *iface); + static bool corresponds(const ProtoT &, const IfaceT *) + { + throw fawkes::Exception( + boost::core::demangle(typeid(pb_converter).name()).c_str(), + "BUG: corresponds(...) must" + "be overridden for every converter used in a sequence."); + } + + private: IfaceT *interface_; }; -template +template class pb_sequence_converter : public pb_convert { public: typedef google::protobuf::RepeatedPtrField sequence_type; pb_sequence_converter() - : sub_converters_(seq_length) + : sub_converters_() + , seq_id_(0) {} virtual void @@ -146,24 +197,32 @@ class pb_sequence_converter : public pb_convert { sequence_type fields = extract_sequence(dynamic_cast(msg)); typename sequence_type::const_iterator field_it = fields.begin(); - while (out_it != sub_converters_.end() && field_it != fields.end()) { + for ( ; field_it != fields.end(); ++field_it) { + // Try to find a corresponding output converter + for (out_it = sub_converters_.begin(); out_it != sub_converters_.end(); ++out_it) { + if (out_it->corresponds_to(*field_it)) + break; + } + + if (out_it == sub_converters_.end()) { + // No corresponding converter found, create new + sub_converters_.emplace_back(); + out_it = --sub_converters_.end(); + } + if (!out_it->is_open()) - out_it->init(blackboard_, logger_); + out_it->init(blackboard_, logger_, seq_id_++); out_it->handle(*field_it); - - ++out_it; - ++field_it; - } - for ( ; out_it != sub_converters_.end(); ++out_it) { - if (out_it->is_open()) - out_it->close(); } + + sub_converters_.erase(out_it + 1, sub_converters_.end()); } virtual const sequence_type &extract_sequence(const ProtoT &msg); private: std::vector sub_converters_; + size_t seq_id_; }; @@ -239,15 +298,6 @@ class BlackboardManager : }; - struct init_interface { - BlackboardManager *manager; - template - void operator() (bb_iface_manager> &iface_mgr) const { - iface_mgr.init(manager->blackboard, manager); - } - }; - - struct handle_messages { BlackboardManager *manager; From e956e3caaf76f75d0ced244c6e13e2d8eedadd26 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Mon, 19 Feb 2018 16:27:53 +0100 Subject: [PATCH 09/20] protoboard: default operators for converter --- src/plugins/protoboard/blackboard_manager.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h index 81ae2ad380..d79c1d239e 100644 --- a/src/plugins/protoboard/blackboard_manager.h +++ b/src/plugins/protoboard/blackboard_manager.h @@ -64,6 +64,9 @@ class pb_convert : public std::enable_shared_from_this { , logger_(nullptr) {} + pb_convert(const pb_convert &) = default; + pb_convert &operator = (const pb_convert &) = default; + virtual ~pb_convert(); virtual void @@ -170,7 +173,7 @@ class pb_converter : public pb_convert { { throw fawkes::Exception( boost::core::demangle(typeid(pb_converter).name()).c_str(), - "BUG: corresponds(...) must" + "BUG: corresponds(...) must " "be overridden for every converter used in a sequence."); } From dddf79fef151bf88f82b8b1f9ccb199d73e486ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Tue, 1 Oct 2019 23:51:58 +0200 Subject: [PATCH 10/20] protoboard-rcll: factor out domain definition protoboard-rcll is effectively a new plugin, but it's based on the protoboard plugin. Only this time, it contains ONLY the domain-specific template specializations & instantiations. All of the generic (i.e. domain-agnostic) functionality has been moved to a plugin template in the core repo. This compiles, but is COMPLETELY UNTESTED yet. --- src/plugins/Makefile | 3 +- src/plugins/protoboard-rcll/Makefile | 29 ++ src/plugins/protoboard-rcll/plugin.cpp | 61 ++++ .../rcll_receiving.cpp} | 230 +++--------- src/plugins/protoboard-rcll/rcll_sending.cpp | 205 +++++++++++ src/plugins/protoboard/Makefile | 50 --- src/plugins/protoboard/blackboard_manager.cpp | 140 -------- src/plugins/protoboard/blackboard_manager.h | 338 ------------------ .../protoboard/interface_definitions.cpp | 54 --- .../protoboard/interface_definitions.h | 89 ----- src/plugins/protoboard/interfaces/Makefile | 24 -- .../protoboard/interfaces/OrderInterface.xml | 51 --- .../interfaces/PrepareMachineInterface.xml | 84 ----- .../interfaces/ProtobufPeerInterface.xml | 35 -- .../interfaces/RCLLGameStateInterface.xml | 33 -- .../interfaces/RecvBeaconInterface.xml | 24 -- .../interfaces/SendBeaconInterface.xml | 32 -- src/plugins/protoboard/plugin.cpp | 14 - src/plugins/protoboard/plugin.h | 15 - src/plugins/protoboard/protobuf_thread.cpp | 265 -------------- src/plugins/protoboard/protobuf_thread.h | 124 ------- 21 files changed, 356 insertions(+), 1544 deletions(-) create mode 100644 src/plugins/protoboard-rcll/Makefile create mode 100644 src/plugins/protoboard-rcll/plugin.cpp rename src/plugins/{protoboard/bb_msg_handlers.cpp => protoboard-rcll/rcll_receiving.cpp} (56%) create mode 100644 src/plugins/protoboard-rcll/rcll_sending.cpp delete mode 100644 src/plugins/protoboard/Makefile delete mode 100644 src/plugins/protoboard/blackboard_manager.cpp delete mode 100644 src/plugins/protoboard/blackboard_manager.h delete mode 100644 src/plugins/protoboard/interface_definitions.cpp delete mode 100644 src/plugins/protoboard/interface_definitions.h delete mode 100644 src/plugins/protoboard/interfaces/Makefile delete mode 100644 src/plugins/protoboard/interfaces/OrderInterface.xml delete mode 100644 src/plugins/protoboard/interfaces/PrepareMachineInterface.xml delete mode 100644 src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml delete mode 100644 src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml delete mode 100644 src/plugins/protoboard/interfaces/RecvBeaconInterface.xml delete mode 100644 src/plugins/protoboard/interfaces/SendBeaconInterface.xml delete mode 100644 src/plugins/protoboard/plugin.cpp delete mode 100644 src/plugins/protoboard/plugin.h delete mode 100644 src/plugins/protoboard/protobuf_thread.cpp delete mode 100644 src/plugins/protoboard/protobuf_thread.h diff --git a/src/plugins/Makefile b/src/plugins/Makefile index 7150b4c298..be29bc303d 100644 --- a/src/plugins/Makefile +++ b/src/plugins/Makefile @@ -31,8 +31,9 @@ SUBDIRS = clips-motor-switch \ mps-laser-gen \ webtools-bridge \ skiller_motor_state \ + protoboard-rcll \ asp-planner - protoboard \ + include $(BUILDSYSDIR)/rules.mk diff --git a/src/plugins/protoboard-rcll/Makefile b/src/plugins/protoboard-rcll/Makefile new file mode 100644 index 0000000000..2bcf4b6046 --- /dev/null +++ b/src/plugins/protoboard-rcll/Makefile @@ -0,0 +1,29 @@ +#***************************************************************************** +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +#***************************************************************************** + +BASEDIR = ../../.. +include $(BASEDIR)/etc/buildsys/config.mk +include $(BUILDSYSDIR)/protobuf.mk +include $(BUILDSYSDIR)/boost.mk + +PRESUBDIRS = interfaces + +LIBS_protoboard_rcll = fawkesprotoboard llsf_msgs \ + OrderInterface PrepareMachineInterface SendBeaconInterface ProtobufPeerInterface \ + RecvBeaconInterface RCLLGameStateInterface + +CFLAGS += $(CFLAGS_CPP11) + +OBJS_protoboard_rcll = rcll_receiving.o rcll_sending.o plugin.o +OBJS_all = $(OBJS_protoboard_rcll) + +PLUGINS_all = $(PLUGINDIR)/protoboard-rcll.$(SOEXT) +PLUGINS_build = $(PLUGINS_all) + +include $(BUILDSYSDIR)/base.mk diff --git a/src/plugins/protoboard-rcll/plugin.cpp b/src/plugins/protoboard-rcll/plugin.cpp new file mode 100644 index 0000000000..bfc9e378fa --- /dev/null +++ b/src/plugins/protoboard-rcll/plugin.cpp @@ -0,0 +1,61 @@ + +/*************************************************************************** + * Protoboard plugin template instantiation for the LLSF + * + * Copyright 2019 Victor Mataré + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#include +#include +#include +#include + +// This is too templatey for auto-formatting ;-) +// clang-format off + + +namespace protoboard { + +std::vector proto_dirs() +{ return { BASEDIR "/src/libs/llsf_msgs" }; } + +} + + +using ProtoboardPluginRCLL = ProtoboardPlugin < + protoboard::bb_iface_manager < + fawkes::SendBeaconInterface, + protoboard::type_list < + fawkes::SendBeaconInterface::SendBeaconMessage, + fawkes::SendBeaconInterface::SetPeerMessage + > + >, + protoboard::bb_iface_manager < + fawkes::PrepareMachineInterface, + protoboard::type_list < + fawkes::PrepareMachineInterface::PrepareBSMessage, + fawkes::PrepareMachineInterface::PrepareCSMessage, + fawkes::PrepareMachineInterface::PrepareDSMessage, + fawkes::PrepareMachineInterface::PrepareRSMessage, + fawkes::PrepareMachineInterface::PrepareSSMessage, + fawkes::PrepareMachineInterface::SetPeerMessage + > + > +>; + +PLUGIN_DESCRIPTION("ProtoBoard plugin for the RoboCup Logistics League") +EXPORT_PLUGIN(ProtoboardPluginRCLL) + diff --git a/src/plugins/protoboard/bb_msg_handlers.cpp b/src/plugins/protoboard-rcll/rcll_receiving.cpp similarity index 56% rename from src/plugins/protoboard/bb_msg_handlers.cpp rename to src/plugins/protoboard-rcll/rcll_receiving.cpp index c1a491c142..60310515a7 100644 --- a/src/plugins/protoboard/bb_msg_handlers.cpp +++ b/src/plugins/protoboard-rcll/rcll_receiving.cpp @@ -1,48 +1,78 @@ -#include "blackboard_manager.h" + +/*************************************************************************** + * Protoboard plugin template instantiation for the LLSF + * - Mappings for received ProtoBuf messages to blackboard interfaces + * + * Copyright 2019 Victor Mataré + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ #include #include #include - #include -#include #include #include #include #include +#include namespace protoboard { - using namespace fawkes; using namespace std; -using namespace boost::fusion; + +// clang-format off std::unordered_map> make_receiving_interfaces_map() { - return { - make_pair( - "llsf_msgs.OrderInfo", - make_shared< - pb_sequence_converter< - llsf_msgs::OrderInfo, - pb_converter - > - >()), - make_pair("llsf_msgs.BeaconSignal", make_shared>()), - make_pair("llsf_msgs.GameState", make_shared>()), - - // Dummy handler, i.e. discard message - make_pair("llsf_msgs.RobotInfo", make_shared()), - make_pair("llsf_msgs.VersionInfo", make_shared()) - }; + return { + { + "llsf_msgs.OrderInfo", + make_shared< + pb_sequence_converter< + llsf_msgs::OrderInfo, + pb_converter + > + >() + }, + { + "llsf_msgs.BeaconSignal", + make_shared>() + }, + { + "llsf_msgs.GameState", + make_shared>() + }, + + // Dummy handler, i.e. discard message + { "llsf_msgs.RobotInfo", make_shared() }, + { "llsf_msgs.VersionInfo", make_shared() } + }; }; + /**************************************** * OrderInterface * ****************************************/ +template<> +std::string iface_id_for_type() +{ return "/protoboard/order/"; } + static const enum_map base_color_enum_order { {llsf_msgs::BaseColor::BASE_BLACK, OrderInterface::BASE_BLACK}, @@ -124,6 +154,10 @@ pb_converter::handle( * RecvBeaconInterface * ****************************************/ +template<> +std::string iface_id_for_type() +{ return "/protoboard/beacon/"; } + static const enum_map team_enum_recv_beacon { {llsf_msgs::Team::CYAN, RecvBeaconInterface::TEAM_COLOR::CYAN}, @@ -149,20 +183,14 @@ pb_converter::handle( } -template<> -void -BlackboardManager::handle_message( - SendBeaconInterface *iface, - SendBeaconInterface::SetPeerMessage *msg) -{ - iface->set_peer_id(msg->peer_id()); -} - - /**************************************** * GameStateInterface * ****************************************/ +template<> +std::string iface_id_for_type() +{ return "/protoboard/game_state/"; } + static const enum_map game_state_enum { {llsf_msgs::GameState::State::GameState_State_INIT, RCLLGameStateInterface::GameState::INIT}, @@ -198,145 +226,5 @@ pb_converter::handle( } -/**************************************** - * SendBeaconMessage * - ****************************************/ - -static const enum_map -team_enum_beacon { - {llsf_msgs::CYAN, SendBeaconInterface::TEAM_COLOR::CYAN}, - {llsf_msgs::MAGENTA, SendBeaconInterface::TEAM_COLOR::MAGENTA} -}; - - -template<> -void -BlackboardManager::handle_message( - SendBeaconInterface *iface, - SendBeaconInterface::SendBeaconMessage *msg) -{ - std::shared_ptr m = std::make_shared(); - - // Set time here to simplify blackboard interface - llsf_msgs::Time *time = new llsf_msgs::Time(); - time->set_sec(clock->now().get_sec()); - time->set_nsec(clock->now().get_nsec()); - llsf_msgs::Pose2D *pose = new llsf_msgs::Pose2D(); - pose->set_x(msg->translation(0)); - pose->set_y(msg->translation(1)); - pose->set_ori(msg->orientation()); - pose->set_allocated_timestamp(time); - - m->set_allocated_pose(pose); - m->set_allocated_team_name(new std::string(msg->team_name())); - m->set_number(msg->number()); - m->set_allocated_peer_name(new std::string(msg->peer_name())); - m->set_team_color(team_enum_beacon.of(msg->team_color())); - - m->set_allocated_time(new llsf_msgs::Time(*time)); - - // Same for sequence number, good idea to count it centrally, anyways - static google::protobuf::uint64 beacon_seq = 0; - m->set_seq(beacon_seq++); - - message_handler_->send(iface->peer_id(), m); } - - -/**************************************** - * Prepare Messages * - ****************************************/ - -static const enum_map -team_enum_prepare { - {llsf_msgs::CYAN, PrepareMachineInterface::Team::CYAN}, - {llsf_msgs::MAGENTA, PrepareMachineInterface::Team::MAGENTA} -}; - -static const enum_map -base_color_enum { - {llsf_msgs::BaseColor::BASE_BLACK, PrepareMachineInterface::BASE_BLACK}, - {llsf_msgs::BaseColor::BASE_RED, PrepareMachineInterface::BASE_RED}, - {llsf_msgs::BaseColor::BASE_SILVER, PrepareMachineInterface::BASE_SILVER} -}; - -static const enum_map -machine_side_enum { - {llsf_msgs::MachineSide::INPUT, PrepareMachineInterface::MachineSide::INPUT}, - {llsf_msgs::MachineSide::OUTPUT, PrepareMachineInterface::MachineSide::OUTPUT} -}; - -static const enum_map -csop_enum { - {llsf_msgs::CSOp::MOUNT_CAP, PrepareMachineInterface::CSOp::MOUNT_CAP}, - {llsf_msgs::CSOp::RETRIEVE_CAP, PrepareMachineInterface::CSOp::RETRIEVE_CAP} -}; - - -template<> -void -BlackboardManager::handle_message( - PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareBSMessage *msg) -{ - std::shared_ptr m = std::make_shared(); - llsf_msgs::PrepareInstructionBS *instr = new llsf_msgs::PrepareInstructionBS(); - instr->set_color(base_color_enum.of(msg->color())); - instr->set_side(machine_side_enum.of(msg->side())); - m->set_allocated_instruction_bs(instr); - m->set_team_color(team_enum_prepare.of(msg->team_color())); - m->set_machine(msg->machine()); - - message_handler_->send(iface->peer_id(), m); -} - - -template<> -void -BlackboardManager::handle_message( - fawkes::PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareCSMessage *msg) -{ - std::shared_ptr m = std::make_shared(); - llsf_msgs::PrepareInstructionCS *instr = new llsf_msgs::PrepareInstructionCS(); - instr->set_operation(csop_enum.of(msg->operation())); - m->set_allocated_instruction_cs(instr); - m->set_team_color(team_enum_prepare.of(msg->team_color())); - m->set_machine(msg->machine()); - - message_handler_->send(iface->peer_id(), m); -} - -template<> -void -BlackboardManager::handle_message( - fawkes::PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareDSMessage *msg) -{} - -template<> -void -BlackboardManager::handle_message( - fawkes::PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareRSMessage *msg) -{} - -template<> -void -BlackboardManager::handle_message( - fawkes::PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareSSMessage *msg) -{} - -template<> -void -BlackboardManager::handle_message( - fawkes::PrepareMachineInterface *iface, - PrepareMachineInterface::SetPeerMessage *msg) -{} - - - -} // namespace protoboard - diff --git a/src/plugins/protoboard-rcll/rcll_sending.cpp b/src/plugins/protoboard-rcll/rcll_sending.cpp new file mode 100644 index 0000000000..4cd1591006 --- /dev/null +++ b/src/plugins/protoboard-rcll/rcll_sending.cpp @@ -0,0 +1,205 @@ + +/*************************************************************************** + * Protoboard plugin template instantiation for the LLSF + * - Mappings for sending ProtoBuf messages via blackboard interfaces + * + * Copyright 2019 Victor Mataré + ****************************************************************************/ + +/* This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Library General Public License for more details. + * + * Read the full text in the LICENSE.GPL file in the doc directory. + */ + +#include +#include +#include +#include +#include + +namespace protoboard { + +using namespace fawkes; +using namespace std; + +// clang-format off + + + +/**************************************** + * SendBeaconMessage * + ****************************************/ + +template<> +std::string iface_id_for_type() +{ return "/protoboard/send_beacon"; } + +static const enum_map +team_enum_beacon { + {llsf_msgs::CYAN, SendBeaconInterface::TEAM_COLOR::CYAN}, + {llsf_msgs::MAGENTA, SendBeaconInterface::TEAM_COLOR::MAGENTA} +}; + +template<> +void BlackboardManager::handle_message( + SendBeaconInterface *iface, + SendBeaconInterface::SendBeaconMessage *msg) +{ + std::shared_ptr m = std::make_shared(); + + // Set time here to simplify blackboard interface + llsf_msgs::Time *time = new llsf_msgs::Time(); + time->set_sec(clock->now().get_sec()); + time->set_nsec(clock->now().get_nsec()); + + llsf_msgs::Pose2D *pose = new llsf_msgs::Pose2D(); + pose->set_x(msg->translation(0)); + pose->set_y(msg->translation(1)); + pose->set_ori(msg->orientation()); + pose->set_allocated_timestamp(time); + + m->set_allocated_pose(pose); + m->set_allocated_team_name(new std::string(msg->team_name())); + m->set_number(msg->number()); + m->set_allocated_peer_name(new std::string(msg->peer_name())); + m->set_team_color(team_enum_beacon.of(msg->team_color())); + + m->set_allocated_time(new llsf_msgs::Time(*time)); + + // Same for sequence number, good idea to count it centrally, anyways + static google::protobuf::uint64 beacon_seq = 0; + m->set_seq(beacon_seq++); + + message_handler_->send(iface->peer_id(), m); +} + + +template<> +void BlackboardManager::handle_message( + SendBeaconInterface *iface, + SendBeaconInterface::SetPeerMessage *msg) +{ + iface->set_peer_id(msg->peer_id()); +} + + + + +/**************************************** + * Prepare Messages * + ****************************************/ + +template<> +std::string iface_id_for_type() +{ return "/protoboard/prepare_machine"; } + +static const enum_map +team_enum_prepare { + {llsf_msgs::CYAN, PrepareMachineInterface::Team::CYAN}, + {llsf_msgs::MAGENTA, PrepareMachineInterface::Team::MAGENTA} +}; + +static const enum_map +base_color_enum { + {llsf_msgs::BaseColor::BASE_BLACK, PrepareMachineInterface::BASE_BLACK}, + {llsf_msgs::BaseColor::BASE_RED, PrepareMachineInterface::BASE_RED}, + {llsf_msgs::BaseColor::BASE_SILVER, PrepareMachineInterface::BASE_SILVER} +}; + +static const enum_map +machine_side_enum { + {llsf_msgs::MachineSide::INPUT, PrepareMachineInterface::MachineSide::INPUT}, + {llsf_msgs::MachineSide::OUTPUT, PrepareMachineInterface::MachineSide::OUTPUT} +}; + +static const enum_map +csop_enum { + {llsf_msgs::CSOp::MOUNT_CAP, PrepareMachineInterface::CSOp::MOUNT_CAP}, + {llsf_msgs::CSOp::RETRIEVE_CAP, PrepareMachineInterface::CSOp::RETRIEVE_CAP} +}; + + +template<> +void BlackboardManager::handle_message( + PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareBSMessage *msg) +{ + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionBS *instr = new llsf_msgs::PrepareInstructionBS(); + instr->set_color(base_color_enum.of(msg->color())); + instr->set_side(machine_side_enum.of(msg->side())); + m->set_allocated_instruction_bs(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); +} + + +template<> +void +BlackboardManager::handle_message( + PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareCSMessage *msg) +{ + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionCS *instr = new llsf_msgs::PrepareInstructionCS(); + instr->set_operation(csop_enum.of(msg->operation())); + m->set_allocated_instruction_cs(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); +} + + +template<> +void +BlackboardManager::handle_message( + PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareDSMessage *msg) +{ + // TODO +} + + +template<> +void +BlackboardManager::handle_message( + PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareRSMessage *msg) +{ + // TODO +} + + +template<> +void +BlackboardManager::handle_message( + PrepareMachineInterface *iface, + PrepareMachineInterface::PrepareSSMessage *msg) +{ + // TODO +} + + +template<> +void +BlackboardManager::handle_message( + PrepareMachineInterface *iface, + PrepareMachineInterface::SetPeerMessage *msg) +{ + iface->set_peer_id(msg->peer_id()); +} + + + +} diff --git a/src/plugins/protoboard/Makefile b/src/plugins/protoboard/Makefile deleted file mode 100644 index 71dc8ce091..0000000000 --- a/src/plugins/protoboard/Makefile +++ /dev/null @@ -1,50 +0,0 @@ -#***************************************************************************** -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -#***************************************************************************** - -BASEDIR = ../../.. -include $(BASEDIR)/etc/buildsys/config.mk -include $(BUILDSYSDIR)/protobuf.mk -include $(BUILDSYSDIR)/boost.mk - -CFLAGS += $(CFLAGS_CPP11) - -ifeq ($(CC),gcc) - ifneq ($(call gcc_atleast_version,4,8),1) - GCC_TOO_OLD=1 - endif -endif - -PRESUBDIRS = interfaces - -LIBS_protoboard = fawkescore fawkesaspects \ - fawkesblackboard fawkesinterface fawkesconfig \ - fawkes_protobuf_comm llsf_msgs \ - OrderInterface PrepareMachineInterface SendBeaconInterface ProtobufPeerInterface \ - RecvBeaconInterface RCLLGameStateInterface - -OBJS_protoboard = blackboard_manager.o protobuf_thread.o interface_definitions.o plugin.o - -ifneq ($(GCC_TOO_OLD),1) - PLUGINS_all = $(PLUGINDIR)/protoboard.$(SOEXT) -else - WARN_TARGETS += warning_old_gcc -endif - -OBJS_all = $(OBJS_protoboard) - -ifeq ($(OBJSSUBMAKE),1) - ifneq ($(WARN_TARGETS),) -all: $(WARN_TARGETS) - endif -.PHONY: warning_old_gcc -warning_old_gcc: - $(SILENT)echo -e "$(INDENT_PRINT)--> $(TRED)Can't build protoboard plugin$(TNORMAL) (GCC too old, have $(GCC_VERSION), required 4.8)" -endif - -include $(BUILDSYSDIR)/base.mk diff --git a/src/plugins/protoboard/blackboard_manager.cpp b/src/plugins/protoboard/blackboard_manager.cpp deleted file mode 100644 index a8056a544c..0000000000 --- a/src/plugins/protoboard/blackboard_manager.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "blackboard_manager.h" -#include "bb_msg_handlers.cpp" - - -namespace protoboard { - - -using namespace fawkes; - - -pb_convert::~pb_convert() -{} - - -BlackboardManager::BlackboardManager(ProtobufThead *msg_handler) - : Thread("ProtoboardBlackboardManager", Thread::OPMODE_WAITFORWAKEUP) - , message_handler_(msg_handler) - , bb_receiving_interfaces_(make_receiving_interfaces_map()) - , on_message_waker_(nullptr) - , next_peer_idx_(0) -{} - - -void BlackboardManager::init() -{ - peer_iface_ = blackboard->open_for_writing("/protoboard/peers"); - - on_message_waker_ = new fawkes::BlackBoardOnMessageWaker(blackboard, peer_iface_, this); - boost::fusion::for_each(bb_sending_interfaces_, [this] (auto &iface_mgr) { - iface_mgr.init(this->blackboard, this); - }); - - for (pb_conversion_map::value_type &c : bb_receiving_interfaces_) - c.second->init(blackboard, logger); -} - - -void BlackboardManager::finalize() -{ - delete on_message_waker_; - blackboard->close(peer_iface_); -} - - -void BlackboardManager::loop() -{ - // Handle CreatePeer* messages - bool did_something = on_interface { peer_iface_, this } - .handle_msg_types < - ProtobufPeerInterface::CreatePeerMessage, - ProtobufPeerInterface::CreatePeerLocalMessage, - ProtobufPeerInterface::CreatePeerCryptoMessage, - ProtobufPeerInterface::CreatePeerLocalCryptoMessage - > (); - - // Handle sending blackboard interfaces - did_something |= boost::fusion::any(bb_sending_interfaces_, handle_messages { this }); - - // Handle receiving blackboard interfaces - while (message_handler_->pb_queue_incoming()) { - ProtobufThead::incoming_message inc = message_handler_->pb_queue_pop(); - pb_conversion_map::iterator it; - - if ((it = bb_receiving_interfaces_.find(inc.msg->GetTypeName())) == bb_receiving_interfaces_.end()) - logger->log_error(name(), "Received message of unregistered type `%s'", inc.msg->GetTypeName().c_str()); - else try { - it->second->handle(inc.msg); - } catch (std::exception &e) { - logger->log_error(name(), "Exception while handling %s: %s", - inc.msg->GetTypeName().c_str(), e.what()); - } - - did_something = true; - } - - if (!did_something) - // Thread woke up, but nothing was handled - logger->log_warn(name(), "Spurious wakeup. WTF?"); -} - - -void BlackboardManager::add_peer(ProtobufPeerInterface *iface, long peer_id) -{ - // TODO: Properly handle overflow. - iface->set_peers(next_peer_idx_++ % iface->maxlenof_peers(), peer_id); - iface->write(); -} - - -template<> void -BlackboardManager::handle_message( - ProtobufPeerInterface *iface, - ProtobufPeerInterface::CreatePeerMessage *msg) -{ add_peer(iface, message_handler_->peer_create(msg->address(), msg->port())); } - - -template<> void -BlackboardManager::handle_message( - ProtobufPeerInterface *iface, - ProtobufPeerInterface::CreatePeerLocalMessage *msg) -{ - add_peer(iface, - message_handler_->peer_create_local( - msg->address(), - msg->send_to_port(), - msg->recv_on_port())); -} - - -template<> void -BlackboardManager::handle_message( - ProtobufPeerInterface *iface, - ProtobufPeerInterface::CreatePeerCryptoMessage *msg) -{ - add_peer(iface, - message_handler_->peer_create_crypto( - msg->address(), - msg->port(), - msg->crypto_key(), - msg->cipher())); -} - - -template<> void -BlackboardManager::handle_message( - ProtobufPeerInterface *iface, - ProtobufPeerInterface::CreatePeerLocalCryptoMessage *msg) -{ - add_peer(iface, - message_handler_->peer_create_local_crypto( - msg->address(), - msg->send_to_port(), - msg->recv_on_port(), - msg->crypto_key(), - msg->cipher())); -} - - -} // namespace protoboard - diff --git a/src/plugins/protoboard/blackboard_manager.h b/src/plugins/protoboard/blackboard_manager.h deleted file mode 100644 index d79c1d239e..0000000000 --- a/src/plugins/protoboard/blackboard_manager.h +++ /dev/null @@ -1,338 +0,0 @@ -#ifndef BLACKBOARD_MANAGER_H -#define BLACKBOARD_MANAGER_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include - -#include -#include -#include - -#include "interface_definitions.h" -#include "protobuf_thread.h" - - - -namespace protoboard { - - -template -struct InterfaceMessageHandler; - - -class BlackboardManager; - - -template -class enum_map { -public: - typedef boost::bimap bimapT; - constexpr enum_map(std::initializer_list init) - : list(init), - map(list.begin(), list.end()) - {} - - constexpr bbEnumT of(pbEnumT v) const - { return map.left.at(v); } - - constexpr pbEnumT of(bbEnumT v) const - { return map.right.at(v); } - -private: - const std::vector list; - const bimapT map; -}; - - -class pb_convert : public std::enable_shared_from_this { -public: - pb_convert() - : blackboard_(nullptr) - , logger_(nullptr) - {} - - pb_convert(const pb_convert &) = default; - pb_convert &operator = (const pb_convert &) = default; - - virtual ~pb_convert(); - - virtual void - init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger, size_t = 0) - { - blackboard_ = blackboard; - logger_ = logger; - } - - virtual void handle(std::shared_ptr msg) - { handle(*msg); } - - virtual void handle(const google::protobuf::Message &) - {} - -protected: - fawkes::BlackBoard *blackboard_; - fawkes::Logger *logger_; -}; - - -template -class pb_converter : public pb_convert { -public: - typedef ProtoT input_type; - typedef IfaceT output_type; - - pb_converter() - : pb_convert() - , interface_(nullptr) - {} - - // Don't copy this - pb_converter(const pb_converter &) = delete; - pb_converter &operator = (const pb_converter &) = delete; - - // Only move! - pb_converter(pb_converter &&o) - : pb_convert(o) - , interface_(std::move(o.interface_)) - { o.interface_ = nullptr; } - - pb_converter &operator = (pb_converter &&o) - { - pb_convert::operator = (o); - this->interface_ = o.interface_; - o.interface_ = nullptr; - return *this; - } - - - virtual ~pb_converter() - { close(); } - - virtual void - init(fawkes::BlackBoard *blackboard, fawkes::Logger *logger, size_t id = 0) override - { - pb_convert::init(blackboard, logger); - interface_ = blackboard_->open_for_writing( - iface_id_for_type(id).c_str()); - logger->log_info( - boost::core::demangle(typeid(*this).name()).c_str(), - "Initialized %s.", - iface_id_for_type(id).c_str()); - } - - virtual void - handle(const google::protobuf::Message &msg) override - { handle(dynamic_cast(msg)); } - - virtual void handle(const ProtoT &msg) - { - handle(msg, interface_); - interface_->write(); - } - - virtual bool is_open() - { return interface_; } - - virtual void close() - { - if (is_open()) { - blackboard_->close(interface_); - interface_ = nullptr; - } - } - - IfaceT *interface() - { return interface_; } - - virtual bool corresponds_to(const ProtoT &msg) - { - if (is_open()) { - interface()->read(); - return corresponds(msg, interface()); - } - else - return true; - } - -protected: - virtual void handle(const ProtoT &msg, IfaceT *iface); - static bool corresponds(const ProtoT &, const IfaceT *) - { - throw fawkes::Exception( - boost::core::demangle(typeid(pb_converter).name()).c_str(), - "BUG: corresponds(...) must " - "be overridden for every converter used in a sequence."); - } - - -private: - IfaceT *interface_; -}; - - -template -class pb_sequence_converter : public pb_convert { -public: - typedef google::protobuf::RepeatedPtrField sequence_type; - - pb_sequence_converter() - : sub_converters_() - , seq_id_(0) - {} - - virtual void - handle(const google::protobuf::Message &msg) override - { - typename std::vector::iterator out_it = sub_converters_.begin(); - sequence_type fields = extract_sequence(dynamic_cast(msg)); - typename sequence_type::const_iterator field_it = fields.begin(); - - for ( ; field_it != fields.end(); ++field_it) { - // Try to find a corresponding output converter - for (out_it = sub_converters_.begin(); out_it != sub_converters_.end(); ++out_it) { - if (out_it->corresponds_to(*field_it)) - break; - } - - if (out_it == sub_converters_.end()) { - // No corresponding converter found, create new - sub_converters_.emplace_back(); - out_it = --sub_converters_.end(); - } - - if (!out_it->is_open()) - out_it->init(blackboard_, logger_, seq_id_++); - out_it->handle(*field_it); - } - - sub_converters_.erase(out_it + 1, sub_converters_.end()); - } - - virtual const sequence_type &extract_sequence(const ProtoT &msg); - -private: - std::vector sub_converters_; - size_t seq_id_; -}; - - -typedef std::unordered_map> pb_conversion_map; - - -pb_conversion_map make_receiving_interfaces_map(); - - -class BlackboardManager : - public fawkes::Thread, - public fawkes::LoggingAspect, - public fawkes::ConfigurableAspect, - public fawkes::BlackBoardAspect, - public fawkes::ClockAspect -{ -public: - BlackboardManager(ProtobufThead *msg_handler); - -protected: - virtual void init() override; - virtual void finalize() override; - virtual void loop() override; - -private: - void add_peer(fawkes::ProtobufPeerInterface *iface, long peer_id); - - template - bool handle_message_type(InterfaceT *iface) - { - if (!iface->msgq_empty()) { - bool rv = false; - while (MessageT *msg = iface->msgq_first_safe(msg)) { - try { - handle_message(iface, msg); - } catch (std::exception &e) { - logger->log_error(name(), "Exception handling %s on %s: %s", - msg->type(), iface->uid(), e.what()); - } - iface->msgq_pop(); - rv = true; - } - iface->write(); - return rv; - } else - return false; - } - - template - void handle_message(InterfaceT *iface, MessageT *msg); - - - template - struct on_interface { - InterfaceT *iface; - BlackboardManager *manager; - - on_interface(InterfaceT *iface, BlackboardManager *manager) - : iface(iface), manager(manager) - {} - - template - bool handle_msg_types () - { - return manager->handle_message_type(iface); - } - - // This template is disabled if MessageTs is {} to resolve ambiguity - template - typename std::enable_if<(sizeof...(MessageTs) > 0), bool>::type - handle_msg_types () - { return handle_msg_types() || handle_msg_types(); } - }; - - - struct handle_messages { - BlackboardManager *manager; - - template - bool operator() (const bb_iface_manager> &pair) const - { return manager->handle_message_type(pair.interface()); } - - template - bool operator() (const bb_iface_manager> &iface_mgr) const - { - return on_interface{iface_mgr.interface(), manager}.template handle_msg_types() - || manager->handle_message_type(iface_mgr.interface()); - } - }; - - - - ProtobufThead *message_handler_; - fawkes::ProtobufPeerInterface *peer_iface_; - - sending_interfaces bb_sending_interfaces_; - - pb_conversion_map bb_receiving_interfaces_; - - fawkes::BlackBoardOnMessageWaker *on_message_waker_; - - unsigned int next_peer_idx_; - -}; - - -} // namespace protoboard - - -#endif // BLACKBOARD_MANAGER_H diff --git a/src/plugins/protoboard/interface_definitions.cpp b/src/plugins/protoboard/interface_definitions.cpp deleted file mode 100644 index 30b3e75e10..0000000000 --- a/src/plugins/protoboard/interface_definitions.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include - -#include "interface_definitions.h" - -#include -#include -#include - - - -namespace protoboard { - - -using namespace fawkes; -using namespace boost::fusion; - - -std::vector proto_dirs() -{ return { BASEDIR "/src/libs/llsf_msgs" }; } - - - -/* - * Sending interfaces: ID is used as-is - */ - -template<> -std::string iface_id_for_type() -{ return "/protoboard/send_beacon"; } - -template<> -std::string iface_id_for_type() -{ return "/protoboard/prepare_machine"; } - - - -/* - * Receiving interfaces: ID ends with slash before index is appended - */ - -template<> -std::string iface_id_for_type() -{ return "/protoboard/order/"; } - -template<> -std::string iface_id_for_type() -{ return "/protoboard/game_state/"; } - -template<> -std::string iface_id_for_type() -{ return "/protoboard/beacon/"; } - - -} // namespace protoboard diff --git a/src/plugins/protoboard/interface_definitions.h b/src/plugins/protoboard/interface_definitions.h deleted file mode 100644 index 9756b21c09..0000000000 --- a/src/plugins/protoboard/interface_definitions.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef INTERFACE_DEFINITIONS_H -#define INTERFACE_DEFINITIONS_H - -#include - -#include -#include -#include - -#include -#include - - -namespace protoboard { - - -template std::string iface_id_for_type(); - -template std::string iface_id_for_type(size_t idx) -{ return iface_id_for_type() + std::to_string(idx); } - - -template -class bb_iface_manager { -public: - bb_iface_manager() - : interface_(nullptr) - , blackboard_(nullptr) - , waker_(nullptr) - {} - - void init(fawkes::BlackBoard *blackboard, fawkes::Thread *thread) - { - blackboard_ = blackboard; - interface_ = blackboard_->open_for_writing(iface_id_for_type().c_str()); - waker_ = new fawkes::BlackBoardOnMessageWaker(blackboard, interface_, thread); - } - - ~bb_iface_manager() - { - if (blackboard_ && interface_) - blackboard_->close(interface_); - if (waker_) - delete waker_; - } - - IfaceT *interface() const { return interface_; } - -private: - IfaceT *interface_; - fawkes::BlackBoard *blackboard_; - fawkes::BlackBoardOnMessageWaker *waker_; -}; - - -template -struct type_list -{}; - - -typedef std::tuple < - bb_iface_manager < - fawkes::SendBeaconInterface, - type_list < - fawkes::SendBeaconInterface::SendBeaconMessage, - fawkes::SendBeaconInterface::SetPeerMessage - > - >, - bb_iface_manager < - fawkes::PrepareMachineInterface, - type_list < - fawkes::PrepareMachineInterface::PrepareBSMessage, - fawkes::PrepareMachineInterface::PrepareCSMessage, - fawkes::PrepareMachineInterface::PrepareDSMessage, - fawkes::PrepareMachineInterface::PrepareRSMessage, - fawkes::PrepareMachineInterface::PrepareSSMessage, - fawkes::PrepareMachineInterface::SetPeerMessage - > - > -> sending_interfaces; - - -std::vector proto_dirs(); - - -} // namespace protoboard - - -#endif // INTERFACE_DEFINITIONS_H diff --git a/src/plugins/protoboard/interfaces/Makefile b/src/plugins/protoboard/interfaces/Makefile deleted file mode 100644 index 8ba2a86f90..0000000000 --- a/src/plugins/protoboard/interfaces/Makefile +++ /dev/null @@ -1,24 +0,0 @@ -#***************************************************************************** -# Makefile Build System for Fawkes: Robotino interfaces -# ------------------- -# Created on Fri Mar 26 17:25:00 2012 -# Copyright (C) 2006-2012 by Tim Niemueller, AllemaniACs RoboCup Team -# -#***************************************************************************** -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -#***************************************************************************** - -BASEDIR = ../../../.. - -include $(BASEDIR)/etc/buildsys/config.mk - -INTERFACES_all = $(notdir $(patsubst %.xml,%,$(wildcard $(SRCDIR)/*.xml))) -include $(BUILDSYSDIR)/interface.mk - -include $(BUILDSYSDIR)/base.mk - diff --git a/src/plugins/protoboard/interfaces/OrderInterface.xml b/src/plugins/protoboard/interfaces/OrderInterface.xml deleted file mode 100644 index 62b1e3e9da..0000000000 --- a/src/plugins/protoboard/interfaces/OrderInterface.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - What color to output - Red base - Black base - Silver base - - - Available ring colors - Blue ring - Green ring - Orange ring - Yellow ring - - - Available cap colors - Black cap - Grey cap - - - Team color - Cyan team - Magenta team - - - Product complexity - No ring - One ring - Two rings - Three rings - - - - Description of a single order - Order ID - Order complexity - Requested base color - Requested rings - Requested cap color - Delivery window start (seconds of game time) - Delivery window end (seconds of game time) - Which gate must be used at the DS - How many of this type must be delivered - How many were delivered by Cyan - How many were delivered by Magenta - - - diff --git a/src/plugins/protoboard/interfaces/PrepareMachineInterface.xml b/src/plugins/protoboard/interfaces/PrepareMachineInterface.xml deleted file mode 100644 index 2a8da2c3ed..0000000000 --- a/src/plugins/protoboard/interfaces/PrepareMachineInterface.xml +++ /dev/null @@ -1,84 +0,0 @@ - - - - - - Side of an MPS - Output side (even tag ID) - Input side (odd tag ID) - - - What color to output - Red base - Black base - Silver base - - - Available ring colors - Blue ring - Green ring - Orange ring - Yellow ring - - - Available cap colors - Black cap - Grey cap - - - Team color - Cyan team - Magenta team - - - Storage Station operation - Store something - Retrieve something - - - Cap Station operation - Retrieve cap - Mount cap - - - - - No data needed really, just store last msgid - Last sent message ID - Peer ID to send to - - - - Set peer ID that messages should be sent to - Peer ID to use - - - Make BS output specified base element at the specified side - The sending team's color - Machine name - Where the base should come out - What base color should come out - - - The sending team's color - Machine name - Delivery gate - - - The sending team's color - Machine name - Operation to perform - Where to store/retrieve - - - The sending team's color - Machine name - Ring color - - - The sending team's color - Machine name - Operation to perform - - - diff --git a/src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml b/src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml deleted file mode 100644 index dd93d5ac11..0000000000 --- a/src/plugins/protoboard/interfaces/ProtobufPeerInterface.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - Current peers maintained by the protoboard plugin - Currently active peers - - - Create a peer without encryption - IP address or host name - Port to send to/receive on - - - Create a local peer without encryption (mainly for simulation) - IP address or host name - Port to send to - Port to receive on - - - Create a peer with encryption - IP address or host name - Port to send to/receive on - Crypto key - Cipher name - - - Create a local peer with encryption - IP address or host name - Port to send to - Port to receive on - Crypto key - Cipher name - - - diff --git a/src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml b/src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml deleted file mode 100644 index 14f5b757ad..0000000000 --- a/src/plugins/protoboard/interfaces/RCLLGameStateInterface.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - Game state - Just after startup of the refbox - Time not yet running - Game is running - Game was paused - - - Game phase - Game hasn't begun - Setup phase - Exploration phase - Production phase - Game has ended - - - - Full game state - Time in seconds since game start - Nanoseconds part - Current game state - Current game phase - Points awarded to cyan - Name of the cyan team - Points awarded to magenta - Name of the magenta team - - - diff --git a/src/plugins/protoboard/interfaces/RecvBeaconInterface.xml b/src/plugins/protoboard/interfaces/RecvBeaconInterface.xml deleted file mode 100644 index 7cf629e644..0000000000 --- a/src/plugins/protoboard/interfaces/RecvBeaconInterface.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - Team color - Cyan team - Magenta team - - - - - Data from last received BeaconSignal - Seconds since game start - Nanoseconds portion - Jersey number (dummy value if rcv'd from refbox) - Team name (dummy value if rcv'd from refbox) - Peer name - Team color - Pose (unused if rcv'd from refbox) - - - - diff --git a/src/plugins/protoboard/interfaces/SendBeaconInterface.xml b/src/plugins/protoboard/interfaces/SendBeaconInterface.xml deleted file mode 100644 index 5d4ee75ff0..0000000000 --- a/src/plugins/protoboard/interfaces/SendBeaconInterface.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - Team color - Cyan team - Magenta team - - - - - No data needed really, just store last msgid - Last sent message ID - Peer ID to send to - - - - Set peer ID that messages should be sent to - Peer ID to use - - - Send a beacon. Contents of the last message are copied into the relevant data fields. - Robot's number - Team name - The Robot's name - Team color - Robot's position on the field - Robot's orientation (yaw) on the field - - - diff --git a/src/plugins/protoboard/plugin.cpp b/src/plugins/protoboard/plugin.cpp deleted file mode 100644 index 0ef1964b86..0000000000 --- a/src/plugins/protoboard/plugin.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "plugin.h" - -#include "blackboard_manager.h" -#include "protobuf_thread.h" - -ProtoboardPlugin::ProtoboardPlugin(fawkes::Configuration *config) - : Plugin(config) -{ - protoboard::ProtobufThead *msg_handler = new protoboard::ProtobufThead(); - protoboard::BlackboardManager *bb_mgr = new protoboard::BlackboardManager(msg_handler); - msg_handler->set_bb_manager(bb_mgr); - thread_list.push_back(bb_mgr); - thread_list.push_back(msg_handler); -} diff --git a/src/plugins/protoboard/plugin.h b/src/plugins/protoboard/plugin.h deleted file mode 100644 index d369429953..0000000000 --- a/src/plugins/protoboard/plugin.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef PROTOBOARD_PLUGIN_H -#define PROTOBOARD_PLUGIN_H - -#include - - -class ProtoboardPlugin : public fawkes::Plugin { -public: - ProtoboardPlugin(fawkes::Configuration *config); -}; - -PLUGIN_DESCRIPTION("Publish LLSF protobuf messages to blackboard") -EXPORT_PLUGIN(ProtoboardPlugin) - -#endif // PROTOBOARD_PLUGIN_H diff --git a/src/plugins/protoboard/protobuf_thread.cpp b/src/plugins/protoboard/protobuf_thread.cpp deleted file mode 100644 index 92ccd163f2..0000000000 --- a/src/plugins/protoboard/protobuf_thread.cpp +++ /dev/null @@ -1,265 +0,0 @@ -#include - -#include -#include -#include - -#include - -#include "protobuf_thread.h" -#include "blackboard_manager.h" - - -using namespace google::protobuf; -using namespace protobuf_comm; - - -namespace protoboard { - - -ProtobufThead::ProtobufThead() - : Thread("ProtoboardMessageHandler", Thread::OPMODE_CONTINUOUS) - , bb_manager_(nullptr) - , message_register_(nullptr) - , server_(nullptr) - , next_client_id_(0) -{} - - -ProtobufThead::~ProtobufThead() -{ - delete message_register_; - delete server_; -} - - -void ProtobufThead::init() -{ - message_register_ = new MessageRegister(proto_dirs()); - - if (!bb_manager_) - throw fawkes::Exception("BUG: %s's reference to blackboard manager thread hasn't been initialized", name()); -} - - - -bool ProtobufThead::pb_queue_incoming() -{ - fawkes::MutexLocker lock(&msgq_mutex_); - return !pb_queue_.empty(); -} - - -ProtobufThead::incoming_message ProtobufThead::pb_queue_pop() -{ - fawkes::MutexLocker lock(&msgq_mutex_); - incoming_message msg = pb_queue_.front(); - pb_queue_.pop(); - return msg; -} - - -/** Enable protobuf peer. - * @param address IP address to send messages to - * @param send_port UDP port to send messages to - * @param recv_port UDP port to receive messages on, 0 to use the same as the @p send_port - * @param crypto_key encryption key - * @param cipher cipher suite, see BufferEncryptor for supported types - * @return peer identifier - */ -long int -ProtobufThead::peer_create_local_crypto(const std::string &address, int send_to_port, int recv_on_port, - const std::string &crypto_key, const std::string &cipher) -{ - if (recv_on_port <= 0) recv_on_port = send_to_port; - - if (send_to_port > 0) { - protobuf_comm::ProtobufBroadcastPeer *peer = - new protobuf_comm::ProtobufBroadcastPeer( - address, send_to_port, recv_on_port, - message_register_, crypto_key, cipher); - - long int peer_id; - { - fawkes::MutexLocker lock(&map_mutex_); - peer_id = ++next_client_id_; - peers_[peer_id] = peer; - } - - peer->signal_received() - .connect(boost::bind(&ProtobufThead::handle_peer_msg, this, peer_id, _1, _2, _3, _4)); - peer->signal_recv_error() - .connect(boost::bind(&ProtobufThead::handle_peer_recv_error, this, peer_id, _1, _2)); - peer->signal_send_error() - .connect(boost::bind(&ProtobufThead::handle_peer_send_error, this, peer_id, _1)); - - return peer_id; - } else { - return 0; - } -} - - -/** Enable protobuf peer. - * @param address IP address to send messages to - * @param port UDP port to send and receive messages - * @param crypto_key encryption key - * @param cipher cipher suite, see BufferEncryptor for supported types - * @return peer identifier - */ -long int -ProtobufThead::peer_create_crypto(const std::string &address, int port, - const std::string &crypto_key, const std::string &cipher) -{ - return peer_create_local_crypto(address, port, port, crypto_key, cipher); -} - - -/** Enable protobuf peer. - * @param address IP address to send messages to - * @param port UDP port to send and receive messages - * @return peer identifier - */ -long int -ProtobufThead::peer_create(const std::string &address, int port) -{ - return peer_create_local_crypto(address, port, port); -} - -/** Enable protobuf peer. - * @param address IP address to send messages to - * @param send_port UDP port to send messages to - * @param recv_port UDP port to receive messages on, 0 to use the same as the @p send_port - * @return peer identifier - */ -long int -ProtobufThead::peer_create_local(const std::string &address, int send_to_port, - int recv_on_port) -{ - return peer_create_local_crypto(address, send_to_port, recv_on_port); -} - - -/** Disable peer. - * @param peer_id ID of the peer to destroy - */ -void -ProtobufThead::peer_destroy(long int peer_id) -{ - if (peers_.find(peer_id) != peers_.end()) { - delete peers_[peer_id]; - peers_.erase(peer_id); - } -} - - -/** Setup crypto for peer. - * @param peer_id ID of the peer to destroy - * @param crypto_key encryption key - * @param cipher cipher suite, see BufferEncryptor for supported types - */ -void -ProtobufThead::peer_setup_crypto(long int peer_id, - const std::string &crypto_key, - const std::string &cipher) -{ - if (peers_.find(peer_id) != peers_.end()) { - peers_[peer_id]->setup_crypto(crypto_key, cipher); - } -} - - -void -ProtobufThead::send(long int peer_id, - std::shared_ptr m) -{ - if (!m) { - if (logger) { - logger->log_warn(name(), "Cannot send broadcast: invalid message"); - } - return; - } - - fawkes::MutexLocker lock(&map_mutex_); - if (peers_.find(peer_id) == peers_.end()) return; - - //logger->log_info(name(), "Broadcasting %s", (*m)->GetTypeName().c_str()); - try { - peers_[peer_id]->send(m); - } catch (google::protobuf::FatalException &e) { - if (logger) { - logger->log_warn(name(), - "Failed to broadcast message of type %s: %s", - m->GetTypeName().c_str(), e.what()); - } - } catch (fawkes::Exception &e) { - if (logger) { - logger->log_warn(name(), - "Failed to broadcast message of type %s: %s", - m->GetTypeName().c_str(), e.what_no_backtrace()); - } - } catch (std::runtime_error &e) { - if (logger) { - logger->log_warn(name(), - "Failed to broadcast message of type %s: %s", - m->GetTypeName().c_str(), e.what()); - } - } -} - - -/** Handle message that came from a peer/robot - * @param endpoint the endpoint from which the message was received - * @param component_id component the message was addressed to - * @param msg_type type of the message - * @param msg the message - */ -void -ProtobufThead::handle_peer_msg(long int peer_id, - boost::asio::ip::udp::endpoint &endpoint, - uint16_t component_id, uint16_t msg_type, - std::shared_ptr msg) -{ - fawkes::MutexLocker lock(&msgq_mutex_); - pb_queue_.push({ - peer_id, - endpoint, - component_id, - msg_type, - std::move(msg) - }); - bb_manager_->wakeup(); -} - - -/** Handle error during peer message processing. - * @param endpoint endpoint of incoming message - * @param msg error message - */ -void -ProtobufThead::handle_peer_recv_error(long int peer_id, - boost::asio::ip::udp::endpoint &endpoint, std::string msg) -{ - if (logger) { - logger->log_warn(name(), - "Failed to receive peer message from %s:%u: %s", - endpoint.address().to_string().c_str(), endpoint.port(), - msg.c_str()); - } -} - - -/** Handle error during peer message processing. - * @param msg error message - */ -void -ProtobufThead::handle_peer_send_error(long int peer_id, std::string msg) -{ - if (logger) { - logger->log_warn(name(), - "Failed to send peer message: %s", msg.c_str()); - } -} - - -} // namespace protoboard; diff --git a/src/plugins/protoboard/protobuf_thread.h b/src/plugins/protoboard/protobuf_thread.h deleted file mode 100644 index 0fdb616868..0000000000 --- a/src/plugins/protoboard/protobuf_thread.h +++ /dev/null @@ -1,124 +0,0 @@ -#ifndef MESSAGE_HANDLER_H -#define MESSAGE_HANDLER_H - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#define CFG_PREFIX "/plugins/protoboard" - -namespace protobuf_comm { - class ProtobufStreamClient; - class ProtobufBroadcastPeer; -} - - -namespace protoboard { - - -class BlackboardManager; - - -class ProtobufThead : - public fawkes::Thread, - public fawkes::LoggingAspect, - public fawkes::ConfigurableAspect -{ -public: - ProtobufThead(); - virtual ~ProtobufThead() override; - - bool pb_queue_incoming(); - - struct incoming_message { - long int peer_id; - boost::asio::ip::udp::endpoint endpoint; - uint16_t component_id; - uint16_t msg_type; - std::shared_ptr msg; - }; - - incoming_message pb_queue_pop(); - - long int peer_create(const std::string &host, int port); - long int peer_create_local(const std::string &host, - int send_to_port, int recv_on_port); - long int peer_create_crypto(const std::string &host, int port, - const std::string &crypto_key = "", const std::string &cipher = ""); - long int peer_create_local_crypto(const std::string &host, - int send_to_port, int recv_on_port, - const std::string &crypto_key = "", const std::string &cipher = ""); - void peer_destroy(long int peer_id); - - void send(long int peer_id, std::shared_ptr msg); - - /** Get the communicator's message register. - * @return message register */ - protobuf_comm::MessageRegister & message_register() - { return *message_register_; } - - void set_bb_manager(BlackboardManager *bb_manager) - { bb_manager_ = bb_manager; } - -protected: - virtual void init() override; - -private: - /** Get protobuf_comm peers. - * @return protobuf_comm peer */ - const std::map & - peers() const - { return peers_; } - - BlackboardManager *bb_manager_; - - /** Signal invoked for a message that has been sent via broadcast. - * @return signal - */ - boost::signals2::signal)> & - signal_peer_sent() { return sig_peer_sent_; } - - - void peer_setup_crypto(long int peer_id, const std::string &crypto_key, const std::string &cipher); - - - void handle_peer_msg(long int peer_id, - boost::asio::ip::udp::endpoint &endpoint, - uint16_t component_id, uint16_t msg_type, - std::shared_ptr msg); - void wait_for_first_ga(long int peer_id, - boost::asio::ip::udp::endpoint &endpoint, - uint16_t component_id, uint16_t msg_type, - std::shared_ptr msg); - void handle_peer_recv_error(long int peer_id, boost::asio::ip::udp::endpoint &endpoint, std::string msg); - void handle_peer_send_error(long int peer_id, std::string msg); - - protobuf_comm::MessageRegister *message_register_; - protobuf_comm::ProtobufStreamServer *server_; - - boost::signals2::signal)> sig_peer_sent_; - - fawkes::Mutex map_mutex_; - fawkes::Mutex msgq_mutex_; - long int next_client_id_; - - std::map peers_; - - std::queue pb_queue_; - -}; - - -} // namespace protoboard - - -#endif // MESSAGE_HANDLER_H From afc01069ca009340775851c754dc391bc5bd52b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Wed, 2 Oct 2019 01:52:18 +0200 Subject: [PATCH 11/20] protoboard-rcll: allow clang-format Well, the result isn't exactly beautiful, but it's acceptable for the sake of consistency. --- src/plugins/protoboard-rcll/plugin.cpp | 47 ++- .../protoboard-rcll/rcll_receiving.cpp | 267 +++++++++--------- src/plugins/protoboard-rcll/rcll_sending.cpp | 201 ++++++------- 3 files changed, 233 insertions(+), 282 deletions(-) diff --git a/src/plugins/protoboard-rcll/plugin.cpp b/src/plugins/protoboard-rcll/plugin.cpp index bfc9e378fa..b4f4ff5e59 100644 --- a/src/plugins/protoboard-rcll/plugin.cpp +++ b/src/plugins/protoboard-rcll/plugin.cpp @@ -23,39 +23,28 @@ #include #include -// This is too templatey for auto-formatting ;-) -// clang-format off - - namespace protoboard { -std::vector proto_dirs() -{ return { BASEDIR "/src/libs/llsf_msgs" }; } - +std::vector +proto_dirs() +{ + return {BASEDIR "/src/libs/llsf_msgs"}; } - -using ProtoboardPluginRCLL = ProtoboardPlugin < - protoboard::bb_iface_manager < - fawkes::SendBeaconInterface, - protoboard::type_list < - fawkes::SendBeaconInterface::SendBeaconMessage, - fawkes::SendBeaconInterface::SetPeerMessage - > - >, - protoboard::bb_iface_manager < - fawkes::PrepareMachineInterface, - protoboard::type_list < - fawkes::PrepareMachineInterface::PrepareBSMessage, - fawkes::PrepareMachineInterface::PrepareCSMessage, - fawkes::PrepareMachineInterface::PrepareDSMessage, - fawkes::PrepareMachineInterface::PrepareRSMessage, - fawkes::PrepareMachineInterface::PrepareSSMessage, - fawkes::PrepareMachineInterface::SetPeerMessage - > - > ->; +} // namespace protoboard + +using ProtoboardPluginRCLL = ProtoboardPlugin< + protoboard::bb_iface_manager>, + protoboard::bb_iface_manager< + fawkes::PrepareMachineInterface, + protoboard::type_list>>; PLUGIN_DESCRIPTION("ProtoBoard plugin for the RoboCup Logistics League") EXPORT_PLUGIN(ProtoboardPluginRCLL) - diff --git a/src/plugins/protoboard-rcll/rcll_receiving.cpp b/src/plugins/protoboard-rcll/rcll_receiving.cpp index 60310515a7..147eb09110 100644 --- a/src/plugins/protoboard-rcll/rcll_receiving.cpp +++ b/src/plugins/protoboard-rcll/rcll_receiving.cpp @@ -34,197 +34,184 @@ namespace protoboard { using namespace fawkes; using namespace std; -// clang-format off - - -std::unordered_map> make_receiving_interfaces_map() { - return { - { - "llsf_msgs.OrderInfo", - make_shared< - pb_sequence_converter< - llsf_msgs::OrderInfo, - pb_converter - > - >() - }, - { - "llsf_msgs.BeaconSignal", - make_shared>() - }, - { - "llsf_msgs.GameState", - make_shared>() - }, - - // Dummy handler, i.e. discard message - { "llsf_msgs.RobotInfo", make_shared() }, - { "llsf_msgs.VersionInfo", make_shared() } - }; +std::unordered_map> +make_receiving_interfaces_map() +{ + return {{"llsf_msgs.OrderInfo", + make_shared>>()}, + {"llsf_msgs.BeaconSignal", + make_shared>()}, + {"llsf_msgs.GameState", + make_shared>()}, + + // Dummy handler, i.e. discard message + {"llsf_msgs.RobotInfo", make_shared()}, + {"llsf_msgs.VersionInfo", make_shared()}}; }; - - /**************************************** * OrderInterface * ****************************************/ -template<> -std::string iface_id_for_type() -{ return "/protoboard/order/"; } +template <> +std::string +iface_id_for_type() +{ + return "/protoboard/order/"; +} -static const enum_map -base_color_enum_order { +static const enum_map base_color_enum_order{ {llsf_msgs::BaseColor::BASE_BLACK, OrderInterface::BASE_BLACK}, {llsf_msgs::BaseColor::BASE_RED, OrderInterface::BASE_RED}, - {llsf_msgs::BaseColor::BASE_SILVER, OrderInterface::BASE_SILVER} -}; + {llsf_msgs::BaseColor::BASE_SILVER, OrderInterface::BASE_SILVER}}; -static const enum_map -ring_color_enum_order { +static const enum_map ring_color_enum_order{ {llsf_msgs::RingColor::RING_BLUE, OrderInterface::RING_BLUE}, {llsf_msgs::RingColor::RING_GREEN, OrderInterface::RING_GREEN}, {llsf_msgs::RingColor::RING_ORANGE, OrderInterface::RING_ORANGE}, {llsf_msgs::RingColor::RING_YELLOW, OrderInterface::RING_YELLOW}, }; -static const enum_map -cap_color_enum_order { +static const enum_map cap_color_enum_order{ {llsf_msgs::CapColor::CAP_BLACK, OrderInterface::CAP_BLACK}, {llsf_msgs::CapColor::CAP_GREY, OrderInterface::CAP_GREY}, }; static const enum_map -complexity_enum_order { - {llsf_msgs::Order::Complexity::Order_Complexity_C0, OrderInterface::C0}, - {llsf_msgs::Order::Complexity::Order_Complexity_C1, OrderInterface::C1}, - {llsf_msgs::Order::Complexity::Order_Complexity_C2, OrderInterface::C2}, - {llsf_msgs::Order::Complexity::Order_Complexity_C3, OrderInterface::C3} -}; - + complexity_enum_order{{llsf_msgs::Order::Complexity::Order_Complexity_C0, OrderInterface::C0}, + {llsf_msgs::Order::Complexity::Order_Complexity_C1, OrderInterface::C1}, + {llsf_msgs::Order::Complexity::Order_Complexity_C2, OrderInterface::C2}, + {llsf_msgs::Order::Complexity::Order_Complexity_C3, OrderInterface::C3}}; -template<> +template <> const google::protobuf::RepeatedPtrField & -pb_sequence_converter>::extract_sequence( - const llsf_msgs::OrderInfo &msg) -{ return msg.orders(); } - +pb_sequence_converter>:: + extract_sequence(const llsf_msgs::OrderInfo &msg) +{ + return msg.orders(); +} -template<> +template <> bool -pb_converter::corresponds(const llsf_msgs::Order &msg, const OrderInterface *iface) -{ return iface->order_id() == msg.id(); } - +pb_converter::corresponds(const llsf_msgs::Order &msg, + const OrderInterface * iface) +{ + return iface->order_id() == msg.id(); +} -template<> +template <> void -pb_converter::handle( - const llsf_msgs::Order &msg, - OrderInterface *iface) +pb_converter::handle(const llsf_msgs::Order &msg, + OrderInterface * iface) { - if (iface->base_color() != base_color_enum_order.of(msg.base_color())) - iface->set_base_color(base_color_enum_order.of(msg.base_color())); - if (iface->cap_color() != cap_color_enum_order.of(msg.cap_color())) - iface->set_cap_color(cap_color_enum_order.of(msg.cap_color())); - if (iface->complexity() != complexity_enum_order.of(msg.complexity())) - iface->set_complexity(complexity_enum_order.of(msg.complexity())); - - for (int i = 0; i < msg.ring_colors_size(); ++i) - if (iface->ring_colors(static_cast(i)) != ring_color_enum_order.of(msg.ring_colors(i))) - iface->set_ring_colors(static_cast(i), ring_color_enum_order.of(msg.ring_colors(i))); - - if (iface->order_id() != msg.id()) - iface->set_order_id(msg.id()); - if (iface->delivery_period_begin() != msg.delivery_period_begin()) - iface->set_delivery_period_begin(msg.delivery_period_begin()); - if (iface->delivery_period_end() != msg.delivery_period_end()) - iface->set_delivery_period_end(msg.delivery_period_end()); - if (iface->delivery_gate() != msg.delivery_gate()) - iface->set_delivery_gate(msg.delivery_gate()); - if (iface->quantity_delivered_cyan() != msg.quantity_delivered_cyan()) - iface->set_quantity_delivered_cyan(msg.quantity_delivered_cyan()); - if (iface->quantity_delivered_magenta() != msg.quantity_delivered_magenta()) - iface->set_quantity_delivered_magenta(msg.quantity_delivered_magenta()); - if (iface->quantity_requested() != msg.quantity_requested()) - iface->set_quantity_requested(msg.quantity_requested()); + if (iface->base_color() != base_color_enum_order.of(msg.base_color())) + iface->set_base_color(base_color_enum_order.of(msg.base_color())); + if (iface->cap_color() != cap_color_enum_order.of(msg.cap_color())) + iface->set_cap_color(cap_color_enum_order.of(msg.cap_color())); + if (iface->complexity() != complexity_enum_order.of(msg.complexity())) + iface->set_complexity(complexity_enum_order.of(msg.complexity())); + + for (int i = 0; i < msg.ring_colors_size(); ++i) + if (iface->ring_colors(static_cast(i)) + != ring_color_enum_order.of(msg.ring_colors(i))) + iface->set_ring_colors(static_cast(i), + ring_color_enum_order.of(msg.ring_colors(i))); + + if (iface->order_id() != msg.id()) + iface->set_order_id(msg.id()); + if (iface->delivery_period_begin() != msg.delivery_period_begin()) + iface->set_delivery_period_begin(msg.delivery_period_begin()); + if (iface->delivery_period_end() != msg.delivery_period_end()) + iface->set_delivery_period_end(msg.delivery_period_end()); + if (iface->delivery_gate() != msg.delivery_gate()) + iface->set_delivery_gate(msg.delivery_gate()); + if (iface->quantity_delivered_cyan() != msg.quantity_delivered_cyan()) + iface->set_quantity_delivered_cyan(msg.quantity_delivered_cyan()); + if (iface->quantity_delivered_magenta() != msg.quantity_delivered_magenta()) + iface->set_quantity_delivered_magenta(msg.quantity_delivered_magenta()); + if (iface->quantity_requested() != msg.quantity_requested()) + iface->set_quantity_requested(msg.quantity_requested()); }; - /**************************************** * RecvBeaconInterface * ****************************************/ -template<> -std::string iface_id_for_type() -{ return "/protoboard/beacon/"; } +template <> +std::string +iface_id_for_type() +{ + return "/protoboard/beacon/"; +} -static const enum_map -team_enum_recv_beacon { +static const enum_map team_enum_recv_beacon{ {llsf_msgs::Team::CYAN, RecvBeaconInterface::TEAM_COLOR::CYAN}, - {llsf_msgs::Team::MAGENTA, RecvBeaconInterface::TEAM_COLOR::MAGENTA} -}; - + {llsf_msgs::Team::MAGENTA, RecvBeaconInterface::TEAM_COLOR::MAGENTA}}; -template<> +template <> void pb_converter::handle( - const llsf_msgs::BeaconSignal &msg, - RecvBeaconInterface *iface) + const llsf_msgs::BeaconSignal &msg, + RecvBeaconInterface * iface) { - iface->set_number(msg.number()); - iface->set_peer_name(msg.peer_name().substr(0, iface->maxlenof_peer_name()).c_str()); - iface->set_pose(0, msg.pose().x()); - iface->set_pose(1, msg.pose().y()); - iface->set_pose(2, msg.pose().ori()); - iface->set_team_color(team_enum_recv_beacon.of(msg.team_color())); - iface->set_team_name(msg.team_name().substr(0, iface->maxlenof_team_name()).c_str()); - iface->set_time_nsec(msg.time().nsec()); - iface->set_time_sec(msg.time().sec()); + iface->set_number(msg.number()); + iface->set_peer_name(msg.peer_name().substr(0, iface->maxlenof_peer_name()).c_str()); + iface->set_pose(0, msg.pose().x()); + iface->set_pose(1, msg.pose().y()); + iface->set_pose(2, msg.pose().ori()); + iface->set_team_color(team_enum_recv_beacon.of(msg.team_color())); + iface->set_team_name(msg.team_name().substr(0, iface->maxlenof_team_name()).c_str()); + iface->set_time_nsec(msg.time().nsec()); + iface->set_time_sec(msg.time().sec()); } - /**************************************** * GameStateInterface * ****************************************/ -template<> -std::string iface_id_for_type() -{ return "/protoboard/game_state/"; } +template <> +std::string +iface_id_for_type() +{ + return "/protoboard/game_state/"; +} static const enum_map -game_state_enum { - {llsf_msgs::GameState::State::GameState_State_INIT, RCLLGameStateInterface::GameState::INIT}, - {llsf_msgs::GameState::State::GameState_State_PAUSED, RCLLGameStateInterface::GameState::PAUSED}, - {llsf_msgs::GameState::State::GameState_State_RUNNING, RCLLGameStateInterface::GameState::RUNNING}, - {llsf_msgs::GameState::State::GameState_State_WAIT_START, RCLLGameStateInterface::GameState::WAIT_START} -}; + game_state_enum{{llsf_msgs::GameState::State::GameState_State_INIT, + RCLLGameStateInterface::GameState::INIT}, + {llsf_msgs::GameState::State::GameState_State_PAUSED, + RCLLGameStateInterface::GameState::PAUSED}, + {llsf_msgs::GameState::State::GameState_State_RUNNING, + RCLLGameStateInterface::GameState::RUNNING}, + {llsf_msgs::GameState::State::GameState_State_WAIT_START, + RCLLGameStateInterface::GameState::WAIT_START}}; static const enum_map -game_phase_enum { - {llsf_msgs::GameState::Phase::GameState_Phase_EXPLORATION, RCLLGameStateInterface::GamePhase::EXPLORATION}, - {llsf_msgs::GameState::Phase::GameState_Phase_POST_GAME, RCLLGameStateInterface::GamePhase::POST_GAME}, - {llsf_msgs::GameState::Phase::GameState_Phase_PRE_GAME, RCLLGameStateInterface::GamePhase::PRE_GAME}, - {llsf_msgs::GameState::Phase::GameState_Phase_PRODUCTION, RCLLGameStateInterface::GamePhase::PRODUCTION}, - {llsf_msgs::GameState::Phase::GameState_Phase_SETUP, RCLLGameStateInterface::GamePhase::SETUP} -}; - - -template<> + game_phase_enum{{llsf_msgs::GameState::Phase::GameState_Phase_EXPLORATION, + RCLLGameStateInterface::GamePhase::EXPLORATION}, + {llsf_msgs::GameState::Phase::GameState_Phase_POST_GAME, + RCLLGameStateInterface::GamePhase::POST_GAME}, + {llsf_msgs::GameState::Phase::GameState_Phase_PRE_GAME, + RCLLGameStateInterface::GamePhase::PRE_GAME}, + {llsf_msgs::GameState::Phase::GameState_Phase_PRODUCTION, + RCLLGameStateInterface::GamePhase::PRODUCTION}, + {llsf_msgs::GameState::Phase::GameState_Phase_SETUP, + RCLLGameStateInterface::GamePhase::SETUP}}; + +template <> void -pb_converter::handle( - const llsf_msgs::GameState &msg, - RCLLGameStateInterface *iface) +pb_converter::handle(const llsf_msgs::GameState &msg, + RCLLGameStateInterface *iface) { - iface->set_game_time_nsec(msg.game_time().nsec()); - iface->set_game_time_sec(msg.game_time().sec()); - iface->set_phase(game_phase_enum.of(msg.phase())); - iface->set_points_cyan(msg.points_cyan()); - iface->set_points_magenta(msg.points_magenta()); - iface->set_state(game_state_enum.of(msg.state())); - iface->set_team_cyan(msg.team_cyan().substr(0, iface->maxlenof_team_cyan()).c_str()); - iface->set_team_magenta(msg.team_magenta().substr(0, iface->maxlenof_team_magenta()).c_str()); + iface->set_game_time_nsec(msg.game_time().nsec()); + iface->set_game_time_sec(msg.game_time().sec()); + iface->set_phase(game_phase_enum.of(msg.phase())); + iface->set_points_cyan(msg.points_cyan()); + iface->set_points_magenta(msg.points_magenta()); + iface->set_state(game_state_enum.of(msg.state())); + iface->set_team_cyan(msg.team_cyan().substr(0, iface->maxlenof_team_cyan()).c_str()); + iface->set_team_magenta(msg.team_magenta().substr(0, iface->maxlenof_team_magenta()).c_str()); } - - -} +} // namespace protoboard diff --git a/src/plugins/protoboard-rcll/rcll_sending.cpp b/src/plugins/protoboard-rcll/rcll_sending.cpp index 4cd1591006..0db6ee8ca2 100644 --- a/src/plugins/protoboard-rcll/rcll_sending.cpp +++ b/src/plugins/protoboard-rcll/rcll_sending.cpp @@ -30,176 +30,151 @@ namespace protoboard { using namespace fawkes; using namespace std; -// clang-format off - - - /**************************************** * SendBeaconMessage * ****************************************/ -template<> -std::string iface_id_for_type() -{ return "/protoboard/send_beacon"; } +template <> +std::string +iface_id_for_type() +{ + return "/protoboard/send_beacon"; +} -static const enum_map -team_enum_beacon { +static const enum_map team_enum_beacon{ {llsf_msgs::CYAN, SendBeaconInterface::TEAM_COLOR::CYAN}, - {llsf_msgs::MAGENTA, SendBeaconInterface::TEAM_COLOR::MAGENTA} -}; + {llsf_msgs::MAGENTA, SendBeaconInterface::TEAM_COLOR::MAGENTA}}; -template<> -void BlackboardManager::handle_message( - SendBeaconInterface *iface, - SendBeaconInterface::SendBeaconMessage *msg) +template <> +void +BlackboardManager::handle_message(SendBeaconInterface * iface, + SendBeaconInterface::SendBeaconMessage *msg) { - std::shared_ptr m = std::make_shared(); + std::shared_ptr m = std::make_shared(); - // Set time here to simplify blackboard interface - llsf_msgs::Time *time = new llsf_msgs::Time(); - time->set_sec(clock->now().get_sec()); - time->set_nsec(clock->now().get_nsec()); + // Set time here to simplify blackboard interface + llsf_msgs::Time *time = new llsf_msgs::Time(); + time->set_sec(clock->now().get_sec()); + time->set_nsec(clock->now().get_nsec()); - llsf_msgs::Pose2D *pose = new llsf_msgs::Pose2D(); - pose->set_x(msg->translation(0)); - pose->set_y(msg->translation(1)); - pose->set_ori(msg->orientation()); - pose->set_allocated_timestamp(time); + llsf_msgs::Pose2D *pose = new llsf_msgs::Pose2D(); + pose->set_x(msg->translation(0)); + pose->set_y(msg->translation(1)); + pose->set_ori(msg->orientation()); + pose->set_allocated_timestamp(time); - m->set_allocated_pose(pose); - m->set_allocated_team_name(new std::string(msg->team_name())); - m->set_number(msg->number()); - m->set_allocated_peer_name(new std::string(msg->peer_name())); - m->set_team_color(team_enum_beacon.of(msg->team_color())); + m->set_allocated_pose(pose); + m->set_allocated_team_name(new std::string(msg->team_name())); + m->set_number(msg->number()); + m->set_allocated_peer_name(new std::string(msg->peer_name())); + m->set_team_color(team_enum_beacon.of(msg->team_color())); - m->set_allocated_time(new llsf_msgs::Time(*time)); + m->set_allocated_time(new llsf_msgs::Time(*time)); - // Same for sequence number, good idea to count it centrally, anyways - static google::protobuf::uint64 beacon_seq = 0; - m->set_seq(beacon_seq++); + // Same for sequence number, good idea to count it centrally, anyways + static google::protobuf::uint64 beacon_seq = 0; + m->set_seq(beacon_seq++); - message_handler_->send(iface->peer_id(), m); + message_handler_->send(iface->peer_id(), m); } - -template<> -void BlackboardManager::handle_message( - SendBeaconInterface *iface, - SendBeaconInterface::SetPeerMessage *msg) +template <> +void +BlackboardManager::handle_message(SendBeaconInterface * iface, + SendBeaconInterface::SetPeerMessage *msg) { - iface->set_peer_id(msg->peer_id()); + iface->set_peer_id(msg->peer_id()); } - - - /**************************************** * Prepare Messages * ****************************************/ -template<> -std::string iface_id_for_type() -{ return "/protoboard/prepare_machine"; } +template <> +std::string +iface_id_for_type() +{ + return "/protoboard/prepare_machine"; +} -static const enum_map -team_enum_prepare { +static const enum_map team_enum_prepare{ {llsf_msgs::CYAN, PrepareMachineInterface::Team::CYAN}, - {llsf_msgs::MAGENTA, PrepareMachineInterface::Team::MAGENTA} -}; + {llsf_msgs::MAGENTA, PrepareMachineInterface::Team::MAGENTA}}; -static const enum_map -base_color_enum { +static const enum_map base_color_enum{ {llsf_msgs::BaseColor::BASE_BLACK, PrepareMachineInterface::BASE_BLACK}, {llsf_msgs::BaseColor::BASE_RED, PrepareMachineInterface::BASE_RED}, - {llsf_msgs::BaseColor::BASE_SILVER, PrepareMachineInterface::BASE_SILVER} -}; + {llsf_msgs::BaseColor::BASE_SILVER, PrepareMachineInterface::BASE_SILVER}}; static const enum_map -machine_side_enum { - {llsf_msgs::MachineSide::INPUT, PrepareMachineInterface::MachineSide::INPUT}, - {llsf_msgs::MachineSide::OUTPUT, PrepareMachineInterface::MachineSide::OUTPUT} -}; + machine_side_enum{{llsf_msgs::MachineSide::INPUT, PrepareMachineInterface::MachineSide::INPUT}, + {llsf_msgs::MachineSide::OUTPUT, PrepareMachineInterface::MachineSide::OUTPUT}}; -static const enum_map -csop_enum { +static const enum_map csop_enum{ {llsf_msgs::CSOp::MOUNT_CAP, PrepareMachineInterface::CSOp::MOUNT_CAP}, - {llsf_msgs::CSOp::RETRIEVE_CAP, PrepareMachineInterface::CSOp::RETRIEVE_CAP} -}; - + {llsf_msgs::CSOp::RETRIEVE_CAP, PrepareMachineInterface::CSOp::RETRIEVE_CAP}}; -template<> -void BlackboardManager::handle_message( - PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareBSMessage *msg) +template <> +void +BlackboardManager::handle_message(PrepareMachineInterface * iface, + PrepareMachineInterface::PrepareBSMessage *msg) { - std::shared_ptr m = std::make_shared(); - llsf_msgs::PrepareInstructionBS *instr = new llsf_msgs::PrepareInstructionBS(); - instr->set_color(base_color_enum.of(msg->color())); - instr->set_side(machine_side_enum.of(msg->side())); - m->set_allocated_instruction_bs(instr); - m->set_team_color(team_enum_prepare.of(msg->team_color())); - m->set_machine(msg->machine()); - - message_handler_->send(iface->peer_id(), m); + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionBS * instr = new llsf_msgs::PrepareInstructionBS(); + instr->set_color(base_color_enum.of(msg->color())); + instr->set_side(machine_side_enum.of(msg->side())); + m->set_allocated_instruction_bs(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); } - -template<> +template <> void -BlackboardManager::handle_message( - PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareCSMessage *msg) +BlackboardManager::handle_message(PrepareMachineInterface * iface, + PrepareMachineInterface::PrepareCSMessage *msg) { - std::shared_ptr m = std::make_shared(); - llsf_msgs::PrepareInstructionCS *instr = new llsf_msgs::PrepareInstructionCS(); - instr->set_operation(csop_enum.of(msg->operation())); - m->set_allocated_instruction_cs(instr); - m->set_team_color(team_enum_prepare.of(msg->team_color())); - m->set_machine(msg->machine()); - - message_handler_->send(iface->peer_id(), m); + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionCS * instr = new llsf_msgs::PrepareInstructionCS(); + instr->set_operation(csop_enum.of(msg->operation())); + m->set_allocated_instruction_cs(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); } - -template<> +template <> void -BlackboardManager::handle_message( - PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareDSMessage *msg) +BlackboardManager::handle_message(PrepareMachineInterface * iface, + PrepareMachineInterface::PrepareDSMessage *msg) { // TODO } - -template<> +template <> void -BlackboardManager::handle_message( - PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareRSMessage *msg) +BlackboardManager::handle_message(PrepareMachineInterface * iface, + PrepareMachineInterface::PrepareRSMessage *msg) { // TODO } - -template<> +template <> void -BlackboardManager::handle_message( - PrepareMachineInterface *iface, - PrepareMachineInterface::PrepareSSMessage *msg) +BlackboardManager::handle_message(PrepareMachineInterface * iface, + PrepareMachineInterface::PrepareSSMessage *msg) { // TODO } - -template<> +template <> void -BlackboardManager::handle_message( - PrepareMachineInterface *iface, - PrepareMachineInterface::SetPeerMessage *msg) +BlackboardManager::handle_message(PrepareMachineInterface * iface, + PrepareMachineInterface::SetPeerMessage *msg) { iface->set_peer_id(msg->peer_id()); } - - -} +} // namespace protoboard From 5786569e64836c128a466e11a6ca71da0bc67fa6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Tue, 8 Oct 2019 18:27:13 +0200 Subject: [PATCH 12/20] protoboard-rcll: add interfaces for RCLL --- .../protoboard-rcll/interfaces/Makefile | 24 ++++++ .../interfaces/OrderInterface.xml | 51 +++++++++++ .../interfaces/PrepareMachineInterface.xml | 84 +++++++++++++++++++ .../interfaces/RCLLGameStateInterface.xml | 33 ++++++++ .../interfaces/RecvBeaconInterface.xml | 24 ++++++ .../interfaces/SendBeaconInterface.xml | 32 +++++++ 6 files changed, 248 insertions(+) create mode 100644 src/plugins/protoboard-rcll/interfaces/Makefile create mode 100644 src/plugins/protoboard-rcll/interfaces/OrderInterface.xml create mode 100644 src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml create mode 100644 src/plugins/protoboard-rcll/interfaces/RCLLGameStateInterface.xml create mode 100644 src/plugins/protoboard-rcll/interfaces/RecvBeaconInterface.xml create mode 100644 src/plugins/protoboard-rcll/interfaces/SendBeaconInterface.xml diff --git a/src/plugins/protoboard-rcll/interfaces/Makefile b/src/plugins/protoboard-rcll/interfaces/Makefile new file mode 100644 index 0000000000..8ba2a86f90 --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/Makefile @@ -0,0 +1,24 @@ +#***************************************************************************** +# Makefile Build System for Fawkes: Robotino interfaces +# ------------------- +# Created on Fri Mar 26 17:25:00 2012 +# Copyright (C) 2006-2012 by Tim Niemueller, AllemaniACs RoboCup Team +# +#***************************************************************************** +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +#***************************************************************************** + +BASEDIR = ../../../.. + +include $(BASEDIR)/etc/buildsys/config.mk + +INTERFACES_all = $(notdir $(patsubst %.xml,%,$(wildcard $(SRCDIR)/*.xml))) +include $(BUILDSYSDIR)/interface.mk + +include $(BUILDSYSDIR)/base.mk + diff --git a/src/plugins/protoboard-rcll/interfaces/OrderInterface.xml b/src/plugins/protoboard-rcll/interfaces/OrderInterface.xml new file mode 100644 index 0000000000..62b1e3e9da --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/OrderInterface.xml @@ -0,0 +1,51 @@ + + + + + + What color to output + Red base + Black base + Silver base + + + Available ring colors + Blue ring + Green ring + Orange ring + Yellow ring + + + Available cap colors + Black cap + Grey cap + + + Team color + Cyan team + Magenta team + + + Product complexity + No ring + One ring + Two rings + Three rings + + + + Description of a single order + Order ID + Order complexity + Requested base color + Requested rings + Requested cap color + Delivery window start (seconds of game time) + Delivery window end (seconds of game time) + Which gate must be used at the DS + How many of this type must be delivered + How many were delivered by Cyan + How many were delivered by Magenta + + + diff --git a/src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml b/src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml new file mode 100644 index 0000000000..2a8da2c3ed --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml @@ -0,0 +1,84 @@ + + + + + + Side of an MPS + Output side (even tag ID) + Input side (odd tag ID) + + + What color to output + Red base + Black base + Silver base + + + Available ring colors + Blue ring + Green ring + Orange ring + Yellow ring + + + Available cap colors + Black cap + Grey cap + + + Team color + Cyan team + Magenta team + + + Storage Station operation + Store something + Retrieve something + + + Cap Station operation + Retrieve cap + Mount cap + + + + + No data needed really, just store last msgid + Last sent message ID + Peer ID to send to + + + + Set peer ID that messages should be sent to + Peer ID to use + + + Make BS output specified base element at the specified side + The sending team's color + Machine name + Where the base should come out + What base color should come out + + + The sending team's color + Machine name + Delivery gate + + + The sending team's color + Machine name + Operation to perform + Where to store/retrieve + + + The sending team's color + Machine name + Ring color + + + The sending team's color + Machine name + Operation to perform + + + diff --git a/src/plugins/protoboard-rcll/interfaces/RCLLGameStateInterface.xml b/src/plugins/protoboard-rcll/interfaces/RCLLGameStateInterface.xml new file mode 100644 index 0000000000..14f5b757ad --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/RCLLGameStateInterface.xml @@ -0,0 +1,33 @@ + + + + + + Game state + Just after startup of the refbox + Time not yet running + Game is running + Game was paused + + + Game phase + Game hasn't begun + Setup phase + Exploration phase + Production phase + Game has ended + + + + Full game state + Time in seconds since game start + Nanoseconds part + Current game state + Current game phase + Points awarded to cyan + Name of the cyan team + Points awarded to magenta + Name of the magenta team + + + diff --git a/src/plugins/protoboard-rcll/interfaces/RecvBeaconInterface.xml b/src/plugins/protoboard-rcll/interfaces/RecvBeaconInterface.xml new file mode 100644 index 0000000000..7cf629e644 --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/RecvBeaconInterface.xml @@ -0,0 +1,24 @@ + + + + + + Team color + Cyan team + Magenta team + + + + + Data from last received BeaconSignal + Seconds since game start + Nanoseconds portion + Jersey number (dummy value if rcv'd from refbox) + Team name (dummy value if rcv'd from refbox) + Peer name + Team color + Pose (unused if rcv'd from refbox) + + + + diff --git a/src/plugins/protoboard-rcll/interfaces/SendBeaconInterface.xml b/src/plugins/protoboard-rcll/interfaces/SendBeaconInterface.xml new file mode 100644 index 0000000000..5d4ee75ff0 --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/SendBeaconInterface.xml @@ -0,0 +1,32 @@ + + + + + + Team color + Cyan team + Magenta team + + + + + No data needed really, just store last msgid + Last sent message ID + Peer ID to send to + + + + Set peer ID that messages should be sent to + Peer ID to use + + + Send a beacon. Contents of the last message are copied into the relevant data fields. + Robot's number + Team name + The Robot's name + Team color + Robot's position on the field + Robot's orientation (yaw) on the field + + + From 5527f34e6e207440c280775017d744028cab0d5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Tue, 8 Oct 2019 21:13:56 +0200 Subject: [PATCH 13/20] protoboard-rcll: simplify iface type -> id mapping --- src/plugins/protoboard-rcll/rcll_receiving.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/plugins/protoboard-rcll/rcll_receiving.cpp b/src/plugins/protoboard-rcll/rcll_receiving.cpp index 147eb09110..311a37b21b 100644 --- a/src/plugins/protoboard-rcll/rcll_receiving.cpp +++ b/src/plugins/protoboard-rcll/rcll_receiving.cpp @@ -58,7 +58,7 @@ template <> std::string iface_id_for_type() { - return "/protoboard/order/"; + return "/protoboard/order"; } static const enum_map base_color_enum_order{ @@ -142,7 +142,7 @@ template <> std::string iface_id_for_type() { - return "/protoboard/beacon/"; + return "/protoboard/beacon"; } static const enum_map team_enum_recv_beacon{ @@ -174,7 +174,7 @@ template <> std::string iface_id_for_type() { - return "/protoboard/game_state/"; + return "/protoboard/game_state"; } static const enum_map From d80e1642a25ba3bf1e3a86686d2d0261afa3b485 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Wed, 9 Oct 2019 04:46:18 +0200 Subject: [PATCH 14/20] skills: add create_peer skill creates a ProtoBuf peer using protoboard-rcll --- src/lua/skills/robotino/create_peer.lua | 100 ++++++++++++++++++++++++ src/lua/skills/robotino/init.lua | 3 + 2 files changed, 103 insertions(+) create mode 100644 src/lua/skills/robotino/create_peer.lua diff --git a/src/lua/skills/robotino/create_peer.lua b/src/lua/skills/robotino/create_peer.lua new file mode 100644 index 0000000000..206cd0c8ac --- /dev/null +++ b/src/lua/skills/robotino/create_peer.lua @@ -0,0 +1,100 @@ + +---------------------------------------------------------------------------- +-- create_peer.lua +-- +-- (c) 2019 Victor Mataré +-- +---------------------------------------------------------------------------- + +-- This program is free software; you can redistribute it and/or modify +-- it under the terms of the GNU General Public License as published by +-- the Free Software Foundation; either version 2 of the License, or +-- (at your option) any later version. +-- +-- This program is distributed in the hope that it will be useful, +-- but WITHOUT ANY WARRANTY; without even the implied warranty of +-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +-- GNU Library General Public License for more details. +-- +-- Read the full text in the LICENSE.GPL file in the doc directory. + +-- Initialize module +module(..., skillenv.module_init) + +-- Crucial skill information +name = "create_peer" +fsm = SkillHSM:new{name=name, start="SEND_MSG"} +depends_skills = { } +depends_interfaces = { + {v = "peer_iface", type="ProtobufPeerInterface", id="/protoboard/peers"}, +} + +documentation = [==[Create and connect to a ProtoBuf peer + +@param address The peer's IP address (string. +@param port The port to send to & receive on. +@param send_to_port The port to send to. Mutually exclusive with the @a port param. +@param recv_on_port The port to receive on. Mutually exclusive with the @a port param. +@param cipher (Optional) Cipher name +@param crypto_key (Optional) The encrypthon key + +]==] + +-- Initialize as skill module +skillenv.skill_module(_M) + + +function input_ok() + local port_ok = fsm.vars.port or (fsm.vars.send_to_port and fsm.vars.recv_on_port) and not( + fsm.vars.port and (fsm.vars.send_to_port or fsm.vars.recv_on_port) + ) + local crypto_ok = (not not fsm.vars.cipher == not not fsm.vars.crypto_key) + return fsm.vars.address and port_ok and crypto_ok +end + +fsm:define_states{ export_to=_M, + closure={ input_ok=input_ok }, + {"SEND_MSG", JumpState} +} + +fsm:add_transitions{ + {"SEND_MSG", "FAILED", precond="not input_ok()", desc="wrong input params"}, + {"SEND_MSG", "FINAL", cond=true} +} + +function SEND_MSG:init() + local msg + if fsm.vars.send_to_port then + if fsm.vars.cipher then + msg = peer_iface.CreatePeerLocalCryptoMessage:new( + fsm.vars.address, + fsm.vars.send_to_port, + fsm.vars.recv_on_port, + fsm.vars.crypto_key, + fsm.vars.cipher + ) + else + msg = peer_iface.CreatePeerLocalMessage:new( + fsm.vars.address, + fsm.vars.send_to_port, + fsm.vars.recv_on_port + ) + end + else + if fsm.vars.cipher then + msg = peer_iface.CreatePeerCryptoMessage:new( + fsm.vars.address, + fsm.vars.port, + fsm.vars.crypto_key, + fsm.vars.cipher + ) + else + msg = peer_iface.CreatePeerMessage:new( + fsm.vars.address, + fsm.vars.port + ) + end + end + peer_iface:msgq_enqueue_copy(msg) +end + diff --git a/src/lua/skills/robotino/init.lua b/src/lua/skills/robotino/init.lua index bd48c7bf29..01db726b8a 100644 --- a/src/lua/skills/robotino/init.lua +++ b/src/lua/skills/robotino/init.lua @@ -55,3 +55,6 @@ skillenv.use_skill("skills.robotino.bring_product_to") skillenv.use_skill("skills.robotino.get_product_from") skillenv.use_skill("skills.robotino.approach_test") skillenv.use_skill("skills.robotino.discard") + +-- ProtoBuf communication +skillenv.use_skill("skills.robotino.create_peer") From 795c7cd3bb25a6ee80449a0d5fac840fa319ce3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Fri, 11 Oct 2019 19:56:52 +0200 Subject: [PATCH 15/20] protoboard-rcll: adapt to API changes in core --- src/plugins/protoboard-rcll/rcll_receiving.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/plugins/protoboard-rcll/rcll_receiving.cpp b/src/plugins/protoboard-rcll/rcll_receiving.cpp index 311a37b21b..f98f09e926 100644 --- a/src/plugins/protoboard-rcll/rcll_receiving.cpp +++ b/src/plugins/protoboard-rcll/rcll_receiving.cpp @@ -93,11 +93,10 @@ pb_sequence_converter -bool -pb_converter::corresponds(const llsf_msgs::Order &msg, - const OrderInterface * iface) +size_t +pb_converter::get_sequence_index(const llsf_msgs::Order &msg) { - return iface->order_id() == msg.id(); + return msg.id(); } template <> From 6a8687934689fd9fdffb8215394a6488692100fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Mon, 14 Oct 2019 04:28:34 +0200 Subject: [PATCH 16/20] protoboard-rcll: handle MachineInfo & RingInfo --- src/plugins/protoboard-rcll/Makefile | 2 +- .../interfaces/MachineInfoInterface.xml | 33 ++++ .../interfaces/RingInfoInterface.xml | 19 +++ .../protoboard-rcll/rcll_receiving.cpp | 156 ++++++++++++++++-- 4 files changed, 195 insertions(+), 15 deletions(-) create mode 100644 src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml create mode 100644 src/plugins/protoboard-rcll/interfaces/RingInfoInterface.xml diff --git a/src/plugins/protoboard-rcll/Makefile b/src/plugins/protoboard-rcll/Makefile index 2bcf4b6046..c5bb1afee7 100644 --- a/src/plugins/protoboard-rcll/Makefile +++ b/src/plugins/protoboard-rcll/Makefile @@ -16,7 +16,7 @@ PRESUBDIRS = interfaces LIBS_protoboard_rcll = fawkesprotoboard llsf_msgs \ OrderInterface PrepareMachineInterface SendBeaconInterface ProtobufPeerInterface \ - RecvBeaconInterface RCLLGameStateInterface + RecvBeaconInterface RCLLGameStateInterface MachineInfoInterface RingInfoInterface CFLAGS += $(CFLAGS_CPP11) diff --git a/src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml b/src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml new file mode 100644 index 0000000000..e821722189 --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml @@ -0,0 +1,33 @@ + + + + + + Team color + Cyan team + Magenta team + + + Available ring colors + Not a RingStation + Blue ring + Green ring + Orange ring + Yellow ring + + + + Information about a machine + Machine name + Machine type + Current state + Team color + 2D Pose (x, y, yaw) + Zone the machine is in + Discretized rotation in degrees + Whether it was correctly reported + TODO: ?? + Available ring colors + + + diff --git a/src/plugins/protoboard-rcll/interfaces/RingInfoInterface.xml b/src/plugins/protoboard-rcll/interfaces/RingInfoInterface.xml new file mode 100644 index 0000000000..29b821fdef --- /dev/null +++ b/src/plugins/protoboard-rcll/interfaces/RingInfoInterface.xml @@ -0,0 +1,19 @@ + + + + + + Available ring colors + Blue ring + Green ring + Orange ring + Yellow ring + + + + How much raw material needs to be paid for which ring color + Ring colors + Raw material costs + + + diff --git a/src/plugins/protoboard-rcll/rcll_receiving.cpp b/src/plugins/protoboard-rcll/rcll_receiving.cpp index f98f09e926..a32a226254 100644 --- a/src/plugins/protoboard-rcll/rcll_receiving.cpp +++ b/src/plugins/protoboard-rcll/rcll_receiving.cpp @@ -19,12 +19,16 @@ * Read the full text in the LICENSE.GPL file in the doc directory. */ +#include #include #include #include +#include #include #include +#include #include +#include #include #include #include @@ -37,17 +41,23 @@ using namespace std; std::unordered_map> make_receiving_interfaces_map() { - return {{"llsf_msgs.OrderInfo", - make_shared>>()}, - {"llsf_msgs.BeaconSignal", - make_shared>()}, - {"llsf_msgs.GameState", - make_shared>()}, - - // Dummy handler, i.e. discard message - {"llsf_msgs.RobotInfo", make_shared()}, - {"llsf_msgs.VersionInfo", make_shared()}}; + return { + {"llsf_msgs.OrderInfo", + make_shared>>()}, + {"llsf_msgs.BeaconSignal", + make_shared>()}, + {"llsf_msgs.GameState", + make_shared>()}, + {"llsf_msgs.MachineInfo", + make_shared>>()}, + {"llsf_msgs.RingInfo", make_shared>()}, + + // Dummy handler, i.e. discard message + {"llsf_msgs.RobotInfo", make_shared()}, + {"llsf_msgs.VersionInfo", make_shared()}, + }; }; /**************************************** @@ -93,10 +103,10 @@ pb_sequence_converter -size_t -pb_converter::get_sequence_index(const llsf_msgs::Order &msg) +std::string +pb_converter::get_sequence_id(const llsf_msgs::Order &msg) { - return msg.id(); + return std::to_string(msg.id()); } template <> @@ -213,4 +223,122 @@ pb_converter::handle(const llsf_ms iface->set_team_magenta(msg.team_magenta().substr(0, iface->maxlenof_team_magenta()).c_str()); } +/**************************************** + * MachineInfoInterface * + ****************************************/ + +template <> +std::string +iface_id_for_type() +{ + return "/protoboard/machine_info"; +} + +static const enum_map + machine_info_ring_color{ + {llsf_msgs::RingColor::RING_BLUE, MachineInfoInterface::RING_BLUE}, + {llsf_msgs::RingColor::RING_GREEN, MachineInfoInterface::RING_GREEN}, + {llsf_msgs::RingColor::RING_ORANGE, MachineInfoInterface::RING_ORANGE}, + {llsf_msgs::RingColor::RING_YELLOW, MachineInfoInterface::RING_YELLOW}, + }; + +static const enum_map mi_team_color_enum{ + {llsf_msgs::CYAN, MachineInfoInterface::Team::CYAN}, + {llsf_msgs::MAGENTA, MachineInfoInterface::Team::MAGENTA}}; + +template <> +std::string +pb_converter::get_sequence_id( + const llsf_msgs::Machine &msg) +{ + return msg.name(); +} + +template <> +const google::protobuf::RepeatedPtrField & +pb_sequence_converter>:: + extract_sequence(const llsf_msgs::MachineInfo &msg) +{ + return msg.machines(); +} + +template <> +void +pb_converter::handle(const llsf_msgs::Machine &msg, + MachineInfoInterface * iface) +{ + float pose[3]{msg.pose().x(), msg.pose().y(), msg.pose().ori()}; + iface->set_pose(pose); + std::string zone = llsf_msgs::Zone_Name(msg.zone()); + for (std::string::size_type idx = zone.find('_'); idx != std::string::npos; idx = zone.find('_')) + zone[idx] = '-'; + iface->set_zone(zone.c_str()); + iface->set_state(msg.state().c_str()); + iface->set_rotation(msg.rotation()); + iface->set_team_color(mi_team_color_enum.of(msg.team_color())); + iface->set_loaded_with(msg.loaded_with()); + + if (msg.ring_colors_size() == 2) { + MachineInfoInterface::RingColor ring_colors[2]{machine_info_ring_color.of(msg.ring_colors(0)), + machine_info_ring_color.of(msg.ring_colors(1))}; + iface->set_ring_colors(ring_colors); + } else if (msg.ring_colors_size() == 0) { + MachineInfoInterface::RingColor ring_colors[2]{MachineInfoInterface::RING_NONE, + MachineInfoInterface::RING_NONE}; + iface->set_ring_colors(ring_colors); + } else { + logger_->log_error(name(), + "Unexpected: MachineInfo message for %s contains %d ring colors", + msg.name().c_str(), + msg.ring_colors_size()); + } + + iface->set_machine_name(msg.name().c_str()); + iface->set_machine_type(msg.type().c_str()); + iface->set_correctly_reported(msg.correctly_reported()); +} + +/**************************************** + * RingInfoInterface * + ****************************************/ + +template <> +std::string +iface_id_for_type() +{ + return "/protoboard/ring_info"; +} + +static const enum_map ring_info_color_enum{ + {llsf_msgs::RingColor::RING_BLUE, RingInfoInterface::RING_BLUE}, + {llsf_msgs::RingColor::RING_GREEN, RingInfoInterface::RING_GREEN}, + {llsf_msgs::RingColor::RING_ORANGE, RingInfoInterface::RING_ORANGE}, + {llsf_msgs::RingColor::RING_YELLOW, RingInfoInterface::RING_YELLOW}, +}; + +template <> +void +pb_converter::handle(const llsf_msgs::RingInfo &msg, + RingInfoInterface * iface) +{ + if (msg.rings_size() != 4) { + logger_->log_error(name(), + "Unexpected: RingInfo message with %d entries instead of %d", + msg.rings_size(), + 4); + } else { + RingInfoInterface::RingColor ring_colors[4]{ring_info_color_enum.of(msg.rings(0).ring_color()), + ring_info_color_enum.of(msg.rings(1).ring_color()), + ring_info_color_enum.of(msg.rings(2).ring_color()), + ring_info_color_enum.of(msg.rings(3).ring_color())}; + uint32_t cost[4]{msg.rings(0).raw_material(), + msg.rings(1).raw_material(), + msg.rings(2).raw_material(), + msg.rings(3).raw_material()}; + iface->set_ring_colors(ring_colors); + iface->set_raw_materials(cost); + } +} + } // namespace protoboard From c39bfc9a1dcd501c0190737b0fe25c51fdfb6bb0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Mon, 14 Oct 2019 13:06:35 +0200 Subject: [PATCH 17/20] protoboard-rcll: handle RS & DS prepare --- .../interfaces/PrepareMachineInterface.xml | 2 +- src/plugins/protoboard-rcll/rcll_sending.cpp | 33 +++++++++++++++++-- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml b/src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml index 2a8da2c3ed..51d6883eab 100644 --- a/src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml +++ b/src/plugins/protoboard-rcll/interfaces/PrepareMachineInterface.xml @@ -62,7 +62,7 @@ The sending team's color Machine name - Delivery gate + Delivered order ID The sending team's color diff --git a/src/plugins/protoboard-rcll/rcll_sending.cpp b/src/plugins/protoboard-rcll/rcll_sending.cpp index 0db6ee8ca2..938ffb5631 100644 --- a/src/plugins/protoboard-rcll/rcll_sending.cpp +++ b/src/plugins/protoboard-rcll/rcll_sending.cpp @@ -114,6 +114,19 @@ static const enum_map csop_enum{ {llsf_msgs::CSOp::MOUNT_CAP, PrepareMachineInterface::CSOp::MOUNT_CAP}, {llsf_msgs::CSOp::RETRIEVE_CAP, PrepareMachineInterface::CSOp::RETRIEVE_CAP}}; +static const enum_map + prepare_ring_color_enum{ + {llsf_msgs::RingColor::RING_BLUE, PrepareMachineInterface::RingColor::RING_BLUE}, + {llsf_msgs::RingColor::RING_GREEN, PrepareMachineInterface::RingColor::RING_GREEN}, + {llsf_msgs::RingColor::RING_ORANGE, PrepareMachineInterface::RingColor::RING_ORANGE}, + {llsf_msgs::RingColor::RING_YELLOW, PrepareMachineInterface::RingColor::RING_YELLOW}, + }; + +static const enum_map ssop_enum{ + {llsf_msgs::SSOp::STORE, PrepareMachineInterface::SSOp::STORE}, + {llsf_msgs::SSOp::RETRIEVE, PrepareMachineInterface::SSOp::RETRIEVE}, +}; + template <> void BlackboardManager::handle_message(PrepareMachineInterface * iface, @@ -150,7 +163,14 @@ void BlackboardManager::handle_message(PrepareMachineInterface * iface, PrepareMachineInterface::PrepareDSMessage *msg) { - // TODO + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionDS * instr = new llsf_msgs::PrepareInstructionDS(); + instr->set_order_id(msg->order_id()); + m->set_allocated_instruction_ds(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); } template <> @@ -158,7 +178,14 @@ void BlackboardManager::handle_message(PrepareMachineInterface * iface, PrepareMachineInterface::PrepareRSMessage *msg) { - // TODO + std::shared_ptr m = std::make_shared(); + llsf_msgs::PrepareInstructionRS * instr = new llsf_msgs::PrepareInstructionRS(); + instr->set_ring_color(prepare_ring_color_enum.of(msg->ring_color())); + m->set_allocated_instruction_rs(instr); + m->set_team_color(team_enum_prepare.of(msg->team_color())); + m->set_machine(msg->machine()); + + message_handler_->send(iface->peer_id(), m); } template <> @@ -166,7 +193,7 @@ void BlackboardManager::handle_message(PrepareMachineInterface * iface, PrepareMachineInterface::PrepareSSMessage *msg) { - // TODO + logger->log_error(name(), "Storage Station is not implemented yet"); } template <> From 36c78293074b2572b07d38883576d7e9f18fb535 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Wed, 16 Oct 2019 21:28:03 +0200 Subject: [PATCH 18/20] protobuf-rcll: add doxygen documentation --- .../protoboard-rcll/rcll_receiving.cpp | 26 +++++++++++++++++++ src/plugins/protoboard-rcll/rcll_sending.cpp | 24 +++++++++++++++++ 2 files changed, 50 insertions(+) diff --git a/src/plugins/protoboard-rcll/rcll_receiving.cpp b/src/plugins/protoboard-rcll/rcll_receiving.cpp index a32a226254..4937dba7cf 100644 --- a/src/plugins/protoboard-rcll/rcll_receiving.cpp +++ b/src/plugins/protoboard-rcll/rcll_receiving.cpp @@ -94,6 +94,9 @@ static const enum_map {llsf_msgs::Order::Complexity::Order_Complexity_C2, OrderInterface::C2}, {llsf_msgs::Order::Complexity::Order_Complexity_C3, OrderInterface::C3}}; +/** @param msg an incoming ProtoBuf OrderInfo message + * @return the repeated orders field + */ template <> const google::protobuf::RepeatedPtrField & pb_sequence_converter>:: @@ -102,6 +105,9 @@ pb_sequence_converter std::string pb_converter::get_sequence_id(const llsf_msgs::Order &msg) @@ -109,6 +115,9 @@ pb_converter::get_sequence_id(const llsf_msgs: return std::to_string(msg.id()); } +/** Convert a ProtoBuf Order message into an OrderInterface + * @param msg The incoming Order message + * @param iface The appropriate blackboard interface */ template <> void pb_converter::handle(const llsf_msgs::Order &msg, @@ -158,6 +167,9 @@ static const enum_map team_enu {llsf_msgs::Team::CYAN, RecvBeaconInterface::TEAM_COLOR::CYAN}, {llsf_msgs::Team::MAGENTA, RecvBeaconInterface::TEAM_COLOR::MAGENTA}}; +/** Convert a ProtoBuf BeaconSignal message into a RecvBeaconInterface + * @param msg The incoming BeaconSignal message + * @param iface The appropriate blackboard interface */ template <> void pb_converter::handle( @@ -208,6 +220,9 @@ static const enum_map void pb_converter::handle(const llsf_msgs::GameState &msg, @@ -246,6 +261,8 @@ static const enum_map mi_team_color {llsf_msgs::CYAN, MachineInfoInterface::Team::CYAN}, {llsf_msgs::MAGENTA, MachineInfoInterface::Team::MAGENTA}}; +/** @param msg an incoming ProtoBuf Machine message + * @return The name of the machine to use as interface ID */ template <> std::string pb_converter::get_sequence_id( @@ -254,6 +271,9 @@ pb_converter::get_sequence_id( return msg.name(); } +/** @param msg an incoming ProtoBuf MachineInfo message + * @return the repeated machines field + */ template <> const google::protobuf::RepeatedPtrField & pb_sequence_converter void pb_converter::handle(const llsf_msgs::Machine &msg, @@ -317,6 +340,9 @@ static const enum_map ring_i {llsf_msgs::RingColor::RING_YELLOW, RingInfoInterface::RING_YELLOW}, }; +/** Convert a ProtoBuf RingInfo message into a RingInfoInterface + * @param msg The incoming RingInfo message + * @param iface The appropriate blackboard interface */ template <> void pb_converter::handle(const llsf_msgs::RingInfo &msg, diff --git a/src/plugins/protoboard-rcll/rcll_sending.cpp b/src/plugins/protoboard-rcll/rcll_sending.cpp index 938ffb5631..d1eeff15e3 100644 --- a/src/plugins/protoboard-rcll/rcll_sending.cpp +++ b/src/plugins/protoboard-rcll/rcll_sending.cpp @@ -45,6 +45,9 @@ static const enum_map team_enu {llsf_msgs::CYAN, SendBeaconInterface::TEAM_COLOR::CYAN}, {llsf_msgs::MAGENTA, SendBeaconInterface::TEAM_COLOR::MAGENTA}}; +/** Send a ProtoBuf BeaconSignal + * @param iface The SendBeaconInterface + * @param msg The SendBeaconMessage that came in on the interface */ template <> void BlackboardManager::handle_message(SendBeaconInterface * iface, @@ -78,6 +81,9 @@ BlackboardManager::handle_message(SendBeaconInterface * iface, message_handler_->send(iface->peer_id(), m); } +/** Set the ProtoBuf peer on the SendBeaconInterface + * @param iface The SendBeaconInterface + * @param msg The SetPeerMessage that came in on the interface */ template <> void BlackboardManager::handle_message(SendBeaconInterface * iface, @@ -127,6 +133,9 @@ static const enum_map ssop_enum{ {llsf_msgs::SSOp::RETRIEVE, PrepareMachineInterface::SSOp::RETRIEVE}, }; +/** Send a ProtoBuf PrepareInstructionBS + * @param iface The PrepareMachineInterface + * @param msg The PrepareBSMessage that came in on the interface */ template <> void BlackboardManager::handle_message(PrepareMachineInterface * iface, @@ -143,6 +152,9 @@ BlackboardManager::handle_message(PrepareMachineInterface * ifa message_handler_->send(iface->peer_id(), m); } +/** Send a ProtoBuf PrepareInstructionCS + * @param iface The PrepareMachineInterface + * @param msg The PrepareCSMessage that came in on the interface */ template <> void BlackboardManager::handle_message(PrepareMachineInterface * iface, @@ -158,6 +170,9 @@ BlackboardManager::handle_message(PrepareMachineInterface * ifa message_handler_->send(iface->peer_id(), m); } +/** Send a ProtoBuf PrepareInstructionCS + * @param iface The PrepareMachineInterface + * @param msg The PrepareCSMessage that came in on the interface */ template <> void BlackboardManager::handle_message(PrepareMachineInterface * iface, @@ -173,6 +188,9 @@ BlackboardManager::handle_message(PrepareMachineInterface * ifa message_handler_->send(iface->peer_id(), m); } +/** Send a ProtoBuf PrepareInstructionRS + * @param iface The PrepareMachineInterface + * @param msg The PrepareRSMessage that came in on the interface */ template <> void BlackboardManager::handle_message(PrepareMachineInterface * iface, @@ -188,6 +206,9 @@ BlackboardManager::handle_message(PrepareMachineInterface * ifa message_handler_->send(iface->peer_id(), m); } +/** Send a ProtoBuf PrepareInstructionSS (not implemented yet) + * @param iface The PrepareMachineInterface + * @param msg The PrepareSSMessage that came in on the interface */ template <> void BlackboardManager::handle_message(PrepareMachineInterface * iface, @@ -196,6 +217,9 @@ BlackboardManager::handle_message(PrepareMachineInterface * ifa logger->log_error(name(), "Storage Station is not implemented yet"); } +/** Set the ProtoBuf peer on the PrepareMachineInterface + * @param iface The PrepareMachineInterface + * @param msg The SetPeerMessage that came in on the interface */ template <> void BlackboardManager::handle_message(PrepareMachineInterface * iface, From 9b41c1dc6d97d649393bbc5a10c790a51309164b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Thu, 5 Dec 2019 18:13:20 +0100 Subject: [PATCH 19/20] fawkes: update submodule to protoboard merge --- fawkes | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fawkes b/fawkes index 7b5a92b07c..ef3c222f63 160000 --- a/fawkes +++ b/fawkes @@ -1 +1 @@ -Subproject commit 7b5a92b07c7b959713233e4955646a20f5e68873 +Subproject commit ef3c222f6310a823d5ed0c9f19e2abfac022d903 From c0a411fdef6b986443f168d543ddf6098710e5e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Matar=C3=A9?= Date: Wed, 11 Dec 2019 15:56:33 +0100 Subject: [PATCH 20/20] protoboard-rcll: fix MachineInfoInterface::loaded_with field descr --- src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml b/src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml index e821722189..d02eaaf65b 100644 --- a/src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml +++ b/src/plugins/protoboard-rcll/interfaces/MachineInfoInterface.xml @@ -26,7 +26,7 @@ Zone the machine is in Discretized rotation in degrees Whether it was correctly reported - TODO: ?? + Bases loaded in slide Available ring colors