From fd7a39a6a360625a252e217d82382f5f32aec849 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 2 Mar 2022 20:21:01 +0100 Subject: [PATCH 01/51] Initial layout MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index e26ad30b5a..1cef82711b 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -102,6 +102,7 @@ add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) add_subdirectory(buoyancy_engine) add_subdirectory(collada_world_exporter) +add_subdirectory(comms) add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) From fbc954d53a538e08f3f4355a4888562dab267811 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 9 Mar 2022 19:45:12 +0100 Subject: [PATCH 02/51] Testing broker MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/comms/Broker.cc | 329 ++++++++++++++++++++++++++++++ src/systems/comms/Broker.hh | 289 ++++++++++++++++++++++++++ src/systems/comms/BrokerPlugin.cc | 66 ++++++ src/systems/comms/BrokerPlugin.hh | 65 ++++++ src/systems/comms/CMakeLists.txt | 7 + src/systems/comms/CommonTypes.hh | 127 ++++++++++++ src/systems/comms/CommsClient.hh | 62 ++++++ 7 files changed, 945 insertions(+) create mode 100644 src/systems/comms/Broker.cc create mode 100644 src/systems/comms/Broker.hh create mode 100644 src/systems/comms/BrokerPlugin.cc create mode 100644 src/systems/comms/BrokerPlugin.hh create mode 100644 src/systems/comms/CMakeLists.txt create mode 100644 src/systems/comms/CommonTypes.hh create mode 100644 src/systems/comms/CommsClient.hh diff --git a/src/systems/comms/Broker.cc b/src/systems/comms/Broker.cc new file mode 100644 index 0000000000..9f2de7c6bb --- /dev/null +++ b/src/systems/comms/Broker.cc @@ -0,0 +1,329 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +// #include +// #include +// #include +// #include +// #include + +// #include "ignition/gazebo/Util.hh" + +#include "Broker.hh" +#include "CommonTypes.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Helper class for managing bidirectional mapping of client IDs and +/// addresses. The class is not thread-safe, so callers must ensure that none +/// of the public methods get called simultaneously. +struct ClientIDs +{ + /// \brief Number of active clients for each address. This structure can be + /// accessed by outer code for reading, but not for modification. + std::unordered_map numActiveClients; + + /// \brief Map of client IDs to addresses. This structure can be accessed by + /// outer code for reading, but not for modification. + std::unordered_map idToAddress; + + /// \brief Add a new client and generate its ID. + /// \param _address Address of the client. + /// \return ID of the client. This method should always succeed and return + /// an ID different from invalidClientID. + ClientID Add(const std::string& _address) + { + const auto clientId = this->NextID(); + this->idToAddress[clientId] = _address; + if (this->numActiveClients.find(_address) == this->numActiveClients.end()) + this->numActiveClients[_address] = 0; + this->numActiveClients[_address]++; + return clientId; + } + + /// \brief Unregister a client. + /// \param _id ID of the client. + /// \return Success of the unregistration. The method can fail e.g. when + /// trying to unregister a client which has not been registered. + bool Remove(const ClientID _id) + { + if (!this->Valid(_id)) + return false; + this->numActiveClients[this->idToAddress[_id]]--; + this->idToAddress.erase(_id); + return true; + } + + /// \brief Clear/reset the structure to be able to work as new. + /// \note This cancels all registrations of all clients and resets the client + /// ID numbering, so it is not valid to mix IDs of clients obtained before and + /// after a Clear() call. + void Clear() + { + this->numActiveClients.clear(); + this->idToAddress.clear(); + this->lastId = invalidClientId; + } + + /// \brief Check validity of a client ID. + /// \param _id ID to check. + /// \return Whether a client with the given ID has been registered. + bool Valid(const ClientID _id) const + { + return _id != invalidClientId && + this->idToAddress.find(_id) != this->idToAddress.end(); + } + + /// \brief Return an ID for a new client. + /// \return The ID. + private: ClientID NextID() + { + return ++this->lastId; + } + + /// \brief Last ID given to a client. + private: ClientID lastId {invalidClientId}; +}; + +class ignition::gazebo::systems::BrokerPrivate +{ + /// \brief An Ignition Transport node for communications. + public: ignition::transport::Node node; + + /// \brief Queue to store the incoming messages received from the clients. + public: std::deque incomingMsgs; + + /// \brief Protect data from races. + public: std::mutex mutex; + + /// \brief Information about the members of the team. + public: TeamMembershipPtr team; + + /// \brief IDs of registered clients. + public: ClientIDs clientIDs; + + /// \brief Buffer to store incoming data packets. + // std::map incomingBuffer; + + /// \brief Buffer to store outgoing data packets. + // std::map outgoingBuffer; +}; + +////////////////////////////////////////////////// +Broker::Broker() + // : team(std::make_shared()), + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +Broker::~Broker() +{ + // cannot use default destructor because of dataPtr +} + +////////////////////////////////////////////////// +void Broker::Start() +{ + // Advertise the service for registering addresses. + if (!this->dataPtr->node.Advertise(kAddrRegistrationSrv, + &Broker::OnAddrRegistration, this)) + { + std::cerr << "Error advertising srv [" << kAddrRegistrationSrv << "]" + << std::endl; + return; + } + + // Advertise the service for unregistering addresses. + if (!this->dataPtr->node.Advertise(kAddrUnregistrationSrv, + &Broker::OnAddrUnregistration, this)) + { + std::cerr << "Error advertising srv [" << kAddrUnregistrationSrv << "]" + << std::endl; + return; + } + + // Advertise the service for registering end points. + if (!this->dataPtr->node.Advertise(kEndPointRegistrationSrv, + &Broker::OnEndPointRegistration, this)) + { + std::cerr << "Error advertising srv [" << kEndPointRegistrationSrv << "]" + << std::endl; + return; + } + + // Advertise the service for unregistering end points. + if (!this->dataPtr->node.Advertise(kEndPointUnregistrationSrv, + &Broker::OnEndPointUnregistration, this)) + { + std::cerr << "Error advertising srv [" << kEndPointUnregistrationSrv << "]" + << std::endl; + return; + } + + // Advertise a oneway service for centralizing all message requests. + if (!this->dataPtr->node.Advertise(kBrokerSrv, &Broker::OnMessage, this)) + { + std::cerr << "Error advertising srv [" << kBrokerSrv << "]" << std::endl; + return; + } + + std::cout << "Started communication broker in Ignition partition " + << this->IgnPartition() << std::endl; +} + +////////////////////////////////////////////////// +std::string Broker::IgnPartition() const +{ + return this->dataPtr->node.Options().Partition(); +} + +////////////////////////////////////////////////// +ClientID Broker::Register(const std::string &_clientAddress) +{ + std::lock_guard lk(this->dataPtr->mutex); + auto kvp = this->dataPtr->team->find(_clientAddress); + if (kvp == this->dataPtr->team->end()) + { + auto newMember = std::make_shared(); + + // Name and address are the same. + newMember->address = _clientAddress; + newMember->name = _clientAddress; + + (*this->dataPtr->team)[_clientAddress] = newMember; + } + + const auto clientId = this->dataPtr->clientIDs.Add(_clientAddress); + + return clientId; +} + +////////////////////////////////////////////////// +bool Broker::Unregister(const ClientID _clientId) +{ + if (!this->dataPtr->clientIDs.Valid(_clientId)) + { + std::cerr << "Broker::Unregister() error: Client ID [" << _clientId + << "] is invalid." << std::endl; + return false; + } + + bool success = true; + + // std::unordered_set endpointIds; + // { + // // make a copy because Unbind() calls will alter the structure + // std::lock_guard lk(this->mutex); + // endpointIds = this->dataPtr->endpointIDs.clientIdToEndpointIds[_clientId]; + // } + + // for (const auto endpointId : endpointIds) + // success = success && this->Unbind(endpointId); + + { + std::lock_guard lk(this->dataPtr->mutex); + + const auto& clientAddress = this->dataPtr->clientIDs.idToAddress[_clientId]; + success = success && this->dataPtr->clientIDs.Remove(_clientId); + + if (this->dataPtr->clientIDs.numActiveClients[clientAddress] == 0u) + this->dataPtr->team->erase(clientAddress); + } + + return success; +} + +////////////////////////////////////////////////// +bool Broker::DispatchMessages() +{ + return true; +} + +///////////////////////////////////////////////// +bool Broker::OnAddrRegistration(const ignition::msgs::StringMsg &_req, + ignition::msgs::UInt32 &_rep) +{ + const auto &address = _req.data(); + + const ClientID result = this->Register(address); + + _rep.set_data(result); + + return result != invalidClientId; +} + +///////////////////////////////////////////////// +bool Broker::OnAddrUnregistration(const ignition::msgs::UInt32 &_req, + ignition::msgs::Boolean &_rep) +{ + uint32_t clientId = _req.data(); + + bool result; + + result = this->Unregister(clientId); + + _rep.set_data(result); + + return result; +} + +///////////////////////////////////////////////// +void Broker::OnMessage(const ignition::msgs::Datagram &_req) +{ + // Just save the message, it will be processed later. + std::lock_guard lk(this->dataPtr->mutex); + + // Save the message. + this->dataPtr->incomingMsgs.push_back(_req); +} + +// ////////////////////////////////////////////////// +// CommsBroker::CommsBroker() +// : dataPtr(std::make_unique()) +// { +// } + +// ////////////////////////////////////////////////// +// void CommsBroker::Configure(const Entity &_entity, +// const std::shared_ptr &_sdf, +// EntityComponentManager &/*_ecm*/, +// EventManager &/*_eventMgr*/) +// { +// } + +// ////////////////////////////////////////////////// +// void CommsBroker::PreUpdate( +// const ignition::gazebo::UpdateInfo &_info, +// ignition::gazebo::EntityComponentManager &_ecm) +// { +// IGN_PROFILE("CommsBroker::PreUpdate"); +// } + +// IGNITION_ADD_PLUGIN(CommsBroker, +// ignition::gazebo::System, +// CommsBroker::ISystemConfigure, +// CommsBroker::ISystemPreUpdate) + +// IGNITION_ADD_PLUGIN_ALIAS(CommsBroker, +// "ignition::gazebo::systems::CommsBroker") diff --git a/src/systems/comms/Broker.hh b/src/systems/comms/Broker.hh new file mode 100644 index 0000000000..3c74fddbea --- /dev/null +++ b/src/systems/comms/Broker.hh @@ -0,0 +1,289 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/// \file Broker.hh +/// \brief Broker for handling message delivery among robots. +#ifndef IGNITION_GAZEBO_SYSTEMS_BROKER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_BROKER_HH_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "CommonTypes.hh" + +// #include +// #include +// #include +// #include + +//#include "CommonTypes" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + +typedef +std::function, double>( + const std::string& name)> pose_update_function; + + /// \brief Stores information about a client broker. + struct BrokerClientInfo + { + /// \brief Address of the client. E.g.: 192.168.2.2 + std::string address; + }; + + /// \def EndPoints_M + /// \brief Map of endpoints + using EndPoints_M = std::map>; + + struct BrokerPrivate; + + /// \brief Store messages, and exposes an API for registering new clients, + /// bind to a particular address, push new messages or get the list of + /// messages already stored in the queue. + class Broker + { + /// \brief Constructor. + public: Broker(); + + /// \brief Destructor. + public: virtual ~Broker(); + + /// \brief Start handling services + /// + /// This function allows us to wait to advertise capabilities to + /// clients until the broker has been entirely initialized. I.e., + /// after SetDefaultRadioConfiguration() has been called. + public: void Start(); + + /// \brief Get the Ignition partition this broker is running in. + /// \return The partition name. + public: std::string IgnPartition() const; + + /// \brief Register a new client for message handling. Multiple clients for + /// the same address are allowed even from a single process. + /// \param[in] _clientAddress Address of the client. + /// \return ID of the client (should be later used for unregistration). + /// If the returned ID is invalidClientId, the registration failed. + public: ClientID Register(const std::string &_clientAddress); + + /// \brief Unregister a client and unbind from all its endpoints. + /// \param[in] _clientId The ID received from the Register() call. + /// \return True if the operation succeeded or false otherwise (if there is + /// no client registered for this ID or the ID is invalid). + public: bool Unregister(ClientID _clientId); + + /// \brief Dispatch all incoming messages. + public: bool DispatchMessages(); + + /// \brief Callback executed when a new registration request is received. + /// \param[in] _req The address contained in the request. + /// \param[out] _rep An ID of the registered client. This ID should be used + /// when unregistering this client. If registration failed, invalidClientId + /// value is returned. + private: bool OnAddrRegistration(const ignition::msgs::StringMsg &_req, + ignition::msgs::UInt32 &_rep); + + /// \brief Callback executed when a new unregistration request is received. + /// \param[in] _req ID of the client to unregister. + /// \param[out] _rep The result of the service. True when the unregistration + /// went OK or false otherwise (e.g.: the ID wasn't registered). + private: bool OnAddrUnregistration(const ignition::msgs::UInt32 &_req, + ignition::msgs::Boolean &_rep); + + /// \brief Callback executed when a new endpoint registration request is + /// received. + /// \param[in] _req The endpoint to register together with ID of the client + /// to which this registration belongs. + /// \param[out] _rep An ID of the endpoint. This ID should be used + /// when unregistering this endpoint. If registration failed, + /// invalidEndpointId value is returned. + private: bool OnEndPointRegistration( + const ignition::msgs::EndpointRegistration &_req, + ignition::msgs::UInt32 &_rep); + + /// \brief Callback executed when a new endpoint unregistration request is + /// received. + /// \param[in] _req ID of the endpoint to unregister. + /// \param[out] _rep The result of the service. True when the unregistration + /// went OK or false otherwise (e.g.: the ID wasn't registered). + private: bool OnEndPointUnregistration( + const ignition::msgs::UInt32 &_req, + ignition::msgs::Boolean &_rep); + + /// \brief Callback executed when a new request is received. + /// \param[in] _req The datagram contained in the request. + private: void OnMessage(const ignition::msgs::Datagram &_req); + + /// \brief Handle reset. + // public: void Reset(); + + // /// \brief Get a mutator to the team. + // /// \return A mutator to the team. + // public: TeamMembershipPtr Team(); + + // /// \brief Send a message to each member + // /// with its updated neighbors list. + // public: void NotifyNeighbors(); + + /// \brief Dispatch all incoming messages. + // public: bool DispatchMessages(); + + // /// \brief This method associates an endpoint with a broker client and its + // /// address. An endpoint is constructed as an address followed by ':', + // /// followed by the port. E.g.: "192.168.1.5:8000" is a valid endpoint. + // /// \param[in] _clientId ID of a registered client. + // /// \param[in] _endpoint End point requested to bind. + // /// \return ID that should be used for unbinding the endpoint. If + // /// invalidEndpointId is returned, the binding request failed (e.g. due to + // /// wrong endpoint specification, or if the _clientId is wrong). + // public: EndpointID Bind(ClientID _clientId, const std::string &_endpoint); + + // /// \brief This method cancels the association between a client and an + // /// endpoint. When all equal endpoints on the same client are unbound, + // /// the client will stop receiving messages for the given endpoint. + // /// \param[in] _endpointId ID of the endpoint to unbind. It has to be an ID + // /// received from a previous call to Bind(). + // /// \return Whether the unbind was successful. It may fail e.g. when the + // /// given ID is invalid and the broker doesn't know it. + // public: bool Unbind(EndpointID _endpointId); + + // /// \brief Register a new client for message handling. Multiple clients for + // /// the same address are allowed even from a single process. + // /// \param[in] _clientAddress Address of the client. + // /// \return ID of the client (should be later used for unregistration). + // /// If the returned ID is invalidClientId, the registration failed. + // public: ClientID Register(const std::string &_clientAddress); + + // /// \brief Unregister a client and unbind from all its endpoints. + // /// \param[in] _clientId The ID received from the Register() call. + // /// \return True if the operation succeeded or false otherwise (if there is + // /// no client registered for this ID or the ID is invalid). + // public: bool Unregister(ClientID _clientId); + + // /// \brief Set the radio configuration for address + // /// \param[in] address + // /// \param[in] radio_configuration + // public: void SetRadioConfiguration(const std::string& address, + // communication_model::radio_configuration config); + + // /// \brief Set the radio configuration + // /// \param[in] radio_configuration + // public: void SetDefaultRadioConfiguration( + // communication_model::radio_configuration config); + + // /// \brief Set the communication function handle to use + // /// \param[in] f Function that evaluates transmission of bytes across the + // /// communication channel. + // public: void SetCommunicationFunction( + // communication_model::communication_function f); + + // /// \brief Set the function to be used for updating pose + // /// \param[in] f Function that finds pose based on name + // public: void SetPoseUpdateFunction(pose_update_function f); + + // /// \brief Callback executed when a new registration request is received. + // /// \param[in] _req The address contained in the request. + // /// \param[out] _rep An ID of the registered client. This ID should be used + // /// when unregistering this client. If registration failed, invalidClientId + // /// value is returned. + // private: bool OnAddrRegistration(const ignition::msgs::StringMsg &_req, + // ignition::msgs::UInt32 &_rep); + + // /// \brief Callback executed when a new unregistration request is received. + // /// \param[in] _req ID of the client to unregister. + // /// \param[out] _rep The result of the service. True when the unregistration + // /// went OK or false otherwise (e.g.: the ID wasn't registered). + // private: bool OnAddrUnregistration(const ignition::msgs::UInt32 &_req, + // ignition::msgs::Boolean &_rep); + + // /// \brief Callback executed when a new endpoint registration request is + // /// received. + // /// \param[in] _req The endpoint to register together with ID of the client + // /// to which this registration belongs. + // /// \param[out] _rep An ID of the endpoint. This ID should be used + // /// when unregistering this endpoint. If registration failed, + // /// invalidEndpointId value is returned. + // private: bool OnEndPointRegistration( + // const subt::msgs::EndpointRegistration &_req, + // ignition::msgs::UInt32 &_rep); + + // /// \brief Callback executed when a new endpoint unregistration request is + // /// received. + // /// \param[in] _req ID of the endpoint to unregister. + // /// \param[out] _rep The result of the service. True when the unregistration + // /// went OK or false otherwise (e.g.: the ID wasn't registered). + // private: bool OnEndPointUnregistration( + // const ignition::msgs::UInt32 &_req, + // ignition::msgs::Boolean &_rep); + + // /// \brief Callback executed when a new request is received. + // /// \param[in] _req The datagram contained in the request. + // private: void OnMessage(const subt::msgs::Datagram &_req); + + // /// \brief Queue to store the incoming messages received from the clients. + // protected: std::deque incomingMsgs; + + // /// \brief List of bound endpoints. The key is an endpoint and the + // /// value is the vector of clients bounded on that endpoint. + // protected: EndPoints_M endpoints; + + // /// \brief Information about the members of the team. + // protected: TeamMembershipPtr team; + + // /// \brief An Ignition Transport node for communications. + // private: ignition::transport::Node node; + + // /// \brief The publisher for notifying neighbor updates. + // private: ignition::transport::Node::Publisher neighborPub; + + // /// \brief Protect data from races. + // private: std::mutex mutex; + + // /// \brief Function handle for evaluating communication + // private: subt::communication_model::communication_function + // communication_function; + + // private: + // /// \brief Default radio configuration + // subt::communication_model::radio_configuration default_radio_configuration; + + // /// \brief Pose update function + // private: pose_update_function pose_update_f; + + /// \brief Private definitions and data + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/comms/BrokerPlugin.cc b/src/systems/comms/BrokerPlugin.cc new file mode 100644 index 0000000000..c88f440a75 --- /dev/null +++ b/src/systems/comms/BrokerPlugin.cc @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include + +#include "ignition/gazebo/Util.hh" + +#include "Broker.hh" +#include "BrokerPlugin.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::BrokerPluginPrivate +{ + /// \brief Broker instance. + public: Broker broker; +}; + +////////////////////////////////////////////////// +BrokerPlugin::BrokerPlugin() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void BrokerPlugin::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ +} + +////////////////////////////////////////////////// +void BrokerPlugin::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("BrokerPlugin::PreUpdate"); +} + +IGNITION_ADD_PLUGIN(BrokerPlugin, + ignition::gazebo::System, + BrokerPlugin::ISystemConfigure, + BrokerPlugin::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(BrokerPlugin, + "ignition::gazebo::systems::BrokerPlugin") diff --git a/src/systems/comms/BrokerPlugin.hh b/src/systems/comms/BrokerPlugin.hh new file mode 100644 index 0000000000..1024f5c44e --- /dev/null +++ b/src/systems/comms/BrokerPlugin.hh @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_BROKERPLUGIN_HH_ +#define IGNITION_GAZEBO_SYSTEMS_BROKERPLUGIN_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class BrokerPluginPrivate; + + /// \brief + class BrokerPlugin + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: BrokerPlugin(); + + /// \brief Destructor + public: ~BrokerPlugin() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/comms/CMakeLists.txt b/src/systems/comms/CMakeLists.txt new file mode 100644 index 0000000000..234e4fc04b --- /dev/null +++ b/src/systems/comms/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(comms-broker + SOURCES + BrokerPlugin.cc + Broker.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/comms/CommonTypes.hh b/src/systems/comms/CommonTypes.hh new file mode 100644 index 0000000000..42ab14ed9a --- /dev/null +++ b/src/systems/comms/CommonTypes.hh @@ -0,0 +1,127 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_COMMONTYPES_HH_ +#define IGNITION_GAZEBO_SYSTEMS_COMMONTYPES_HH_ + +#include +#include +#include +// #include + +// #include +// #include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + +// /// \def Neighbors_M +// /// \brief Map of neighbors +// /// \todo Fix second term to hold channel information (RSS, bitrate) +// using Neighbors_M = std::map; + +/// \brief Structure used to store information about a member of the +/// team. +struct TeamMember +{ + /// \brief Name used for this agent + std::string name; + + /// \brief Address of this agent. E.g.: 192.168.1.2 + std::string address; + + // /// \brief List of neighbors and comms probabilities for this robot. + // Neighbors_M neighbors; + + // /// \brief Static configuration of radio for communication + // subt::communication_model::radio_configuration radio; + + // /// \brief State of the radio, e.g., pose + // subt::rf_interface::radio_state rf_state; +}; + +/// \def TeamMemberPtr +/// \brief Shared pointer to TeamMember +using TeamMemberPtr = std::shared_ptr; + +/// \def TeamMembership_M +/// \brief Map containing information about the members of the team. +/// The key is the robot address. The value is a pointer to a TeamMember +/// object that contains multiple information about the robot. +using TeamMembership_M = std::map; + +/// \def TeamMembershipPtr +/// \brief A shared pointer to the membership data structure. +/// \sa TeamMembership_M +using TeamMembershipPtr = std::shared_ptr; + +// /// \brief Address used to send a message to all the members of the team +// /// listening on a specific port. +// const std::string kBroadcast = "broadcast"; + +// /// \brief Address used to bind to a multicast group. Note that we do not +// /// support multiple multicast groups, only one. +// const std::string kMulticast = "multicast"; + +// /// \brief Service used to centralize all messages sent from the agents. +const std::string kBrokerSrv = "/broker"; + +/// \brief Service used to validate an address. +const std::string kAddrRegistrationSrv = "/address/register"; + +/// \brief Service used to invalidate an address. +const std::string kAddrUnregistrationSrv = "/address/unregister"; + +/// \brief Service used to register an end point. +const std::string kEndPointRegistrationSrv = "/end_point/register"; + +/// \brief Service used to unregister an end point. +const std::string kEndPointUnregistrationSrv = "/end_point/unregister"; + +/// \brief ID of a broker client. +typedef uint32_t ClientID; + +/// \brief ID denoting an invalid broker client. +constexpr ClientID invalidClientId {0}; + +// /// \brief Address used to receive neighbor updates. +// const std::string kNeighborsTopic = "/neighbors"; + +// /// \brief Default port. +// const uint32_t kDefaultPort = 4100u; + +// /// \brief ID of a broker client. +// typedef uint32_t ClientID; +// /// \brief ID denoting an invalid broker client. +// constexpr ClientID invalidClientId {0}; + +// /// \brief ID of a bound endpoint belonging to a particular client. +// typedef uint32_t EndpointID; +// /// \brief ID denoting an invalid endpoint. +// constexpr EndpointID invalidEndpointId {0}; + +} +} +} +} + +#endif \ No newline at end of file diff --git a/src/systems/comms/CommsClient.hh b/src/systems/comms/CommsClient.hh new file mode 100644 index 0000000000..a276acee32 --- /dev/null +++ b/src/systems/comms/CommsClient.hh @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_COMMSCLIENT_HH_ +#define IGNITION_GAZEBO_SYSTEMS_COMMSCLIENT_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // \brief + class CommsClient + { + ////////////////////////////////////////////////// + /// \brief Constructor + public: CommsClient() + { + } + + ////////////////////////////////////////////////// + /// \brief Send a message + public: void SendPacket() + { + } + + ////////////////////////////////////////////////// + /// \brief + private: void ReceivedPacket() + { + } + }; + } +} +} +} + +#endif From 135ceef8740d10bf450872488cc741318059fce2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sun, 13 Mar 2022 19:42:54 +0100 Subject: [PATCH 03/51] Working version. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/worlds/comms.sdf | 120 +++++++ src/systems/comms/AddressManager.cc | 170 ++++++++++ src/systems/comms/AddressManager.hh | 110 +++++++ src/systems/comms/Broker.cc | 294 ++++-------------- src/systems/comms/Broker.hh | 250 ++------------- src/systems/comms/BrokerPlugin.cc | 66 ---- src/systems/comms/CMakeLists.txt | 8 +- src/systems/comms/CommonTypes.hh | 83 +---- src/systems/comms/CommsClient.hh | 5 - src/systems/comms/CommsModel.hh | 58 ++++ src/systems/comms/models/CMakeLists.txt | 8 + src/systems/comms/models/PerfectCommsModel.cc | 110 +++++++ .../PerfectCommsModel.hh} | 28 +- 13 files changed, 677 insertions(+), 633 deletions(-) create mode 100644 examples/worlds/comms.sdf create mode 100644 src/systems/comms/AddressManager.cc create mode 100644 src/systems/comms/AddressManager.hh delete mode 100644 src/systems/comms/BrokerPlugin.cc create mode 100644 src/systems/comms/CommsModel.hh create mode 100644 src/systems/comms/models/CMakeLists.txt create mode 100644 src/systems/comms/models/PerfectCommsModel.cc rename src/systems/comms/{BrokerPlugin.hh => models/PerfectCommsModel.hh} (71%) diff --git a/examples/worlds/comms.sdf b/examples/worlds/comms.sdf new file mode 100644 index 0000000000..aec4ac52f1 --- /dev/null +++ b/examples/worlds/comms.sdf @@ -0,0 +1,120 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 2 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + + + + + diff --git a/src/systems/comms/AddressManager.cc b/src/systems/comms/AddressManager.cc new file mode 100644 index 0000000000..01f705f907 --- /dev/null +++ b/src/systems/comms/AddressManager.cc @@ -0,0 +1,170 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "AddressManager.hh" +#include "CommonTypes.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private AddressManager data class. +class ignition::gazebo::systems::AddressManager::Implementation +{ + /// \brief Buffer to store the content associated to each address. + /// The key is an address. The value contains all the information associated + /// to the address. + public: std::map data; + + /// \brief An Ignition Transport node for communications. + public: ignition::transport::Node node; +}; + +////////////////////////////////////////////////// +AddressManager::AddressManager() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +AddressManager::~AddressManager() +{ + // cannot use default destructor because of dataPtr +} + +////////////////////////////////////////////////// +void AddressManager::AddSubscriber(const std::string &_address, + const std::string &_topic) +{ + ignition::transport::Node::Publisher publisher = + this->dataPtr->node.Advertise(_topic); + + this->dataPtr->data[_address].subscriptions[_topic] = publisher; +} + +////////////////////////////////////////////////// +// std::unordered_set AddressManager::Subscribers( +// const std::string &_address) +// { +// std::unordered_set result; + +// if (this->dataPtr->data.find(_address) != this->dataPtr->data.end()) +// { +// result = this->dataPtr->data[_address].subscriptions; +// } + +// return result; +// } + +////////////////////////////////////////////////// +void AddressManager::AddInbound(const std::string &_address, + const std::shared_ptr &_msg) +{ + this->dataPtr->data[_address].inboundMsgs.push_back(_msg); +} + +////////////////////////////////////////////////// +void AddressManager::AddOutbound(const std::string &_address, + const std::shared_ptr &_msg) +{ + this->dataPtr->data[_address].outboundMsgs.push_back(_msg); +} + +////////////////////////////////////////////////// +bool AddressManager::RemoveSubscriber(const std::string &_address, + const std::string &_topic) +{ + // Iterate over all the topics. + if (this->dataPtr->data.find(_address) == + this->dataPtr->data.end()) + { + return false; + } + + return this->dataPtr->data[_address].subscriptions.erase(_topic) > 0; +} + +////////////////////////////////////////////////// +void AddressManager::RemoveInbound(const std::string &_address, + const std::shared_ptr &_msg) +{ + if (this->dataPtr->data.find(_address) != + this->dataPtr->data.end()) + { + auto &q = this->dataPtr->data[_address].inboundMsgs; + q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); + } +} + +////////////////////////////////////////////////// +void AddressManager::RemoveOutbound(const std::string &_address, + const std::shared_ptr &_msg) +{ + if (this->dataPtr->data.find(_address) != this->dataPtr->data.end()) + { + auto &q = this->dataPtr->data[_address].outboundMsgs; + q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); + } +} + +////////////////////////////////////////////////// +void AddressManager::DeliverMsgs() +{ + for (auto & [address, content] : this->dataPtr->data) + { + // Reference to the inbound queue for this address. + auto &inbound = content.inboundMsgs; + + // All these messages need to be delivered. + for (auto &msg : inbound) + { + // Use the publisher associated to the destination address. + for (auto & [topic, publisher] : content.subscriptions) + publisher.Publish(*msg); + } + + content.inboundMsgs.clear(); + } +} + +////////////////////////////////////////////////// +std::map &AddressManager::Data() +{ + return this->dataPtr->data; +} + +////////////////////////////////////////////////// +std::map AddressManager::Copy() +{ + return this->dataPtr->data; +} + +////////////////////////////////////////////////// +void AddressManager::Set(std::map &_newContent) +{ +this->dataPtr->data = _newContent; +} diff --git a/src/systems/comms/AddressManager.hh b/src/systems/comms/AddressManager.hh new file mode 100644 index 0000000000..7c4fadd34b --- /dev/null +++ b/src/systems/comms/AddressManager.hh @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_ADDRESSMANAGER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_ADDRESSMANAGER_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + +struct AddressContent +{ + /// \brief Queue of inbound messages. + public: std::deque> inboundMsgs; + + /// \brief Queue of outbound messages. + public: std::deque> outboundMsgs; + + /// \brief A map where the key is the topic subscribed to this address and + /// the value is a publisher to reach that topic. + public: std::map subscriptions; +}; + +/// \brief +class AddressManager +{ + /// \brief Default constructor. + public: AddressManager(); + + /// \brief Destructor. + public: virtual ~AddressManager(); + + /// \brief + public: void AddSubscriber(const std::string &_address, + const std::string &_topic); + + /// \brief + // public: std::unordered_set Subscribers( + // const std::string &_address); + + /// \brief + public: void AddInbound(const std::string &_address, + const std::shared_ptr &_msg); + + /// \brief + public: void AddOutbound(const std::string &_address, + const std::shared_ptr &_msg); + + /// \brief + public: bool RemoveSubscriber(const std::string &_address, + const std::string &_topic); + + /// \brief + public: void RemoveInbound(const std::string &_address, + const std::shared_ptr &_msg); + + /// \brief + public: void RemoveOutbound(const std::string &_address, + const std::shared_ptr &_msg); + + /// \brief ToDo. + public: void DeliverMsgs(); + + /// \brief ToDo. + public: std::map &Data(); + + /// \brief ToDo. + public: std::map Copy(); + + /// \brief ToDo. + public: void Set(std::map &_newContent); + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) +}; +} +} +} +} + +#endif diff --git a/src/systems/comms/Broker.cc b/src/systems/comms/Broker.cc index 9f2de7c6bb..1accd10354 100644 --- a/src/systems/comms/Broker.cc +++ b/src/systems/comms/Broker.cc @@ -17,16 +17,16 @@ #include +#include #include +#include #include -// #include -// #include -// #include -// #include -// #include +#include -// #include "ignition/gazebo/Util.hh" +#include +#include +#include "AddressManager.hh" #include "Broker.hh" #include "CommonTypes.hh" @@ -34,105 +34,22 @@ using namespace ignition; using namespace gazebo; using namespace systems; -/// \brief Helper class for managing bidirectional mapping of client IDs and -/// addresses. The class is not thread-safe, so callers must ensure that none -/// of the public methods get called simultaneously. -struct ClientIDs -{ - /// \brief Number of active clients for each address. This structure can be - /// accessed by outer code for reading, but not for modification. - std::unordered_map numActiveClients; - - /// \brief Map of client IDs to addresses. This structure can be accessed by - /// outer code for reading, but not for modification. - std::unordered_map idToAddress; - - /// \brief Add a new client and generate its ID. - /// \param _address Address of the client. - /// \return ID of the client. This method should always succeed and return - /// an ID different from invalidClientID. - ClientID Add(const std::string& _address) - { - const auto clientId = this->NextID(); - this->idToAddress[clientId] = _address; - if (this->numActiveClients.find(_address) == this->numActiveClients.end()) - this->numActiveClients[_address] = 0; - this->numActiveClients[_address]++; - return clientId; - } - - /// \brief Unregister a client. - /// \param _id ID of the client. - /// \return Success of the unregistration. The method can fail e.g. when - /// trying to unregister a client which has not been registered. - bool Remove(const ClientID _id) - { - if (!this->Valid(_id)) - return false; - this->numActiveClients[this->idToAddress[_id]]--; - this->idToAddress.erase(_id); - return true; - } - - /// \brief Clear/reset the structure to be able to work as new. - /// \note This cancels all registrations of all clients and resets the client - /// ID numbering, so it is not valid to mix IDs of clients obtained before and - /// after a Clear() call. - void Clear() - { - this->numActiveClients.clear(); - this->idToAddress.clear(); - this->lastId = invalidClientId; - } - - /// \brief Check validity of a client ID. - /// \param _id ID to check. - /// \return Whether a client with the given ID has been registered. - bool Valid(const ClientID _id) const - { - return _id != invalidClientId && - this->idToAddress.find(_id) != this->idToAddress.end(); - } - - /// \brief Return an ID for a new client. - /// \return The ID. - private: ClientID NextID() - { - return ++this->lastId; - } - - /// \brief Last ID given to a client. - private: ClientID lastId {invalidClientId}; -}; - -class ignition::gazebo::systems::BrokerPrivate +/// \brief Private Broker data class. +class ignition::gazebo::systems::Broker::Implementation { /// \brief An Ignition Transport node for communications. public: ignition::transport::Node node; - /// \brief Queue to store the incoming messages received from the clients. - public: std::deque incomingMsgs; + /// \brief The message manager. + public: AddressManager data; /// \brief Protect data from races. public: std::mutex mutex; - - /// \brief Information about the members of the team. - public: TeamMembershipPtr team; - - /// \brief IDs of registered clients. - public: ClientIDs clientIDs; - - /// \brief Buffer to store incoming data packets. - // std::map incomingBuffer; - - /// \brief Buffer to store outgoing data packets. - // std::map outgoingBuffer; }; ////////////////////////////////////////////////// Broker::Broker() - // : team(std::make_shared()), - : dataPtr(std::make_unique()) + : dataPtr(ignition::utils::MakeUniqueImpl()) { } @@ -145,185 +62,90 @@ Broker::~Broker() ////////////////////////////////////////////////// void Broker::Start() { - // Advertise the service for registering addresses. - if (!this->dataPtr->node.Advertise(kAddrRegistrationSrv, - &Broker::OnAddrRegistration, this)) + // Advertise the service for binding addresses. + if (!this->dataPtr->node.Advertise(kAddrBindSrv, &Broker::OnBind, this)) { - std::cerr << "Error advertising srv [" << kAddrRegistrationSrv << "]" - << std::endl; + ignerr << "Error advertising srv [" << kAddrBindSrv << "]" << std::endl; return; } - // Advertise the service for unregistering addresses. - if (!this->dataPtr->node.Advertise(kAddrUnregistrationSrv, - &Broker::OnAddrUnregistration, this)) + // Advertise the service for unbinding addresses. + if (!this->dataPtr->node.Advertise(kAddrUnbindSrv, &Broker::OnUnbind, this)) { - std::cerr << "Error advertising srv [" << kAddrUnregistrationSrv << "]" + ignerr << "Error advertising srv [" << kAddrUnbindSrv << "]" << std::endl; return; } - // Advertise the service for registering end points. - if (!this->dataPtr->node.Advertise(kEndPointRegistrationSrv, - &Broker::OnEndPointRegistration, this)) + // Advertise the topic for receiving data messages. + if (!this->dataPtr->node.Subscribe(kBrokerTopic, &Broker::OnMsg, this)) { - std::cerr << "Error advertising srv [" << kEndPointRegistrationSrv << "]" - << std::endl; - return; + ignerr << "Error subscribing to topic [" << kBrokerTopic << "]" + << std::endl; } - - // Advertise the service for unregistering end points. - if (!this->dataPtr->node.Advertise(kEndPointUnregistrationSrv, - &Broker::OnEndPointUnregistration, this)) - { - std::cerr << "Error advertising srv [" << kEndPointUnregistrationSrv << "]" - << std::endl; - return; - } - - // Advertise a oneway service for centralizing all message requests. - if (!this->dataPtr->node.Advertise(kBrokerSrv, &Broker::OnMessage, this)) - { - std::cerr << "Error advertising srv [" << kBrokerSrv << "]" << std::endl; - return; - } - - std::cout << "Started communication broker in Ignition partition " - << this->IgnPartition() << std::endl; } ////////////////////////////////////////////////// -std::string Broker::IgnPartition() const +void Broker::OnBind(const ignition::msgs::StringMsg_V &_req) { - return this->dataPtr->node.Options().Partition(); -} + auto count = _req.data_size(); + if (count != 2) + ignerr << "Receive incorrect number of arguments. " + << "Expecting 2 and receive " << count << std::endl; -////////////////////////////////////////////////// -ClientID Broker::Register(const std::string &_clientAddress) -{ std::lock_guard lk(this->dataPtr->mutex); - auto kvp = this->dataPtr->team->find(_clientAddress); - if (kvp == this->dataPtr->team->end()) - { - auto newMember = std::make_shared(); - - // Name and address are the same. - newMember->address = _clientAddress; - newMember->name = _clientAddress; - - (*this->dataPtr->team)[_clientAddress] = newMember; - } - - const auto clientId = this->dataPtr->clientIDs.Add(_clientAddress); + this->dataPtr->data.AddSubscriber(_req.data(0), _req.data(1)); - return clientId; + ignmsg << "Address [" << _req.data(0) << "] bound on topic [" + << _req.data(1) << "]" << std::endl; } ////////////////////////////////////////////////// -bool Broker::Unregister(const ClientID _clientId) +void Broker::OnUnbind(const ignition::msgs::StringMsg_V &_req) { - if (!this->dataPtr->clientIDs.Valid(_clientId)) - { - std::cerr << "Broker::Unregister() error: Client ID [" << _clientId - << "] is invalid." << std::endl; - return false; - } - - bool success = true; + auto count = _req.data_size(); + if (count != 2) + ignerr << "Receive incorrect number of arguments. " + << "Expecting 2 and receive " << count << std::endl; - // std::unordered_set endpointIds; - // { - // // make a copy because Unbind() calls will alter the structure - // std::lock_guard lk(this->mutex); - // endpointIds = this->dataPtr->endpointIDs.clientIdToEndpointIds[_clientId]; - // } - - // for (const auto endpointId : endpointIds) - // success = success && this->Unbind(endpointId); - - { - std::lock_guard lk(this->dataPtr->mutex); + std::lock_guard lk(this->dataPtr->mutex); + this->dataPtr->data.RemoveSubscriber(_req.data(0), _req.data(1)); - const auto& clientAddress = this->dataPtr->clientIDs.idToAddress[_clientId]; - success = success && this->dataPtr->clientIDs.Remove(_clientId); + ignmsg << "Address [" << _req.data(0) << "] unbound on topic [" + << _req.data(1) << "]" << std::endl; +} - if (this->dataPtr->clientIDs.numActiveClients[clientAddress] == 0u) - this->dataPtr->team->erase(clientAddress); - } +////////////////////////////////////////////////// +void Broker::OnMsg(const ignition::msgs::Datagram &_msg) +{ + // Place the message in the outbound queue of the sender. + auto msgPtr = std::make_shared(_msg); - return success; + std::lock_guard lk(this->dataPtr->mutex); + this->dataPtr->data.AddOutbound(_msg.src_address(), msgPtr); } ////////////////////////////////////////////////// -bool Broker::DispatchMessages() +void Broker::DeliverMsgs() { - return true; + std::lock_guard lk(this->dataPtr->mutex); + this->dataPtr->data.DeliverMsgs(); } -///////////////////////////////////////////////// -bool Broker::OnAddrRegistration(const ignition::msgs::StringMsg &_req, - ignition::msgs::UInt32 &_rep) +////////////////////////////////////////////////// +AddressManager &Broker::Data() { - const auto &address = _req.data(); - - const ClientID result = this->Register(address); - - _rep.set_data(result); - - return result != invalidClientId; + return this->dataPtr->data; } -///////////////////////////////////////////////// -bool Broker::OnAddrUnregistration(const ignition::msgs::UInt32 &_req, - ignition::msgs::Boolean &_rep) +////////////////////////////////////////////////// +void Broker::Lock() { - uint32_t clientId = _req.data(); - - bool result; - - result = this->Unregister(clientId); - - _rep.set_data(result); - - return result; + this->dataPtr->mutex.lock(); } -///////////////////////////////////////////////// -void Broker::OnMessage(const ignition::msgs::Datagram &_req) +////////////////////////////////////////////////// +void Broker::Unlock() { - // Just save the message, it will be processed later. - std::lock_guard lk(this->dataPtr->mutex); - - // Save the message. - this->dataPtr->incomingMsgs.push_back(_req); + this->dataPtr->mutex.unlock(); } - -// ////////////////////////////////////////////////// -// CommsBroker::CommsBroker() -// : dataPtr(std::make_unique()) -// { -// } - -// ////////////////////////////////////////////////// -// void CommsBroker::Configure(const Entity &_entity, -// const std::shared_ptr &_sdf, -// EntityComponentManager &/*_ecm*/, -// EventManager &/*_eventMgr*/) -// { -// } - -// ////////////////////////////////////////////////// -// void CommsBroker::PreUpdate( -// const ignition::gazebo::UpdateInfo &_info, -// ignition::gazebo::EntityComponentManager &_ecm) -// { -// IGN_PROFILE("CommsBroker::PreUpdate"); -// } - -// IGNITION_ADD_PLUGIN(CommsBroker, -// ignition::gazebo::System, -// CommsBroker::ISystemConfigure, -// CommsBroker::ISystemPreUpdate) - -// IGNITION_ADD_PLUGIN_ALIAS(CommsBroker, -// "ignition::gazebo::systems::CommsBroker") diff --git a/src/systems/comms/Broker.hh b/src/systems/comms/Broker.hh index 3c74fddbea..8121bab9af 100644 --- a/src/systems/comms/Broker.hh +++ b/src/systems/comms/Broker.hh @@ -20,24 +20,12 @@ #ifndef IGNITION_GAZEBO_SYSTEMS_BROKER_HH_ #define IGNITION_GAZEBO_SYSTEMS_BROKER_HH_ +#include #include +#include +#include -#include -#include -#include -#include -#include -#include -#include - -#include "CommonTypes.hh" - -// #include -// #include -// #include -// #include - -//#include "CommonTypes" +#include "AddressManager.hh" namespace ignition { @@ -47,24 +35,6 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - -typedef -std::function, double>( - const std::string& name)> pose_update_function; - - /// \brief Stores information about a client broker. - struct BrokerClientInfo - { - /// \brief Address of the client. E.g.: 192.168.2.2 - std::string address; - }; - - /// \def EndPoints_M - /// \brief Map of endpoints - using EndPoints_M = std::map>; - - struct BrokerPrivate; - /// \brief Store messages, and exposes an API for registering new clients, /// bind to a particular address, push new messages or get the list of /// messages already stored in the queue. @@ -76,210 +46,32 @@ std::function, double>( /// \brief Destructor. public: virtual ~Broker(); - /// \brief Start handling services - /// - /// This function allows us to wait to advertise capabilities to - /// clients until the broker has been entirely initialized. I.e., - /// after SetDefaultRadioConfiguration() has been called. + /// \brief Start. public: void Start(); - /// \brief Get the Ignition partition this broker is running in. - /// \return The partition name. - public: std::string IgnPartition() const; - - /// \brief Register a new client for message handling. Multiple clients for - /// the same address are allowed even from a single process. - /// \param[in] _clientAddress Address of the client. - /// \return ID of the client (should be later used for unregistration). - /// If the returned ID is invalidClientId, the registration failed. - public: ClientID Register(const std::string &_clientAddress); - - /// \brief Unregister a client and unbind from all its endpoints. - /// \param[in] _clientId The ID received from the Register() call. - /// \return True if the operation succeeded or false otherwise (if there is - /// no client registered for this ID or the ID is invalid). - public: bool Unregister(ClientID _clientId); - - /// \brief Dispatch all incoming messages. - public: bool DispatchMessages(); - - /// \brief Callback executed when a new registration request is received. - /// \param[in] _req The address contained in the request. - /// \param[out] _rep An ID of the registered client. This ID should be used - /// when unregistering this client. If registration failed, invalidClientId - /// value is returned. - private: bool OnAddrRegistration(const ignition::msgs::StringMsg &_req, - ignition::msgs::UInt32 &_rep); - - /// \brief Callback executed when a new unregistration request is received. - /// \param[in] _req ID of the client to unregister. - /// \param[out] _rep The result of the service. True when the unregistration - /// went OK or false otherwise (e.g.: the ID wasn't registered). - private: bool OnAddrUnregistration(const ignition::msgs::UInt32 &_req, - ignition::msgs::Boolean &_rep); - - /// \brief Callback executed when a new endpoint registration request is - /// received. - /// \param[in] _req The endpoint to register together with ID of the client - /// to which this registration belongs. - /// \param[out] _rep An ID of the endpoint. This ID should be used - /// when unregistering this endpoint. If registration failed, - /// invalidEndpointId value is returned. - private: bool OnEndPointRegistration( - const ignition::msgs::EndpointRegistration &_req, - ignition::msgs::UInt32 &_rep); - - /// \brief Callback executed when a new endpoint unregistration request is - /// received. - /// \param[in] _req ID of the endpoint to unregister. - /// \param[out] _rep The result of the service. True when the unregistration - /// went OK or false otherwise (e.g.: the ID wasn't registered). - private: bool OnEndPointUnregistration( - const ignition::msgs::UInt32 &_req, - ignition::msgs::Boolean &_rep); - - /// \brief Callback executed when a new request is received. - /// \param[in] _req The datagram contained in the request. - private: void OnMessage(const ignition::msgs::Datagram &_req); - - /// \brief Handle reset. - // public: void Reset(); - - // /// \brief Get a mutator to the team. - // /// \return A mutator to the team. - // public: TeamMembershipPtr Team(); - - // /// \brief Send a message to each member - // /// with its updated neighbors list. - // public: void NotifyNeighbors(); - - /// \brief Dispatch all incoming messages. - // public: bool DispatchMessages(); - - // /// \brief This method associates an endpoint with a broker client and its - // /// address. An endpoint is constructed as an address followed by ':', - // /// followed by the port. E.g.: "192.168.1.5:8000" is a valid endpoint. - // /// \param[in] _clientId ID of a registered client. - // /// \param[in] _endpoint End point requested to bind. - // /// \return ID that should be used for unbinding the endpoint. If - // /// invalidEndpointId is returned, the binding request failed (e.g. due to - // /// wrong endpoint specification, or if the _clientId is wrong). - // public: EndpointID Bind(ClientID _clientId, const std::string &_endpoint); - - // /// \brief This method cancels the association between a client and an - // /// endpoint. When all equal endpoints on the same client are unbound, - // /// the client will stop receiving messages for the given endpoint. - // /// \param[in] _endpointId ID of the endpoint to unbind. It has to be an ID - // /// received from a previous call to Bind(). - // /// \return Whether the unbind was successful. It may fail e.g. when the - // /// given ID is invalid and the broker doesn't know it. - // public: bool Unbind(EndpointID _endpointId); - - // /// \brief Register a new client for message handling. Multiple clients for - // /// the same address are allowed even from a single process. - // /// \param[in] _clientAddress Address of the client. - // /// \return ID of the client (should be later used for unregistration). - // /// If the returned ID is invalidClientId, the registration failed. - // public: ClientID Register(const std::string &_clientAddress); - - // /// \brief Unregister a client and unbind from all its endpoints. - // /// \param[in] _clientId The ID received from the Register() call. - // /// \return True if the operation succeeded or false otherwise (if there is - // /// no client registered for this ID or the ID is invalid). - // public: bool Unregister(ClientID _clientId); - - // /// \brief Set the radio configuration for address - // /// \param[in] address - // /// \param[in] radio_configuration - // public: void SetRadioConfiguration(const std::string& address, - // communication_model::radio_configuration config); - - // /// \brief Set the radio configuration - // /// \param[in] radio_configuration - // public: void SetDefaultRadioConfiguration( - // communication_model::radio_configuration config); - - // /// \brief Set the communication function handle to use - // /// \param[in] f Function that evaluates transmission of bytes across the - // /// communication channel. - // public: void SetCommunicationFunction( - // communication_model::communication_function f); - - // /// \brief Set the function to be used for updating pose - // /// \param[in] f Function that finds pose based on name - // public: void SetPoseUpdateFunction(pose_update_function f); - - // /// \brief Callback executed when a new registration request is received. - // /// \param[in] _req The address contained in the request. - // /// \param[out] _rep An ID of the registered client. This ID should be used - // /// when unregistering this client. If registration failed, invalidClientId - // /// value is returned. - // private: bool OnAddrRegistration(const ignition::msgs::StringMsg &_req, - // ignition::msgs::UInt32 &_rep); - - // /// \brief Callback executed when a new unregistration request is received. - // /// \param[in] _req ID of the client to unregister. - // /// \param[out] _rep The result of the service. True when the unregistration - // /// went OK or false otherwise (e.g.: the ID wasn't registered). - // private: bool OnAddrUnregistration(const ignition::msgs::UInt32 &_req, - // ignition::msgs::Boolean &_rep); - - // /// \brief Callback executed when a new endpoint registration request is - // /// received. - // /// \param[in] _req The endpoint to register together with ID of the client - // /// to which this registration belongs. - // /// \param[out] _rep An ID of the endpoint. This ID should be used - // /// when unregistering this endpoint. If registration failed, - // /// invalidEndpointId value is returned. - // private: bool OnEndPointRegistration( - // const subt::msgs::EndpointRegistration &_req, - // ignition::msgs::UInt32 &_rep); - - // /// \brief Callback executed when a new endpoint unregistration request is - // /// received. - // /// \param[in] _req ID of the endpoint to unregister. - // /// \param[out] _rep The result of the service. True when the unregistration - // /// went OK or false otherwise (e.g.: the ID wasn't registered). - // private: bool OnEndPointUnregistration( - // const ignition::msgs::UInt32 &_req, - // ignition::msgs::Boolean &_rep); - - // /// \brief Callback executed when a new request is received. - // /// \param[in] _req The datagram contained in the request. - // private: void OnMessage(const subt::msgs::Datagram &_req); - - // /// \brief Queue to store the incoming messages received from the clients. - // protected: std::deque incomingMsgs; - - // /// \brief List of bound endpoints. The key is an endpoint and the - // /// value is the vector of clients bounded on that endpoint. - // protected: EndPoints_M endpoints; - - // /// \brief Information about the members of the team. - // protected: TeamMembershipPtr team; + /// \brief ToDo. + public: void OnBind(const ignition::msgs::StringMsg_V &_req); - // /// \brief An Ignition Transport node for communications. - // private: ignition::transport::Node node; + /// \brief ToDo. + public: void OnUnbind(const ignition::msgs::StringMsg_V &_req); - // /// \brief The publisher for notifying neighbor updates. - // private: ignition::transport::Node::Publisher neighborPub; + /// \brief ToDo. + public: void OnMsg(const ignition::msgs::Datagram &_msg); - // /// \brief Protect data from races. - // private: std::mutex mutex; + /// \brief ToDo. + public: void DeliverMsgs(); - // /// \brief Function handle for evaluating communication - // private: subt::communication_model::communication_function - // communication_function; + /// \brief ToDo. + public: AddressManager &Data(); - // private: - // /// \brief Default radio configuration - // subt::communication_model::radio_configuration default_radio_configuration; + /// \brief ToDo. + public: void Lock(); - // /// \brief Pose update function - // private: pose_update_function pose_update_f; + /// \brief ToDo. + public: void Unlock(); - /// \brief Private definitions and data - private: std::unique_ptr dataPtr; + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } diff --git a/src/systems/comms/BrokerPlugin.cc b/src/systems/comms/BrokerPlugin.cc deleted file mode 100644 index c88f440a75..0000000000 --- a/src/systems/comms/BrokerPlugin.cc +++ /dev/null @@ -1,66 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#include -#include -#include -#include - -#include "ignition/gazebo/Util.hh" - -#include "Broker.hh" -#include "BrokerPlugin.hh" - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -class ignition::gazebo::systems::BrokerPluginPrivate -{ - /// \brief Broker instance. - public: Broker broker; -}; - -////////////////////////////////////////////////// -BrokerPlugin::BrokerPlugin() - : dataPtr(std::make_unique()) -{ -} - -////////////////////////////////////////////////// -void BrokerPlugin::Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &/*_ecm*/, - EventManager &/*_eventMgr*/) -{ -} - -////////////////////////////////////////////////// -void BrokerPlugin::PreUpdate( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) -{ - IGN_PROFILE("BrokerPlugin::PreUpdate"); -} - -IGNITION_ADD_PLUGIN(BrokerPlugin, - ignition::gazebo::System, - BrokerPlugin::ISystemConfigure, - BrokerPlugin::ISystemPreUpdate) - -IGNITION_ADD_PLUGIN_ALIAS(BrokerPlugin, - "ignition::gazebo::systems::BrokerPlugin") diff --git a/src/systems/comms/CMakeLists.txt b/src/systems/comms/CMakeLists.txt index 234e4fc04b..d8c0039db3 100644 --- a/src/systems/comms/CMakeLists.txt +++ b/src/systems/comms/CMakeLists.txt @@ -1,7 +1 @@ -gz_add_system(comms-broker - SOURCES - BrokerPlugin.cc - Broker.cc - PUBLIC_LINK_LIBS - ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} -) +add_subdirectory(models/) diff --git a/src/systems/comms/CommonTypes.hh b/src/systems/comms/CommonTypes.hh index 42ab14ed9a..f468d66e15 100644 --- a/src/systems/comms/CommonTypes.hh +++ b/src/systems/comms/CommonTypes.hh @@ -17,13 +17,7 @@ #ifndef IGNITION_GAZEBO_SYSTEMS_COMMONTYPES_HH_ #define IGNITION_GAZEBO_SYSTEMS_COMMONTYPES_HH_ -#include -#include #include -// #include - -// #include -// #include namespace ignition { @@ -34,46 +28,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { -// /// \def Neighbors_M -// /// \brief Map of neighbors -// /// \todo Fix second term to hold channel information (RSS, bitrate) -// using Neighbors_M = std::map; - -/// \brief Structure used to store information about a member of the -/// team. -struct TeamMember -{ - /// \brief Name used for this agent - std::string name; - - /// \brief Address of this agent. E.g.: 192.168.1.2 - std::string address; - - // /// \brief List of neighbors and comms probabilities for this robot. - // Neighbors_M neighbors; - - // /// \brief Static configuration of radio for communication - // subt::communication_model::radio_configuration radio; - - // /// \brief State of the radio, e.g., pose - // subt::rf_interface::radio_state rf_state; -}; - -/// \def TeamMemberPtr -/// \brief Shared pointer to TeamMember -using TeamMemberPtr = std::shared_ptr; - -/// \def TeamMembership_M -/// \brief Map containing information about the members of the team. -/// The key is the robot address. The value is a pointer to a TeamMember -/// object that contains multiple information about the robot. -using TeamMembership_M = std::map; - -/// \def TeamMembershipPtr -/// \brief A shared pointer to the membership data structure. -/// \sa TeamMembership_M -using TeamMembershipPtr = std::shared_ptr; - // /// \brief Address used to send a message to all the members of the team // /// listening on a specific port. // const std::string kBroadcast = "broadcast"; @@ -82,43 +36,14 @@ using TeamMembershipPtr = std::shared_ptr; // /// support multiple multicast groups, only one. // const std::string kMulticast = "multicast"; -// /// \brief Service used to centralize all messages sent from the agents. -const std::string kBrokerSrv = "/broker"; +// /// \brief Topic used to centralize all messages sent from the agents. +const std::string kBrokerTopic = "/broker"; /// \brief Service used to validate an address. -const std::string kAddrRegistrationSrv = "/address/register"; +const std::string kAddrBindSrv = "/broker/bind"; /// \brief Service used to invalidate an address. -const std::string kAddrUnregistrationSrv = "/address/unregister"; - -/// \brief Service used to register an end point. -const std::string kEndPointRegistrationSrv = "/end_point/register"; - -/// \brief Service used to unregister an end point. -const std::string kEndPointUnregistrationSrv = "/end_point/unregister"; - -/// \brief ID of a broker client. -typedef uint32_t ClientID; - -/// \brief ID denoting an invalid broker client. -constexpr ClientID invalidClientId {0}; - -// /// \brief Address used to receive neighbor updates. -// const std::string kNeighborsTopic = "/neighbors"; - -// /// \brief Default port. -// const uint32_t kDefaultPort = 4100u; - -// /// \brief ID of a broker client. -// typedef uint32_t ClientID; -// /// \brief ID denoting an invalid broker client. -// constexpr ClientID invalidClientId {0}; - -// /// \brief ID of a bound endpoint belonging to a particular client. -// typedef uint32_t EndpointID; -// /// \brief ID denoting an invalid endpoint. -// constexpr EndpointID invalidEndpointId {0}; - +const std::string kAddrUnbindSrv = "/broker/unbind"; } } } diff --git a/src/systems/comms/CommsClient.hh b/src/systems/comms/CommsClient.hh index a276acee32..93b781fd2c 100644 --- a/src/systems/comms/CommsClient.hh +++ b/src/systems/comms/CommsClient.hh @@ -15,11 +15,6 @@ * */ -/* - * Development of this module has been funded by the Monterey Bay Aquarium - * Research Institute (MBARI) and the David and Lucile Packard Foundation - */ - #ifndef IGNITION_GAZEBO_SYSTEMS_COMMSCLIENT_HH_ #define IGNITION_GAZEBO_SYSTEMS_COMMSCLIENT_HH_ diff --git a/src/systems/comms/CommsModel.hh b/src/systems/comms/CommsModel.hh new file mode 100644 index 0000000000..b5dfa904c9 --- /dev/null +++ b/src/systems/comms/CommsModel.hh @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_COMMS_COMMSMODEL_HH__ +#define IGNITION_GAZEBO_SYSTEMS_COMMS_COMMSMODEL_HH__ + +#include +#include +#include + +#include "AddressManager.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + /// \brief Abstract interface to define how the environment should handle + /// communication simulation. This class should be responsible for + /// handling dropouts, decay and packet collisions. + class ICommsModel + { + /// \brief This method is called when there is a timestep in the simulator + /// override this to update your data structures as needed. + /// \param[in] _info - Simulator information about the current timestep. + /// \param[in] _ecm - Ignition's ECM. + /// \param[in] _messageMgr - Use this to mark the message as arrived. + public: virtual void Step( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm, + AddressManager &_messageMgr) = 0; + + /// \brief Destructor + public: virtual ~ICommsModel() = default; + }; + } +} +} +} + +#endif diff --git a/src/systems/comms/models/CMakeLists.txt b/src/systems/comms/models/CMakeLists.txt new file mode 100644 index 0000000000..6134a572c6 --- /dev/null +++ b/src/systems/comms/models/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(perfect-comms-model + SOURCES + PerfectCommsModel.cc + ../AddressManager.cc + ../Broker.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/comms/models/PerfectCommsModel.cc b/src/systems/comms/models/PerfectCommsModel.cc new file mode 100644 index 0000000000..491f39293d --- /dev/null +++ b/src/systems/comms/models/PerfectCommsModel.cc @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include + +#include "ignition/gazebo/Util.hh" + +#include "../Broker.hh" +#include "PerfectCommsModel.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::PerfectCommsModel::Implementation +{ + /// \brief Broker instance. + public: Broker broker; +}; + +////////////////////////////////////////////////// +PerfectCommsModel::PerfectCommsModel() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ + ignerr << "Broker plugin running" << std::endl; +} + +///////////////////////////PerfectCommsModel/////////////////////// +PerfectCommsModel::~PerfectCommsModel() +{ + // cannot use default destructor because of dataPtr +} + +////////////////////////////////////////////////// +void PerfectCommsModel::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->broker.Start(); +} + +////////////////////////////////////////////////// +void PerfectCommsModel::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("PerfectCommsModel::PreUpdate"); + + // Step the comms model. + this->Step(_info, _ecm, this->dataPtr->broker.Data()); + + // Deliver the inbound messages. + this->dataPtr->broker.DeliverMsgs(); +} + +void PerfectCommsModel::Step( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm, + AddressManager &_messageMgr) +{ + this->dataPtr->broker.Lock(); + + // Check if there are new messages to process. + auto &allData = this->dataPtr->broker.Data().Data(); + + // Create a copy to modify while we iterate. + auto allDataCopy = this->dataPtr->broker.Data().Copy(); + + for (auto & [address, content] : allData) + { + // Reference to the outbound queue for this address. + auto &outbound = content.outboundMsgs; + + // All these messages need to be processed. + for (auto &msg : outbound) + allDataCopy[msg->dst_address()].inboundMsgs.push_back(msg); + + allDataCopy[address].outboundMsgs.clear(); + } + + this->dataPtr->broker.Data().Set(allDataCopy); + + this->dataPtr->broker.Unlock(); +} + +IGNITION_ADD_PLUGIN(PerfectCommsModel, + ignition::gazebo::System, + PerfectCommsModel::ISystemConfigure, + PerfectCommsModel::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(PerfectCommsModel, + "ignition::gazebo::systems::PerfectCommsModel") diff --git a/src/systems/comms/BrokerPlugin.hh b/src/systems/comms/models/PerfectCommsModel.hh similarity index 71% rename from src/systems/comms/BrokerPlugin.hh rename to src/systems/comms/models/PerfectCommsModel.hh index 1024f5c44e..5492833eef 100644 --- a/src/systems/comms/BrokerPlugin.hh +++ b/src/systems/comms/models/PerfectCommsModel.hh @@ -14,11 +14,13 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_BROKERPLUGIN_HH_ -#define IGNITION_GAZEBO_SYSTEMS_BROKERPLUGIN_HH_ +#ifndef IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMSMODEL_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMSMODEL_HH_ #include -#include +#include +#include "../AddressManager.hh" +#include "../CommsModel.hh" namespace ignition { @@ -28,20 +30,18 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - // Forward declaration - class BrokerPluginPrivate; - /// \brief - class BrokerPlugin + class PerfectCommsModel : public System, + public ICommsModel, public ISystemConfigure, public ISystemPreUpdate { /// \brief Constructor - public: BrokerPlugin(); + public: PerfectCommsModel(); /// \brief Destructor - public: ~BrokerPlugin() override = default; + public: ~PerfectCommsModel(); // Documentation inherited public: void Configure(const Entity &_entity, @@ -54,8 +54,14 @@ namespace systems const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; - /// \brief Private data pointer - private: std::unique_ptr dataPtr; + /// \brief + public: void Step( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm, + AddressManager &_messageMgr); + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } From 80678b3776803bc488e6ae614fb75ea8abc87d88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 14 Mar 2022 19:11:30 +0100 Subject: [PATCH 04/51] Layout updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/standalone/comms/CMakeLists.txt | 14 +++ examples/standalone/comms/publisher.cc | 92 +++++++++++++++++++ examples/standalone/comms/subscriber.cc | 57 ++++++++++++ examples/worlds/comms.sdf | 4 +- .../ignition/gazebo}/Broker.hh | 12 +-- .../ignition/gazebo}/CommonTypes.hh | 8 +- .../ignition/gazebo}/CommsModel.hh | 12 +-- .../ignition/gazebo/MsgManager.hh | 21 ++--- src/{systems/comms => }/Broker.cc | 14 ++- src/CMakeLists.txt | 3 + .../comms/AddressManager.cc => MsgManager.cc} | 43 ++++----- src/systems/CMakeLists.txt | 2 +- src/systems/comms/CMakeLists.txt | 1 - src/systems/perfect_comms/CMakeLists.txt | 6 ++ .../{comms => perfect_comms}/CommsClient.hh | 0 .../PerfectComms.cc} | 38 ++++---- .../PerfectComms.hh} | 16 ++-- .../models/CMakeLists.txt | 2 +- 18 files changed, 249 insertions(+), 96 deletions(-) create mode 100644 examples/standalone/comms/CMakeLists.txt create mode 100644 examples/standalone/comms/publisher.cc create mode 100644 examples/standalone/comms/subscriber.cc rename {src/systems/comms => include/ignition/gazebo}/Broker.hh (91%) rename {src/systems/comms => include/ignition/gazebo}/CommonTypes.hh (92%) rename {src/systems/comms => include/ignition/gazebo}/CommsModel.hh (89%) rename src/systems/comms/AddressManager.hh => include/ignition/gazebo/MsgManager.hh (86%) rename src/{systems/comms => }/Broker.cc (94%) rename src/{systems/comms/AddressManager.cc => MsgManager.cc} (79%) delete mode 100644 src/systems/comms/CMakeLists.txt create mode 100644 src/systems/perfect_comms/CMakeLists.txt rename src/systems/{comms => perfect_comms}/CommsClient.hh (100%) rename src/systems/{comms/models/PerfectCommsModel.cc => perfect_comms/PerfectComms.cc} (77%) rename src/systems/{comms/models/PerfectCommsModel.hh => perfect_comms/PerfectComms.hh} (85%) rename src/systems/{comms => perfect_comms}/models/CMakeLists.txt (88%) diff --git a/examples/standalone/comms/CMakeLists.txt b/examples/standalone/comms/CMakeLists.txt new file mode 100644 index 0000000000..404b2ecb8b --- /dev/null +++ b/examples/standalone/comms/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(ign-gazebo-comms) + +find_package(ignition-transport11 QUIET REQUIRED) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) + +add_executable(publisher publisher.cc) +target_link_libraries(publisher + ignition-transport${IGN_TRANSPORT_VER}::core) + +add_executable(subscriber subscriber.cc) +target_link_libraries(subscriber + ignition-transport${IGN_TRANSPORT_VER}::core) diff --git a/examples/standalone/comms/publisher.cc b/examples/standalone/comms/publisher.cc new file mode 100644 index 0000000000..2f7c88918c --- /dev/null +++ b/examples/standalone/comms/publisher.cc @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +// Example: ./publisher addr1 + +#include +#include +#include +#include +#include +#include +#include +#include + +/// \brief Flag used to break the publisher loop and terminate the program. +static std::atomic g_terminatePub(false); + +////////////////////////////////////////////////// +/// \brief Function callback executed when a SIGINT or SIGTERM signals are +/// captured. This is used to break the infinite loop that publishes messages +/// and exit the program smoothly. +void signal_handler(int _signal) +{ + if (_signal == SIGINT || _signal == SIGTERM) + g_terminatePub = true; +} + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + // Install a signal handler for SIGINT and SIGTERM. + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + // Create a transport node and advertise a topic. + ignition::transport::Node node; + std::string topic = "/broker"; + + auto pub = node.Advertise(topic); + if (!pub) + { + std::cerr << "Error advertising topic [" << topic << "]" << std::endl; + return -1; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Prepare the message. + ignition::msgs::Datagram msg; + msg.set_src_address("unused"); + msg.set_dst_address(argv[1]); + + // Publish messages at 1Hz. + int counter = 0; + while (!g_terminatePub) + { + // Prepare the payload. + ignition::msgs::StringMsg payload; + payload.set_data("hello world " + std::to_string(counter)); + std::string serializedData; + if (!payload.SerializeToString(&serializedData)) + { + std::cerr << "Error serializing message\n" + << payload.DebugString() << std::endl; + } + msg.set_data(serializedData); + + if (!pub.Publish(msg)) + break; + + counter++; + + std::cout << "Publishing hello on topic [" << topic << "]" << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + return 0; +} diff --git a/examples/standalone/comms/subscriber.cc b/examples/standalone/comms/subscriber.cc new file mode 100644 index 0000000000..d65a79c5d7 --- /dev/null +++ b/examples/standalone/comms/subscriber.cc @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +// Example: ./subscriber addr1 addr1/rx + +#include +#include +#include + +////////////////////////////////////////////////// +/// \brief Function called each time a topic update is received. +void cb(const ignition::msgs::Datagram &_msg) +{ + std::cout << "Msg: " << _msg.data() << std::endl << std::endl; +} + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + // Create a transport node. + ignition::transport::Node node; + std::string addr = argv[1]; + std::string topic = argv[2]; + + // Subscribe to a topic by registering a callback. + if (!node.Subscribe(topic, cb)) + { + std::cerr << "Error subscribing to topic [" << topic << "]" << std::endl; + return -1; + } + + // Prepare the input parameters. + ignition::msgs::StringMsg_V req; + req.add_data(addr); + req.add_data(topic); + + // Bind. + if (!node.Request("/broker/bind", req)) + std::cerr << "Service call failed" << std::endl; + + // Zzzzzz. + ignition::transport::waitForShutdown(); +} diff --git a/examples/worlds/comms.sdf b/examples/worlds/comms.sdf index aec4ac52f1..ef212fad6e 100644 --- a/examples/worlds/comms.sdf +++ b/examples/worlds/comms.sdf @@ -110,8 +110,8 @@ + filename="ignition-gazebo-perfect-comms-system" + name="ignition::gazebo::systems::PerfectComms"> diff --git a/src/systems/comms/Broker.hh b/include/ignition/gazebo/Broker.hh similarity index 91% rename from src/systems/comms/Broker.hh rename to include/ignition/gazebo/Broker.hh index 8121bab9af..b8840cc387 100644 --- a/src/systems/comms/Broker.hh +++ b/include/ignition/gazebo/Broker.hh @@ -17,24 +17,21 @@ /// \file Broker.hh /// \brief Broker for handling message delivery among robots. -#ifndef IGNITION_GAZEBO_SYSTEMS_BROKER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_BROKER_HH_ +#ifndef IGNITION_GAZEBO_BROKER_HH_ +#define IGNITION_GAZEBO_BROKER_HH_ #include +#include #include #include #include -#include "AddressManager.hh" - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ /// \brief Store messages, and exposes an API for registering new clients, /// bind to a particular address, push new messages or get the list of /// messages already stored in the queue. @@ -62,7 +59,7 @@ namespace systems public: void DeliverMsgs(); /// \brief ToDo. - public: AddressManager &Data(); + public: ignition::gazebo::MsgManager &Data(); /// \brief ToDo. public: void Lock(); @@ -73,7 +70,6 @@ namespace systems /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; - } } } } diff --git a/src/systems/comms/CommonTypes.hh b/include/ignition/gazebo/CommonTypes.hh similarity index 92% rename from src/systems/comms/CommonTypes.hh rename to include/ignition/gazebo/CommonTypes.hh index f468d66e15..73aa0e6af0 100644 --- a/src/systems/comms/CommonTypes.hh +++ b/include/ignition/gazebo/CommonTypes.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_COMMONTYPES_HH_ -#define IGNITION_GAZEBO_SYSTEMS_COMMONTYPES_HH_ +#ifndef IGNITION_GAZEBO_COMMONTYPES_HH_ +#define IGNITION_GAZEBO_COMMONTYPES_HH_ #include @@ -25,9 +25,6 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ - // /// \brief Address used to send a message to all the members of the team // /// listening on a specific port. // const std::string kBroadcast = "broadcast"; @@ -47,6 +44,5 @@ const std::string kAddrUnbindSrv = "/broker/unbind"; } } } -} #endif \ No newline at end of file diff --git a/src/systems/comms/CommsModel.hh b/include/ignition/gazebo/CommsModel.hh similarity index 89% rename from src/systems/comms/CommsModel.hh rename to include/ignition/gazebo/CommsModel.hh index b5dfa904c9..704abb3423 100644 --- a/src/systems/comms/CommsModel.hh +++ b/include/ignition/gazebo/CommsModel.hh @@ -15,23 +15,20 @@ * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_COMMS_COMMSMODEL_HH__ -#define IGNITION_GAZEBO_SYSTEMS_COMMS_COMMSMODEL_HH__ +#ifndef IGNITION_GAZEBO_COMMSMODEL_HH__ +#define IGNITION_GAZEBO_COMMSMODEL_HH__ #include +#include #include #include -#include "AddressManager.hh" - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ /// \brief Abstract interface to define how the environment should handle /// communication simulation. This class should be responsible for /// handling dropouts, decay and packet collisions. @@ -45,12 +42,11 @@ namespace systems public: virtual void Step( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm, - AddressManager &_messageMgr) = 0; + ignition::gazebo::MsgManager &_messageMgr) = 0; /// \brief Destructor public: virtual ~ICommsModel() = default; }; - } } } } diff --git a/src/systems/comms/AddressManager.hh b/include/ignition/gazebo/MsgManager.hh similarity index 86% rename from src/systems/comms/AddressManager.hh rename to include/ignition/gazebo/MsgManager.hh index 7c4fadd34b..32dd513e49 100644 --- a/src/systems/comms/AddressManager.hh +++ b/include/ignition/gazebo/MsgManager.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_ADDRESSMANAGER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_ADDRESSMANAGER_HH_ +#ifndef IGNITION_GAZEBO_MSGMANAGER_HH_ +#define IGNITION_GAZEBO_MSGMANAGER_HH_ #include #include @@ -33,10 +33,8 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ -struct AddressContent +struct MsgContent { /// \brief Queue of inbound messages. public: std::deque> inboundMsgs; @@ -51,13 +49,13 @@ struct AddressContent }; /// \brief -class AddressManager +class MsgManager { /// \brief Default constructor. - public: AddressManager(); + public: MsgManager(); /// \brief Destructor. - public: virtual ~AddressManager(); + public: virtual ~MsgManager(); /// \brief public: void AddSubscriber(const std::string &_address, @@ -91,13 +89,13 @@ class AddressManager public: void DeliverMsgs(); /// \brief ToDo. - public: std::map &Data(); + public: std::map &Data(); /// \brief ToDo. - public: std::map Copy(); + public: std::map Copy(); /// \brief ToDo. - public: void Set(std::map &_newContent); + public: void Set(std::map &_newContent); /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) @@ -105,6 +103,5 @@ class AddressManager } } } -} #endif diff --git a/src/systems/comms/Broker.cc b/src/Broker.cc similarity index 94% rename from src/systems/comms/Broker.cc rename to src/Broker.cc index 1accd10354..a03cd84c1a 100644 --- a/src/systems/comms/Broker.cc +++ b/src/Broker.cc @@ -23,25 +23,23 @@ #include #include +#include +#include +#include #include #include -#include "AddressManager.hh" -#include "Broker.hh" -#include "CommonTypes.hh" - using namespace ignition; using namespace gazebo; -using namespace systems; /// \brief Private Broker data class. -class ignition::gazebo::systems::Broker::Implementation +class ignition::gazebo::Broker::Implementation { /// \brief An Ignition Transport node for communications. public: ignition::transport::Node node; /// \brief The message manager. - public: AddressManager data; + public: ignition::gazebo::MsgManager data; /// \brief Protect data from races. public: std::mutex mutex; @@ -133,7 +131,7 @@ void Broker::DeliverMsgs() } ////////////////////////////////////////////////// -AddressManager &Broker::Data() +MsgManager &Broker::Data() { return this->dataPtr->data; } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c609bec040..8115003468 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -47,11 +47,13 @@ target_link_libraries(${ign_lib_target} set (sources Barrier.cc BaseView.cc + Broker.cc Conversions.cc EntityComponentManager.cc LevelManager.cc Link.cc Model.cc + MsgManager.cc Primitives.cc SdfEntityCreator.cc SdfGenerator.cc @@ -67,6 +69,7 @@ set (sources cmd/ModelCommandAPI.cc ${PROTO_PRIVATE_SRC} ${network_sources} + ${comms_sources} ) set (gtest_sources diff --git a/src/systems/comms/AddressManager.cc b/src/MsgManager.cc similarity index 79% rename from src/systems/comms/AddressManager.cc rename to src/MsgManager.cc index 01f705f907..ab051db73d 100644 --- a/src/systems/comms/AddressManager.cc +++ b/src/MsgManager.cc @@ -22,42 +22,40 @@ #include #include #include +#include +#include #include #include -#include "AddressManager.hh" -#include "CommonTypes.hh" - using namespace ignition; using namespace gazebo; -using namespace systems; -/// \brief Private AddressManager data class. -class ignition::gazebo::systems::AddressManager::Implementation +/// \brief Private MsgManager data class. +class ignition::gazebo::MsgManager::Implementation { /// \brief Buffer to store the content associated to each address. /// The key is an address. The value contains all the information associated /// to the address. - public: std::map data; + public: std::map data; /// \brief An Ignition Transport node for communications. public: ignition::transport::Node node; }; ////////////////////////////////////////////////// -AddressManager::AddressManager() +MsgManager::MsgManager() : dataPtr(ignition::utils::MakeUniqueImpl()) { } ////////////////////////////////////////////////// -AddressManager::~AddressManager() +MsgManager::~MsgManager() { // cannot use default destructor because of dataPtr } ////////////////////////////////////////////////// -void AddressManager::AddSubscriber(const std::string &_address, +void MsgManager::AddSubscriber(const std::string &_address, const std::string &_topic) { ignition::transport::Node::Publisher publisher = @@ -67,7 +65,7 @@ void AddressManager::AddSubscriber(const std::string &_address, } ////////////////////////////////////////////////// -// std::unordered_set AddressManager::Subscribers( +// std::unordered_set MsgManager::Subscribers( // const std::string &_address) // { // std::unordered_set result; @@ -81,21 +79,21 @@ void AddressManager::AddSubscriber(const std::string &_address, // } ////////////////////////////////////////////////// -void AddressManager::AddInbound(const std::string &_address, +void MsgManager::AddInbound(const std::string &_address, const std::shared_ptr &_msg) { this->dataPtr->data[_address].inboundMsgs.push_back(_msg); } ////////////////////////////////////////////////// -void AddressManager::AddOutbound(const std::string &_address, +void MsgManager::AddOutbound(const std::string &_address, const std::shared_ptr &_msg) { this->dataPtr->data[_address].outboundMsgs.push_back(_msg); } ////////////////////////////////////////////////// -bool AddressManager::RemoveSubscriber(const std::string &_address, +bool MsgManager::RemoveSubscriber(const std::string &_address, const std::string &_topic) { // Iterate over all the topics. @@ -109,7 +107,7 @@ bool AddressManager::RemoveSubscriber(const std::string &_address, } ////////////////////////////////////////////////// -void AddressManager::RemoveInbound(const std::string &_address, +void MsgManager::RemoveInbound(const std::string &_address, const std::shared_ptr &_msg) { if (this->dataPtr->data.find(_address) != @@ -121,7 +119,7 @@ void AddressManager::RemoveInbound(const std::string &_address, } ////////////////////////////////////////////////// -void AddressManager::RemoveOutbound(const std::string &_address, +void MsgManager::RemoveOutbound(const std::string &_address, const std::shared_ptr &_msg) { if (this->dataPtr->data.find(_address) != this->dataPtr->data.end()) @@ -132,7 +130,7 @@ void AddressManager::RemoveOutbound(const std::string &_address, } ////////////////////////////////////////////////// -void AddressManager::DeliverMsgs() +void MsgManager::DeliverMsgs() { for (auto & [address, content] : this->dataPtr->data) { @@ -144,7 +142,10 @@ void AddressManager::DeliverMsgs() { // Use the publisher associated to the destination address. for (auto & [topic, publisher] : content.subscriptions) + { publisher.Publish(*msg); + ignmsg << "Publishing msg [" << msg->DebugString() << "]" << std::endl; + } } content.inboundMsgs.clear(); @@ -152,19 +153,19 @@ void AddressManager::DeliverMsgs() } ////////////////////////////////////////////////// -std::map &AddressManager::Data() +std::map &MsgManager::Data() { return this->dataPtr->data; } ////////////////////////////////////////////////// -std::map AddressManager::Copy() +std::map MsgManager::Copy() { return this->dataPtr->data; } ////////////////////////////////////////////////// -void AddressManager::Set(std::map &_newContent) +void MsgManager::Set(std::map &_newContent) { -this->dataPtr->data = _newContent; + this->dataPtr->data = _newContent; } diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 1cef82711b..4c39caeeeb 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -102,7 +102,6 @@ add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) add_subdirectory(buoyancy_engine) add_subdirectory(collada_world_exporter) -add_subdirectory(comms) add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) @@ -137,6 +136,7 @@ add_subdirectory(particle_emitter) add_subdirectory(particle_emitter2) add_subdirectory(performer_detector) add_subdirectory(physics) +add_subdirectory(perfect_comms) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) diff --git a/src/systems/comms/CMakeLists.txt b/src/systems/comms/CMakeLists.txt deleted file mode 100644 index d8c0039db3..0000000000 --- a/src/systems/comms/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -add_subdirectory(models/) diff --git a/src/systems/perfect_comms/CMakeLists.txt b/src/systems/perfect_comms/CMakeLists.txt new file mode 100644 index 0000000000..fae61ef801 --- /dev/null +++ b/src/systems/perfect_comms/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(perfect-comms + SOURCES + PerfectComms.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/comms/CommsClient.hh b/src/systems/perfect_comms/CommsClient.hh similarity index 100% rename from src/systems/comms/CommsClient.hh rename to src/systems/perfect_comms/CommsClient.hh diff --git a/src/systems/comms/models/PerfectCommsModel.cc b/src/systems/perfect_comms/PerfectComms.cc similarity index 77% rename from src/systems/comms/models/PerfectCommsModel.cc rename to src/systems/perfect_comms/PerfectComms.cc index 491f39293d..6a051b980e 100644 --- a/src/systems/comms/models/PerfectCommsModel.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -16,40 +16,38 @@ */ #include -#include +#include #include +#include #include -#include "ignition/gazebo/Util.hh" - -#include "../Broker.hh" -#include "PerfectCommsModel.hh" +#include "PerfectComms.hh" using namespace ignition; using namespace gazebo; using namespace systems; -class ignition::gazebo::systems::PerfectCommsModel::Implementation +class ignition::gazebo::systems::PerfectComms::Implementation { /// \brief Broker instance. - public: Broker broker; + public: ignition::gazebo::Broker broker; }; ////////////////////////////////////////////////// -PerfectCommsModel::PerfectCommsModel() +PerfectComms::PerfectComms() : dataPtr(ignition::utils::MakeUniqueImpl()) { ignerr << "Broker plugin running" << std::endl; } -///////////////////////////PerfectCommsModel/////////////////////// -PerfectCommsModel::~PerfectCommsModel() +////////////////////////////////////////////////// +PerfectComms::~PerfectComms() { // cannot use default destructor because of dataPtr } ////////////////////////////////////////////////// -void PerfectCommsModel::Configure(const Entity &_entity, +void PerfectComms::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &/*_ecm*/, EventManager &/*_eventMgr*/) @@ -58,11 +56,11 @@ void PerfectCommsModel::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void PerfectCommsModel::PreUpdate( +void PerfectComms::PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { - IGN_PROFILE("PerfectCommsModel::PreUpdate"); + IGN_PROFILE("PerfectComms::PreUpdate"); // Step the comms model. this->Step(_info, _ecm, this->dataPtr->broker.Data()); @@ -71,10 +69,10 @@ void PerfectCommsModel::PreUpdate( this->dataPtr->broker.DeliverMsgs(); } -void PerfectCommsModel::Step( +void PerfectComms::Step( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm, - AddressManager &_messageMgr) + MsgManager &_messageMgr) { this->dataPtr->broker.Lock(); @@ -101,10 +99,10 @@ void PerfectCommsModel::Step( this->dataPtr->broker.Unlock(); } -IGNITION_ADD_PLUGIN(PerfectCommsModel, +IGNITION_ADD_PLUGIN(PerfectComms, ignition::gazebo::System, - PerfectCommsModel::ISystemConfigure, - PerfectCommsModel::ISystemPreUpdate) + PerfectComms::ISystemConfigure, + PerfectComms::ISystemPreUpdate) -IGNITION_ADD_PLUGIN_ALIAS(PerfectCommsModel, - "ignition::gazebo::systems::PerfectCommsModel") +IGNITION_ADD_PLUGIN_ALIAS(PerfectComms, + "ignition::gazebo::systems::PerfectComms") diff --git a/src/systems/comms/models/PerfectCommsModel.hh b/src/systems/perfect_comms/PerfectComms.hh similarity index 85% rename from src/systems/comms/models/PerfectCommsModel.hh rename to src/systems/perfect_comms/PerfectComms.hh index 5492833eef..0a88c415ba 100644 --- a/src/systems/comms/models/PerfectCommsModel.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -14,13 +14,13 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMSMODEL_HH_ -#define IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMSMODEL_HH_ +#ifndef IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ +#include +#include #include #include -#include "../AddressManager.hh" -#include "../CommsModel.hh" namespace ignition { @@ -31,17 +31,17 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { /// \brief - class PerfectCommsModel + class PerfectComms : public System, public ICommsModel, public ISystemConfigure, public ISystemPreUpdate { /// \brief Constructor - public: PerfectCommsModel(); + public: PerfectComms(); /// \brief Destructor - public: ~PerfectCommsModel(); + public: ~PerfectComms(); // Documentation inherited public: void Configure(const Entity &_entity, @@ -58,7 +58,7 @@ namespace systems public: void Step( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm, - AddressManager &_messageMgr); + MsgManager &_messageMgr); /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) diff --git a/src/systems/comms/models/CMakeLists.txt b/src/systems/perfect_comms/models/CMakeLists.txt similarity index 88% rename from src/systems/comms/models/CMakeLists.txt rename to src/systems/perfect_comms/models/CMakeLists.txt index 6134a572c6..c1b72abb68 100644 --- a/src/systems/comms/models/CMakeLists.txt +++ b/src/systems/perfect_comms/models/CMakeLists.txt @@ -1,7 +1,7 @@ gz_add_system(perfect-comms-model SOURCES PerfectCommsModel.cc - ../AddressManager.cc + ../MsgManager.cc ../Broker.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} From 971efab3152ae770ba5e07c94279abeb90e1260f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 14 Mar 2022 17:56:01 -0700 Subject: [PATCH 05/51] move comms files to comms dir and update namespace Signed-off-by: Ian Chen --- include/ignition/gazebo/{ => comms}/Broker.hh | 10 ++++++++-- .../ignition/gazebo/{ => comms}/CommonTypes.hh | 7 ++++++- .../ignition/gazebo/{ => comms}/CommsModel.hh | 17 ++++++++++------- .../ignition/gazebo/{ => comms}/MsgManager.hh | 8 ++++++-- src/CMakeLists.txt | 9 +++++++-- src/{ => comms}/Broker.cc | 18 ++++++++++-------- src/{ => comms}/MsgManager.cc | 18 ++++++++++-------- src/systems/perfect_comms/PerfectComms.cc | 15 ++++++++------- src/systems/perfect_comms/PerfectComms.hh | 11 ++++++----- 9 files changed, 71 insertions(+), 42 deletions(-) rename include/ignition/gazebo/{ => comms}/Broker.hh (93%) rename include/ignition/gazebo/{ => comms}/CommonTypes.hh (95%) rename include/ignition/gazebo/{ => comms}/CommsModel.hh (82%) rename include/ignition/gazebo/{ => comms}/MsgManager.hh (96%) rename src/{ => comms}/Broker.cc (93%) rename src/{ => comms}/MsgManager.cc (92%) diff --git a/include/ignition/gazebo/Broker.hh b/include/ignition/gazebo/comms/Broker.hh similarity index 93% rename from include/ignition/gazebo/Broker.hh rename to include/ignition/gazebo/comms/Broker.hh index b8840cc387..1a9cf9559c 100644 --- a/include/ignition/gazebo/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -21,17 +21,22 @@ #define IGNITION_GAZEBO_BROKER_HH_ #include -#include #include #include #include +#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/config.hh" + namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace comms +{ + /// \brief Store messages, and exposes an API for registering new clients, /// bind to a particular address, push new messages or get the list of /// messages already stored in the queue. @@ -59,7 +64,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void DeliverMsgs(); /// \brief ToDo. - public: ignition::gazebo::MsgManager &Data(); + public: MsgManager &Data(); /// \brief ToDo. public: void Lock(); @@ -73,5 +78,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { } } } +} #endif diff --git a/include/ignition/gazebo/CommonTypes.hh b/include/ignition/gazebo/comms/CommonTypes.hh similarity index 95% rename from include/ignition/gazebo/CommonTypes.hh rename to include/ignition/gazebo/comms/CommonTypes.hh index 73aa0e6af0..6231385754 100644 --- a/include/ignition/gazebo/CommonTypes.hh +++ b/include/ignition/gazebo/comms/CommonTypes.hh @@ -19,12 +19,16 @@ #include +#include "ignition/gazebo/config.hh" + namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace comms +{ // /// \brief Address used to send a message to all the members of the team // /// listening on a specific port. // const std::string kBroadcast = "broadcast"; @@ -44,5 +48,6 @@ const std::string kAddrUnbindSrv = "/broker/unbind"; } } } +} -#endif \ No newline at end of file +#endif diff --git a/include/ignition/gazebo/CommsModel.hh b/include/ignition/gazebo/comms/CommsModel.hh similarity index 82% rename from include/ignition/gazebo/CommsModel.hh rename to include/ignition/gazebo/comms/CommsModel.hh index 704abb3423..de00b35846 100644 --- a/include/ignition/gazebo/CommsModel.hh +++ b/include/ignition/gazebo/comms/CommsModel.hh @@ -18,17 +18,21 @@ #ifndef IGNITION_GAZEBO_COMMSMODEL_HH__ #define IGNITION_GAZEBO_COMMSMODEL_HH__ -#include -#include -#include #include +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/System.hh" +#include "ignition/gazebo/comms/MsgManager.hh" + namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace comms +{ /// \brief Abstract interface to define how the environment should handle /// communication simulation. This class should be responsible for /// handling dropouts, decay and packet collisions. @@ -39,10 +43,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _info - Simulator information about the current timestep. /// \param[in] _ecm - Ignition's ECM. /// \param[in] _messageMgr - Use this to mark the message as arrived. - public: virtual void Step( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::MsgManager &_messageMgr) = 0; + public: virtual void Step(const UpdateInfo &_info, + EntityComponentManager &_ecm, MsgManager &_messageMgr) = 0; /// \brief Destructor public: virtual ~ICommsModel() = default; @@ -50,5 +52,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { } } } +} #endif diff --git a/include/ignition/gazebo/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh similarity index 96% rename from include/ignition/gazebo/MsgManager.hh rename to include/ignition/gazebo/comms/MsgManager.hh index 32dd513e49..12b5d91f09 100644 --- a/include/ignition/gazebo/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -23,17 +23,20 @@ #include #include #include -#include #include #include +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/System.hh" + namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { - +namespace comms +{ struct MsgContent { /// \brief Queue of inbound messages. @@ -103,5 +106,6 @@ class MsgManager } } } +} #endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8115003468..a2b2389424 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -26,6 +26,11 @@ set(network_sources network/PeerTracker.cc ) +set(comms_sources + comms/Broker.cc + comms/MsgManager.cc +) + set(gui_sources ${gui_sources} PARENT_SCOPE @@ -47,13 +52,13 @@ target_link_libraries(${ign_lib_target} set (sources Barrier.cc BaseView.cc - Broker.cc +# Broker.cc Conversions.cc EntityComponentManager.cc LevelManager.cc Link.cc Model.cc - MsgManager.cc +# MsgManager.cc Primitives.cc SdfEntityCreator.cc SdfGenerator.cc diff --git a/src/Broker.cc b/src/comms/Broker.cc similarity index 93% rename from src/Broker.cc rename to src/comms/Broker.cc index a03cd84c1a..374dd7227b 100644 --- a/src/Broker.cc +++ b/src/comms/Broker.cc @@ -23,28 +23,30 @@ #include #include -#include -#include -#include -#include #include -using namespace ignition; -using namespace gazebo; +#include "ignition/gazebo/comms/Broker.hh" +#include "ignition/gazebo/comms/CommonTypes.hh" +#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/Util.hh" /// \brief Private Broker data class. -class ignition::gazebo::Broker::Implementation +class ignition::gazebo::comms::Broker::Implementation { /// \brief An Ignition Transport node for communications. public: ignition::transport::Node node; /// \brief The message manager. - public: ignition::gazebo::MsgManager data; + public: MsgManager data; /// \brief Protect data from races. public: std::mutex mutex; }; +using namespace ignition; +using namespace gazebo; +using namespace comms; + ////////////////////////////////////////////////// Broker::Broker() : dataPtr(ignition::utils::MakeUniqueImpl()) diff --git a/src/MsgManager.cc b/src/comms/MsgManager.cc similarity index 92% rename from src/MsgManager.cc rename to src/comms/MsgManager.cc index ab051db73d..2e8b9b35bd 100644 --- a/src/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -22,16 +22,14 @@ #include #include #include -#include -#include #include #include -using namespace ignition; -using namespace gazebo; +#include "ignition/gazebo/comms/CommonTypes.hh" +#include "ignition/gazebo/comms/MsgManager.hh" /// \brief Private MsgManager data class. -class ignition::gazebo::MsgManager::Implementation +class ignition::gazebo::comms::MsgManager::Implementation { /// \brief Buffer to store the content associated to each address. /// The key is an address. The value contains all the information associated @@ -42,6 +40,10 @@ class ignition::gazebo::MsgManager::Implementation public: ignition::transport::Node node; }; +using namespace ignition; +using namespace gazebo; +using namespace comms; + ////////////////////////////////////////////////// MsgManager::MsgManager() : dataPtr(ignition::utils::MakeUniqueImpl()) @@ -153,19 +155,19 @@ void MsgManager::DeliverMsgs() } ////////////////////////////////////////////////// -std::map &MsgManager::Data() +std::map &MsgManager::Data() { return this->dataPtr->data; } ////////////////////////////////////////////////// -std::map MsgManager::Copy() +std::map MsgManager::Copy() { return this->dataPtr->data; } ////////////////////////////////////////////////// -void MsgManager::Set(std::map &_newContent) +void MsgManager::Set(std::map &_newContent) { this->dataPtr->data = _newContent; } diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index 6a051b980e..8359be25c3 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -15,12 +15,13 @@ * */ -#include -#include -#include #include #include +#include "ignition/gazebo/comms/Broker.hh" +#include "ignition/common/Profiler.hh" +#include "ignition/gazebo/Util.hh" + #include "PerfectComms.hh" using namespace ignition; @@ -30,7 +31,7 @@ using namespace systems; class ignition::gazebo::systems::PerfectComms::Implementation { /// \brief Broker instance. - public: ignition::gazebo::Broker broker; + public: comms::Broker broker; }; ////////////////////////////////////////////////// @@ -70,9 +71,9 @@ void PerfectComms::PreUpdate( } void PerfectComms::Step( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm, - MsgManager &_messageMgr) + const UpdateInfo &_info, + EntityComponentManager &_ecm, + comms::MsgManager &_messageMgr) { this->dataPtr->broker.Lock(); diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index 0a88c415ba..c083b13be8 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -17,11 +17,12 @@ #ifndef IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ #define IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ -#include -#include -#include #include +#include "ignition/gazebo/comms/CommsModel.hh" +#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/System.hh" + namespace ignition { namespace gazebo @@ -33,7 +34,7 @@ namespace systems /// \brief class PerfectComms : public System, - public ICommsModel, + public comms::ICommsModel, public ISystemConfigure, public ISystemPreUpdate { @@ -58,7 +59,7 @@ namespace systems public: void Step( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm, - MsgManager &_messageMgr); + comms::MsgManager &_messageMgr); /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) From 4fe15bfb1186e1e186c97ef232c48365fcfd146a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 15 Mar 2022 12:42:54 -0700 Subject: [PATCH 06/51] add unit tests Signed-off-by: Ian Chen --- src/CMakeLists.txt | 2 + src/comms/Broker_TEST.cc | 78 +++++++++++++++++++++++++++ src/comms/MsgManager_TEST.cc | 100 +++++++++++++++++++++++++++++++++++ 3 files changed, 180 insertions(+) create mode 100644 src/comms/Broker_TEST.cc create mode 100644 src/comms/MsgManager_TEST.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a2b2389424..123a7fadd1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -101,6 +101,8 @@ set (gtest_sources Util_TEST.cc World_TEST.cc ign_TEST.cc + comms/Broker_TEST.cc + comms/MsgManager_TEST.cc network/NetworkConfig_TEST.cc network/PeerTracker_TEST.cc network/NetworkManager_TEST.cc diff --git a/src/comms/Broker_TEST.cc b/src/comms/Broker_TEST.cc new file mode 100644 index 0000000000..85224a1817 --- /dev/null +++ b/src/comms/Broker_TEST.cc @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include "ignition/gazebo/comms/Broker.hh" + +#include "helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Tests for Broker class +class BrokerTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(BrokerTest, Broker) +{ + comms::Broker broker; + + // test locking / unlocking and accessing data + EXPECT_NO_THROW(broker.Start()); + EXPECT_NO_THROW(broker.Lock()); + auto &allData = broker.Data().Data(); + EXPECT_TRUE(allData.empty()); + EXPECT_NO_THROW(broker.Unlock()); + + // test manually binding address and topic + msgs::StringMsg_V reqBind; + reqBind.add_data("addr1"); + reqBind.add_data("topic"); + broker.OnBind(reqBind); + allData = broker.Data().Data(); + EXPECT_EQ(1u, allData.size()); + EXPECT_EQ(1u, allData["addr1"].subscriptions.size()); + EXPECT_NE(allData["addr1"].subscriptions.end(), + allData["addr1"].subscriptions.find("topic")); + + // test manually adding a msg + EXPECT_TRUE(allData["addr1"].outboundMsgs.empty()); + msgs::Datagram msg; + msg.set_src_address("addr1"); + broker.OnMsg(msg); + EXPECT_EQ(1u, allData["addr1"].outboundMsgs.size()); + EXPECT_EQ("addr1", allData["addr1"].outboundMsgs[0u]->src_address()); + + // test manually unbinding address and topic + msgs::StringMsg_V reqUnbind; + reqUnbind.add_data("addr1"); + reqUnbind.add_data("topic"); + broker.OnUnbind(reqUnbind); + EXPECT_EQ(1u, allData.size()); + EXPECT_TRUE(allData["addr1"].subscriptions.empty()); + + // test msg delivery + auto msgIn = std::make_shared(); + allData["addr2"].inboundMsgs.push_back(msgIn); + EXPECT_EQ(1u, allData["addr2"].inboundMsgs.size()); + broker.DeliverMsgs(); + EXPECT_TRUE(allData["addr2"].inboundMsgs.empty()); +} diff --git a/src/comms/MsgManager_TEST.cc b/src/comms/MsgManager_TEST.cc new file mode 100644 index 0000000000..31ebb13b8c --- /dev/null +++ b/src/comms/MsgManager_TEST.cc @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include "ignition/gazebo/comms/MsgManager.hh" + +#include "helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Tests for MsgManager class +class MsgManagerTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(MsgManagerTest, MsgManager) +{ + comms::MsgManager msgManager; + + EXPECT_TRUE(msgManager.Data().empty()); + EXPECT_TRUE(msgManager.Copy().empty()); + + // test subscriber + msgManager.AddSubscriber("addr1", "topic1"); + EXPECT_EQ(1u, msgManager.Data().size()); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); + EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), + msgManager.Data()["addr1"].subscriptions.find("topic1")); + + // test inbound + auto msgIn = std::make_shared(); + EXPECT_EQ(msgManager.Data().end(), msgManager.Data().find("addr2")); + msgManager.AddInbound("addr2", msgIn); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr2")); + EXPECT_FALSE(msgManager.Data()["addr2"].inboundMsgs.empty()); + EXPECT_EQ(msgIn, msgManager.Data()["addr2"].inboundMsgs[0]); + + // test outbound + auto msgOut = std::make_shared(); + EXPECT_EQ(msgManager.Data().end(), msgManager.Data().find("addr3")); + msgManager.AddOutbound("addr3", msgOut); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr3")); + EXPECT_FALSE(msgManager.Data()["addr3"].outboundMsgs.empty()); + EXPECT_EQ(msgOut, msgManager.Data()["addr3"].outboundMsgs[0]); + + // test msg removal + msgManager.RemoveInbound("addr2", msgIn); + EXPECT_TRUE(msgManager.Data()["addr2"].inboundMsgs.empty()); + msgManager.RemoveOutbound("addr3", msgOut); + EXPECT_TRUE(msgManager.Data()["addr3"].outboundMsgs.empty()); + + // test msg delivery + msgManager.AddInbound("addr4", msgIn); + EXPECT_FALSE(msgManager.Data()["addr4"].inboundMsgs.empty()); + msgManager.DeliverMsgs(); + EXPECT_TRUE(msgManager.Data()["addr4"].inboundMsgs.empty()); + + // test subscriber removal + msgManager.RemoveSubscriber("addr1", "topic1"); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); + EXPECT_TRUE(msgManager.Data()["addr1"].subscriptions.empty()); + + // test setting msg content + auto msgIn2 = std::make_shared(); + auto msgOut2 = std::make_shared(); + std::map content; + content["addr6"].inboundMsgs.push_back(msgIn2); + content["addr6"].outboundMsgs.push_back(msgOut2); + msgManager.Set(content); + EXPECT_TRUE(msgManager.Data()["addr6"].subscriptions.empty()); + EXPECT_EQ(1u, msgManager.Data()["addr6"].inboundMsgs.size()); + EXPECT_EQ(msgIn2, msgManager.Data()["addr6"].inboundMsgs[0u]); + EXPECT_EQ(1u, msgManager.Data()["addr6"].outboundMsgs.size()); + EXPECT_EQ(msgOut2, msgManager.Data()["addr6"].outboundMsgs[0u]); + + // test copy + EXPECT_TRUE(msgManager.Copy()["addr6"].subscriptions.empty()); + EXPECT_EQ(1u, msgManager.Copy()["addr6"].inboundMsgs.size()); + EXPECT_EQ(msgIn2, msgManager.Copy()["addr6"].inboundMsgs[0u]); + EXPECT_EQ(1u, msgManager.Copy()["addr6"].outboundMsgs.size()); + EXPECT_EQ(msgOut2, msgManager.Copy()["addr6"].outboundMsgs[0u]); +} From 68628d8a14ebfc9c58ad8b60094b78b4de5898d8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 15 Mar 2022 13:55:17 -0700 Subject: [PATCH 07/51] add integration test Signed-off-by: Ian Chen --- test/integration/CMakeLists.txt | 1 + test/integration/comms.cc | 122 ++++++++++++++++++++++++++++++++ 2 files changed, 123 insertions(+) create mode 100644 test/integration/comms.cc diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 4a8bee7003..ac6642f644 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -10,6 +10,7 @@ set(tests buoyancy.cc buoyancy_engine.cc collada_world_exporter.cc + comms.cc components.cc contact_system.cc detachable_joint.cc diff --git a/test/integration/comms.cc b/test/integration/comms.cc new file mode 100644 index 0000000000..25b78b69c3 --- /dev/null +++ b/test/integration/comms.cc @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" // NOLINT(build/include) + +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +class CommsTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(CommsTest, Comms) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/examples/worlds/comms.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + size_t iters = 1000; + server.Run(true, iters, false); + + unsigned int msgCounter = 0u; + std::mutex mutex; + auto cb = [&](const msgs::Datagram &_msg) -> void + { + // verify msg content + std::lock_guard lock(mutex); + std::string expected = "hello world " + std::to_string(msgCounter); + + ignition::msgs::StringMsg receivedMsg; + receivedMsg.ParseFromString(_msg.data()); + EXPECT_EQ(expected, receivedMsg.data()); + msgCounter++; + }; + + // create subscriber + ignition::transport::Node node; + std::string addr = "addr1"; + std::string topic = "topic1"; + + // Subscribe to a topic by registering a callback. + auto cbFunc = std::function(cb); + EXPECT_TRUE(node.Subscribe(topic, cbFunc)) + << "Error subscribing to topic [" << topic << "]"; + + // Prepare the input parameters. + ignition::msgs::StringMsg_V req; + req.add_data(addr); + req.add_data(topic); + + // Bind. + EXPECT_TRUE(node.Request("/broker/bind", req)); + + // create publisher + auto pub = node.Advertise(topic); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Prepare the message. + ignition::msgs::Datagram msg; + msg.set_src_address("unused"); + msg.set_dst_address(addr); + + // publish 10 messages + ignition::msgs::StringMsg payload; + unsigned int pubCount = 10u; + for (unsigned int i = 0u; i < pubCount; ++i) + { + // Prepare the payload. + payload.set_data("hello world " + std::to_string(i)); + std::string serializedData; + EXPECT_TRUE(payload.SerializeToString(&serializedData)) + << payload.DebugString(); + msg.set_data(serializedData); + EXPECT_TRUE(pub.Publish(msg)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // verify subscriber received all msgs + int sleep = 0; + bool done = false; + while (!done && sleep++ < 10) + { + std::lock_guard lock(mutex); + done = msgCounter == pubCount; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_EQ(pubCount, msgCounter); +} From ca49142cc8f2a22c5dfad4d84a0bfc03f3da7c4d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 15 Mar 2022 15:12:00 -0700 Subject: [PATCH 08/51] refactor CommsModel class Signed-off-by: Ian Chen --- include/ignition/gazebo/comms/CommsModel.hh | 45 ++++++++++++++++++++- src/systems/perfect_comms/PerfectComms.cc | 43 ++++++-------------- src/systems/perfect_comms/PerfectComms.hh | 25 ++++-------- 3 files changed, 64 insertions(+), 49 deletions(-) diff --git a/include/ignition/gazebo/comms/CommsModel.hh b/include/ignition/gazebo/comms/CommsModel.hh index de00b35846..eb92bbb75f 100644 --- a/include/ignition/gazebo/comms/CommsModel.hh +++ b/include/ignition/gazebo/comms/CommsModel.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_GAZEBO_COMMSMODEL_HH__ -#define IGNITION_GAZEBO_COMMSMODEL_HH__ +#ifndef IGNITION_GAZEBO_COMMSMODEL_HH_ +#define IGNITION_GAZEBO_COMMSMODEL_HH_ #include @@ -37,7 +37,45 @@ namespace comms /// communication simulation. This class should be responsible for /// handling dropouts, decay and packet collisions. class ICommsModel + : public System, + public ISystemConfigure, + public ISystemPreUpdate { + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override + { + this->Load(_entity, _sdf, _ecm, _eventMgr); + this->broker.Start(); + } + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override + { + // Step the comms model. + this->Step(_info, _ecm, this->broker.Data()); + + // Deliver the inbound messages. + this->broker.DeliverMsgs(); + } + + /// \brief This method is called when the system is being configured + /// override this to load any additional params for the comms model + /// \param[in] _entity The entity this plugin is attached to. + /// \param[in] _sdf The SDF Element associated with this system plugin. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + /// \param[in] _eventMgr The EventManager of the given simulation + /// instance. + public: virtual void Load(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) = 0; + /// \brief This method is called when there is a timestep in the simulator /// override this to update your data structures as needed. /// \param[in] _info - Simulator information about the current timestep. @@ -48,6 +86,9 @@ namespace comms /// \brief Destructor public: virtual ~ICommsModel() = default; + + /// \brief Broker instance + public: Broker broker; }; } } diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index 8359be25c3..ae2fcc0039 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -30,15 +30,12 @@ using namespace systems; class ignition::gazebo::systems::PerfectComms::Implementation { - /// \brief Broker instance. - public: comms::Broker broker; }; ////////////////////////////////////////////////// PerfectComms::PerfectComms() : dataPtr(ignition::utils::MakeUniqueImpl()) { - ignerr << "Broker plugin running" << std::endl; } ////////////////////////////////////////////////// @@ -48,40 +45,26 @@ PerfectComms::~PerfectComms() } ////////////////////////////////////////////////// -void PerfectComms::Configure(const Entity &_entity, - const std::shared_ptr &_sdf, +// void PerfectComms::Configure(const Entity &_entity, +void PerfectComms::Load(const Entity &/*_entity*/, + const std::shared_ptr &/*_sdf*/, EntityComponentManager &/*_ecm*/, EventManager &/*_eventMgr*/) { - this->dataPtr->broker.Start(); -} - -////////////////////////////////////////////////// -void PerfectComms::PreUpdate( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) -{ - IGN_PROFILE("PerfectComms::PreUpdate"); - - // Step the comms model. - this->Step(_info, _ecm, this->dataPtr->broker.Data()); - - // Deliver the inbound messages. - this->dataPtr->broker.DeliverMsgs(); } void PerfectComms::Step( - const UpdateInfo &_info, - EntityComponentManager &_ecm, - comms::MsgManager &_messageMgr) + const UpdateInfo &/*_info*/, + EntityComponentManager &/*_ecm*/, + comms::MsgManager &/*_messageMgr*/) { - this->dataPtr->broker.Lock(); + this->broker.Lock(); // Check if there are new messages to process. - auto &allData = this->dataPtr->broker.Data().Data(); + auto &allData = this->broker.Data().Data(); // Create a copy to modify while we iterate. - auto allDataCopy = this->dataPtr->broker.Data().Copy(); + auto allDataCopy = this->broker.Data().Copy(); for (auto & [address, content] : allData) { @@ -95,15 +78,15 @@ void PerfectComms::Step( allDataCopy[address].outboundMsgs.clear(); } - this->dataPtr->broker.Data().Set(allDataCopy); + this->broker.Data().Set(allDataCopy); - this->dataPtr->broker.Unlock(); + this->broker.Unlock(); } IGNITION_ADD_PLUGIN(PerfectComms, ignition::gazebo::System, - PerfectComms::ISystemConfigure, - PerfectComms::ISystemPreUpdate) + comms::ICommsModel::ISystemConfigure, + comms::ICommsModel::ISystemPreUpdate) IGNITION_ADD_PLUGIN_ALIAS(PerfectComms, "ignition::gazebo::systems::PerfectComms") diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index c083b13be8..fd6586c70e 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -33,10 +33,7 @@ namespace systems { /// \brief class PerfectComms - : public System, - public comms::ICommsModel, - public ISystemConfigure, - public ISystemPreUpdate + : public comms::ICommsModel { /// \brief Constructor public: PerfectComms(); @@ -45,21 +42,15 @@ namespace systems public: ~PerfectComms(); // Documentation inherited - public: void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) override; - - // Documentation inherited - public: void PreUpdate( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + public: void Load(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; /// \brief - public: void Step( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm, - comms::MsgManager &_messageMgr); + public: void Step(const ignition::gazebo::UpdateInfo &_info, + EntityComponentManager &_ecm, + comms::MsgManager &_messageMgr); /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) From b38f470a9ec5d5e797d14ef78b7472d41986e58d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sat, 26 Mar 2022 20:26:11 +0100 Subject: [PATCH 09/51] Updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/standalone/comms/publisher.cc | 21 ++- examples/standalone/comms/subscriber.cc | 45 ++++-- examples/worlds/comms.sdf | 80 ++++++++-- include/ignition/gazebo/comms/Broker.hh | 101 +++++++++--- include/ignition/gazebo/comms/CommonTypes.hh | 53 ------- .../comms/{CommsModel.hh => ICommsModel.hh} | 61 ++++---- include/ignition/gazebo/comms/MsgManager.hh | 86 ++++++++--- src/comms/Broker.cc | 88 ++++++++--- src/comms/Broker_TEST.cc | 24 +-- src/comms/MsgManager.cc | 94 ++++++------ src/comms/MsgManager_TEST.cc | 13 +- src/systems/CMakeLists.txt | 1 + src/systems/comms_endpoint/CMakeLists.txt | 6 + src/systems/comms_endpoint/CommsEndpoint.cc | 145 ++++++++++++++++++ src/systems/comms_endpoint/CommsEndpoint.hh | 64 ++++++++ src/systems/perfect_comms/CommsClient.hh | 57 ------- src/systems/perfect_comms/PerfectComms.cc | 19 +-- src/systems/perfect_comms/PerfectComms.hh | 21 ++- test/integration/comms.cc | 8 +- 19 files changed, 674 insertions(+), 313 deletions(-) delete mode 100644 include/ignition/gazebo/comms/CommonTypes.hh rename include/ignition/gazebo/comms/{CommsModel.hh => ICommsModel.hh} (70%) create mode 100644 src/systems/comms_endpoint/CMakeLists.txt create mode 100644 src/systems/comms_endpoint/CommsEndpoint.cc create mode 100644 src/systems/comms_endpoint/CommsEndpoint.hh delete mode 100644 src/systems/perfect_comms/CommsClient.hh diff --git a/examples/standalone/comms/publisher.cc b/examples/standalone/comms/publisher.cc index 2f7c88918c..810307f022 100644 --- a/examples/standalone/comms/publisher.cc +++ b/examples/standalone/comms/publisher.cc @@ -29,6 +29,13 @@ /// \brief Flag used to break the publisher loop and terminate the program. static std::atomic g_terminatePub(false); +////////////////////////////////////////////////// +/// \brief Usage function. +void usage() +{ + std::cerr << "./publisher " << std::endl; +} + ////////////////////////////////////////////////// /// \brief Function callback executed when a SIGINT or SIGTERM signals are /// captured. This is used to break the infinite loop that publishes messages @@ -42,15 +49,21 @@ void signal_handler(int _signal) ////////////////////////////////////////////////// int main(int argc, char **argv) { + if (argc != 2) + { + usage(); + return -1; + } + // Install a signal handler for SIGINT and SIGTERM. std::signal(SIGINT, signal_handler); std::signal(SIGTERM, signal_handler); // Create a transport node and advertise a topic. ignition::transport::Node node; - std::string topic = "/broker"; + std::string topic = "/broker/msgs"; - auto pub = node.Advertise(topic); + auto pub = node.Advertise(topic); if (!pub) { std::cerr << "Error advertising topic [" << topic << "]" << std::endl; @@ -60,7 +73,7 @@ int main(int argc, char **argv) std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Prepare the message. - ignition::msgs::Datagram msg; + ignition::msgs::Dataframe msg; msg.set_src_address("unused"); msg.set_dst_address(argv[1]); @@ -82,7 +95,7 @@ int main(int argc, char **argv) if (!pub.Publish(msg)) break; - counter++; + ++counter; std::cout << "Publishing hello on topic [" << topic << "]" << std::endl; std::this_thread::sleep_for(std::chrono::milliseconds(1000)); diff --git a/examples/standalone/comms/subscriber.cc b/examples/standalone/comms/subscriber.cc index d65a79c5d7..23c661d9d2 100644 --- a/examples/standalone/comms/subscriber.cc +++ b/examples/standalone/comms/subscriber.cc @@ -15,15 +15,23 @@ * */ -// Example: ./subscriber addr1 addr1/rx +// Example: ./subscriber addr1 box1 addr1/rx #include #include #include +////////////////////////////////////////////////// +/// \brief Usage function. +void usage() +{ + std::cerr << "./subscriber " + << std::endl; +} + ////////////////////////////////////////////////// /// \brief Function called each time a topic update is received. -void cb(const ignition::msgs::Datagram &_msg) +void cb(const ignition::msgs::Dataframe &_msg) { std::cout << "Msg: " << _msg.data() << std::endl << std::endl; } @@ -31,10 +39,17 @@ void cb(const ignition::msgs::Datagram &_msg) ////////////////////////////////////////////////// int main(int argc, char **argv) { + if (argc != 4) + { + usage(); + return -1; + } + // Create a transport node. ignition::transport::Node node; - std::string addr = argv[1]; - std::string topic = argv[2]; + std::string addr = argv[1]; + std::string model = argv[2]; + std::string topic = argv[3]; // Subscribe to a topic by registering a callback. if (!node.Subscribe(topic, cb)) @@ -43,15 +58,25 @@ int main(int argc, char **argv) return -1; } - // Prepare the input parameters. - ignition::msgs::StringMsg_V req; - req.add_data(addr); - req.add_data(topic); + // Prepare the bind parameters. + ignition::msgs::StringMsg_V bindReq; + bindReq.add_data(addr); + bindReq.add_data(model); + bindReq.add_data(topic); // Bind. - if (!node.Request("/broker/bind", req)) - std::cerr << "Service call failed" << std::endl; + // if (!node.Request("/broker/bind", bindReq)) + // std::cerr << "Bind call failed" << std::endl; // Zzzzzz. ignition::transport::waitForShutdown(); + + // Prepare the unbind parameters. + ignition::msgs::StringMsg_V unbindReq; + unbindReq.add_data(addr); + unbindReq.add_data(topic); + + // Unbind. + // if (!node.Request("/broker/unbind", unbindReq)) + // std::cerr << "Unbind call failed" << std::endl; } diff --git a/examples/worlds/comms.sdf b/examples/worlds/comms.sdf index ef212fad6e..5ec4495d56 100644 --- a/examples/worlds/comms.sdf +++ b/examples/worlds/comms.sdf @@ -1,19 +1,22 @@ - + 0.001 @@ -31,6 +34,10 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> + + true @@ -73,7 +80,7 @@ - + 2 0 0.5 0 0 0 @@ -110,10 +117,59 @@ + filename="ignition-gazebo-comms-endpoint-system" + name="ignition::gazebo::systems::CommsEndpoint"> +
addr1
+ addr1/rx + + + + -2 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + +
addr2
+ addr2/rx + + /broker/bind + /broker/unbind + + diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 1a9cf9559c..8b0a0c7ebb 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -15,31 +15,70 @@ * */ -/// \file Broker.hh -/// \brief Broker for handling message delivery among robots. #ifndef IGNITION_GAZEBO_BROKER_HH_ #define IGNITION_GAZEBO_BROKER_HH_ -#include -#include -#include +#include #include - -#include "ignition/gazebo/comms/MsgManager.hh" +#include #include "ignition/gazebo/config.hh" namespace ignition { +namespace msgs +{ + // Forward declarations. + class Dataframe; + class StringMsg_V; +} namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace comms { - - /// \brief Store messages, and exposes an API for registering new clients, - /// bind to a particular address, push new messages or get the list of - /// messages already stored in the queue. + // Forward declarations. + class MsgManager; + + /// \brief A class to store messages to be delivered using a comms model. + /// This class should be used in combination with a specific comms model that + /// implements the ICommsModel interface. + /// \sa ICommsModel.hh + /// The broker maintains two queues: inbound and outbound. When a client + /// sends a communication request, we'll store it in the outbound queue of + /// the sender's address. When the comms model decides that a message needs + /// to be delivered to one of the destination, it'll be stored in the inbound + /// queue of the destination's address. + /// + /// The main goal of this class is to receive the comms requests and place + /// them in the appropriate outbound queue, as well as deliver the messages + /// that are in the inbound queues. + /// + /// The instance of the comms model is responsible for moving around the + /// messages from the outbound queues to the inbound queues. + /// + /// The broker can be configured with the following SDF parameters: + /// + /// * Optional parameters: + /// Element used to capture the broker parameters. This block can + /// contain any of the next parameters: + /// : Topic name where the broker receives all the incoming + /// messages. The default value is "/broker/msgs" + /// : Service name used to bind an address. + /// The default value is "/broker/bind" + /// : Service name used to unbind from an address. + /// The default value is "/broker/unbind" + /// + /// Here's an example: + /// + /// + /// /broker/inbound + /// /broker/bind_address + /// /broker/unbind_address + /// + /// class Broker { /// \brief Constructor. @@ -48,28 +87,48 @@ namespace comms /// \brief Destructor. public: virtual ~Broker(); - /// \brief Start. + /// \brief Configure the broker via SDF. + /// \param[in] _sdf The SDF Element associated with the broker parameters. + public: void Load(std::shared_ptr _sdf); + + /// \brief Start handling comms services. + /// + /// This function allows us to wait to advertise capabilities to + /// clients until the broker has been entirely initialized. public: void Start(); - /// \brief ToDo. + /// \brief This method associates an address with a client topic used as + /// callback for receiving messages. This is a client requirement to + /// start receiving messages. + /// \param[in] _req Bind request containing the following content: + /// _req[0] Client address. + /// _req[1] Model name associated to the address. + /// _req[2] Client subscription topic. public: void OnBind(const ignition::msgs::StringMsg_V &_req); - /// \brief ToDo. + /// \brief Unbind a given client address. The client associated to this + /// address will not receive any more messages. + /// \param[in] _req Bind request containing the following content: + /// _req[0] Client address. + /// _req[1] Client subscription topic. public: void OnUnbind(const ignition::msgs::StringMsg_V &_req); - /// \brief ToDo. - public: void OnMsg(const ignition::msgs::Datagram &_msg); + /// \brief Callback executed to process a communication request from one of + /// the clients. + /// \param[in] _msg The message from the client. + public: void OnMsg(const ignition::msgs::Dataframe &_msg); - /// \brief ToDo. + /// \brief Process all the messages in the inbound queue and deliver them + /// to the destination clients. public: void DeliverMsgs(); - /// \brief ToDo. - public: MsgManager &Data(); + /// \brief Get a mutable reference to the message manager. + public: MsgManager &DataManager(); - /// \brief ToDo. + /// \brief Lock the mutex to access the message manager. public: void Lock(); - /// \brief ToDo. + /// \brief Unlock the mutex to access the message manager. public: void Unlock(); /// \brief Private data pointer. diff --git a/include/ignition/gazebo/comms/CommonTypes.hh b/include/ignition/gazebo/comms/CommonTypes.hh deleted file mode 100644 index 6231385754..0000000000 --- a/include/ignition/gazebo/comms/CommonTypes.hh +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ -#ifndef IGNITION_GAZEBO_COMMONTYPES_HH_ -#define IGNITION_GAZEBO_COMMONTYPES_HH_ - -#include - -#include "ignition/gazebo/config.hh" - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace comms -{ -// /// \brief Address used to send a message to all the members of the team -// /// listening on a specific port. -// const std::string kBroadcast = "broadcast"; - -// /// \brief Address used to bind to a multicast group. Note that we do not -// /// support multiple multicast groups, only one. -// const std::string kMulticast = "multicast"; - -// /// \brief Topic used to centralize all messages sent from the agents. -const std::string kBrokerTopic = "/broker"; - -/// \brief Service used to validate an address. -const std::string kAddrBindSrv = "/broker/bind"; - -/// \brief Service used to invalidate an address. -const std::string kAddrUnbindSrv = "/broker/unbind"; -} -} -} -} - -#endif diff --git a/include/ignition/gazebo/comms/CommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh similarity index 70% rename from include/ignition/gazebo/comms/CommsModel.hh rename to include/ignition/gazebo/comms/ICommsModel.hh index eb92bbb75f..bdd9ba6c80 100644 --- a/include/ignition/gazebo/comms/CommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -15,15 +15,14 @@ * */ -#ifndef IGNITION_GAZEBO_COMMSMODEL_HH_ -#define IGNITION_GAZEBO_COMMSMODEL_HH_ +#ifndef IGNITION_GAZEBO_ICOMMSMODEL_HH_ +#define IGNITION_GAZEBO_ICOMMSMODEL_HH_ #include - +#include "ignition/gazebo/comms/Broker.hh" #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/System.hh" -#include "ignition/gazebo/comms/MsgManager.hh" namespace ignition { @@ -33,6 +32,9 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace comms { + // Forward declarations. + class MsgManager; + /// \brief Abstract interface to define how the environment should handle /// communication simulation. This class should be responsible for /// handling dropouts, decay and packet collisions. @@ -41,27 +43,37 @@ namespace comms public ISystemConfigure, public ISystemPreUpdate { - // Documentation inherited + /// \brief Destructor. + public: virtual ~ICommsModel() = default; + + // Documentation inherited. public: void Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) override - { - this->Load(_entity, _sdf, _ecm, _eventMgr); - this->broker.Start(); - } + { + sdf::ElementPtr sdfClone = _sdf->Clone(); + this->Load(_entity, sdfClone, _ecm, _eventMgr); + this->broker.Load(sdfClone); + this->broker.Start(); + } - // Documentation inherited + // Documentation inherited. public: void PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override - { - // Step the comms model. - this->Step(_info, _ecm, this->broker.Data()); + { + // We lock while we manipulate the queues. + this->broker.Lock(); + + // Step the comms model. + this->Step(_info, _ecm, this->broker.DataManager()); - // Deliver the inbound messages. - this->broker.DeliverMsgs(); - } + this->broker.Unlock(); + + // Deliver the inbound messages. + this->broker.DeliverMsgs(); + } /// \brief This method is called when the system is being configured /// override this to load any additional params for the comms model @@ -69,12 +81,11 @@ namespace comms /// \param[in] _sdf The SDF Element associated with this system plugin. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - /// \param[in] _eventMgr The EventManager of the given simulation - /// instance. + /// \param[in] _eventMgr The EventManager of the given simulation instance. public: virtual void Load(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) = 0; + std::shared_ptr _sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) = 0; /// \brief This method is called when there is a timestep in the simulator /// override this to update your data structures as needed. @@ -82,12 +93,10 @@ namespace comms /// \param[in] _ecm - Ignition's ECM. /// \param[in] _messageMgr - Use this to mark the message as arrived. public: virtual void Step(const UpdateInfo &_info, - EntityComponentManager &_ecm, MsgManager &_messageMgr) = 0; - - /// \brief Destructor - public: virtual ~ICommsModel() = default; + EntityComponentManager &_ecm, + MsgManager &_messageMgr) = 0; - /// \brief Broker instance + /// \brief Broker instance. public: Broker broker; }; } diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 12b5d91f09..7121e30188 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -18,40 +18,63 @@ #ifndef IGNITION_GAZEBO_MSGMANAGER_HH_ #define IGNITION_GAZEBO_MSGMANAGER_HH_ -#include #include #include #include #include + #include #include - #include "ignition/gazebo/config.hh" #include "ignition/gazebo/System.hh" namespace ignition { +namespace msgs +{ + // Forward declarations. + class Dataframe; +} namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace comms { + +/// \brief ToDo. struct MsgContent { /// \brief Queue of inbound messages. - public: std::deque> inboundMsgs; + public: std::deque> inboundMsgs; /// \brief Queue of outbound messages. - public: std::deque> outboundMsgs; + public: std::deque> outboundMsgs; /// \brief A map where the key is the topic subscribed to this address and /// the value is a publisher to reach that topic. public: std::map subscriptions; + + /// \brief Model name associated to this address. + public: std::string modelName; }; -/// \brief +// class MsgManagerData +// { +// /// \brief Default constructor. +// public: MsgManagerData(); + +// /// \brief Destructor. +// public: virtual ~MsgManagerData(); + +// public: + +// /// \brief Private data pointer. +// IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) +// }; + +/// \brief ToDo. class MsgManager { /// \brief Default constructor. @@ -60,44 +83,61 @@ class MsgManager /// \brief Destructor. public: virtual ~MsgManager(); - /// \brief + /// \brief Add a new subscriber + /// \param[in] _address The subscriber address. + /// \param[in] _modelName The model name. + /// \param[in] _topic The subscriber topic. public: void AddSubscriber(const std::string &_address, + const std::string &_modelName, const std::string &_topic); - /// \brief - // public: std::unordered_set Subscribers( - // const std::string &_address); - - /// \brief + /// \brief Add a new message to the inbound queue. + /// \param[in] _address The destination address. + /// \param[in] _msg The message. public: void AddInbound(const std::string &_address, - const std::shared_ptr &_msg); + const std::shared_ptr &_msg); - /// \brief + /// \brief Add a new message to the outbound queue. + /// \param[in] _address The sender address. + /// \param[in] _msg The message. public: void AddOutbound(const std::string &_address, - const std::shared_ptr &_msg); + const std::shared_ptr &_msg); - /// \brief + /// \brief Remove an existing subscriber. + /// \param The subscriber address. + /// \param The Subscriber topic. + /// \return True if the subscriber was removed or false otherwise. public: bool RemoveSubscriber(const std::string &_address, const std::string &_topic); - /// \brief + /// \brief Remove a message from the inbound queue. + /// \param[in] _address The destination address. + /// \param[in] _Msg Message pointer to remove. public: void RemoveInbound(const std::string &_address, - const std::shared_ptr &_msg); + const std::shared_ptr &_msg); - /// \brief + /// \brief Remove a message from the outbound queue. + /// \param[in] _address The sender address. + /// \param[in] _msg Message pointer to remove. public: void RemoveOutbound(const std::string &_address, - const std::shared_ptr &_msg); + const std::shared_ptr &_msg); - /// \brief ToDo. + /// \brief This function delivers all the messages in the inbound queue to + /// the appropriate subscribers. This function also clears the inbound queue. public: void DeliverMsgs(); - /// \brief ToDo. + /// \brief Get a mutable reference to the data containing subscriptions and + /// data queues. + /// \return A mutable reference to the data. public: std::map &Data(); - /// \brief ToDo. + /// \brief Get a copy of the data structure containing subscriptions and data + /// queues. + /// \return A copy of the data. public: std::map Copy(); - /// \brief ToDo. + /// \brief Set the data structure containing subscriptions and data queues. + /// \param[in] _newContent New content to be set. public: void Set(std::map &_newContent); /// \brief Private data pointer. diff --git a/src/comms/Broker.cc b/src/comms/Broker.cc index 374dd7227b..5700195b0b 100644 --- a/src/comms/Broker.cc +++ b/src/comms/Broker.cc @@ -15,18 +15,17 @@ * */ -#include +#include #include #include #include #include +#include #include #include - #include "ignition/gazebo/comms/Broker.hh" -#include "ignition/gazebo/comms/CommonTypes.hh" #include "ignition/gazebo/comms/MsgManager.hh" #include "ignition/gazebo/Util.hh" @@ -41,6 +40,15 @@ class ignition::gazebo::comms::Broker::Implementation /// \brief Protect data from races. public: std::mutex mutex; + + /// \brief Topic used to centralize all messages sent from the agents. + public: std::string msgTopic = "/broker/msgs"; + + /// \brief Service used to bind to an address. + public: std::string bindSrv = "/broker/bind"; + + /// \brief Service used to unbind from an address. + public: std::string unbindSrv = "/broker/unbind"; }; using namespace ignition; @@ -59,45 +67,76 @@ Broker::~Broker() // cannot use default destructor because of dataPtr } +////////////////////////////////////////////////// +void Broker::Load(std::shared_ptr _sdf) +{ + if (!_sdf->HasElement("broker")) + return; + + sdf::ElementPtr elem = _sdf->Clone()->GetElement("broker"); + this->dataPtr->msgTopic = + elem->Get("messages_topic", this->dataPtr->msgTopic).first; + this->dataPtr->bindSrv = + elem->Get("bind_service", this->dataPtr->bindSrv).first; + this->dataPtr->unbindSrv = + elem->Get("unbind_service", this->dataPtr->unbindSrv).first; +} + ////////////////////////////////////////////////// void Broker::Start() { // Advertise the service for binding addresses. - if (!this->dataPtr->node.Advertise(kAddrBindSrv, &Broker::OnBind, this)) + if (!this->dataPtr->node.Advertise(this->dataPtr->bindSrv, + &Broker::OnBind, this)) { - ignerr << "Error advertising srv [" << kAddrBindSrv << "]" << std::endl; + ignerr << "Error advertising srv [" << this->dataPtr->bindSrv << "]" + << std::endl; return; } // Advertise the service for unbinding addresses. - if (!this->dataPtr->node.Advertise(kAddrUnbindSrv, &Broker::OnUnbind, this)) + if (!this->dataPtr->node.Advertise(this->dataPtr->unbindSrv, + &Broker::OnUnbind, this)) { - ignerr << "Error advertising srv [" << kAddrUnbindSrv << "]" - << std::endl; + ignerr << "Error advertising srv [" << this->dataPtr->unbindSrv << "]" + << std::endl; return; } // Advertise the topic for receiving data messages. - if (!this->dataPtr->node.Subscribe(kBrokerTopic, &Broker::OnMsg, this)) + if (!this->dataPtr->node.Subscribe(this->dataPtr->msgTopic, + &Broker::OnMsg, this)) { - ignerr << "Error subscribing to topic [" << kBrokerTopic << "]" + ignerr << "Error subscribing to topic [" << this->dataPtr->msgTopic << "]" << std::endl; + return; } + + igndbg << "Broker services:" << std::endl; + igndbg << " Bind: [" << this->dataPtr->bindSrv << "]" << std::endl; + igndbg << " Unbind: [" << this->dataPtr->unbindSrv << "]" << std::endl; + igndbg << "Broker topics:" << std::endl; + igndbg << " Incoming messages: [" << this->dataPtr->msgTopic << "]" + << std::endl; } ////////////////////////////////////////////////// void Broker::OnBind(const ignition::msgs::StringMsg_V &_req) { auto count = _req.data_size(); - if (count != 2) + if (count != 3) ignerr << "Receive incorrect number of arguments. " - << "Expecting 2 and receive " << count << std::endl; + << "Expecting 3 and receive " << count << std::endl; + + std::string address = _req.data(0); + std::string model = _req.data(1); + std::string topic = _req.data(2); std::lock_guard lk(this->dataPtr->mutex); - this->dataPtr->data.AddSubscriber(_req.data(0), _req.data(1)); + this->DataManager().AddSubscriber(address, model, topic); - ignmsg << "Address [" << _req.data(0) << "] bound on topic [" - << _req.data(1) << "]" << std::endl; + ignmsg << "Address [" << address << "] bound to model [" << model + << "] on topic [" << topic << "]" << std::endl; } ////////////////////////////////////////////////// @@ -108,32 +147,35 @@ void Broker::OnUnbind(const ignition::msgs::StringMsg_V &_req) ignerr << "Receive incorrect number of arguments. " << "Expecting 2 and receive " << count << std::endl; + std::string address = _req.data(0); + std::string topic = _req.data(1); + std::lock_guard lk(this->dataPtr->mutex); - this->dataPtr->data.RemoveSubscriber(_req.data(0), _req.data(1)); + this->DataManager().RemoveSubscriber(address, topic); - ignmsg << "Address [" << _req.data(0) << "] unbound on topic [" - << _req.data(1) << "]" << std::endl; + ignmsg << "Address [" << address << "] unbound on topic [" + << topic << "]" << std::endl; } ////////////////////////////////////////////////// -void Broker::OnMsg(const ignition::msgs::Datagram &_msg) +void Broker::OnMsg(const ignition::msgs::Dataframe &_msg) { // Place the message in the outbound queue of the sender. - auto msgPtr = std::make_shared(_msg); + auto msgPtr = std::make_shared(_msg); std::lock_guard lk(this->dataPtr->mutex); - this->dataPtr->data.AddOutbound(_msg.src_address(), msgPtr); + this->DataManager().AddOutbound(_msg.src_address(), msgPtr); } ////////////////////////////////////////////////// void Broker::DeliverMsgs() { std::lock_guard lk(this->dataPtr->mutex); - this->dataPtr->data.DeliverMsgs(); + this->DataManager().DeliverMsgs(); } ////////////////////////////////////////////////// -MsgManager &Broker::Data() +MsgManager &Broker::DataManager() { return this->dataPtr->data; } diff --git a/src/comms/Broker_TEST.cc b/src/comms/Broker_TEST.cc index 85224a1817..9d6d8410e9 100644 --- a/src/comms/Broker_TEST.cc +++ b/src/comms/Broker_TEST.cc @@ -16,10 +16,11 @@ */ #include -#include +#include +#include #include "ignition/gazebo/comms/Broker.hh" - +#include "ignition/gazebo/comms/MsgManager.hh" #include "helpers/EnvTestFixture.hh" using namespace ignition; @@ -35,33 +36,34 @@ TEST_F(BrokerTest, Broker) { comms::Broker broker; - // test locking / unlocking and accessing data + // Test locking / unlocking and accessing data. EXPECT_NO_THROW(broker.Start()); EXPECT_NO_THROW(broker.Lock()); - auto &allData = broker.Data().Data(); + auto &allData = broker.DataManager().Data(); EXPECT_TRUE(allData.empty()); EXPECT_NO_THROW(broker.Unlock()); - // test manually binding address and topic + // Test manually binding address and topic. msgs::StringMsg_V reqBind; reqBind.add_data("addr1"); + reqBind.add_data("model1"); reqBind.add_data("topic"); broker.OnBind(reqBind); - allData = broker.Data().Data(); + allData = broker.DataManager().Data(); EXPECT_EQ(1u, allData.size()); EXPECT_EQ(1u, allData["addr1"].subscriptions.size()); EXPECT_NE(allData["addr1"].subscriptions.end(), allData["addr1"].subscriptions.find("topic")); - // test manually adding a msg + // Test manually adding a msg. EXPECT_TRUE(allData["addr1"].outboundMsgs.empty()); - msgs::Datagram msg; + msgs::Dataframe msg; msg.set_src_address("addr1"); broker.OnMsg(msg); EXPECT_EQ(1u, allData["addr1"].outboundMsgs.size()); EXPECT_EQ("addr1", allData["addr1"].outboundMsgs[0u]->src_address()); - // test manually unbinding address and topic + // Test manually unbinding address and topic. msgs::StringMsg_V reqUnbind; reqUnbind.add_data("addr1"); reqUnbind.add_data("topic"); @@ -69,8 +71,8 @@ TEST_F(BrokerTest, Broker) EXPECT_EQ(1u, allData.size()); EXPECT_TRUE(allData["addr1"].subscriptions.empty()); - // test msg delivery - auto msgIn = std::make_shared(); + // Test msg delivery. + auto msgIn = std::make_shared(); allData["addr2"].inboundMsgs.push_back(msgIn); EXPECT_EQ(1u, allData["addr2"].inboundMsgs.size()); broker.DeliverMsgs(); diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc index 2e8b9b35bd..03f8647aee 100644 --- a/src/comms/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -15,19 +15,34 @@ * */ -#include +#include #include #include #include #include #include + #include #include - -#include "ignition/gazebo/comms/CommonTypes.hh" +#include "ignition/gazebo/config.hh" #include "ignition/gazebo/comms/MsgManager.hh" +// /// \brief Private MsgManagerData data class. +// class ignition::gazebo::comms::MsgManagerData::Implementation +// { +// /// \brief Queue of inbound messages. +// public: std::deque> inboundMsgs; + +// /// \brief Queue of outbound messages. +// public: std::deque> outboundMsgs; + +// /// \brief A map where the key is the topic subscribed to this address and +// /// the value is a publisher to reach that topic. +// public: std::map subscriptions; +// }; + /// \brief Private MsgManager data class. class ignition::gazebo::comms::MsgManager::Implementation { @@ -44,6 +59,18 @@ using namespace ignition; using namespace gazebo; using namespace comms; +// ////////////////////////////////////////////////// +// MsgManagerData::MsgManagerData() +// : dataPtr(ignition::utils::MakeUniqueImpl()) +// { +// } + +// ////////////////////////////////////////////////// +// MsgManagerData::~MsgManagerData() +// { +// // cannot use default destructor because of dataPtr +// } + ////////////////////////////////////////////////// MsgManager::MsgManager() : dataPtr(ignition::utils::MakeUniqueImpl()) @@ -58,77 +85,61 @@ MsgManager::~MsgManager() ////////////////////////////////////////////////// void MsgManager::AddSubscriber(const std::string &_address, - const std::string &_topic) + const std::string &_modelName, + const std::string &_topic) { + this->dataPtr->data[_address].modelName = _modelName; + ignition::transport::Node::Publisher publisher = - this->dataPtr->node.Advertise(_topic); + this->dataPtr->node.Advertise(_topic); this->dataPtr->data[_address].subscriptions[_topic] = publisher; } -////////////////////////////////////////////////// -// std::unordered_set MsgManager::Subscribers( -// const std::string &_address) -// { -// std::unordered_set result; - -// if (this->dataPtr->data.find(_address) != this->dataPtr->data.end()) -// { -// result = this->dataPtr->data[_address].subscriptions; -// } - -// return result; -// } - ////////////////////////////////////////////////// void MsgManager::AddInbound(const std::string &_address, - const std::shared_ptr &_msg) + const std::shared_ptr &_msg) { this->dataPtr->data[_address].inboundMsgs.push_back(_msg); } ////////////////////////////////////////////////// void MsgManager::AddOutbound(const std::string &_address, - const std::shared_ptr &_msg) + const std::shared_ptr &_msg) { this->dataPtr->data[_address].outboundMsgs.push_back(_msg); } ////////////////////////////////////////////////// bool MsgManager::RemoveSubscriber(const std::string &_address, - const std::string &_topic) + const std::string &_topic) { - // Iterate over all the topics. - if (this->dataPtr->data.find(_address) == - this->dataPtr->data.end()) - { + if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) return false; - } return this->dataPtr->data[_address].subscriptions.erase(_topic) > 0; } ////////////////////////////////////////////////// void MsgManager::RemoveInbound(const std::string &_address, - const std::shared_ptr &_msg) + const std::shared_ptr &_msg) { - if (this->dataPtr->data.find(_address) != - this->dataPtr->data.end()) - { - auto &q = this->dataPtr->data[_address].inboundMsgs; - q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); - } + if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) + return; + + auto &q = this->dataPtr->data[_address].inboundMsgs; + q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); } ////////////////////////////////////////////////// void MsgManager::RemoveOutbound(const std::string &_address, - const std::shared_ptr &_msg) + const std::shared_ptr &_msg) { - if (this->dataPtr->data.find(_address) != this->dataPtr->data.end()) - { - auto &q = this->dataPtr->data[_address].outboundMsgs; - q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); - } + if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) + return; + + auto &q = this->dataPtr->data[_address].outboundMsgs; + q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); } ////////////////////////////////////////////////// @@ -144,10 +155,7 @@ void MsgManager::DeliverMsgs() { // Use the publisher associated to the destination address. for (auto & [topic, publisher] : content.subscriptions) - { publisher.Publish(*msg); - ignmsg << "Publishing msg [" << msg->DebugString() << "]" << std::endl; - } } content.inboundMsgs.clear(); diff --git a/src/comms/MsgManager_TEST.cc b/src/comms/MsgManager_TEST.cc index 31ebb13b8c..82061e01ab 100644 --- a/src/comms/MsgManager_TEST.cc +++ b/src/comms/MsgManager_TEST.cc @@ -17,9 +17,8 @@ #include #include - +#include #include "ignition/gazebo/comms/MsgManager.hh" - #include "helpers/EnvTestFixture.hh" using namespace ignition; @@ -39,14 +38,14 @@ TEST_F(MsgManagerTest, MsgManager) EXPECT_TRUE(msgManager.Copy().empty()); // test subscriber - msgManager.AddSubscriber("addr1", "topic1"); + msgManager.AddSubscriber("addr1", "model1", "topic1"); EXPECT_EQ(1u, msgManager.Data().size()); EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), msgManager.Data()["addr1"].subscriptions.find("topic1")); // test inbound - auto msgIn = std::make_shared(); + auto msgIn = std::make_shared(); EXPECT_EQ(msgManager.Data().end(), msgManager.Data().find("addr2")); msgManager.AddInbound("addr2", msgIn); EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr2")); @@ -54,7 +53,7 @@ TEST_F(MsgManagerTest, MsgManager) EXPECT_EQ(msgIn, msgManager.Data()["addr2"].inboundMsgs[0]); // test outbound - auto msgOut = std::make_shared(); + auto msgOut = std::make_shared(); EXPECT_EQ(msgManager.Data().end(), msgManager.Data().find("addr3")); msgManager.AddOutbound("addr3", msgOut); EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr3")); @@ -79,8 +78,8 @@ TEST_F(MsgManagerTest, MsgManager) EXPECT_TRUE(msgManager.Data()["addr1"].subscriptions.empty()); // test setting msg content - auto msgIn2 = std::make_shared(); - auto msgOut2 = std::make_shared(); + auto msgIn2 = std::make_shared(); + auto msgOut2 = std::make_shared(); std::map content; content["addr6"].inboundMsgs.push_back(msgIn2); content["addr6"].outboundMsgs.push_back(msgOut2); diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 4c39caeeeb..abad89748c 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -102,6 +102,7 @@ add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) add_subdirectory(buoyancy_engine) add_subdirectory(collada_world_exporter) +add_subdirectory(comms_endpoint) add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) diff --git a/src/systems/comms_endpoint/CMakeLists.txt b/src/systems/comms_endpoint/CMakeLists.txt new file mode 100644 index 0000000000..c0ca01e034 --- /dev/null +++ b/src/systems/comms_endpoint/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(comms-endpoint + SOURCES + CommsEndpoint.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/comms_endpoint/CommsEndpoint.cc b/src/systems/comms_endpoint/CommsEndpoint.cc new file mode 100644 index 0000000000..ba21bdbfba --- /dev/null +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +#include "CommsEndpoint.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::CommsEndpoint::Implementation +{ + /// \brief The address. + public: std::string address; + + /// \brief The topic where the messages will be delivered. + public: std::string topic; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief True when the address has been bound in the broker. + public: bool bound{false}; + + /// \brief Service where the broker is listening bind requests. + public: std::string bindSrv = "/broker/bind"; + + /// \brief Service where the broker is listening unbind requests. + public: std::string unbindSrv = "/broker/unbind"; + + /// \brief The ignition transport node. + public: ignition::transport::Node node; +}; + + +////////////////////////////////////////////////// +CommsEndpoint::CommsEndpoint() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +CommsEndpoint::~CommsEndpoint() +{ + if (!this->dataPtr->bound) + return; + + // Prepare the unbind parameters. + ignition::msgs::StringMsg_V unbindReq; + unbindReq.add_data(this->dataPtr->address); + unbindReq.add_data(this->dataPtr->topic); + + // Unbind. + if (!this->dataPtr->node.Request("/broker/unbind", unbindReq)) + ignerr << "Bind call failed" << std::endl; + +} + +////////////////////////////////////////////////// +void CommsEndpoint::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + // Parse
. + if (!_sdf->HasElement("address")) + { + ignerr << "No
specified." << std::endl; + return; + } + this->dataPtr->address = _sdf->Get("address"); + + // Parse . + if (!_sdf->HasElement("topic")) + { + ignerr << "No specified." << std::endl; + return; + } + this->dataPtr->topic = _sdf->Get("topic"); + + if (_sdf->HasElement("broker")) + { + sdf::ElementPtr elem = _sdf->Clone()->GetElement("broker"); + this->dataPtr->bindSrv = + elem->Get("bind_service", this->dataPtr->bindSrv).first; + this->dataPtr->unbindSrv = + elem->Get("unbind_service", this->dataPtr->unbindSrv).first; + } + + // Set model. + this->dataPtr->model = Model(_entity); +} + +////////////////////////////////////////////////// +void CommsEndpoint::PreUpdate( + const ignition::gazebo::UpdateInfo &/*_info*/, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("CommsEndpoint::PreUpdate"); + + if (this->dataPtr->bound) + return; + + // Prepare the bind parameters. + ignition::msgs::StringMsg_V bindReq; + bindReq.add_data(this->dataPtr->address); + bindReq.add_data(this->dataPtr->model.Name(_ecm)); + bindReq.add_data(this->dataPtr->topic); + + // Bind. + if (this->dataPtr->node.Request("/broker/bind", bindReq)) + this->dataPtr->bound = true; + else + ignerr << "Bind call failed" << std::endl; +} + +IGNITION_ADD_PLUGIN(CommsEndpoint, + ignition::gazebo::System, + CommsEndpoint::ISystemConfigure, + CommsEndpoint::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(CommsEndpoint, + "ignition::gazebo::systems::CommsEndpoint") diff --git a/src/systems/comms_endpoint/CommsEndpoint.hh b/src/systems/comms_endpoint/CommsEndpoint.hh new file mode 100644 index 0000000000..b9e26f9e81 --- /dev/null +++ b/src/systems/comms_endpoint/CommsEndpoint.hh @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_COMMSENDPOINT_HH_ +#define IGNITION_GAZEBO_SYSTEMS_COMMSENDPOINT_HH_ + +#include +#include +#include + +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + class CommsEndpoint + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: CommsEndpoint(); + + /// \brief Destructor + public: ~CommsEndpoint(); + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + } +} +} +} + +#endif diff --git a/src/systems/perfect_comms/CommsClient.hh b/src/systems/perfect_comms/CommsClient.hh deleted file mode 100644 index 93b781fd2c..0000000000 --- a/src/systems/perfect_comms/CommsClient.hh +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef IGNITION_GAZEBO_SYSTEMS_COMMSCLIENT_HH_ -#define IGNITION_GAZEBO_SYSTEMS_COMMSCLIENT_HH_ - -#include - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ - // \brief - class CommsClient - { - ////////////////////////////////////////////////// - /// \brief Constructor - public: CommsClient() - { - } - - ////////////////////////////////////////////////// - /// \brief Send a message - public: void SendPacket() - { - } - - ////////////////////////////////////////////////// - /// \brief - private: void ReceivedPacket() - { - } - }; - } -} -} -} - -#endif diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index ae2fcc0039..b8c84e2808 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -15,13 +15,14 @@ * */ -#include #include +#include +#include + #include "ignition/gazebo/comms/Broker.hh" -#include "ignition/common/Profiler.hh" +#include "ignition/gazebo/comms/MsgManager.hh" #include "ignition/gazebo/Util.hh" - #include "PerfectComms.hh" using namespace ignition; @@ -47,7 +48,7 @@ PerfectComms::~PerfectComms() ////////////////////////////////////////////////// // void PerfectComms::Configure(const Entity &_entity, void PerfectComms::Load(const Entity &/*_entity*/, - const std::shared_ptr &/*_sdf*/, + std::shared_ptr /*_sdf*/, EntityComponentManager &/*_ecm*/, EventManager &/*_eventMgr*/) { @@ -58,13 +59,11 @@ void PerfectComms::Step( EntityComponentManager &/*_ecm*/, comms::MsgManager &/*_messageMgr*/) { - this->broker.Lock(); - // Check if there are new messages to process. - auto &allData = this->broker.Data().Data(); + auto &allData = this->broker.DataManager().Data(); // Create a copy to modify while we iterate. - auto allDataCopy = this->broker.Data().Copy(); + auto allDataCopy = this->broker.DataManager().Copy(); for (auto & [address, content] : allData) { @@ -78,9 +77,7 @@ void PerfectComms::Step( allDataCopy[address].outboundMsgs.clear(); } - this->broker.Data().Set(allDataCopy); - - this->broker.Unlock(); + this->broker.DataManager().Set(allDataCopy); } IGNITION_ADD_PLUGIN(PerfectComms, diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index fd6586c70e..ef270140ca 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -17,10 +17,11 @@ #ifndef IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ #define IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ +#include #include +#include -#include "ignition/gazebo/comms/CommsModel.hh" -#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/comms/ICommsModel.hh" #include "ignition/gazebo/System.hh" namespace ignition @@ -31,23 +32,27 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - /// \brief + // Forward declarations. + class MsgManager; + + /// \brief An example of a comms model. + /// This model always delivers any message to its destination. class PerfectComms : public comms::ICommsModel { - /// \brief Constructor + /// \brief Constructor. public: PerfectComms(); - /// \brief Destructor + /// \brief Destructor. public: ~PerfectComms(); - // Documentation inherited + // Documentation inherited. public: void Load(const Entity &_entity, - const std::shared_ptr &_sdf, + std::shared_ptr _sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) override; - /// \brief + // Documentation inherited. public: void Step(const ignition::gazebo::UpdateInfo &_info, EntityComponentManager &_ecm, comms::MsgManager &_messageMgr); diff --git a/test/integration/comms.cc b/test/integration/comms.cc index 25b78b69c3..19fbc0be80 100644 --- a/test/integration/comms.cc +++ b/test/integration/comms.cc @@ -56,7 +56,7 @@ TEST_F(CommsTest, Comms) unsigned int msgCounter = 0u; std::mutex mutex; - auto cb = [&](const msgs::Datagram &_msg) -> void + auto cb = [&](const msgs::Dataframe &_msg) -> void { // verify msg content std::lock_guard lock(mutex); @@ -74,7 +74,7 @@ TEST_F(CommsTest, Comms) std::string topic = "topic1"; // Subscribe to a topic by registering a callback. - auto cbFunc = std::function(cb); + auto cbFunc = std::function(cb); EXPECT_TRUE(node.Subscribe(topic, cbFunc)) << "Error subscribing to topic [" << topic << "]"; @@ -87,10 +87,10 @@ TEST_F(CommsTest, Comms) EXPECT_TRUE(node.Request("/broker/bind", req)); // create publisher - auto pub = node.Advertise(topic); + auto pub = node.Advertise(topic); std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Prepare the message. - ignition::msgs::Datagram msg; + ignition::msgs::Dataframe msg; msg.set_src_address("unused"); msg.set_dst_address(addr); From d3c7395735cc8eee9e74f6892f00c4c6308d36cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sun, 27 Mar 2022 20:19:39 +0200 Subject: [PATCH 10/51] More updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/standalone/comms/CMakeLists.txt | 4 - examples/standalone/comms/subscriber.cc | 82 ---------------- examples/worlds/comms.sdf | 9 +- include/ignition/gazebo/comms/Broker.hh | 6 +- include/ignition/gazebo/comms/ICommsModel.hh | 25 +++-- include/ignition/gazebo/comms/MsgManager.hh | 56 +++++------ src/comms/Broker.cc | 9 +- src/comms/Broker_TEST.cc | 6 +- src/comms/MsgManager.cc | 49 +++------- src/comms/MsgManager_TEST.cc | 2 +- src/systems/comms_endpoint/CommsEndpoint.cc | 94 ++++++++++++++----- src/systems/comms_endpoint/CommsEndpoint.hh | 31 ++++++ src/systems/perfect_comms/PerfectComms.cc | 22 ++--- src/systems/perfect_comms/PerfectComms.hh | 5 +- .../perfect_comms/models/CMakeLists.txt | 8 -- 15 files changed, 196 insertions(+), 212 deletions(-) delete mode 100644 examples/standalone/comms/subscriber.cc delete mode 100644 src/systems/perfect_comms/models/CMakeLists.txt diff --git a/examples/standalone/comms/CMakeLists.txt b/examples/standalone/comms/CMakeLists.txt index 404b2ecb8b..a52dbfefea 100644 --- a/examples/standalone/comms/CMakeLists.txt +++ b/examples/standalone/comms/CMakeLists.txt @@ -8,7 +8,3 @@ set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) add_executable(publisher publisher.cc) target_link_libraries(publisher ignition-transport${IGN_TRANSPORT_VER}::core) - -add_executable(subscriber subscriber.cc) -target_link_libraries(subscriber - ignition-transport${IGN_TRANSPORT_VER}::core) diff --git a/examples/standalone/comms/subscriber.cc b/examples/standalone/comms/subscriber.cc deleted file mode 100644 index 23c661d9d2..0000000000 --- a/examples/standalone/comms/subscriber.cc +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -// Example: ./subscriber addr1 box1 addr1/rx - -#include -#include -#include - -////////////////////////////////////////////////// -/// \brief Usage function. -void usage() -{ - std::cerr << "./subscriber " - << std::endl; -} - -////////////////////////////////////////////////// -/// \brief Function called each time a topic update is received. -void cb(const ignition::msgs::Dataframe &_msg) -{ - std::cout << "Msg: " << _msg.data() << std::endl << std::endl; -} - -////////////////////////////////////////////////// -int main(int argc, char **argv) -{ - if (argc != 4) - { - usage(); - return -1; - } - - // Create a transport node. - ignition::transport::Node node; - std::string addr = argv[1]; - std::string model = argv[2]; - std::string topic = argv[3]; - - // Subscribe to a topic by registering a callback. - if (!node.Subscribe(topic, cb)) - { - std::cerr << "Error subscribing to topic [" << topic << "]" << std::endl; - return -1; - } - - // Prepare the bind parameters. - ignition::msgs::StringMsg_V bindReq; - bindReq.add_data(addr); - bindReq.add_data(model); - bindReq.add_data(topic); - - // Bind. - // if (!node.Request("/broker/bind", bindReq)) - // std::cerr << "Bind call failed" << std::endl; - - // Zzzzzz. - ignition::transport::waitForShutdown(); - - // Prepare the unbind parameters. - ignition::msgs::StringMsg_V unbindReq; - unbindReq.add_data(addr); - unbindReq.add_data(topic); - - // Unbind. - // if (!node.Request("/broker/unbind", unbindReq)) - // std::cerr << "Unbind call failed" << std::endl; -} diff --git a/examples/worlds/comms.sdf b/examples/worlds/comms.sdf index 5ec4495d56..a67c432ed7 100644 --- a/examples/worlds/comms.sdf +++ b/examples/worlds/comms.sdf @@ -2,17 +2,17 @@ @@ -170,6 +170,7 @@ /broker/unbind + diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 8b0a0c7ebb..867e8f8617 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -28,6 +28,7 @@ namespace ignition namespace msgs { // Forward declarations. + class Boolean; class Dataframe; class StringMsg_V; } @@ -104,7 +105,10 @@ namespace comms /// _req[0] Client address. /// _req[1] Model name associated to the address. /// _req[2] Client subscription topic. - public: void OnBind(const ignition::msgs::StringMsg_V &_req); + /// \param[out] _rep Unused + /// \return True when the bind service succeeded or false otherwise. + public: bool OnBind(const ignition::msgs::StringMsg_V &_req, + ignition::msgs::Boolean &_rep); /// \brief Unbind a given client address. The client associated to this /// address will not receive any more messages. diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index bdd9ba6c80..6c658d6f77 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -19,6 +19,7 @@ #define IGNITION_GAZEBO_ICOMMSMODEL_HH_ #include +#include #include "ignition/gazebo/comms/Broker.hh" #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -36,8 +37,8 @@ namespace comms class MsgManager; /// \brief Abstract interface to define how the environment should handle - /// communication simulation. This class should be responsible for - /// handling dropouts, decay and packet collisions. + /// communication simulation. As an example, this class could be responsible + /// for handling dropouts, decay and packet collisions. class ICommsModel : public System, public ISystemConfigure, @@ -63,11 +64,16 @@ namespace comms const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override { - // We lock while we manipulate the queues. + IGN_PROFILE("ICommsModel::PreUpdate"); + + // We lock while we manipulate data. this->broker.Lock(); // Step the comms model. - this->Step(_info, _ecm, this->broker.DataManager()); + const Registry ¤tRegistry = this->broker.DataManager().DataConst(); + Registry newRegistry = this->broker.DataManager().Copy(); + this->Step(_info, currentRegistry, newRegistry, _ecm); + this->broker.DataManager().Set(newRegistry); this->broker.Unlock(); @@ -89,12 +95,15 @@ namespace comms /// \brief This method is called when there is a timestep in the simulator /// override this to update your data structures as needed. - /// \param[in] _info - Simulator information about the current timestep. + /// \param[in] _info Simulator information about the current timestep. + /// \param[in] _currentRegistry The current registry. + /// \param[in] _newRegistry The new registry. When Step() is finished this + /// will become the new registry. /// \param[in] _ecm - Ignition's ECM. - /// \param[in] _messageMgr - Use this to mark the message as arrived. public: virtual void Step(const UpdateInfo &_info, - EntityComponentManager &_ecm, - MsgManager &_messageMgr) = 0; + const Registry &_currentRegistry, + Registry &_newRegistry, + EntityComponentManager &_ecm) = 0; /// \brief Broker instance. public: Broker broker; diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 7121e30188..65a7b2927b 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -42,37 +42,32 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace comms { -/// \brief ToDo. -struct MsgContent +/// \brief A queue of message pointers. +using DataQueue = std::deque; + +/// \brief A map where the key is the topic subscribed to an address and +/// the value is a publisher to reach that topic. +using SubscriptionHandler = std::map; + +/// \brief All the information associated to an address. +struct AddressContent { /// \brief Queue of inbound messages. - public: std::deque> inboundMsgs; + public: DataQueue inboundMsgs; /// \brief Queue of outbound messages. - public: std::deque> outboundMsgs; + public: DataQueue outboundMsgs; - /// \brief A map where the key is the topic subscribed to this address and - /// the value is a publisher to reach that topic. - public: std::map subscriptions; + /// \brief Subscribers. + public: SubscriptionHandler subscriptions; /// \brief Model name associated to this address. public: std::string modelName; }; -// class MsgManagerData -// { -// /// \brief Default constructor. -// public: MsgManagerData(); - -// /// \brief Destructor. -// public: virtual ~MsgManagerData(); - -// public: - -// /// \brief Private data pointer. -// IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) -// }; +/// \brief A map where the key is an address and the value is all the +/// information associated to each address (subscribers, queues, ...). +using Registry = std::map; /// \brief ToDo. class MsgManager @@ -95,13 +90,13 @@ class MsgManager /// \param[in] _address The destination address. /// \param[in] _msg The message. public: void AddInbound(const std::string &_address, - const std::shared_ptr &_msg); + const msgs::DataframeSharedPtr &_msg); /// \brief Add a new message to the outbound queue. /// \param[in] _address The sender address. /// \param[in] _msg The message. public: void AddOutbound(const std::string &_address, - const std::shared_ptr &_msg); + const msgs::DataframeSharedPtr &_msg); /// \brief Remove an existing subscriber. /// \param The subscriber address. @@ -114,13 +109,13 @@ class MsgManager /// \param[in] _address The destination address. /// \param[in] _Msg Message pointer to remove. public: void RemoveInbound(const std::string &_address, - const std::shared_ptr &_msg); + const msgs::DataframeSharedPtr &_msg); /// \brief Remove a message from the outbound queue. /// \param[in] _address The sender address. /// \param[in] _msg Message pointer to remove. public: void RemoveOutbound(const std::string &_address, - const std::shared_ptr &_msg); + const msgs::DataframeSharedPtr &_msg); /// \brief This function delivers all the messages in the inbound queue to /// the appropriate subscribers. This function also clears the inbound queue. @@ -129,16 +124,21 @@ class MsgManager /// \brief Get a mutable reference to the data containing subscriptions and /// data queues. /// \return A mutable reference to the data. - public: std::map &Data(); + public: const Registry &DataConst(); + + /// \brief Get a mutable reference to the data containing subscriptions and + /// data queues. + /// \return A mutable reference to the data. + public: Registry &Data(); /// \brief Get a copy of the data structure containing subscriptions and data /// queues. /// \return A copy of the data. - public: std::map Copy(); + public: Registry Copy(); /// \brief Set the data structure containing subscriptions and data queues. /// \param[in] _newContent New content to be set. - public: void Set(std::map &_newContent); + public: void Set(Registry &_newContent); /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) diff --git a/src/comms/Broker.cc b/src/comms/Broker.cc index 5700195b0b..0de93b77ca 100644 --- a/src/comms/Broker.cc +++ b/src/comms/Broker.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -121,12 +122,16 @@ void Broker::Start() } ////////////////////////////////////////////////// -void Broker::OnBind(const ignition::msgs::StringMsg_V &_req) +bool Broker::OnBind(const ignition::msgs::StringMsg_V &_req, + ignition::msgs::Boolean &/*_rep*/) { auto count = _req.data_size(); if (count != 3) + { ignerr << "Receive incorrect number of arguments. " << "Expecting 3 and receive " << count << std::endl; + return false; + } std::string address = _req.data(0); std::string model = _req.data(1); @@ -137,6 +142,8 @@ void Broker::OnBind(const ignition::msgs::StringMsg_V &_req) ignmsg << "Address [" << address << "] bound to model [" << model << "] on topic [" << topic << "]" << std::endl; + + return true; } ////////////////////////////////////////////////// diff --git a/src/comms/Broker_TEST.cc b/src/comms/Broker_TEST.cc index 9d6d8410e9..3ba2d4383c 100644 --- a/src/comms/Broker_TEST.cc +++ b/src/comms/Broker_TEST.cc @@ -17,6 +17,7 @@ #include +#include #include #include #include "ignition/gazebo/comms/Broker.hh" @@ -48,7 +49,10 @@ TEST_F(BrokerTest, Broker) reqBind.add_data("addr1"); reqBind.add_data("model1"); reqBind.add_data("topic"); - broker.OnBind(reqBind); + ignition::msgs::Boolean rep; + bool result; + result = broker.OnBind(reqBind, rep); + EXPECT_TRUE(result); allData = broker.DataManager().Data(); EXPECT_EQ(1u, allData.size()); EXPECT_EQ(1u, allData["addr1"].subscriptions.size()); diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc index 03f8647aee..77eeb562db 100644 --- a/src/comms/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -28,28 +28,13 @@ #include "ignition/gazebo/config.hh" #include "ignition/gazebo/comms/MsgManager.hh" -// /// \brief Private MsgManagerData data class. -// class ignition::gazebo::comms::MsgManagerData::Implementation -// { -// /// \brief Queue of inbound messages. -// public: std::deque> inboundMsgs; - -// /// \brief Queue of outbound messages. -// public: std::deque> outboundMsgs; - -// /// \brief A map where the key is the topic subscribed to this address and -// /// the value is a publisher to reach that topic. -// public: std::map subscriptions; -// }; - /// \brief Private MsgManager data class. class ignition::gazebo::comms::MsgManager::Implementation { /// \brief Buffer to store the content associated to each address. /// The key is an address. The value contains all the information associated /// to the address. - public: std::map data; + public: Registry data; /// \brief An Ignition Transport node for communications. public: ignition::transport::Node node; @@ -59,18 +44,6 @@ using namespace ignition; using namespace gazebo; using namespace comms; -// ////////////////////////////////////////////////// -// MsgManagerData::MsgManagerData() -// : dataPtr(ignition::utils::MakeUniqueImpl()) -// { -// } - -// ////////////////////////////////////////////////// -// MsgManagerData::~MsgManagerData() -// { -// // cannot use default destructor because of dataPtr -// } - ////////////////////////////////////////////////// MsgManager::MsgManager() : dataPtr(ignition::utils::MakeUniqueImpl()) @@ -98,14 +71,14 @@ void MsgManager::AddSubscriber(const std::string &_address, ////////////////////////////////////////////////// void MsgManager::AddInbound(const std::string &_address, - const std::shared_ptr &_msg) + const msgs::DataframeSharedPtr &_msg) { this->dataPtr->data[_address].inboundMsgs.push_back(_msg); } ////////////////////////////////////////////////// void MsgManager::AddOutbound(const std::string &_address, - const std::shared_ptr &_msg) + const msgs::DataframeSharedPtr &_msg) { this->dataPtr->data[_address].outboundMsgs.push_back(_msg); } @@ -122,7 +95,7 @@ bool MsgManager::RemoveSubscriber(const std::string &_address, ////////////////////////////////////////////////// void MsgManager::RemoveInbound(const std::string &_address, - const std::shared_ptr &_msg) + const msgs::DataframeSharedPtr &_msg) { if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) return; @@ -133,7 +106,7 @@ void MsgManager::RemoveInbound(const std::string &_address, ////////////////////////////////////////////////// void MsgManager::RemoveOutbound(const std::string &_address, - const std::shared_ptr &_msg) + const msgs::DataframeSharedPtr &_msg) { if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) return; @@ -163,19 +136,25 @@ void MsgManager::DeliverMsgs() } ////////////////////////////////////////////////// -std::map &MsgManager::Data() +const Registry &MsgManager::DataConst() +{ + return this->dataPtr->data; +} + +////////////////////////////////////////////////// +Registry &MsgManager::Data() { return this->dataPtr->data; } ////////////////////////////////////////////////// -std::map MsgManager::Copy() +Registry MsgManager::Copy() { return this->dataPtr->data; } ////////////////////////////////////////////////// -void MsgManager::Set(std::map &_newContent) +void MsgManager::Set(Registry &_newContent) { this->dataPtr->data = _newContent; } diff --git a/src/comms/MsgManager_TEST.cc b/src/comms/MsgManager_TEST.cc index 82061e01ab..da82f91a74 100644 --- a/src/comms/MsgManager_TEST.cc +++ b/src/comms/MsgManager_TEST.cc @@ -80,7 +80,7 @@ TEST_F(MsgManagerTest, MsgManager) // test setting msg content auto msgIn2 = std::make_shared(); auto msgOut2 = std::make_shared(); - std::map content; + std::map content; content["addr6"].inboundMsgs.push_back(msgIn2); content["addr6"].outboundMsgs.push_back(msgOut2); msgManager.Set(content); diff --git a/src/systems/comms_endpoint/CommsEndpoint.cc b/src/systems/comms_endpoint/CommsEndpoint.cc index ba21bdbfba..3120ed97b3 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.cc +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -15,6 +15,11 @@ * */ +#include +#include + +#include +#include #include #include #include @@ -32,6 +37,15 @@ using namespace systems; class ignition::gazebo::systems::CommsEndpoint::Implementation { + /// \brief Send the bind request. + public: void Bind(); + + /// \brief Service response callback. + /// \brief \param[in] _rep Unused. + /// \brief \param[in] _result Bind result. + public: void BindCallback(const ignition::msgs::Boolean &_rep, + const bool _result); + /// \brief The address. public: std::string address; @@ -42,7 +56,7 @@ class ignition::gazebo::systems::CommsEndpoint::Implementation public: Model model{kNullEntity}; /// \brief True when the address has been bound in the broker. - public: bool bound{false}; + public: std::atomic_bool bound{false}; /// \brief Service where the broker is listening bind requests. public: std::string bindSrv = "/broker/bind"; @@ -50,10 +64,39 @@ class ignition::gazebo::systems::CommsEndpoint::Implementation /// \brief Service where the broker is listening unbind requests. public: std::string unbindSrv = "/broker/unbind"; + /// \brief Message to send the bind request. + public: ignition::msgs::StringMsg_V bindReq; + + /// \brief Message to send the unbind request. + public: ignition::msgs::StringMsg_V unbindReq; + + /// \brief Time between bind retries (secs). + public: std::chrono::steady_clock::duration bindRequestPeriod{1}; + + /// \brief Last simulation time we tried to bind. + public: std::chrono::steady_clock::duration lastBindRequestTime{-2}; + /// \brief The ignition transport node. public: ignition::transport::Node node; }; +////////////////////////////////////////////////// +void CommsEndpoint::Implementation::BindCallback( + const ignition::msgs::Boolean &/*_rep*/, const bool _result) +{ + if (_result) + this->bound = true; + + igndbg << "Succesfuly bound to [" << this->address << "] on topic [" + << this->topic << "]" << std::endl; +} + +////////////////////////////////////////////////// +void CommsEndpoint::Implementation::Bind() +{ + this->node.Request(this->bindSrv, this->bindReq, + &CommsEndpoint::Implementation::BindCallback, this); +} ////////////////////////////////////////////////// CommsEndpoint::CommsEndpoint() @@ -67,21 +110,17 @@ CommsEndpoint::~CommsEndpoint() if (!this->dataPtr->bound) return; - // Prepare the unbind parameters. - ignition::msgs::StringMsg_V unbindReq; - unbindReq.add_data(this->dataPtr->address); - unbindReq.add_data(this->dataPtr->topic); - // Unbind. - if (!this->dataPtr->node.Request("/broker/unbind", unbindReq)) - ignerr << "Bind call failed" << std::endl; - + // Note the we send the request as a one way request because we're not going + // to be alive to check the result or retry. + this->dataPtr->node.Request( + this->dataPtr->unbindSrv, this->dataPtr->unbindReq); } ////////////////////////////////////////////////// void CommsEndpoint::Configure(const Entity &_entity, const std::shared_ptr &_sdf, - EntityComponentManager &/*_ecm*/, + EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) { // Parse
. @@ -100,6 +139,7 @@ void CommsEndpoint::Configure(const Entity &_entity, } this->dataPtr->topic = _sdf->Get("topic"); + // Parse . if (_sdf->HasElement("broker")) { sdf::ElementPtr elem = _sdf->Clone()->GetElement("broker"); @@ -111,29 +151,37 @@ void CommsEndpoint::Configure(const Entity &_entity, // Set model. this->dataPtr->model = Model(_entity); + + // Prepare the bind parameters. + this->dataPtr->bindReq.add_data(this->dataPtr->address); + this->dataPtr->bindReq.add_data(this->dataPtr->model.Name(_ecm)); + this->dataPtr->bindReq.add_data(this->dataPtr->topic); + + // Prepare the unbind parameters. + this->dataPtr->unbindReq.add_data(this->dataPtr->address); + this->dataPtr->unbindReq.add_data(this->dataPtr->topic); } ////////////////////////////////////////////////// void CommsEndpoint::PreUpdate( - const ignition::gazebo::UpdateInfo &/*_info*/, - ignition::gazebo::EntityComponentManager &_ecm) + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &/*_ecm*/) { IGN_PROFILE("CommsEndpoint::PreUpdate"); if (this->dataPtr->bound) return; - // Prepare the bind parameters. - ignition::msgs::StringMsg_V bindReq; - bindReq.add_data(this->dataPtr->address); - bindReq.add_data(this->dataPtr->model.Name(_ecm)); - bindReq.add_data(this->dataPtr->topic); - - // Bind. - if (this->dataPtr->node.Request("/broker/bind", bindReq)) - this->dataPtr->bound = true; - else - ignerr << "Bind call failed" << std::endl; + auto elapsed = _info.simTime - this->dataPtr->lastBindRequestTime; + if (elapsed > std::chrono::steady_clock::duration::zero() && + elapsed < this->dataPtr->bindRequestPeriod) + { + return; + } + this->dataPtr->lastBindRequestTime = _info.simTime; + + // Let's try to bind. + this->dataPtr->Bind(); } IGNITION_ADD_PLUGIN(CommsEndpoint, diff --git a/src/systems/comms_endpoint/CommsEndpoint.hh b/src/systems/comms_endpoint/CommsEndpoint.hh index b9e26f9e81..f5aa327362 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.hh +++ b/src/systems/comms_endpoint/CommsEndpoint.hh @@ -31,6 +31,37 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { + /// \brief A system that registers in the comms broker an endpoint. + /// You're creating an address attached to the model where the plugin is + /// running. The system will bind this address in the broker automatically + /// for you and unbind it when the model is destroyed. + /// + /// The endpoint can be configured with the following SDF parameters: + /// + /// * Required parameters: + ///
An identifier used to receive messages (string). + /// The topic name where you want to receive the messages targeted to + /// this address. + /// + /// * Optional parameters: + /// Element used to capture where are the broker services. + /// This block can contain any of the next optional parameters: + /// : Service name used to bind an address. + /// The default value is "/broker/bind" + /// : Service name used to unbind from an address. + /// The default value is "/broker/unbind" + /// + /// Here's an example: + /// + ///
addr1
+ /// addr1/rx + /// + /// /broker/bind + /// /broker/unbind + /// + /// class CommsEndpoint : public System, public ISystemConfigure, diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index b8c84e2808..0038bfaf9c 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -46,7 +46,6 @@ PerfectComms::~PerfectComms() } ////////////////////////////////////////////////// -// void PerfectComms::Configure(const Entity &_entity, void PerfectComms::Load(const Entity &/*_entity*/, std::shared_ptr /*_sdf*/, EntityComponentManager &/*_ecm*/, @@ -54,30 +53,25 @@ void PerfectComms::Load(const Entity &/*_entity*/, { } +////////////////////////////////////////////////// void PerfectComms::Step( const UpdateInfo &/*_info*/, - EntityComponentManager &/*_ecm*/, - comms::MsgManager &/*_messageMgr*/) + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + EntityComponentManager &/*_ecm*/) { - // Check if there are new messages to process. - auto &allData = this->broker.DataManager().Data(); - - // Create a copy to modify while we iterate. - auto allDataCopy = this->broker.DataManager().Copy(); - - for (auto & [address, content] : allData) + for (auto & [address, content] : _currentRegistry) { // Reference to the outbound queue for this address. auto &outbound = content.outboundMsgs; // All these messages need to be processed. for (auto &msg : outbound) - allDataCopy[msg->dst_address()].inboundMsgs.push_back(msg); + _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); - allDataCopy[address].outboundMsgs.clear(); + // Clear the outbound queue. + _newRegistry[address].outboundMsgs.clear(); } - - this->broker.DataManager().Set(allDataCopy); } IGNITION_ADD_PLUGIN(PerfectComms, diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index ef270140ca..3643ec0905 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -54,8 +54,9 @@ namespace systems // Documentation inherited. public: void Step(const ignition::gazebo::UpdateInfo &_info, - EntityComponentManager &_ecm, - comms::MsgManager &_messageMgr); + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + EntityComponentManager &_ecm); /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) diff --git a/src/systems/perfect_comms/models/CMakeLists.txt b/src/systems/perfect_comms/models/CMakeLists.txt deleted file mode 100644 index c1b72abb68..0000000000 --- a/src/systems/perfect_comms/models/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -gz_add_system(perfect-comms-model - SOURCES - PerfectCommsModel.cc - ../MsgManager.cc - ../Broker.cc - PUBLIC_LINK_LIBS - ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} -) From 9baf15e02c9fd6665cc5e5b1000219bc32509b3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sun, 27 Mar 2022 20:48:05 +0200 Subject: [PATCH 11/51] Updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/Broker.hh | 4 ++++ include/ignition/gazebo/comms/ICommsModel.hh | 3 +++ src/comms/Broker.cc | 25 +++++++++++++++----- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 867e8f8617..3c3ee49ef8 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -98,6 +98,10 @@ namespace comms /// clients until the broker has been entirely initialized. public: void Start(); + /// \brief Set the current time. + /// \param[in] _time Current time. + public: void SetTime(const std::chrono::steady_clock::duration &_time); + /// \brief This method associates an address with a client topic used as /// callback for receiving messages. This is a client requirement to /// start receiving messages. diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 6c658d6f77..3a44b13ea2 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -69,6 +69,9 @@ namespace comms // We lock while we manipulate data. this->broker.Lock(); + // Update the time in the broker. + this->broker.SetTime(_info.simTime); + // Step the comms model. const Registry ¤tRegistry = this->broker.DataManager().DataConst(); Registry newRegistry = this->broker.DataManager().Copy(); diff --git a/src/comms/Broker.cc b/src/comms/Broker.cc index 0de93b77ca..f198423ba7 100644 --- a/src/comms/Broker.cc +++ b/src/comms/Broker.cc @@ -17,25 +17,21 @@ #include #include +#include -#include -#include #include #include #include -#include #include #include "ignition/gazebo/comms/Broker.hh" #include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/Util.hh" /// \brief Private Broker data class. class ignition::gazebo::comms::Broker::Implementation { - /// \brief An Ignition Transport node for communications. - public: ignition::transport::Node node; - /// \brief The message manager. public: MsgManager data; @@ -50,6 +46,12 @@ class ignition::gazebo::comms::Broker::Implementation /// \brief Service used to unbind from an address. public: std::string unbindSrv = "/broker/unbind"; + + /// \brief The current time. + public: std::chrono::steady_clock::duration time{0}; + + /// \brief An Ignition Transport node for communications. + public: ignition::transport::Node node; }; using namespace ignition; @@ -121,6 +123,12 @@ void Broker::Start() << std::endl; } +////////////////////////////////////////////////// +void Broker::SetTime(const std::chrono::steady_clock::duration &_time) +{ + this->dataPtr->time = _time; +} + ////////////////////////////////////////////////// bool Broker::OnBind(const ignition::msgs::StringMsg_V &_req, ignition::msgs::Boolean &/*_rep*/) @@ -171,6 +179,11 @@ void Broker::OnMsg(const ignition::msgs::Dataframe &_msg) auto msgPtr = std::make_shared(_msg); std::lock_guard lk(this->dataPtr->mutex); + + // Stamp the time. + msgPtr->mutable_header()->mutable_stamp()->CopyFrom( + gazebo::convert(this->dataPtr->time)); + this->DataManager().AddOutbound(_msg.src_address(), msgPtr); } From 9d2e2e34262075d8466c411dbbc6cebeee5f338a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 28 Mar 2022 18:26:21 +0200 Subject: [PATCH 12/51] Tweak tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/Broker.hh | 10 ++- include/ignition/gazebo/comms/ICommsModel.hh | 3 + include/ignition/gazebo/comms/MsgManager.hh | 14 ++-- src/comms/Broker.cc | 13 +++- src/comms/Broker_TEST.cc | 29 ++++++-- src/comms/MsgManager.cc | 57 +++++++++++++--- src/comms/MsgManager_TEST.cc | 70 ++++++++++++++++---- src/systems/comms_endpoint/CommsEndpoint.cc | 2 +- test/integration/comms.cc | 31 ++++----- 9 files changed, 172 insertions(+), 57 deletions(-) diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 3c3ee49ef8..c20e2707a3 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -51,9 +51,9 @@ namespace comms /// to be delivered to one of the destination, it'll be stored in the inbound /// queue of the destination's address. /// - /// The main goal of this class is to receive the comms requests and place - /// them in the appropriate outbound queue, as well as deliver the messages - /// that are in the inbound queues. + /// The main goal of this class is to receive the comms requests, stamp the + /// time, and place them in the appropriate outbound queue, as well as deliver + /// the messages that are in the inbound queues. /// /// The instance of the comms model is responsible for moving around the /// messages from the outbound queues to the inbound queues. @@ -98,6 +98,10 @@ namespace comms /// clients until the broker has been entirely initialized. public: void Start(); + /// \brief Get the current time. + /// \return Current time. + public: std::chrono::steady_clock::duration Time() const; + /// \brief Set the current time. /// \param[in] _time Current time. public: void SetTime(const std::chrono::steady_clock::duration &_time); diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 3a44b13ea2..4feb5617c0 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -66,6 +66,9 @@ namespace comms { IGN_PROFILE("ICommsModel::PreUpdate"); + if (_info.paused) + return; + // We lock while we manipulate data. this->broker.Lock(); diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 65a7b2927b..ae0b32259d 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -78,11 +78,15 @@ class MsgManager /// \brief Destructor. public: virtual ~MsgManager(); - /// \brief Add a new subscriber + /// \brief Add a new subscriber. It's possible to associate multiple topics + /// to the same address/model pair. However, the same address cannot be + /// attached to multiple models. When all the subscribers are removed, it's + /// posible to bind to this address using a different model. /// \param[in] _address The subscriber address. /// \param[in] _modelName The model name. /// \param[in] _topic The subscriber topic. - public: void AddSubscriber(const std::string &_address, + /// \return True if the subscriber was successfully added or false otherwise. + public: bool AddSubscriber(const std::string &_address, const std::string &_modelName, const std::string &_topic); @@ -108,13 +112,15 @@ class MsgManager /// \brief Remove a message from the inbound queue. /// \param[in] _address The destination address. /// \param[in] _Msg Message pointer to remove. - public: void RemoveInbound(const std::string &_address, + /// \return True if the message was removed or false otherwise. + public: bool RemoveInbound(const std::string &_address, const msgs::DataframeSharedPtr &_msg); /// \brief Remove a message from the outbound queue. /// \param[in] _address The sender address. /// \param[in] _msg Message pointer to remove. - public: void RemoveOutbound(const std::string &_address, + /// \return True if the message was removed or false otherwise. + public: bool RemoveOutbound(const std::string &_address, const msgs::DataframeSharedPtr &_msg); /// \brief This function delivers all the messages in the inbound queue to diff --git a/src/comms/Broker.cc b/src/comms/Broker.cc index f198423ba7..f3a3571322 100644 --- a/src/comms/Broker.cc +++ b/src/comms/Broker.cc @@ -123,6 +123,12 @@ void Broker::Start() << std::endl; } +////////////////////////////////////////////////// +std::chrono::steady_clock::duration Broker::Time() const +{ + return this->dataPtr->time; +} + ////////////////////////////////////////////////// void Broker::SetTime(const std::chrono::steady_clock::duration &_time) { @@ -146,7 +152,9 @@ bool Broker::OnBind(const ignition::msgs::StringMsg_V &_req, std::string topic = _req.data(2); std::lock_guard lk(this->dataPtr->mutex); - this->DataManager().AddSubscriber(address, model, topic); + + if (!this->DataManager().AddSubscriber(address, model, topic)) + return false; ignmsg << "Address [" << address << "] bound to model [" << model << "] on topic [" << topic << "]" << std::endl; @@ -159,8 +167,11 @@ void Broker::OnUnbind(const ignition::msgs::StringMsg_V &_req) { auto count = _req.data_size(); if (count != 2) + { ignerr << "Receive incorrect number of arguments. " << "Expecting 2 and receive " << count << std::endl; + return; + } std::string address = _req.data(0); std::string topic = _req.data(1); diff --git a/src/comms/Broker_TEST.cc b/src/comms/Broker_TEST.cc index 3ba2d4383c..5b22430c1a 100644 --- a/src/comms/Broker_TEST.cc +++ b/src/comms/Broker_TEST.cc @@ -44,16 +44,21 @@ TEST_F(BrokerTest, Broker) EXPECT_TRUE(allData.empty()); EXPECT_NO_THROW(broker.Unlock()); + // Test manually binding with an incorrect number of arguments. + msgs::StringMsg_V wrongReqBind; + wrongReqBind.add_data("addr1"); + wrongReqBind.add_data("model1"); + ignition::msgs::Boolean unused; + EXPECT_FALSE(broker.OnBind(wrongReqBind, unused)); + allData = broker.DataManager().Data(); + EXPECT_EQ(0u, allData.size()); + // Test manually binding address and topic. msgs::StringMsg_V reqBind; reqBind.add_data("addr1"); reqBind.add_data("model1"); reqBind.add_data("topic"); - ignition::msgs::Boolean rep; - bool result; - result = broker.OnBind(reqBind, rep); - EXPECT_TRUE(result); - allData = broker.DataManager().Data(); + EXPECT_TRUE(broker.OnBind(reqBind, unused)); EXPECT_EQ(1u, allData.size()); EXPECT_EQ(1u, allData["addr1"].subscriptions.size()); EXPECT_NE(allData["addr1"].subscriptions.end(), @@ -67,6 +72,13 @@ TEST_F(BrokerTest, Broker) EXPECT_EQ(1u, allData["addr1"].outboundMsgs.size()); EXPECT_EQ("addr1", allData["addr1"].outboundMsgs[0u]->src_address()); + // Test manually unbinding with an incorrect number of arguments. + msgs::StringMsg_V wrongReqUnbind; + wrongReqUnbind.add_data("addr1"); + broker.OnUnbind(wrongReqUnbind); + EXPECT_EQ(1u, allData.size()); + EXPECT_FALSE(allData["addr1"].subscriptions.empty()); + // Test manually unbinding address and topic. msgs::StringMsg_V reqUnbind; reqUnbind.add_data("addr1"); @@ -81,4 +93,11 @@ TEST_F(BrokerTest, Broker) EXPECT_EQ(1u, allData["addr2"].inboundMsgs.size()); broker.DeliverMsgs(); EXPECT_TRUE(allData["addr2"].inboundMsgs.empty()); + + // Test time. + const std::chrono::steady_clock::duration time0{0}; + const std::chrono::steady_clock::duration time1{1}; + EXPECT_EQ(time0, broker.Time()); + broker.SetTime(time1); + EXPECT_EQ(time1, broker.Time()); } diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc index 77eeb562db..9fd93ceab5 100644 --- a/src/comms/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -57,16 +57,27 @@ MsgManager::~MsgManager() } ////////////////////////////////////////////////// -void MsgManager::AddSubscriber(const std::string &_address, +bool MsgManager::AddSubscriber(const std::string &_address, const std::string &_modelName, const std::string &_topic) { + auto it = this->dataPtr->data.find(_address); + if (it != this->dataPtr->data.end()) + { + if (!it->second.modelName.empty() && it->second.modelName != _modelName) + { + ignerr << "AddSubscriber() error: Address already attached to a different" + << " model" << std::endl; + return false; + } + } this->dataPtr->data[_address].modelName = _modelName; ignition::transport::Node::Publisher publisher = this->dataPtr->node.Advertise(_topic); this->dataPtr->data[_address].subscriptions[_topic] = publisher; + return true; } ////////////////////////////////////////////////// @@ -87,32 +98,56 @@ void MsgManager::AddOutbound(const std::string &_address, bool MsgManager::RemoveSubscriber(const std::string &_address, const std::string &_topic) { - if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) + auto it = this->dataPtr->data.find(_address); + if (it == this->dataPtr->data.end()) + { + ignerr << "RemoveSubscriber() error: Unable to find address [" + << _address << "]" << std::endl; return false; + } + + auto res = it->second.subscriptions.erase(_topic) > 0; - return this->dataPtr->data[_address].subscriptions.erase(_topic) > 0; + // It there are no subscribers we clear the model name. This way the address + // can be bound to a separate model. We also clear the queues. + if (it->second.subscriptions.empty()) + it->second.modelName = ""; + + return res; } ////////////////////////////////////////////////// -void MsgManager::RemoveInbound(const std::string &_address, +bool MsgManager::RemoveInbound(const std::string &_address, const msgs::DataframeSharedPtr &_msg) { - if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) - return; + auto it = this->dataPtr->data.find(_address); + if (it == this->dataPtr->data.end()) + { + ignerr << "RemoveInbound() error: Unable to find address [" + << _address << "]" << std::endl; + return false; + } - auto &q = this->dataPtr->data[_address].inboundMsgs; + auto &q = it->second.inboundMsgs; q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); + return true; } ////////////////////////////////////////////////// -void MsgManager::RemoveOutbound(const std::string &_address, +bool MsgManager::RemoveOutbound(const std::string &_address, const msgs::DataframeSharedPtr &_msg) { - if (this->dataPtr->data.find(_address) == this->dataPtr->data.end()) - return; + auto it = this->dataPtr->data.find(_address); + if (it == this->dataPtr->data.end()) + { + ignerr << "RemoveOutbound() error: Unable to find address [" + << _address << "]" << std::endl; + return false; + } - auto &q = this->dataPtr->data[_address].outboundMsgs; + auto &q = it->second.outboundMsgs; q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); + return true; } ////////////////////////////////////////////////// diff --git a/src/comms/MsgManager_TEST.cc b/src/comms/MsgManager_TEST.cc index da82f91a74..9e96d4e8d8 100644 --- a/src/comms/MsgManager_TEST.cc +++ b/src/comms/MsgManager_TEST.cc @@ -16,7 +16,6 @@ */ #include -#include #include #include "ignition/gazebo/comms/MsgManager.hh" #include "helpers/EnvTestFixture.hh" @@ -37,14 +36,36 @@ TEST_F(MsgManagerTest, MsgManager) EXPECT_TRUE(msgManager.Data().empty()); EXPECT_TRUE(msgManager.Copy().empty()); - // test subscriber - msgManager.AddSubscriber("addr1", "model1", "topic1"); + // Test subscriber. + EXPECT_TRUE(msgManager.AddSubscriber("addr1", "model1", "topic1")); EXPECT_EQ(1u, msgManager.Data().size()); EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); + EXPECT_EQ(1u, msgManager.Data()["addr1"].subscriptions.size()); EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), msgManager.Data()["addr1"].subscriptions.find("topic1")); + EXPECT_EQ("model1", msgManager.Data()["addr1"].modelName); - // test inbound + // Try to bind to an address attached to another model. + EXPECT_FALSE(msgManager.AddSubscriber("addr1", "model2", "topic2")); + EXPECT_EQ(1u, msgManager.Data().size()); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); + EXPECT_EQ(1u, msgManager.Data()["addr1"].subscriptions.size()); + EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), + msgManager.Data()["addr1"].subscriptions.find("topic1")); + EXPECT_EQ("model1", msgManager.Data()["addr1"].modelName); + + // Add an additional topic. + EXPECT_TRUE(msgManager.AddSubscriber("addr1", "model1", "topic2")); + EXPECT_EQ(1u, msgManager.Data().size()); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); + EXPECT_EQ(2u, msgManager.Data()["addr1"].subscriptions.size()); + EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), + msgManager.Data()["addr1"].subscriptions.find("topic1")); + EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), + msgManager.Data()["addr1"].subscriptions.find("topic2")); + EXPECT_EQ("model1", msgManager.Data()["addr1"].modelName); + + // Test inbound. auto msgIn = std::make_shared(); EXPECT_EQ(msgManager.Data().end(), msgManager.Data().find("addr2")); msgManager.AddInbound("addr2", msgIn); @@ -52,7 +73,7 @@ TEST_F(MsgManagerTest, MsgManager) EXPECT_FALSE(msgManager.Data()["addr2"].inboundMsgs.empty()); EXPECT_EQ(msgIn, msgManager.Data()["addr2"].inboundMsgs[0]); - // test outbound + // Test outbound. auto msgOut = std::make_shared(); EXPECT_EQ(msgManager.Data().end(), msgManager.Data().find("addr3")); msgManager.AddOutbound("addr3", msgOut); @@ -60,24 +81,42 @@ TEST_F(MsgManagerTest, MsgManager) EXPECT_FALSE(msgManager.Data()["addr3"].outboundMsgs.empty()); EXPECT_EQ(msgOut, msgManager.Data()["addr3"].outboundMsgs[0]); - // test msg removal - msgManager.RemoveInbound("addr2", msgIn); + // Test msg removal. + EXPECT_FALSE(msgManager.RemoveInbound("not_found", msgIn)); + EXPECT_TRUE(msgManager.RemoveInbound("addr2", msgIn)); EXPECT_TRUE(msgManager.Data()["addr2"].inboundMsgs.empty()); - msgManager.RemoveOutbound("addr3", msgOut); + EXPECT_FALSE(msgManager.RemoveOutbound("not_found", msgOut)); + EXPECT_TRUE(msgManager.RemoveOutbound("addr3", msgOut)); EXPECT_TRUE(msgManager.Data()["addr3"].outboundMsgs.empty()); - // test msg delivery + // Test msg delivery. msgManager.AddInbound("addr4", msgIn); EXPECT_FALSE(msgManager.Data()["addr4"].inboundMsgs.empty()); msgManager.DeliverMsgs(); EXPECT_TRUE(msgManager.Data()["addr4"].inboundMsgs.empty()); - // test subscriber removal + // Test subscriber removal. msgManager.RemoveSubscriber("addr1", "topic1"); EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); - EXPECT_TRUE(msgManager.Data()["addr1"].subscriptions.empty()); + EXPECT_EQ(1u, msgManager.Data()["addr1"].subscriptions.size()); + EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), + msgManager.Data()["addr1"].subscriptions.find("topic2")); + msgManager.RemoveSubscriber("addr1", "topic2"); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); + EXPECT_EQ(0u, msgManager.Data()["addr1"].subscriptions.size()); + EXPECT_TRUE(msgManager.Data()["addr1"].modelName.empty()); - // test setting msg content + // Without subscribers, we should be able to recycle the address for a + // different model. + EXPECT_TRUE(msgManager.AddSubscriber("addr1", "model2", "topic2")); + EXPECT_EQ(4u, msgManager.Data().size()); + EXPECT_NE(msgManager.Data().end(), msgManager.Data().find("addr1")); + EXPECT_EQ(1u, msgManager.Data()["addr1"].subscriptions.size()); + EXPECT_NE(msgManager.Data()["addr1"].subscriptions.end(), + msgManager.Data()["addr1"].subscriptions.find("topic2")); + EXPECT_EQ("model2", msgManager.Data()["addr1"].modelName); + + // Test setting msg content. auto msgIn2 = std::make_shared(); auto msgOut2 = std::make_shared(); std::map content; @@ -90,10 +129,15 @@ TEST_F(MsgManagerTest, MsgManager) EXPECT_EQ(1u, msgManager.Data()["addr6"].outboundMsgs.size()); EXPECT_EQ(msgOut2, msgManager.Data()["addr6"].outboundMsgs[0u]); - // test copy + // Test copy. EXPECT_TRUE(msgManager.Copy()["addr6"].subscriptions.empty()); EXPECT_EQ(1u, msgManager.Copy()["addr6"].inboundMsgs.size()); EXPECT_EQ(msgIn2, msgManager.Copy()["addr6"].inboundMsgs[0u]); EXPECT_EQ(1u, msgManager.Copy()["addr6"].outboundMsgs.size()); EXPECT_EQ(msgOut2, msgManager.Copy()["addr6"].outboundMsgs[0u]); + + // Test DataConst. + auto it = msgManager.DataConst().find("addr6"); + EXPECT_TRUE(it->second.subscriptions.empty()); + } diff --git a/src/systems/comms_endpoint/CommsEndpoint.cc b/src/systems/comms_endpoint/CommsEndpoint.cc index 3120ed97b3..ebcf55f0c2 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.cc +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -111,7 +111,7 @@ CommsEndpoint::~CommsEndpoint() return; // Unbind. - // Note the we send the request as a one way request because we're not going + // We use a oneway request because we're not going // to be alive to check the result or retry. this->dataPtr->node.Request( this->dataPtr->unbindSrv, this->dataPtr->unbindReq); diff --git a/test/integration/comms.cc b/test/integration/comms.cc index 19fbc0be80..485f76c0db 100644 --- a/test/integration/comms.cc +++ b/test/integration/comms.cc @@ -58,7 +58,7 @@ TEST_F(CommsTest, Comms) std::mutex mutex; auto cb = [&](const msgs::Dataframe &_msg) -> void { - // verify msg content + // Verify msg content std::lock_guard lock(mutex); std::string expected = "hello world " + std::to_string(msgCounter); @@ -68,33 +68,26 @@ TEST_F(CommsTest, Comms) msgCounter++; }; - // create subscriber + // Create subscriber. ignition::transport::Node node; - std::string addr = "addr1"; - std::string topic = "topic1"; + std::string addr = "addr1"; + std::string subscriptionTopic = "addr1/rx"; // Subscribe to a topic by registering a callback. auto cbFunc = std::function(cb); - EXPECT_TRUE(node.Subscribe(topic, cbFunc)) - << "Error subscribing to topic [" << topic << "]"; + EXPECT_TRUE(node.Subscribe(subscriptionTopic, cbFunc)) + << "Error subscribing to topic [" << subscriptionTopic << "]"; - // Prepare the input parameters. - ignition::msgs::StringMsg_V req; - req.add_data(addr); - req.add_data(topic); - - // Bind. - EXPECT_TRUE(node.Request("/broker/bind", req)); - - // create publisher - auto pub = node.Advertise(topic); + // Create publisher. + std::string publicationTopic = "/broker/msgs"; + auto pub = node.Advertise(publicationTopic); std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Prepare the message. ignition::msgs::Dataframe msg; msg.set_src_address("unused"); msg.set_dst_address(addr); - // publish 10 messages + // Publish 10 messages. ignition::msgs::StringMsg payload; unsigned int pubCount = 10u; for (unsigned int i = 0u; i < pubCount; ++i) @@ -106,10 +99,10 @@ TEST_F(CommsTest, Comms) << payload.DebugString(); msg.set_data(serializedData); EXPECT_TRUE(pub.Publish(msg)); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + server.Run(true, 100, false); } - // verify subscriber received all msgs + // Verify subscriber received all msgs. int sleep = 0; bool done = false; while (!done && sleep++ < 10) From 5363fca7c478635e53eba7128fc052d3307a1df9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 28 Mar 2022 20:54:50 +0200 Subject: [PATCH 13/51] Style. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 1 + src/CMakeLists.txt | 2 -- src/systems/CMakeLists.txt | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 4feb5617c0..e59f7b8213 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -18,6 +18,7 @@ #ifndef IGNITION_GAZEBO_ICOMMSMODEL_HH_ #define IGNITION_GAZEBO_ICOMMSMODEL_HH_ +#include #include #include #include "ignition/gazebo/comms/Broker.hh" diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 123a7fadd1..3c3af60e00 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -52,13 +52,11 @@ target_link_libraries(${ign_lib_target} set (sources Barrier.cc BaseView.cc -# Broker.cc Conversions.cc EntityComponentManager.cc LevelManager.cc Link.cc Model.cc -# MsgManager.cc Primitives.cc SdfEntityCreator.cc SdfGenerator.cc diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index abad89748c..1e4389cfe8 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -136,8 +136,8 @@ add_subdirectory(optical_tactile_plugin) add_subdirectory(particle_emitter) add_subdirectory(particle_emitter2) add_subdirectory(performer_detector) -add_subdirectory(physics) add_subdirectory(perfect_comms) +add_subdirectory(physics) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) From 5e7d4a87d4b5820d62578637e7306b9eba7ab8c0 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 30 Mar 2022 10:47:27 +0800 Subject: [PATCH 14/51] switch to unordered_map Signed-off-by: Arjo Chakravarty --- include/ignition/gazebo/comms/MsgManager.hh | 5 +++-- src/comms/MsgManager_TEST.cc | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 65a7b2927b..ed5597158a 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -47,7 +48,7 @@ using DataQueue = std::deque; /// \brief A map where the key is the topic subscribed to an address and /// the value is a publisher to reach that topic. -using SubscriptionHandler = std::map; +using SubscriptionHandler = std::unordered_map; /// \brief All the information associated to an address. struct AddressContent @@ -67,7 +68,7 @@ struct AddressContent /// \brief A map where the key is an address and the value is all the /// information associated to each address (subscribers, queues, ...). -using Registry = std::map; +using Registry = std::unordered_map; /// \brief ToDo. class MsgManager diff --git a/src/comms/MsgManager_TEST.cc b/src/comms/MsgManager_TEST.cc index da82f91a74..ee666875ee 100644 --- a/src/comms/MsgManager_TEST.cc +++ b/src/comms/MsgManager_TEST.cc @@ -80,7 +80,7 @@ TEST_F(MsgManagerTest, MsgManager) // test setting msg content auto msgIn2 = std::make_shared(); auto msgOut2 = std::make_shared(); - std::map content; + std::unordered_map content; content["addr6"].inboundMsgs.push_back(msgIn2); content["addr6"].outboundMsgs.push_back(msgOut2); msgManager.Set(content); From beefb6bf950dc48ffb783c2c17ca2357ca8ad846 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 30 Mar 2022 20:46:25 +0200 Subject: [PATCH 15/51] Step size and doxygen. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/standalone/comms/publisher.cc | 1 + include/ignition/gazebo/comms/Broker.hh | 2 + include/ignition/gazebo/comms/ICommsModel.hh | 83 +++++++++++++++++++- include/ignition/gazebo/comms/MsgManager.hh | 8 +- src/comms/Broker_TEST.cc | 2 +- src/comms/MsgManager.cc | 3 - src/comms/MsgManager_TEST.cc | 3 + src/systems/comms_endpoint/CommsEndpoint.cc | 3 +- src/systems/comms_endpoint/CommsEndpoint.hh | 2 +- src/systems/perfect_comms/PerfectComms.cc | 1 - src/systems/perfect_comms/PerfectComms.hh | 2 +- test/integration/comms.cc | 8 +- 12 files changed, 100 insertions(+), 18 deletions(-) diff --git a/examples/standalone/comms/publisher.cc b/examples/standalone/comms/publisher.cc index 810307f022..bbf981d40f 100644 --- a/examples/standalone/comms/publisher.cc +++ b/examples/standalone/comms/publisher.cc @@ -23,6 +23,7 @@ #include #include #include + #include #include diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index c20e2707a3..7359924434 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -19,6 +19,7 @@ #define IGNITION_GAZEBO_BROKER_HH_ #include + #include #include #include "ignition/gazebo/config.hh" @@ -135,6 +136,7 @@ namespace comms public: void DeliverMsgs(); /// \brief Get a mutable reference to the message manager. + /// \return The mutable reference. public: MsgManager &DataManager(); /// \brief Lock the mutex to access the message manager. diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index e59f7b8213..702100d83e 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -18,7 +18,9 @@ #ifndef IGNITION_GAZEBO_ICOMMSMODEL_HH_ #define IGNITION_GAZEBO_ICOMMSMODEL_HH_ +#include #include + #include #include #include "ignition/gazebo/comms/Broker.hh" @@ -40,6 +42,34 @@ namespace comms /// \brief Abstract interface to define how the environment should handle /// communication simulation. As an example, this class could be responsible /// for handling dropouts, decay and packet collisions. + /// + /// The derived comms models can be configured with the following SDF + /// parameters: + /// + /// * Optional parameters: + /// If defined this will allow the comms model to run at a + /// higher frequency than the physics engine. This is useful when dealing + /// with ranging. If the is set larger than the physics engine dt + /// then the comms model step size will default to dt. + /// Note: for consistency it is adviced that the dt is a multiple of timestep. + /// Units are in seconds. + /// : Topic name where the broker receives all the incoming + /// messages. The default value is "/broker/msgs" + /// : Service name used to bind an address. + /// The default value is "/broker/bind" + /// : Service name used to unbind from an address. + /// The default value is "/broker/unbind" + /// + /// Here's an example: + /// + /// 2 + /// 1.0 + /// + /// + /// 1 + /// class ICommsModel : public System, public ISystemConfigure, @@ -55,6 +85,14 @@ namespace comms EventManager &_eventMgr) override { sdf::ElementPtr sdfClone = _sdf->Clone(); + + // Parse the optional . + if (sdfClone->HasElement("step_size")) + { + this->timeStep = std::chrono::duration( + static_cast(sdfClone->Get("step_size") * 1e9)); + } + this->Load(_entity, sdfClone, _ecm, _eventMgr); this->broker.Load(sdfClone); this->broker.Start(); @@ -70,6 +108,42 @@ namespace comms if (_info.paused) return; + this->currentTime = + std::chrono::steady_clock::time_point(_info.simTime); + + if (!this->timeStep.has_value()) + { + // If no step_size is defined simply execute one step of the comms model + this->StepImpl(_info, _ecm); + } + else + { + // Otherwise step at the specified time step until we converge on the + // final timestep. If the timestep is larger than the dt, then dt will + // be used. + auto endTime = this->currentTime + _info.dt; + + while (this->currentTime < endTime) + { + ignition::gazebo::UpdateInfo info(_info); + info.dt = std::min(this->timeStep.value(), _info.dt); + info.simTime = this->currentTime.time_since_epoch(); + this->StepImpl(_info, _ecm); + this->currentTime += info.dt; + } + } + } + + /// \brief This method is called when there is a timestep in the simulator + /// override this to update your data structures as needed. + /// \param[in] _info Simulator information about the current timestep. + /// \param[in] _currentRegistry The current registry. + /// \param[in] _newRegistry The new registry. When Step() is finished this + /// will become the new registry. + /// \param[in] _ecm - Ignition's ECM. + public: virtual void StepImpl(const UpdateInfo &_info, + EntityComponentManager &_ecm) + { // We lock while we manipulate data. this->broker.Lock(); @@ -113,7 +187,14 @@ namespace comms EntityComponentManager &_ecm) = 0; /// \brief Broker instance. - public: Broker broker; + protected: Broker broker; + + /// \brief The step size for each step iteration. + protected: std::optional + timeStep = std::nullopt; + + /// \brief Current time. + protected: std::chrono::steady_clock::time_point currentTime; }; } } diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index e455600ad1..1c22e6cc33 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -19,7 +19,6 @@ #define IGNITION_GAZEBO_MSGMANAGER_HH_ #include -#include #include #include #include @@ -48,7 +47,8 @@ using DataQueue = std::deque; /// \brief A map where the key is the topic subscribed to an address and /// the value is a publisher to reach that topic. -using SubscriptionHandler = std::unordered_map; +using SubscriptionHandler = + std::unordered_map; /// \brief All the information associated to an address. struct AddressContent @@ -104,8 +104,8 @@ class MsgManager const msgs::DataframeSharedPtr &_msg); /// \brief Remove an existing subscriber. - /// \param The subscriber address. - /// \param The Subscriber topic. + /// \param[in] _address The subscriber address. + /// \param[in] _topic The Subscriber topic. /// \return True if the subscriber was removed or false otherwise. public: bool RemoveSubscriber(const std::string &_address, const std::string &_topic); diff --git a/src/comms/Broker_TEST.cc b/src/comms/Broker_TEST.cc index 5b22430c1a..66e263fa9b 100644 --- a/src/comms/Broker_TEST.cc +++ b/src/comms/Broker_TEST.cc @@ -16,10 +16,10 @@ */ #include - #include #include #include + #include "ignition/gazebo/comms/Broker.hh" #include "ignition/gazebo/comms/MsgManager.hh" #include "helpers/EnvTestFixture.hh" diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc index 9fd93ceab5..60859cfbc5 100644 --- a/src/comms/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -18,10 +18,7 @@ #include #include -#include -#include #include -#include #include #include diff --git a/src/comms/MsgManager_TEST.cc b/src/comms/MsgManager_TEST.cc index d5866a0535..b0e0989405 100644 --- a/src/comms/MsgManager_TEST.cc +++ b/src/comms/MsgManager_TEST.cc @@ -17,6 +17,9 @@ #include #include + +#include + #include "ignition/gazebo/comms/MsgManager.hh" #include "helpers/EnvTestFixture.hh" diff --git a/src/systems/comms_endpoint/CommsEndpoint.cc b/src/systems/comms_endpoint/CommsEndpoint.cc index ebcf55f0c2..2756bea200 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.cc +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -21,14 +21,13 @@ #include #include #include + #include #include #include #include - #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" - #include "CommsEndpoint.hh" using namespace ignition; diff --git a/src/systems/comms_endpoint/CommsEndpoint.hh b/src/systems/comms_endpoint/CommsEndpoint.hh index f5aa327362..a21e9b3089 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.hh +++ b/src/systems/comms_endpoint/CommsEndpoint.hh @@ -18,9 +18,9 @@ #define IGNITION_GAZEBO_SYSTEMS_COMMSENDPOINT_HH_ #include + #include #include - #include "ignition/gazebo/System.hh" namespace ignition diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index 0038bfaf9c..5b9a44fe6c 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -19,7 +19,6 @@ #include #include - #include "ignition/gazebo/comms/Broker.hh" #include "ignition/gazebo/comms/MsgManager.hh" #include "ignition/gazebo/Util.hh" diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index 3643ec0905..9e37697e8f 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -18,9 +18,9 @@ #define IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ #include + #include #include - #include "ignition/gazebo/comms/ICommsModel.hh" #include "ignition/gazebo/System.hh" diff --git a/test/integration/comms.cc b/test/integration/comms.cc index 485f76c0db..b21dd41e86 100644 --- a/test/integration/comms.cc +++ b/test/integration/comms.cc @@ -16,6 +16,7 @@ */ #include + #include #include #include @@ -23,10 +24,8 @@ #include #include #include - #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) - #include "../helpers/EnvTestFixture.hh" using namespace ignition; @@ -42,8 +41,9 @@ TEST_F(CommsTest, Comms) { // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/examples/worlds/comms.sdf"; + const auto sdfFile = + ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "comms.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); From c531b254bb6a893976aeeb1b8025e6510f0f02b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sat, 2 Apr 2022 16:38:55 +0200 Subject: [PATCH 16/51] Style. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 702100d83e..bd8821870b 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -89,8 +89,8 @@ namespace comms // Parse the optional . if (sdfClone->HasElement("step_size")) { - this->timeStep = std::chrono::duration( - static_cast(sdfClone->Get("step_size") * 1e9)); + this->timeStep = std::chrono::duration( + static_cast(sdfClone->Get("step_size") * 1e9)); } this->Load(_entity, sdfClone, _ecm, _eventMgr); From c73d1b42058204248f62353cc340fbc0bc6e6883 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sat, 2 Apr 2022 20:38:13 +0200 Subject: [PATCH 17/51] Data structures, SDF parsing and pose updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/standalone/comms/publisher.cc | 2 +- examples/worlds/comms.sdf | 30 ++- src/systems/CMakeLists.txt | 1 + src/systems/comms_endpoint/CommsEndpoint.cc | 15 +- src/systems/wireless_comms/CMakeLists.txt | 6 + src/systems/wireless_comms/WirelessComms.cc | 266 ++++++++++++++++++++ src/systems/wireless_comms/WirelessComms.hh | 69 +++++ 7 files changed, 385 insertions(+), 4 deletions(-) create mode 100644 src/systems/wireless_comms/CMakeLists.txt create mode 100644 src/systems/wireless_comms/WirelessComms.cc create mode 100644 src/systems/wireless_comms/WirelessComms.hh diff --git a/examples/standalone/comms/publisher.cc b/examples/standalone/comms/publisher.cc index bbf981d40f..0579da04bf 100644 --- a/examples/standalone/comms/publisher.cc +++ b/examples/standalone/comms/publisher.cc @@ -75,7 +75,7 @@ int main(int argc, char **argv) // Prepare the message. ignition::msgs::Dataframe msg; - msg.set_src_address("unused"); + msg.set_src_address("addr1"); msg.set_dst_address(argv[1]); // Publish messages at 1Hz. diff --git a/examples/worlds/comms.sdf b/examples/worlds/comms.sdf index a67c432ed7..624f38dc00 100644 --- a/examples/worlds/comms.sdf +++ b/examples/worlds/comms.sdf @@ -35,8 +35,20 @@ name="ignition::gazebo::systems::SceneBroadcaster"> + filename="ignition-gazebo-wireless-comms-system" + name="ignition::gazebo::systems::WirelessComms"> + + 500.0 + 1.5 + 40 + 10.0 + + + 1000000 + 20 + -90 + QPSK + @@ -119,9 +131,22 @@ + box_link
addr1
addr1/rx + + box_link + true + 10 + 10 + + 0 + 500 + + @@ -163,6 +188,7 @@ + box_link
addr2
addr2/rx diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 1e4389cfe8..990d180da5 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -153,3 +153,4 @@ add_subdirectory(user_commands) add_subdirectory(velocity_control) add_subdirectory(wheel_slip) add_subdirectory(wind_effects) +add_subdirectory(wireless_comms) diff --git a/src/systems/comms_endpoint/CommsEndpoint.cc b/src/systems/comms_endpoint/CommsEndpoint.cc index 2756bea200..e20857e2c0 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.cc +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -54,6 +54,9 @@ class ignition::gazebo::systems::CommsEndpoint::Implementation /// \brief Model interface public: Model model{kNullEntity}; + /// \brief ToDo. + public: std::string linkName; + /// \brief True when the address has been bound in the broker. public: std::atomic_bool bound{false}; @@ -138,6 +141,14 @@ void CommsEndpoint::Configure(const Entity &_entity, } this->dataPtr->topic = _sdf->Get("topic"); + // Parse . + if (!_sdf->HasElement("link_name")) + { + ignerr << "No specified." << std::endl; + return; + } + this->dataPtr->linkName = _sdf->Get("link_name"); + // Parse . if (_sdf->HasElement("broker")) { @@ -153,7 +164,9 @@ void CommsEndpoint::Configure(const Entity &_entity, // Prepare the bind parameters. this->dataPtr->bindReq.add_data(this->dataPtr->address); - this->dataPtr->bindReq.add_data(this->dataPtr->model.Name(_ecm)); + // this->dataPtr->bindReq.add_data(this->dataPtr->model.Name(_ecm)); + this->dataPtr->bindReq.add_data(this->dataPtr->model.Name(_ecm) + + "::" + this->dataPtr->linkName); this->dataPtr->bindReq.add_data(this->dataPtr->topic); // Prepare the unbind parameters. diff --git a/src/systems/wireless_comms/CMakeLists.txt b/src/systems/wireless_comms/CMakeLists.txt new file mode 100644 index 0000000000..e21e1b6162 --- /dev/null +++ b/src/systems/wireless_comms/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(wireless-comms + SOURCES + WirelessComms.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/wireless_comms/WirelessComms.cc b/src/systems/wireless_comms/WirelessComms.cc new file mode 100644 index 0000000000..56fa656167 --- /dev/null +++ b/src/systems/wireless_comms/WirelessComms.cc @@ -0,0 +1,266 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +#include "ignition/gazebo/comms/Broker.hh" +#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" +#include "WirelessComms.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Parameters for simple log-normal fading model. +struct RFConfiguration +{ + /// \brief Hard limit on range. + double maxRange = 50.0; + /// \brief Fading exponent. + double fadingExponent = 2.5; + /// \brief Received power at 1m (in dBm). + double l0 = 40; + /// \beief Standard deviation for received power. + double sigma = 10; + + /// Output stream operator. + /// @param[out] _oss Stream. + /// @param[in] _config configuration to output. + friend std::ostream &operator<<(std::ostream &_oss, + const RFConfiguration &_config) + { + _oss << "RF Configuration (range-based)" << std::endl + << "-- max_range: " << _config.maxRange << std::endl + << "-- fading_exponent: " << _config.fadingExponent << std::endl + << "-- L0: " << _config.l0 << std::endl + << "-- sigma: " << _config.sigma << std::endl; + + return _oss; + } +}; + +/// \brief Radio configuration parameters. +/// +/// In addition to static parameters such as channel capacity and +/// default transmit power, this structure holds a function which can +/// be called to compute the pathloss between two antenna poses. +struct RadioConfiguration +{ + /// \brief Capacity of radio in bits-per-second. + double capacity = 54000000; + /// \brief Default transmit power in dBm. Default is 27dBm or 500mW. + double txPower = 27; + /// \brief Modulation scheme, e.g., QPSK (Quadrature Phase Shift Keyring). + std::string modulation = "QPSK"; + /// \brief Noise floor of the radio in dBm. + double noiseFloor = -90; + /// \brief Function handle for computing pathloss. + //rf_interface::pathloss_function pathloss_f; + + /// Output stream operator. + /// @param _oss Stream. + /// @param _config configuration to output. + friend std::ostream &operator<<(std::ostream &_oss, + const RadioConfiguration &_config) + { + _oss << "Radio Configuration" << std::endl + << "-- capacity: " << _config.capacity << std::endl + << "-- tx_power: " << _config.txPower << std::endl + << "-- noise_floor: " << _config.noiseFloor << std::endl + << "-- modulation: " << _config.modulation << std::endl; + + return _oss; + } +}; + +/// \brief Store radio state +/// +/// Structure to hold radio state including the pose and book-keeping +/// necessary to implement bitrate limits. +struct RadioState +{ + /// \brief Timestamp of last update. + double updateStamp; + + /// \brief Pose of the radio. + ignition::math::Pose3 pose; + + /// \brief Recent sent packet history. + std::list> bytesSent; + + /// \brief Accumulation of bytes sent in an epoch. + uint64_t bytesSentThisEpoch = 0; + + /// \brief Recent received packet history. + std::list> bytesReceived; + + /// \brief Accumulation of bytes received in an epoch. + uint64_t bytesReceivedThisEpoch = 0; + + /// \brief Isotropic antenna gain. + double antennaGain; +}; + +/// \brief Type for holding RF power as a Normally distributed random variable. +struct RFPower +{ + /// \brief Expected value of RF power. + double mean; + + /// \brief Variance of RF power. + double variance; + + /// \brief ToDo. + operator double() { return mean; } +}; + +/// \brief Private WirelessComms data class. +class ignition::gazebo::systems::WirelessComms::Implementation +{ + /// \brief Range configuration. + public: RFConfiguration rangeConfig; + + /// \brief Radio configuration. + public: RadioConfiguration radioConfig; + + /// \brief A map where the key is the model name and the value is the + /// Model instance associated. + public: std::unordered_map links; +}; + +////////////////////////////////////////////////// +WirelessComms::WirelessComms() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +WirelessComms::~WirelessComms() +{ + // cannot use default destructor because of dataPtr +} + +////////////////////////////////////////////////// +void WirelessComms::Load(const Entity &/*_entity*/, + std::shared_ptr _sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + if (_sdf->HasElement("range_config")) + { + sdf::ElementPtr elem = _sdf->Clone()->GetElement("range_config"); + + this->dataPtr->rangeConfig.maxRange = + elem->Get("max_range", this->dataPtr->rangeConfig.maxRange).first; + + this->dataPtr->rangeConfig.fadingExponent = + elem->Get("fading_exponent", + this->dataPtr->rangeConfig.fadingExponent).first; + + this->dataPtr->rangeConfig.l0 = + elem->Get("l0", this->dataPtr->rangeConfig.l0).first; + + this->dataPtr->rangeConfig.sigma = + elem->Get("sigma", this->dataPtr->rangeConfig.sigma).first; + } + + if (_sdf->HasElement("radio_config")) + { + sdf::ElementPtr elem = _sdf->Clone()->GetElement("radio_config"); + + this->dataPtr->radioConfig.capacity = + elem->Get("capacity", this->dataPtr->radioConfig.capacity).first; + + this->dataPtr->radioConfig.txPower = + elem->Get("tx_power", this->dataPtr->radioConfig.txPower).first; + + this->dataPtr->radioConfig.modulation = + elem->Get("modulation", + this->dataPtr->radioConfig.modulation).first; + + this->dataPtr->radioConfig.noiseFloor = + elem->Get("noise_floor", + this->dataPtr->radioConfig.noiseFloor).first; + } + + igndbg << "Range configuration:" << std::endl + << this->dataPtr->rangeConfig << std::endl; + + igndbg << "Radio configuration:" << std::endl + << this->dataPtr->radioConfig << std::endl; +} + +////////////////////////////////////////////////// +void WirelessComms::Step( + const UpdateInfo &/*_info*/, + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + EntityComponentManager &_ecm) +{ + // Make sure that all addresses have its corresponding Link initialized. + for (auto & [address, content] : _currentRegistry) + { + std::string linkName = content.modelName; + if (this->dataPtr->links.find(address) != this->dataPtr->links.end()) + continue; + + auto entities = gazebo::entitiesFromScopedName(linkName, _ecm); + if (entities.empty()) + continue; + + auto entityId = *(entities.begin()); + this->dataPtr->links[address] = gazebo::Link(entityId); + enableComponent(_ecm, entityId); + } + + for (auto & [address, content] : _currentRegistry) + { + // Reference to the outbound queue for this address. + auto &outbound = content.outboundMsgs; + + // All these messages need to be processed. + for (auto &msg : outbound) + { + igndbg << "Pose src: " + << *(this->dataPtr->links[msg->src_address()].WorldPose(_ecm)) + << std::endl; + + igndbg << "Pose dst: " + << *(this->dataPtr->links[msg->dst_address()].WorldPose(_ecm)) + << std::endl; + + _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + } + + // Clear the outbound queue. + _newRegistry[address].outboundMsgs.clear(); + } +} + +IGNITION_ADD_PLUGIN(WirelessComms, + ignition::gazebo::System, + comms::ICommsModel::ISystemConfigure, + comms::ICommsModel::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(WirelessComms, + "ignition::gazebo::systems::WirelessComms") diff --git a/src/systems/wireless_comms/WirelessComms.hh b/src/systems/wireless_comms/WirelessComms.hh new file mode 100644 index 0000000000..c65dc14199 --- /dev/null +++ b/src/systems/wireless_comms/WirelessComms.hh @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_WIRELESSCOMMS_HH_ +#define IGNITION_GAZEBO_SYSTEMS_WIRELESSCOMMS_HH_ + +#include + +#include +#include +#include "ignition/gazebo/comms/ICommsModel.hh" +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class MsgManager; + + /// \brief An example of a comms model. + /// This model always delivers any message to its destination. + class WirelessComms + : public comms::ICommsModel + { + /// \brief Constructor. + public: WirelessComms(); + + /// \brief Destructor. + public: ~WirelessComms(); + + // Documentation inherited. + public: void Load(const Entity &_entity, + std::shared_ptr _sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited. + public: void Step(const ignition::gazebo::UpdateInfo &_info, + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + EntityComponentManager &_ecm); + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + } +} +} +} + +#endif From 6b844042e4d984f8ea6d88271b009c0323019e99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sun, 3 Apr 2022 19:29:20 +0200 Subject: [PATCH 18/51] Working version. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/worlds/comms.sdf | 17 +- src/systems/CMakeLists.txt | 2 +- .../CMakeLists.txt | 4 +- src/systems/rf_comms/RFComms.cc | 461 ++++++++++++++++++ src/systems/rf_comms/RFComms.hh | 109 +++++ src/systems/wireless_comms/WirelessComms.cc | 266 ---------- src/systems/wireless_comms/WirelessComms.hh | 69 --- 7 files changed, 582 insertions(+), 346 deletions(-) rename src/systems/{wireless_comms => rf_comms}/CMakeLists.txt (68%) create mode 100644 src/systems/rf_comms/RFComms.cc create mode 100644 src/systems/rf_comms/RFComms.hh delete mode 100644 src/systems/wireless_comms/WirelessComms.cc delete mode 100644 src/systems/wireless_comms/WirelessComms.hh diff --git a/examples/worlds/comms.sdf b/examples/worlds/comms.sdf index 624f38dc00..7fba33803f 100644 --- a/examples/worlds/comms.sdf +++ b/examples/worlds/comms.sdf @@ -35,12 +35,12 @@ name="ignition::gazebo::systems::SceneBroadcaster"> + filename="ignition-gazebo-rf-comms-system" + name="ignition::gazebo::systems::RFComms"> - 500.0 + 500000.0 1.5 - 40 + 40 10.0 @@ -93,7 +93,8 @@ - 2 0 0.5 0 0 0 + true + 30000 0 0.5 0 0 0 @@ -135,7 +136,7 @@
addr1
addr1/rx - box_link @@ -144,9 +145,9 @@ 10 0 - 500 + 5000 - + --> diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 990d180da5..676c19dbeb 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -139,6 +139,7 @@ add_subdirectory(performer_detector) add_subdirectory(perfect_comms) add_subdirectory(physics) add_subdirectory(pose_publisher) +add_subdirectory(rf_comms) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) add_subdirectory(shader_param) @@ -153,4 +154,3 @@ add_subdirectory(user_commands) add_subdirectory(velocity_control) add_subdirectory(wheel_slip) add_subdirectory(wind_effects) -add_subdirectory(wireless_comms) diff --git a/src/systems/wireless_comms/CMakeLists.txt b/src/systems/rf_comms/CMakeLists.txt similarity index 68% rename from src/systems/wireless_comms/CMakeLists.txt rename to src/systems/rf_comms/CMakeLists.txt index e21e1b6162..e5d151aaa6 100644 --- a/src/systems/wireless_comms/CMakeLists.txt +++ b/src/systems/rf_comms/CMakeLists.txt @@ -1,6 +1,6 @@ -gz_add_system(wireless-comms +gz_add_system(rf-comms SOURCES - WirelessComms.cc + RFComms.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc new file mode 100644 index 0000000000..d95e2311f2 --- /dev/null +++ b/src/systems/rf_comms/RFComms.cc @@ -0,0 +1,461 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" +#include "RFComms.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Parameters for simple log-normal fading model. +struct RangeConfiguration +{ + /// \brief Hard limit on range. + double maxRange = 50.0; + + /// \brief Fading exponent. + double fadingExponent = 2.5; + + /// \brief Received power at 1m (in dBm). + double l0 = 40; + + /// \beief Standard deviation for received power. + double sigma = 10; + + /// Output stream operator. + /// @param[out] _oss Stream. + /// @param[in] _config configuration to output. + friend std::ostream &operator<<(std::ostream &_oss, + const RangeConfiguration &_config) + { + _oss << "RF Configuration (range-based)" << std::endl + << "-- max_range: " << _config.maxRange << std::endl + << "-- fading_exponent: " << _config.fadingExponent << std::endl + << "-- l0: " << _config.l0 << std::endl + << "-- sigma: " << _config.sigma << std::endl; + + return _oss; + } +}; + +/// \brief Radio configuration parameters. +/// +/// Static parameters such as channel capacity and transmit power. +struct RadioConfiguration +{ + /// \brief Capacity of radio in bits-per-second. + double capacity = 54000000; + + /// \brief Default transmit power in dBm. Default is 27dBm or 500mW. + double txPower = 27; + + /// \brief Modulation scheme, e.g., QPSK (Quadrature Phase Shift Keyring). + std::string modulation = "QPSK"; + + /// \brief Noise floor of the radio in dBm. + double noiseFloor = -90; + + /// Output stream operator. + /// @param _oss Stream. + /// @param _config configuration to output. + friend std::ostream &operator<<(std::ostream &_oss, + const RadioConfiguration &_config) + { + _oss << "Radio Configuration" << std::endl + << "-- capacity: " << _config.capacity << std::endl + << "-- tx_power: " << _config.txPower << std::endl + << "-- noise_floor: " << _config.noiseFloor << std::endl + << "-- modulation: " << _config.modulation << std::endl; + + return _oss; + } +}; + +/// \brief Store radio state +/// +/// Structure to hold radio state including the pose and book-keeping +/// necessary to implement bitrate limits. +struct RadioState +{ + /// \brief Timestamp of last update. + double timeStamp; + + /// \brief Pose of the radio. + ignition::math::Pose3 pose; + + /// \brief Recent sent packet history. + std::list> bytesSent; + + /// \brief Accumulation of bytes sent in an epoch. + uint64_t bytesSentThisEpoch = 0; + + /// \brief Recent received packet history. + std::list> bytesReceived; + + /// \brief Accumulation of bytes received in an epoch. + uint64_t bytesReceivedThisEpoch = 0; +}; + +/// \brief Type for holding RF power as a Normally distributed random variable. +struct RFPower +{ + /// \brief Expected value of RF power. + double mean; + + /// \brief Variance of RF power. + double variance; + + /// \brief double operator. + /// \return the RFPower as a double. + operator double() + { + return mean; + } +}; + +/// \brief Private RFComms data class. +class ignition::gazebo::systems::RFComms::Implementation +{ + /// \brief Attempt communication between two nodes. + /// + /// The radio configuration, transmitter and receiver state, and + /// packet size are all used to compute the probability of successful + /// communication (i.e., based on both SNR and bitrate + /// limitations). This probability is then used to determine if the + /// packet is successfully communicated. + /// + /// @param radio Static configuration for the radio + /// \param[in out] _txState Current state of the transmitter. + /// \param[in out] _rxState Current state of the receiver. + /// \param[in] _numBytes Size of the packet. + /// \return std::tuple reporting if the packet should be + /// delivered and the received signal strength (in dBm). + public: std::tuple AttemptSend(RadioState &_txState, + RadioState &_rxState, + const uint64_t &_numBytes); + + /// \brief Convert from dBm to power. + /// \param[in] _dBm Input in dBm. + /// \return Power in watts (W). + private: double DbmToPow(double _dBm) const; + + /// \brief Compute the bit error rate (BER). + /// \param[in] _power Rx power (dBm). + /// \param[in] _noise Noise value (dBm). + /// \return Based on rx_power, noise value, and modulation, compute the bit + // error rate (BER). + private: double QPSKPowerToBER(double _power, + double _noise) const; + + /// \brief Function to compute the pathloss between two antenna poses. + /// \param[in] _txPower Tx power. + /// \param[in] _txState Radio state of the transmitter. + /// \param[in] _rxState Radio state of the receiver. + /// \return The RFPower pathloss distribution of the two antenna poses. + private: RFPower LogNormalReceivedPower(const double &_txPower, + const RadioState &_txState, + const RadioState &_rxState) const; + + /// \brief Range configuration. + public: RangeConfiguration rangeConfig; + + /// \brief Radio configuration. + public: RadioConfiguration radioConfig; + + /// \brief A map where the key is the address and the value is the + /// Link instance associated. + public: std::unordered_map links; + + /// \brief A map where the key is the address and the value is its radio state + public: std::unordered_map radioStates; + + /// \brief Duration of an epoch (seconds). + public: double epochDuration = 1.0; + + /// \brief Random number generator. + public: std::default_random_engine rndEngine; +}; + +///////////////////////////////////////////// +double RFComms::Implementation::DbmToPow(double _dBm) const +{ + return 0.001 * pow(10., _dBm / 10.); +} + +//////////////////////////////////////////// +double RFComms::Implementation::QPSKPowerToBER( + double _power, double _noise) const +{ + return erfc(sqrt(_power / _noise)); +} + +///////////////////////////////////////////// +RFPower RFComms::Implementation::LogNormalReceivedPower( + const double &_txPower, const RadioState &_txState, + const RadioState &_rxState) const +{ + double range = _txState.pose.Pos().Distance(_rxState.pose.Pos()); + + if (this->rangeConfig.maxRange > 0.0 && range > this->rangeConfig.maxRange) + return {-std::numeric_limits::infinity(), 0.0}; + + double PL = + this->rangeConfig.l0 + 10 * this->rangeConfig.fadingExponent * log10(range); + + return {_txPower - PL, this->rangeConfig.sigma}; +} + +///////////////////////////////////////////// +std::tuple RFComms::Implementation::AttemptSend( + RadioState &_txState, RadioState &_rxState, const uint64_t &_numBytes) +{ + double now = _txState.timeStamp; + + // Maintain running window of bytes sent over the last epoch, e.g., 1s. + while (!_txState.bytesSent.empty() && + _txState.bytesSent.front().first < now - this->epochDuration) + { + _txState.bytesSentThisEpoch -= _txState.bytesSent.front().second; + _txState.bytesSent.pop_front(); + } + + // igndbg << "Bytes sent: " << _txState.bytesSentThisEpoch << " + " + // << _numBytes << " = " + // << _txState.bytesSentThisEpoch + _numBytes << std::endl; + + // Compute prospective accumulated bits along with time window + // (including this packet). + double bitsSent = (_txState.bytesSentThisEpoch + _numBytes) * 8; + + // Check current epoch bitrate vs capacity and fail to send accordingly + if (bitsSent > this->radioConfig.capacity * this->epochDuration) + { + ignwarn << "Bitrate limited: " << bitsSent << "bits sent (limit: " + << this->radioConfig.capacity * this->epochDuration << std::endl; + return std::make_tuple(false, std::numeric_limits::lowest()); + } + + // Record these bytes. + _txState.bytesSent.push_back(std::make_pair(now, _numBytes)); + _txState.bytesSentThisEpoch += _numBytes; + + // Get the received power based on TX power and position of each node. + auto rxPowerDist = + this->LogNormalReceivedPower(this->radioConfig.txPower, _txState, _rxState); + + double rxPower = rxPowerDist.mean; + if (rxPowerDist.variance > 0.0) + { + std::normal_distribution<> d{rxPowerDist.mean, sqrt(rxPowerDist.variance)}; + rxPower = d(this->rndEngine); + } + + // Based on rx_power, noise value, and modulation, compute the bit + // error rate (BER). + double ber = this->QPSKPowerToBER( + this->DbmToPow(rxPower), this->DbmToPow(this->radioConfig.noiseFloor)); + + double packetDropProb = 1.0 - exp(_numBytes * log(1 - ber)); + + igndbg << "TX power (dBm): " << this->radioConfig.txPower << "\n" << + "RX power (dBm): " << rxPower << "\n" << + "BER: " << ber << "\n" << + "# Bytes: " << _numBytes << "\n" << + "PER: " << packetDropProb << std::endl; + + double randDraw = (rand() % 1000) / 1000.0; + + bool packetReceived = randDraw > packetDropProb; + + if (!packetReceived) + return std::make_tuple(false, std::numeric_limits::lowest()); + + // Maintain running window of bytes received over the last epoch, e.g., 1s. + while (!_rxState.bytesReceived.empty() && + _rxState.bytesReceived.front().first < now - this->epochDuration) + { + _rxState.bytesReceivedThisEpoch -= _rxState.bytesReceived.front().second; + _rxState.bytesReceived.pop_front(); + } + + // igndbg << "bytes received: " << _rxState.bytesReceivedThisEpoch + // << " + " << _numBytes + // << " = " << _rxState.bytesReceivedThisEpoch + _numBytes << std::endl; + + // Compute prospective accumulated bits along with time window + // (including this packet). + double bitsReceived = (_rxState.bytesReceivedThisEpoch + _numBytes) * 8; + + // Check current epoch bitrate vs capacity and fail to send accordingly. + if (bitsReceived > this->radioConfig.capacity * this->epochDuration) + { + // ignwarn << "Bitrate limited: " << bitsReceived + // << "bits received (limit: " + // << this->radioConfig.capacity * this->epochDuration << std::endl; + return std::make_tuple(false, std::numeric_limits::lowest()); + } + + // Record these bytes. + _rxState.bytesReceived.push_back(std::make_pair(now, _numBytes)); + _rxState.bytesReceivedThisEpoch += _numBytes; + + return std::make_tuple(true, rxPower); +} + +////////////////////////////////////////////////// +RFComms::RFComms() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +RFComms::~RFComms() +{ + // cannot use default destructor because of dataPtr +} + +////////////////////////////////////////////////// +void RFComms::Load(const Entity &/*_entity*/, + std::shared_ptr _sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + if (_sdf->HasElement("range_config")) + { + sdf::ElementPtr elem = _sdf->Clone()->GetElement("range_config"); + + this->dataPtr->rangeConfig.maxRange = + elem->Get("max_range", this->dataPtr->rangeConfig.maxRange).first; + + this->dataPtr->rangeConfig.fadingExponent = + elem->Get("fading_exponent", + this->dataPtr->rangeConfig.fadingExponent).first; + + this->dataPtr->rangeConfig.l0 = + elem->Get("l0", this->dataPtr->rangeConfig.l0).first; + + this->dataPtr->rangeConfig.sigma = + elem->Get("sigma", this->dataPtr->rangeConfig.sigma).first; + } + + if (_sdf->HasElement("radio_config")) + { + sdf::ElementPtr elem = _sdf->Clone()->GetElement("radio_config"); + + this->dataPtr->radioConfig.capacity = + elem->Get("capacity", this->dataPtr->radioConfig.capacity).first; + + this->dataPtr->radioConfig.txPower = + elem->Get("tx_power", this->dataPtr->radioConfig.txPower).first; + + this->dataPtr->radioConfig.modulation = + elem->Get("modulation", + this->dataPtr->radioConfig.modulation).first; + + this->dataPtr->radioConfig.noiseFloor = + elem->Get("noise_floor", + this->dataPtr->radioConfig.noiseFloor).first; + } + + igndbg << "Range configuration:" << std::endl + << this->dataPtr->rangeConfig << std::endl; + + igndbg << "Radio configuration:" << std::endl + << this->dataPtr->radioConfig << std::endl; +} + +////////////////////////////////////////////////// +void RFComms::Step( + const UpdateInfo &_info, + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + EntityComponentManager &_ecm) +{ + // Make sure that all addresses have its corresponding Link initialized. + for (auto & [address, content] : _currentRegistry) + { + std::string linkName = content.modelName; + if (this->dataPtr->links.find(address) != this->dataPtr->links.end()) + continue; + + auto entities = gazebo::entitiesFromScopedName(linkName, _ecm); + if (entities.empty()) + continue; + + auto entityId = *(entities.begin()); + this->dataPtr->links[address] = gazebo::Link(entityId); + enableComponent(_ecm, entityId); + } + + for (auto & [address, content] : _currentRegistry) + { + // Reference to the outbound queue for this address. + auto &outbound = content.outboundMsgs; + + // All these messages need to be processed. + for (auto &msg : outbound) + { + // Update radio state. + const auto kSrcAddress = msg->src_address(); + const auto kDstAddress = msg->dst_address(); + + this->dataPtr->radioStates[kSrcAddress].pose = + *(this->dataPtr->links[kSrcAddress].WorldPose(_ecm)); + this->dataPtr->radioStates[kSrcAddress].timeStamp = + std::chrono::duration(_info.simTime).count(); + + this->dataPtr->radioStates[kDstAddress].pose = + *(this->dataPtr->links[kDstAddress].WorldPose(_ecm)); + this->dataPtr->radioStates[kDstAddress].timeStamp = + std::chrono::duration(_info.simTime).count(); + + auto [sendPacket, rssi] = this->dataPtr->AttemptSend( + this->dataPtr->radioStates[kSrcAddress], + this->dataPtr->radioStates[kDstAddress], + msg->data().size()); + + if (sendPacket) + _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + } + + // Clear the outbound queue. + _newRegistry[address].outboundMsgs.clear(); + } +} + +IGNITION_ADD_PLUGIN(RFComms, + ignition::gazebo::System, + comms::ICommsModel::ISystemConfigure, + comms::ICommsModel::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(RFComms, + "ignition::gazebo::systems::RFComms") diff --git a/src/systems/rf_comms/RFComms.hh b/src/systems/rf_comms/RFComms.hh new file mode 100644 index 0000000000..cd4e05e9c4 --- /dev/null +++ b/src/systems/rf_comms/RFComms.hh @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_RFCOMMS_HH_ +#define IGNITION_GAZEBO_SYSTEMS_RFCOMMS_HH_ + +#include + +#include +#include +#include "ignition/gazebo/comms/ICommsModel.hh" +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + /// \brief An example of a comms model. + /// This model simulates communication using radio frequency (RF) devices. + /// The model uses a log-distance path loss function. + /// + /// This sytem can be configured with the following SDF parameters: + /// + /// * Optional parameters: + /// Element used to capture the range configuration based on a + /// log-normal distribution. This block can contain any of the + /// next parameters: + /// * : Hard limit on range (meters). No communication will + /// happen beyond this range. Default is 50. + /// * : Fading exponent used in the normal distribution. + /// Default is 2.5. + /// * : Path loss at the reference distance (1 meter) in dBm. + /// Default is 40. + /// * : Standard deviation of the normal distribution. + /// Default is 10. + /// + /// Element used to capture the radio configuration. + /// This block can contain any of the + /// next parameters: + /// * : Capacity of radio in bits-per-second. + /// Default is 54000000 (54 Mbps). + /// * : Transmitter power in dBm. Default is 27dBm (500mW). + /// * : Noise floor in dBm. Default is -90dBm. + /// * : Supported modulations: ["QPSK"]. Default is "QPSK". + /// + /// Here's an example: + /// + /// + /// 500000.0 + /// 1.5 + /// 40 + /// 10.0 + /// + /// + /// 1000000 + /// 20 + /// -90 + /// QPSK + /// + /// + class RFComms + : public comms::ICommsModel + { + /// \brief Constructor. + public: RFComms(); + + /// \brief Destructor. + public: ~RFComms(); + + // Documentation inherited. + public: void Load(const Entity &_entity, + std::shared_ptr _sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited. + public: void Step(const ignition::gazebo::UpdateInfo &_info, + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + EntityComponentManager &_ecm); + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + } +} +} +} + +#endif diff --git a/src/systems/wireless_comms/WirelessComms.cc b/src/systems/wireless_comms/WirelessComms.cc deleted file mode 100644 index 56fa656167..0000000000 --- a/src/systems/wireless_comms/WirelessComms.cc +++ /dev/null @@ -1,266 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#include - -#include -#include -#include "ignition/gazebo/comms/Broker.hh" -#include "ignition/gazebo/comms/MsgManager.hh" -#include "ignition/gazebo/components/Pose.hh" -#include "ignition/gazebo/Link.hh" -#include "ignition/gazebo/Model.hh" -#include "ignition/gazebo/Util.hh" -#include "WirelessComms.hh" - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -/// \brief Parameters for simple log-normal fading model. -struct RFConfiguration -{ - /// \brief Hard limit on range. - double maxRange = 50.0; - /// \brief Fading exponent. - double fadingExponent = 2.5; - /// \brief Received power at 1m (in dBm). - double l0 = 40; - /// \beief Standard deviation for received power. - double sigma = 10; - - /// Output stream operator. - /// @param[out] _oss Stream. - /// @param[in] _config configuration to output. - friend std::ostream &operator<<(std::ostream &_oss, - const RFConfiguration &_config) - { - _oss << "RF Configuration (range-based)" << std::endl - << "-- max_range: " << _config.maxRange << std::endl - << "-- fading_exponent: " << _config.fadingExponent << std::endl - << "-- L0: " << _config.l0 << std::endl - << "-- sigma: " << _config.sigma << std::endl; - - return _oss; - } -}; - -/// \brief Radio configuration parameters. -/// -/// In addition to static parameters such as channel capacity and -/// default transmit power, this structure holds a function which can -/// be called to compute the pathloss between two antenna poses. -struct RadioConfiguration -{ - /// \brief Capacity of radio in bits-per-second. - double capacity = 54000000; - /// \brief Default transmit power in dBm. Default is 27dBm or 500mW. - double txPower = 27; - /// \brief Modulation scheme, e.g., QPSK (Quadrature Phase Shift Keyring). - std::string modulation = "QPSK"; - /// \brief Noise floor of the radio in dBm. - double noiseFloor = -90; - /// \brief Function handle for computing pathloss. - //rf_interface::pathloss_function pathloss_f; - - /// Output stream operator. - /// @param _oss Stream. - /// @param _config configuration to output. - friend std::ostream &operator<<(std::ostream &_oss, - const RadioConfiguration &_config) - { - _oss << "Radio Configuration" << std::endl - << "-- capacity: " << _config.capacity << std::endl - << "-- tx_power: " << _config.txPower << std::endl - << "-- noise_floor: " << _config.noiseFloor << std::endl - << "-- modulation: " << _config.modulation << std::endl; - - return _oss; - } -}; - -/// \brief Store radio state -/// -/// Structure to hold radio state including the pose and book-keeping -/// necessary to implement bitrate limits. -struct RadioState -{ - /// \brief Timestamp of last update. - double updateStamp; - - /// \brief Pose of the radio. - ignition::math::Pose3 pose; - - /// \brief Recent sent packet history. - std::list> bytesSent; - - /// \brief Accumulation of bytes sent in an epoch. - uint64_t bytesSentThisEpoch = 0; - - /// \brief Recent received packet history. - std::list> bytesReceived; - - /// \brief Accumulation of bytes received in an epoch. - uint64_t bytesReceivedThisEpoch = 0; - - /// \brief Isotropic antenna gain. - double antennaGain; -}; - -/// \brief Type for holding RF power as a Normally distributed random variable. -struct RFPower -{ - /// \brief Expected value of RF power. - double mean; - - /// \brief Variance of RF power. - double variance; - - /// \brief ToDo. - operator double() { return mean; } -}; - -/// \brief Private WirelessComms data class. -class ignition::gazebo::systems::WirelessComms::Implementation -{ - /// \brief Range configuration. - public: RFConfiguration rangeConfig; - - /// \brief Radio configuration. - public: RadioConfiguration radioConfig; - - /// \brief A map where the key is the model name and the value is the - /// Model instance associated. - public: std::unordered_map links; -}; - -////////////////////////////////////////////////// -WirelessComms::WirelessComms() - : dataPtr(ignition::utils::MakeUniqueImpl()) -{ -} - -////////////////////////////////////////////////// -WirelessComms::~WirelessComms() -{ - // cannot use default destructor because of dataPtr -} - -////////////////////////////////////////////////// -void WirelessComms::Load(const Entity &/*_entity*/, - std::shared_ptr _sdf, - EntityComponentManager &/*_ecm*/, - EventManager &/*_eventMgr*/) -{ - if (_sdf->HasElement("range_config")) - { - sdf::ElementPtr elem = _sdf->Clone()->GetElement("range_config"); - - this->dataPtr->rangeConfig.maxRange = - elem->Get("max_range", this->dataPtr->rangeConfig.maxRange).first; - - this->dataPtr->rangeConfig.fadingExponent = - elem->Get("fading_exponent", - this->dataPtr->rangeConfig.fadingExponent).first; - - this->dataPtr->rangeConfig.l0 = - elem->Get("l0", this->dataPtr->rangeConfig.l0).first; - - this->dataPtr->rangeConfig.sigma = - elem->Get("sigma", this->dataPtr->rangeConfig.sigma).first; - } - - if (_sdf->HasElement("radio_config")) - { - sdf::ElementPtr elem = _sdf->Clone()->GetElement("radio_config"); - - this->dataPtr->radioConfig.capacity = - elem->Get("capacity", this->dataPtr->radioConfig.capacity).first; - - this->dataPtr->radioConfig.txPower = - elem->Get("tx_power", this->dataPtr->radioConfig.txPower).first; - - this->dataPtr->radioConfig.modulation = - elem->Get("modulation", - this->dataPtr->radioConfig.modulation).first; - - this->dataPtr->radioConfig.noiseFloor = - elem->Get("noise_floor", - this->dataPtr->radioConfig.noiseFloor).first; - } - - igndbg << "Range configuration:" << std::endl - << this->dataPtr->rangeConfig << std::endl; - - igndbg << "Radio configuration:" << std::endl - << this->dataPtr->radioConfig << std::endl; -} - -////////////////////////////////////////////////// -void WirelessComms::Step( - const UpdateInfo &/*_info*/, - const comms::Registry &_currentRegistry, - comms::Registry &_newRegistry, - EntityComponentManager &_ecm) -{ - // Make sure that all addresses have its corresponding Link initialized. - for (auto & [address, content] : _currentRegistry) - { - std::string linkName = content.modelName; - if (this->dataPtr->links.find(address) != this->dataPtr->links.end()) - continue; - - auto entities = gazebo::entitiesFromScopedName(linkName, _ecm); - if (entities.empty()) - continue; - - auto entityId = *(entities.begin()); - this->dataPtr->links[address] = gazebo::Link(entityId); - enableComponent(_ecm, entityId); - } - - for (auto & [address, content] : _currentRegistry) - { - // Reference to the outbound queue for this address. - auto &outbound = content.outboundMsgs; - - // All these messages need to be processed. - for (auto &msg : outbound) - { - igndbg << "Pose src: " - << *(this->dataPtr->links[msg->src_address()].WorldPose(_ecm)) - << std::endl; - - igndbg << "Pose dst: " - << *(this->dataPtr->links[msg->dst_address()].WorldPose(_ecm)) - << std::endl; - - _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); - } - - // Clear the outbound queue. - _newRegistry[address].outboundMsgs.clear(); - } -} - -IGNITION_ADD_PLUGIN(WirelessComms, - ignition::gazebo::System, - comms::ICommsModel::ISystemConfigure, - comms::ICommsModel::ISystemPreUpdate) - -IGNITION_ADD_PLUGIN_ALIAS(WirelessComms, - "ignition::gazebo::systems::WirelessComms") diff --git a/src/systems/wireless_comms/WirelessComms.hh b/src/systems/wireless_comms/WirelessComms.hh deleted file mode 100644 index c65dc14199..0000000000 --- a/src/systems/wireless_comms/WirelessComms.hh +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ -#ifndef IGNITION_GAZEBO_SYSTEMS_WIRELESSCOMMS_HH_ -#define IGNITION_GAZEBO_SYSTEMS_WIRELESSCOMMS_HH_ - -#include - -#include -#include -#include "ignition/gazebo/comms/ICommsModel.hh" -#include "ignition/gazebo/System.hh" - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ - // Forward declarations. - class MsgManager; - - /// \brief An example of a comms model. - /// This model always delivers any message to its destination. - class WirelessComms - : public comms::ICommsModel - { - /// \brief Constructor. - public: WirelessComms(); - - /// \brief Destructor. - public: ~WirelessComms(); - - // Documentation inherited. - public: void Load(const Entity &_entity, - std::shared_ptr _sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) override; - - // Documentation inherited. - public: void Step(const ignition::gazebo::UpdateInfo &_info, - const comms::Registry &_currentRegistry, - comms::Registry &_newRegistry, - EntityComponentManager &_ecm); - - /// \brief Private data pointer. - IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) - }; - } -} -} -} - -#endif From f6fde6b7fc37da1eae189ab923aa11d97c4aa1d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sun, 3 Apr 2022 20:26:08 +0200 Subject: [PATCH 19/51] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/worlds/comms.sdf | 2 - include/ignition/gazebo/comms/MsgManager.hh | 4 ++ src/systems/comms_endpoint/CommsEndpoint.cc | 15 +---- src/systems/rf_comms/RFComms.cc | 65 +++++++++------------ src/systems/rf_comms/RFComms.hh | 10 ++-- 5 files changed, 39 insertions(+), 57 deletions(-) diff --git a/examples/worlds/comms.sdf b/examples/worlds/comms.sdf index 7fba33803f..efa117939f 100644 --- a/examples/worlds/comms.sdf +++ b/examples/worlds/comms.sdf @@ -132,7 +132,6 @@ - box_link
addr1
addr1/rx @@ -189,7 +188,6 @@ - box_link
addr2
addr2/rx diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 1c22e6cc33..92820923b7 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -26,6 +26,7 @@ #include #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/System.hh" namespace ignition @@ -64,6 +65,9 @@ struct AddressContent /// \brief Model name associated to this address. public: std::string modelName; + + // \brief Entity of the model associated to this address. + public: gazebo::Entity entity; }; /// \brief A map where the key is an address and the value is all the diff --git a/src/systems/comms_endpoint/CommsEndpoint.cc b/src/systems/comms_endpoint/CommsEndpoint.cc index e20857e2c0..2756bea200 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.cc +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -54,9 +54,6 @@ class ignition::gazebo::systems::CommsEndpoint::Implementation /// \brief Model interface public: Model model{kNullEntity}; - /// \brief ToDo. - public: std::string linkName; - /// \brief True when the address has been bound in the broker. public: std::atomic_bool bound{false}; @@ -141,14 +138,6 @@ void CommsEndpoint::Configure(const Entity &_entity, } this->dataPtr->topic = _sdf->Get("topic"); - // Parse . - if (!_sdf->HasElement("link_name")) - { - ignerr << "No specified." << std::endl; - return; - } - this->dataPtr->linkName = _sdf->Get("link_name"); - // Parse . if (_sdf->HasElement("broker")) { @@ -164,9 +153,7 @@ void CommsEndpoint::Configure(const Entity &_entity, // Prepare the bind parameters. this->dataPtr->bindReq.add_data(this->dataPtr->address); - // this->dataPtr->bindReq.add_data(this->dataPtr->model.Name(_ecm)); - this->dataPtr->bindReq.add_data(this->dataPtr->model.Name(_ecm) + - "::" + this->dataPtr->linkName); + this->dataPtr->bindReq.add_data(this->dataPtr->model.Name(_ecm)); this->dataPtr->bindReq.add_data(this->dataPtr->topic); // Prepare the unbind parameters. diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index d95e2311f2..c13955645e 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -18,7 +18,9 @@ #include #include #include +#include #include +#include #include #include @@ -191,11 +193,7 @@ class ignition::gazebo::systems::RFComms::Implementation /// \brief Radio configuration. public: RadioConfiguration radioConfig; - /// \brief A map where the key is the address and the value is the - /// Link instance associated. - public: std::unordered_map links; - - /// \brief A map where the key is the address and the value is its radio state + /// \brief A map where the key is the address and the value its radio state. public: std::unordered_map radioStates; /// \brief Duration of an epoch (seconds). @@ -223,15 +221,15 @@ RFPower RFComms::Implementation::LogNormalReceivedPower( const double &_txPower, const RadioState &_txState, const RadioState &_rxState) const { - double range = _txState.pose.Pos().Distance(_rxState.pose.Pos()); + const double kRange = _txState.pose.Pos().Distance(_rxState.pose.Pos()); - if (this->rangeConfig.maxRange > 0.0 && range > this->rangeConfig.maxRange) + if (this->rangeConfig.maxRange > 0.0 && kRange > this->rangeConfig.maxRange) return {-std::numeric_limits::infinity(), 0.0}; - double PL = - this->rangeConfig.l0 + 10 * this->rangeConfig.fadingExponent * log10(range); + const double kPL = this->rangeConfig.l0 + + 10 * this->rangeConfig.fadingExponent * log10(kRange); - return {_txPower - PL, this->rangeConfig.sigma}; + return {_txPower - kPL, this->rangeConfig.sigma}; } ///////////////////////////////////////////// @@ -400,20 +398,27 @@ void RFComms::Step( comms::Registry &_newRegistry, EntityComponentManager &_ecm) { - // Make sure that all addresses have its corresponding Link initialized. + // Update ratio states. for (auto & [address, content] : _currentRegistry) { - std::string linkName = content.modelName; - if (this->dataPtr->links.find(address) != this->dataPtr->links.end()) - continue; + // Associate entity if needed. + if (content.entity == kNullEntity) + { + auto entities = gazebo::entitiesFromScopedName(content.modelName, _ecm); + if (entities.empty()) + continue; - auto entities = gazebo::entitiesFromScopedName(linkName, _ecm); - if (entities.empty()) - continue; + auto entityId = *(entities.begin()); + _newRegistry[address].entity = entityId; + + enableComponent(_ecm, entityId); + } - auto entityId = *(entities.begin()); - this->dataPtr->links[address] = gazebo::Link(entityId); - enableComponent(_ecm, entityId); + // Update radio state. + const auto kPose = gazebo::worldPose(content.entity, _ecm); + this->dataPtr->radioStates[address].pose = kPose; + this->dataPtr->radioStates[address].timeStamp = + std::chrono::duration(_info.simTime).count(); } for (auto & [address, content] : _currentRegistry) @@ -422,25 +427,11 @@ void RFComms::Step( auto &outbound = content.outboundMsgs; // All these messages need to be processed. - for (auto &msg : outbound) + for (const auto &msg : outbound) { - // Update radio state. - const auto kSrcAddress = msg->src_address(); - const auto kDstAddress = msg->dst_address(); - - this->dataPtr->radioStates[kSrcAddress].pose = - *(this->dataPtr->links[kSrcAddress].WorldPose(_ecm)); - this->dataPtr->radioStates[kSrcAddress].timeStamp = - std::chrono::duration(_info.simTime).count(); - - this->dataPtr->radioStates[kDstAddress].pose = - *(this->dataPtr->links[kDstAddress].WorldPose(_ecm)); - this->dataPtr->radioStates[kDstAddress].timeStamp = - std::chrono::duration(_info.simTime).count(); - auto [sendPacket, rssi] = this->dataPtr->AttemptSend( - this->dataPtr->radioStates[kSrcAddress], - this->dataPtr->radioStates[kDstAddress], + this->dataPtr->radioStates[msg->src_address()], + this->dataPtr->radioStates[msg->dst_address()], msg->data().size()); if (sendPacket) diff --git a/src/systems/rf_comms/RFComms.hh b/src/systems/rf_comms/RFComms.hh index cd4e05e9c4..0013d63f98 100644 --- a/src/systems/rf_comms/RFComms.hh +++ b/src/systems/rf_comms/RFComms.hh @@ -32,11 +32,13 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - /// \brief An example of a comms model. - /// This model simulates communication using radio frequency (RF) devices. - /// The model uses a log-distance path loss function. + /// \brief A comms model that simulates communication using radio frequency + /// (RF) devices. The model uses a log-distance path loss function. /// - /// This sytem can be configured with the following SDF parameters: + /// This communication model has been ported from: + /// https://github.com/osrf/subt . + /// + /// This system can be configured with the following SDF parameters: /// /// * Optional parameters: /// Element used to capture the range configuration based on a From 0398ceebd8539193a647d1ed77547caca8b9c2eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 5 Apr 2022 17:34:51 +0200 Subject: [PATCH 20/51] rand_r MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/rf_comms/RFComms.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index c13955645e..039548a909 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -290,8 +290,7 @@ std::tuple RFComms::Implementation::AttemptSend( "# Bytes: " << _numBytes << "\n" << "PER: " << packetDropProb << std::endl; - double randDraw = (rand() % 1000) / 1000.0; - + double randDraw = (rand_r() % 1000) / 1000.0; bool packetReceived = randDraw > packetDropProb; if (!packetReceived) From 6ac5f9d01b4fe7fde03383af6ec660a63ed70296 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 5 Apr 2022 17:42:55 +0200 Subject: [PATCH 21/51] Rename MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/worlds/{comms.sdf => perfect_comms.sdf} | 2 +- test/integration/CMakeLists.txt | 2 +- test/integration/{comms.cc => perfect_comms.cc} | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) rename examples/worlds/{comms.sdf => perfect_comms.sdf} (99%) rename test/integration/{comms.cc => perfect_comms.cc} (95%) diff --git a/examples/worlds/comms.sdf b/examples/worlds/perfect_comms.sdf similarity index 99% rename from examples/worlds/comms.sdf rename to examples/worlds/perfect_comms.sdf index a67c432ed7..ff88ce7bea 100644 --- a/examples/worlds/comms.sdf +++ b/examples/worlds/perfect_comms.sdf @@ -16,7 +16,7 @@ ./publisher addr1 --> - + 0.001 diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 3199607765..236bdc2498 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -10,7 +10,6 @@ set(tests buoyancy.cc buoyancy_engine.cc collada_world_exporter.cc - comms.cc components.cc contact_system.cc detachable_joint.cc @@ -45,6 +44,7 @@ set(tests odometry_publisher.cc particle_emitter.cc particle_emitter2.cc + perfect_comms.cc performer_detector.cc physics_system.cc play_pause.cc diff --git a/test/integration/comms.cc b/test/integration/perfect_comms.cc similarity index 95% rename from test/integration/comms.cc rename to test/integration/perfect_comms.cc index b21dd41e86..1a0525fe25 100644 --- a/test/integration/comms.cc +++ b/test/integration/perfect_comms.cc @@ -32,18 +32,18 @@ using namespace ignition; using namespace gazebo; ///////////////////////////////////////////////// -class CommsTest : public InternalFixture<::testing::Test> +class PerfectCommsTest : public InternalFixture<::testing::Test> { }; ///////////////////////////////////////////////// -TEST_F(CommsTest, Comms) +TEST_F(PerfectCommsTest, PerfectComms) { // Start server ServerConfig serverConfig; const auto sdfFile = ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "examples", "worlds", "comms.sdf"); + "examples", "worlds", "perfect_comms.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); From 06123e8aa60ed48207cf97aac48f356396161c78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 5 Apr 2022 20:11:01 +0200 Subject: [PATCH 22/51] Update step MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/worlds/perfect_comms.sdf | 32 ++------------- src/systems/perfect_comms/PerfectComms.cc | 47 +++++++++++++++++++++-- src/systems/rf_comms/RFComms.cc | 45 ++++++++++++++-------- test/integration/CMakeLists.txt | 1 + test/integration/perfect_comms.cc | 2 +- 5 files changed, 78 insertions(+), 49 deletions(-) diff --git a/examples/worlds/perfect_comms.sdf b/examples/worlds/perfect_comms.sdf index baae2faa35..dd64ac3ba8 100644 --- a/examples/worlds/perfect_comms.sdf +++ b/examples/worlds/perfect_comms.sdf @@ -1,6 +1,6 @@ diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index 5b9a44fe6c..e21d42c5f6 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -57,16 +57,55 @@ void PerfectComms::Step( const UpdateInfo &/*_info*/, const comms::Registry &_currentRegistry, comms::Registry &_newRegistry, - EntityComponentManager &/*_ecm*/) + EntityComponentManager &_ecm) { + // Initialize entity if needed. + for (auto & [address, content] : _currentRegistry) + { + if (content.entity == kNullEntity) + { + auto entities = gazebo::entitiesFromScopedName(content.modelName, _ecm); + if (entities.empty()) + continue; + + auto entityId = *(entities.begin()); + if (entityId == kNullEntity) + continue; + + _newRegistry[address].entity = entityId; + } + } + for (auto & [address, content] : _currentRegistry) { // Reference to the outbound queue for this address. auto &outbound = content.outboundMsgs; - // All these messages need to be processed. - for (auto &msg : outbound) - _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + // Is the source address bound? + auto itSrc = _currentRegistry.find(address); + bool srcAddressBound = itSrc != _currentRegistry.end(); + + // Is the source address attached to a model? + bool srcAddressAttachedToModel = + srcAddressBound && itSrc->second.entity != kNullEntity; + + if (srcAddressAttachedToModel) + { + // All these messages need to be processed. + for (auto &msg : outbound) + { + // Is the destination address bound? + auto itDst = _currentRegistry.find(msg->dst_address()); + bool dstAddressBound = itDst != _currentRegistry.end(); + + // Is the destination address attached to a model? + bool dstAddressAttachedToModel = + dstAddressBound && itDst->second.entity != kNullEntity; + + if (dstAddressAttachedToModel) + _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + } + } // Clear the outbound queue. _newRegistry[address].outboundMsgs.clear(); diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 039548a909..c3640ed6cb 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -290,7 +290,7 @@ std::tuple RFComms::Implementation::AttemptSend( "# Bytes: " << _numBytes << "\n" << "PER: " << packetDropProb << std::endl; - double randDraw = (rand_r() % 1000) / 1000.0; + double randDraw = (rand() % 1000) / 1000.0; bool packetReceived = randDraw > packetDropProb; if (!packetReceived) @@ -408,16 +408,21 @@ void RFComms::Step( continue; auto entityId = *(entities.begin()); + if (entityId == kNullEntity) + continue; + _newRegistry[address].entity = entityId; enableComponent(_ecm, entityId); } - - // Update radio state. - const auto kPose = gazebo::worldPose(content.entity, _ecm); - this->dataPtr->radioStates[address].pose = kPose; - this->dataPtr->radioStates[address].timeStamp = - std::chrono::duration(_info.simTime).count(); + else + { + // Update radio state. + const auto kPose = gazebo::worldPose(content.entity, _ecm); + this->dataPtr->radioStates[address].pose = kPose; + this->dataPtr->radioStates[address].timeStamp = + std::chrono::duration(_info.simTime).count(); + } } for (auto & [address, content] : _currentRegistry) @@ -425,16 +430,24 @@ void RFComms::Step( // Reference to the outbound queue for this address. auto &outbound = content.outboundMsgs; - // All these messages need to be processed. - for (const auto &msg : outbound) + // The source address needs to be attached to a robot. + auto itSrc = this->dataPtr->radioStates.find(address); + if (itSrc != this->dataPtr->radioStates.end()) { - auto [sendPacket, rssi] = this->dataPtr->AttemptSend( - this->dataPtr->radioStates[msg->src_address()], - this->dataPtr->radioStates[msg->dst_address()], - msg->data().size()); - - if (sendPacket) - _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + // All these messages need to be processed. + for (const auto &msg : outbound) + { + // The destination address needs to be attached to a robot. + auto itDst = this->dataPtr->radioStates.find(msg->dst_address()); + if (itDst == this->dataPtr->radioStates.end()) + continue; + + auto [sendPacket, rssi] = this->dataPtr->AttemptSend( + itSrc->second, itDst->second, msg->data().size()); + + if (sendPacket) + _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + } } // Clear the outbound queue. diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 236bdc2498..32133dc5a7 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -49,6 +49,7 @@ set(tests physics_system.cc play_pause.cc pose_publisher_system.cc + rf_comms.cc recreate_entities.cc save_world.cc scene_broadcaster_system.cc diff --git a/test/integration/perfect_comms.cc b/test/integration/perfect_comms.cc index 1a0525fe25..36d29acb84 100644 --- a/test/integration/perfect_comms.cc +++ b/test/integration/perfect_comms.cc @@ -84,7 +84,7 @@ TEST_F(PerfectCommsTest, PerfectComms) std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Prepare the message. ignition::msgs::Dataframe msg; - msg.set_src_address("unused"); + msg.set_src_address("addr2"); msg.set_dst_address(addr); // Publish 10 messages. From 327eb47eacff2228c84bd241d6a0ed2c0a246e93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 5 Apr 2022 20:16:00 +0200 Subject: [PATCH 23/51] Updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/standalone/comms/publisher.cc | 2 +- include/ignition/gazebo/comms/MsgManager.hh | 3 ++ src/systems/perfect_comms/PerfectComms.cc | 47 +++++++++++++++++++-- test/integration/perfect_comms.cc | 2 +- 4 files changed, 48 insertions(+), 6 deletions(-) diff --git a/examples/standalone/comms/publisher.cc b/examples/standalone/comms/publisher.cc index bbf981d40f..0579da04bf 100644 --- a/examples/standalone/comms/publisher.cc +++ b/examples/standalone/comms/publisher.cc @@ -75,7 +75,7 @@ int main(int argc, char **argv) // Prepare the message. ignition::msgs::Dataframe msg; - msg.set_src_address("unused"); + msg.set_src_address("addr1"); msg.set_dst_address(argv[1]); // Publish messages at 1Hz. diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 1c22e6cc33..68295db9d7 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -64,6 +64,9 @@ struct AddressContent /// \brief Model name associated to this address. public: std::string modelName; + + /// \brief Entity of the model associated to this address. + public: gazebo::Entity entity; }; /// \brief A map where the key is an address and the value is all the diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index 5b9a44fe6c..e21d42c5f6 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -57,16 +57,55 @@ void PerfectComms::Step( const UpdateInfo &/*_info*/, const comms::Registry &_currentRegistry, comms::Registry &_newRegistry, - EntityComponentManager &/*_ecm*/) + EntityComponentManager &_ecm) { + // Initialize entity if needed. + for (auto & [address, content] : _currentRegistry) + { + if (content.entity == kNullEntity) + { + auto entities = gazebo::entitiesFromScopedName(content.modelName, _ecm); + if (entities.empty()) + continue; + + auto entityId = *(entities.begin()); + if (entityId == kNullEntity) + continue; + + _newRegistry[address].entity = entityId; + } + } + for (auto & [address, content] : _currentRegistry) { // Reference to the outbound queue for this address. auto &outbound = content.outboundMsgs; - // All these messages need to be processed. - for (auto &msg : outbound) - _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + // Is the source address bound? + auto itSrc = _currentRegistry.find(address); + bool srcAddressBound = itSrc != _currentRegistry.end(); + + // Is the source address attached to a model? + bool srcAddressAttachedToModel = + srcAddressBound && itSrc->second.entity != kNullEntity; + + if (srcAddressAttachedToModel) + { + // All these messages need to be processed. + for (auto &msg : outbound) + { + // Is the destination address bound? + auto itDst = _currentRegistry.find(msg->dst_address()); + bool dstAddressBound = itDst != _currentRegistry.end(); + + // Is the destination address attached to a model? + bool dstAddressAttachedToModel = + dstAddressBound && itDst->second.entity != kNullEntity; + + if (dstAddressAttachedToModel) + _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + } + } // Clear the outbound queue. _newRegistry[address].outboundMsgs.clear(); diff --git a/test/integration/perfect_comms.cc b/test/integration/perfect_comms.cc index 1a0525fe25..36d29acb84 100644 --- a/test/integration/perfect_comms.cc +++ b/test/integration/perfect_comms.cc @@ -84,7 +84,7 @@ TEST_F(PerfectCommsTest, PerfectComms) std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Prepare the message. ignition::msgs::Dataframe msg; - msg.set_src_address("unused"); + msg.set_src_address("addr2"); msg.set_dst_address(addr); // Publish 10 messages. From 9e626e742ef46753d5ab3efc80fa24dd91dc8525 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 5 Apr 2022 20:17:42 +0200 Subject: [PATCH 24/51] Missing include MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/MsgManager.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 68295db9d7..e23c167514 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -26,6 +26,7 @@ #include #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/System.hh" namespace ignition From 2099ec706cba8669d429e1614c2fb5a0c50588eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 5 Apr 2022 20:44:55 +0200 Subject: [PATCH 25/51] Add test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/rf_comms/RFComms.cc | 10 +-- test/integration/rf_comms.cc | 116 ++++++++++++++++++++++++++++++++ 2 files changed, 121 insertions(+), 5 deletions(-) create mode 100644 test/integration/rf_comms.cc diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index c3640ed6cb..751a10b0c5 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -284,11 +284,11 @@ std::tuple RFComms::Implementation::AttemptSend( double packetDropProb = 1.0 - exp(_numBytes * log(1 - ber)); - igndbg << "TX power (dBm): " << this->radioConfig.txPower << "\n" << - "RX power (dBm): " << rxPower << "\n" << - "BER: " << ber << "\n" << - "# Bytes: " << _numBytes << "\n" << - "PER: " << packetDropProb << std::endl; + // igndbg << "TX power (dBm): " << this->radioConfig.txPower << "\n" << + // "RX power (dBm): " << rxPower << "\n" << + // "BER: " << ber << "\n" << + // "# Bytes: " << _numBytes << "\n" << + // "PER: " << packetDropProb << std::endl; double randDraw = (rand() % 1000) / 1000.0; bool packetReceived = randDraw > packetDropProb; diff --git a/test/integration/rf_comms.cc b/test/integration/rf_comms.cc new file mode 100644 index 0000000000..015e4b5380 --- /dev/null +++ b/test/integration/rf_comms.cc @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include + +#include +#include +#include +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" // NOLINT(build/include) +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +class RFCommsTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(RFCommsTest, RFComms) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = + ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "rf_comms.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + size_t iters = 1000; + server.Run(true, iters, false); + + unsigned int msgCounter = 0u; + std::mutex mutex; + auto cb = [&](const msgs::Dataframe &_msg) -> void + { + // Verify msg content + std::lock_guard lock(mutex); + std::string expected = "hello world " + std::to_string(msgCounter); + + ignition::msgs::StringMsg receivedMsg; + receivedMsg.ParseFromString(_msg.data()); + EXPECT_EQ(expected, receivedMsg.data()); + msgCounter++; + }; + + // Create subscriber. + ignition::transport::Node node; + std::string addr = "addr1"; + std::string subscriptionTopic = "addr1/rx"; + + // Subscribe to a topic by registering a callback. + auto cbFunc = std::function(cb); + EXPECT_TRUE(node.Subscribe(subscriptionTopic, cbFunc)) + << "Error subscribing to topic [" << subscriptionTopic << "]"; + + // Create publisher. + std::string publicationTopic = "/broker/msgs"; + auto pub = node.Advertise(publicationTopic); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Prepare the message. + ignition::msgs::Dataframe msg; + msg.set_src_address("addr2"); + msg.set_dst_address(addr); + + // Publish 10 messages. + ignition::msgs::StringMsg payload; + unsigned int pubCount = 10u; + for (unsigned int i = 0u; i < pubCount; ++i) + { + // Prepare the payload. + payload.set_data("hello world " + std::to_string(i)); + std::string serializedData; + EXPECT_TRUE(payload.SerializeToString(&serializedData)) + << payload.DebugString(); + msg.set_data(serializedData); + EXPECT_TRUE(pub.Publish(msg)); + server.Run(true, 100, false); + } + + // Verify subscriber received all msgs. + int sleep = 0; + bool done = false; + while (!done && sleep++ < 10) + { + std::lock_guard lock(mutex); + done = msgCounter == pubCount; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_EQ(pubCount, msgCounter); +} From 1ff2b39ef80834049dbcd9801078076ba6999f67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 5 Apr 2022 20:48:50 +0200 Subject: [PATCH 26/51] Example world. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/worlds/perfect_comms.sdf | 1 - examples/worlds/rf_comms.sdf | 190 ++++++++++++++++++++++++++++++ 2 files changed, 190 insertions(+), 1 deletion(-) create mode 100644 examples/worlds/rf_comms.sdf diff --git a/examples/worlds/perfect_comms.sdf b/examples/worlds/perfect_comms.sdf index dd64ac3ba8..23cbb95268 100644 --- a/examples/worlds/perfect_comms.sdf +++ b/examples/worlds/perfect_comms.sdf @@ -54,7 +54,6 @@ - true diff --git a/examples/worlds/rf_comms.sdf b/examples/worlds/rf_comms.sdf new file mode 100644 index 0000000000..6cec69d1dd --- /dev/null +++ b/examples/worlds/rf_comms.sdf @@ -0,0 +1,190 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + 500000.0 + 2.6 + 40 + 10.0 + + + 1000000 + 20 + -90 + QPSK + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + true + 50 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + +
addr1
+ addr1/rx + + + + + -2 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + +
addr2
+ addr2/rx + + /broker/bind + /broker/unbind + + + + + + + From 5472d97e1993d96147318f3a6c834f1026067232 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 7 Apr 2022 17:13:25 +0200 Subject: [PATCH 27/51] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 2 -- include/ignition/gazebo/comms/MsgManager.hh | 12 ++++++------ src/comms/MsgManager.cc | 6 +++--- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index bd8821870b..b2b5e148ec 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -137,8 +137,6 @@ namespace comms /// \brief This method is called when there is a timestep in the simulator /// override this to update your data structures as needed. /// \param[in] _info Simulator information about the current timestep. - /// \param[in] _currentRegistry The current registry. - /// \param[in] _newRegistry The new registry. When Step() is finished this /// will become the new registry. /// \param[in] _ecm - Ignition's ECM. public: virtual void StepImpl(const UpdateInfo &_info, diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index e23c167514..16022bfdcc 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -74,7 +74,7 @@ struct AddressContent /// information associated to each address (subscribers, queues, ...). using Registry = std::unordered_map; -/// \brief ToDo. +/// \brief Class to handle messages and subscriptions. class MsgManager { /// \brief Default constructor. @@ -132,10 +132,10 @@ class MsgManager /// the appropriate subscribers. This function also clears the inbound queue. public: void DeliverMsgs(); - /// \brief Get a mutable reference to the data containing subscriptions and + /// \brief Get an inmutable reference to the data containing subscriptions and /// data queues. - /// \return A mutable reference to the data. - public: const Registry &DataConst(); + /// \return A const reference to the data. + public: const Registry &DataConst() const; /// \brief Get a mutable reference to the data containing subscriptions and /// data queues. @@ -145,11 +145,11 @@ class MsgManager /// \brief Get a copy of the data structure containing subscriptions and data /// queues. /// \return A copy of the data. - public: Registry Copy(); + public: Registry Copy() const; /// \brief Set the data structure containing subscriptions and data queues. /// \param[in] _newContent New content to be set. - public: void Set(Registry &_newContent); + public: void Set(const Registry &_newContent); /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc index 60859cfbc5..2d406a8846 100644 --- a/src/comms/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -168,7 +168,7 @@ void MsgManager::DeliverMsgs() } ////////////////////////////////////////////////// -const Registry &MsgManager::DataConst() +const Registry &MsgManager::DataConst() const { return this->dataPtr->data; } @@ -180,13 +180,13 @@ Registry &MsgManager::Data() } ////////////////////////////////////////////////// -Registry MsgManager::Copy() +Registry MsgManager::Copy() const { return this->dataPtr->data; } ////////////////////////////////////////////////// -void MsgManager::Set(Registry &_newContent) +void MsgManager::Set(const Registry &_newContent) { this->dataPtr->data = _newContent; } From 991b6906a169f4d63bbcdf5b4bd72ace57848d6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 7 Apr 2022 17:21:35 +0200 Subject: [PATCH 28/51] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/worlds/perfect_comms.sdf | 6 +++--- examples/worlds/rf_comms.sdf | 4 ++-- src/systems/rf_comms/RFComms.cc | 19 ++++++------------- src/systems/rf_comms/RFComms.hh | 6 +++--- 4 files changed, 14 insertions(+), 21 deletions(-) diff --git a/examples/worlds/perfect_comms.sdf b/examples/worlds/perfect_comms.sdf index 23cbb95268..090d71aa45 100644 --- a/examples/worlds/perfect_comms.sdf +++ b/examples/worlds/perfect_comms.sdf @@ -10,10 +10,10 @@ make Try launching a comms subscriber: - ign topic -e -t addr1/rx + ign topic -e -t addr2/rx Try launching a comms publisher: - ./publisher addr1 + ./publisher addr2 --> @@ -54,6 +54,7 @@ + true @@ -80,7 +81,6 @@ - true 2 0 0.5 0 0 0 diff --git a/examples/worlds/rf_comms.sdf b/examples/worlds/rf_comms.sdf index 6cec69d1dd..4338b56601 100644 --- a/examples/worlds/rf_comms.sdf +++ b/examples/worlds/rf_comms.sdf @@ -10,10 +10,10 @@ make Try launching a comms subscriber: - ign topic -e -t addr1/rx + ign topic -e -t addr2/rx Try launching a comms publisher: - ./publisher addr1 + ./publisher addr2 --> diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 751a10b0c5..2e63116a29 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -50,12 +50,12 @@ struct RangeConfiguration /// \brief Received power at 1m (in dBm). double l0 = 40; - /// \beief Standard deviation for received power. + /// \brief Standard deviation for received power. double sigma = 10; /// Output stream operator. - /// @param[out] _oss Stream. - /// @param[in] _config configuration to output. + /// \param[out] _oss Stream. + /// \param[in] _config configuration to output. friend std::ostream &operator<<(std::ostream &_oss, const RangeConfiguration &_config) { @@ -87,8 +87,8 @@ struct RadioConfiguration double noiseFloor = -90; /// Output stream operator. - /// @param _oss Stream. - /// @param _config configuration to output. + /// \param _oss Stream. + /// \param _config configuration to output. friend std::ostream &operator<<(std::ostream &_oss, const RadioConfiguration &_config) { @@ -138,7 +138,7 @@ struct RFPower /// \brief double operator. /// \return the RFPower as a double. - operator double() + operator double() const { return mean; } @@ -155,7 +155,6 @@ class ignition::gazebo::systems::RFComms::Implementation /// limitations). This probability is then used to determine if the /// packet is successfully communicated. /// - /// @param radio Static configuration for the radio /// \param[in out] _txState Current state of the transmitter. /// \param[in out] _rxState Current state of the receiver. /// \param[in] _numBytes Size of the packet. @@ -334,12 +333,6 @@ RFComms::RFComms() { } -////////////////////////////////////////////////// -RFComms::~RFComms() -{ - // cannot use default destructor because of dataPtr -} - ////////////////////////////////////////////////// void RFComms::Load(const Entity &/*_entity*/, std::shared_ptr _sdf, diff --git a/src/systems/rf_comms/RFComms.hh b/src/systems/rf_comms/RFComms.hh index 0013d63f98..6c10fd601c 100644 --- a/src/systems/rf_comms/RFComms.hh +++ b/src/systems/rf_comms/RFComms.hh @@ -20,7 +20,7 @@ #include #include -#include +#include #include "ignition/gazebo/comms/ICommsModel.hh" #include "ignition/gazebo/System.hh" @@ -86,7 +86,7 @@ namespace systems public: RFComms(); /// \brief Destructor. - public: ~RFComms(); + public: ~RFComms() override = default; // Documentation inherited. public: void Load(const Entity &_entity, @@ -98,7 +98,7 @@ namespace systems public: void Step(const ignition::gazebo::UpdateInfo &_info, const comms::Registry &_currentRegistry, comms::Registry &_newRegistry, - EntityComponentManager &_ecm); + EntityComponentManager &_ecm) override; /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) From 0162d741c7fe284a30a8b6006b60238f24868a12 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 8 Apr 2022 14:32:57 -0700 Subject: [PATCH 29/51] fix sending empty msgs by changing AttemptSend to use msg->ByteSizeLong() instead of msg->data().size() Signed-off-by: Ian Chen --- src/systems/rf_comms/RFComms.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 2e63116a29..7b6adf6b83 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -436,7 +436,7 @@ void RFComms::Step( continue; auto [sendPacket, rssi] = this->dataPtr->AttemptSend( - itSrc->second, itDst->second, msg->data().size()); + itSrc->second, itDst->second, msg->ByteSizeLong()); if (sendPacket) _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); From 1140b565c175cbdae114a39158f4639a373b04db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 15:09:38 +0200 Subject: [PATCH 30/51] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 19 +++++-------------- src/comms/Broker.cc | 12 ++++++------ 2 files changed, 11 insertions(+), 20 deletions(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index b2b5e148ec..81a64a8530 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -53,12 +53,6 @@ namespace comms /// then the comms model step size will default to dt. /// Note: for consistency it is adviced that the dt is a multiple of timestep. /// Units are in seconds. - /// : Topic name where the broker receives all the incoming - /// messages. The default value is "/broker/msgs" - /// : Service name used to bind an address. - /// The default value is "/broker/bind" - /// : Service name used to unbind from an address. - /// The default value is "/broker/unbind" /// /// Here's an example: /// @@ -84,17 +78,15 @@ namespace comms EntityComponentManager &_ecm, EventManager &_eventMgr) override { - sdf::ElementPtr sdfClone = _sdf->Clone(); - // Parse the optional . - if (sdfClone->HasElement("step_size")) + if (_sdf->HasElement("step_size")) { this->timeStep = std::chrono::duration( - static_cast(sdfClone->Get("step_size") * 1e9)); + static_cast(_sdf->Get("step_size") * 1e9)); } - this->Load(_entity, sdfClone, _ecm, _eventMgr); - this->broker.Load(sdfClone); + this->Load(_entity, _sdf, _ecm, _eventMgr); + this->broker.Load(_sdf); this->broker.Start(); } @@ -134,8 +126,7 @@ namespace comms } } - /// \brief This method is called when there is a timestep in the simulator - /// override this to update your data structures as needed. + /// \brief This method is called when there is a timestep in the simulator. /// \param[in] _info Simulator information about the current timestep. /// will become the new registry. /// \param[in] _ecm - Ignition's ECM. diff --git a/src/comms/Broker.cc b/src/comms/Broker.cc index f3a3571322..c06b6440a6 100644 --- a/src/comms/Broker.cc +++ b/src/comms/Broker.cc @@ -151,7 +151,7 @@ bool Broker::OnBind(const ignition::msgs::StringMsg_V &_req, std::string model = _req.data(1); std::string topic = _req.data(2); - std::lock_guard lk(this->dataPtr->mutex); + std::lock_guard lock(this->dataPtr->mutex); if (!this->DataManager().AddSubscriber(address, model, topic)) return false; @@ -168,15 +168,15 @@ void Broker::OnUnbind(const ignition::msgs::StringMsg_V &_req) auto count = _req.data_size(); if (count != 2) { - ignerr << "Receive incorrect number of arguments. " - << "Expecting 2 and receive " << count << std::endl; + ignerr << "Received incorrect number of arguments. " + << "Expecting 2 and received " << count << std::endl; return; } std::string address = _req.data(0); std::string topic = _req.data(1); - std::lock_guard lk(this->dataPtr->mutex); + std::lock_guard lock(this->dataPtr->mutex); this->DataManager().RemoveSubscriber(address, topic); ignmsg << "Address [" << address << "] unbound on topic [" @@ -189,7 +189,7 @@ void Broker::OnMsg(const ignition::msgs::Dataframe &_msg) // Place the message in the outbound queue of the sender. auto msgPtr = std::make_shared(_msg); - std::lock_guard lk(this->dataPtr->mutex); + std::lock_guard lock(this->dataPtr->mutex); // Stamp the time. msgPtr->mutable_header()->mutable_stamp()->CopyFrom( @@ -201,7 +201,7 @@ void Broker::OnMsg(const ignition::msgs::Dataframe &_msg) ////////////////////////////////////////////////// void Broker::DeliverMsgs() { - std::lock_guard lk(this->dataPtr->mutex); + std::lock_guard lock(this->dataPtr->mutex); this->DataManager().DeliverMsgs(); } From c56d1779074de4d2a4e2429a5803110bb1cf7478 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 15:48:57 +0200 Subject: [PATCH 31/51] Add ICommsModel.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 101 +++------------ src/CMakeLists.txt | 1 + src/comms/ICommsModel.cc | 128 +++++++++++++++++++ 3 files changed, 147 insertions(+), 83 deletions(-) create mode 100644 src/comms/ICommsModel.cc diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 81a64a8530..be77331feb 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -18,14 +18,12 @@ #ifndef IGNITION_GAZEBO_ICOMMSMODEL_HH_ #define IGNITION_GAZEBO_ICOMMSMODEL_HH_ -#include #include -#include -#include -#include "ignition/gazebo/comms/Broker.hh" +#include +#include +#include "ignition/gazebo/comms/MsgManager.hh" #include "ignition/gazebo/config.hh" -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/System.hh" namespace ignition @@ -34,11 +32,13 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + // Forward declarations + class EntityComponentManager; + class EventManager; + namespace comms { - // Forward declarations. - class MsgManager; - /// \brief Abstract interface to define how the environment should handle /// communication simulation. As an example, this class could be responsible /// for handling dropouts, decay and packet collisions. @@ -69,87 +69,29 @@ namespace comms public ISystemConfigure, public ISystemPreUpdate { + /// \brief Constructor. + public: ICommsModel(); + /// \brief Destructor. - public: virtual ~ICommsModel() = default; + public: ~ICommsModel() override = default; // Documentation inherited. public: void Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &_eventMgr) override - { - // Parse the optional . - if (_sdf->HasElement("step_size")) - { - this->timeStep = std::chrono::duration( - static_cast(_sdf->Get("step_size") * 1e9)); - } - - this->Load(_entity, _sdf, _ecm, _eventMgr); - this->broker.Load(_sdf); - this->broker.Start(); - } + EventManager &_eventMgr) override; // Documentation inherited. public: void PreUpdate( const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override - { - IGN_PROFILE("ICommsModel::PreUpdate"); - - if (_info.paused) - return; - - this->currentTime = - std::chrono::steady_clock::time_point(_info.simTime); - - if (!this->timeStep.has_value()) - { - // If no step_size is defined simply execute one step of the comms model - this->StepImpl(_info, _ecm); - } - else - { - // Otherwise step at the specified time step until we converge on the - // final timestep. If the timestep is larger than the dt, then dt will - // be used. - auto endTime = this->currentTime + _info.dt; - - while (this->currentTime < endTime) - { - ignition::gazebo::UpdateInfo info(_info); - info.dt = std::min(this->timeStep.value(), _info.dt); - info.simTime = this->currentTime.time_since_epoch(); - this->StepImpl(_info, _ecm); - this->currentTime += info.dt; - } - } - } + ignition::gazebo::EntityComponentManager &_ecm) override; /// \brief This method is called when there is a timestep in the simulator. /// \param[in] _info Simulator information about the current timestep. /// will become the new registry. /// \param[in] _ecm - Ignition's ECM. public: virtual void StepImpl(const UpdateInfo &_info, - EntityComponentManager &_ecm) - { - // We lock while we manipulate data. - this->broker.Lock(); - - // Update the time in the broker. - this->broker.SetTime(_info.simTime); - - // Step the comms model. - const Registry ¤tRegistry = this->broker.DataManager().DataConst(); - Registry newRegistry = this->broker.DataManager().Copy(); - this->Step(_info, currentRegistry, newRegistry, _ecm); - this->broker.DataManager().Set(newRegistry); - - this->broker.Unlock(); - - // Deliver the inbound messages. - this->broker.DeliverMsgs(); - } + EntityComponentManager &_ecm); /// \brief This method is called when the system is being configured /// override this to load any additional params for the comms model @@ -167,7 +109,7 @@ namespace comms /// override this to update your data structures as needed. /// \param[in] _info Simulator information about the current timestep. /// \param[in] _currentRegistry The current registry. - /// \param[in] _newRegistry The new registry. When Step() is finished this + /// \param[out] _newRegistry The new registry. When Step() is finished this /// will become the new registry. /// \param[in] _ecm - Ignition's ECM. public: virtual void Step(const UpdateInfo &_info, @@ -175,15 +117,8 @@ namespace comms Registry &_newRegistry, EntityComponentManager &_ecm) = 0; - /// \brief Broker instance. - protected: Broker broker; - - /// \brief The step size for each step iteration. - protected: std::optional - timeStep = std::nullopt; - - /// \brief Current time. - protected: std::chrono::steady_clock::time_point currentTime; + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 50601595fa..2aef8d0996 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -28,6 +28,7 @@ set(network_sources set(comms_sources comms/Broker.cc + comms/ICommsModel.cc comms/MsgManager.cc ) diff --git a/src/comms/ICommsModel.cc b/src/comms/ICommsModel.cc new file mode 100644 index 0000000000..7b0074821c --- /dev/null +++ b/src/comms/ICommsModel.cc @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include + +#include +#include +#include "ignition/gazebo/comms/Broker.hh" +#include "ignition/gazebo/comms/ICommsModel.hh" +#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" + +using namespace ignition; +using namespace gazebo; +using namespace comms; + +/// \brief Private ICommsModel data class. +class ignition::gazebo::comms::ICommsModel::Implementation +{ + /// \brief Broker instance. + public: Broker broker; + + /// \brief The step size for each step iteration. + public: std::optional + timeStep = std::nullopt; + + /// \brief Current time. + public: std::chrono::steady_clock::time_point currentTime; +}; + +////////////////////////////////////////////////// +ICommsModel::ICommsModel() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +void ICommsModel::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + // Parse the optional . + if (_sdf->HasElement("step_size")) + { + this->dataPtr->timeStep = std::chrono::duration( + static_cast(_sdf->Get("step_size") * 1e9)); + } + this->Load(_entity, _sdf, _ecm, _eventMgr); + this->dataPtr->broker.Load(_sdf); + this->dataPtr->broker.Start(); +} + +////////////////////////////////////////////////// +void ICommsModel::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("ICommsModel::PreUpdate"); + + if (_info.paused) + return; + + this->dataPtr->currentTime = + std::chrono::steady_clock::time_point(_info.simTime); + + if (!this->dataPtr->timeStep.has_value()) + { + // If no step_size is defined simply execute one step of the comms model + this->StepImpl(_info, _ecm); + } + else + { + // Otherwise step at the specified time step until we converge on the + // final timestep. If the timestep is larger than the dt, then dt will + // be used. + auto endTime = this->dataPtr->currentTime + _info.dt; + + while (this->dataPtr->currentTime < endTime) + { + ignition::gazebo::UpdateInfo info(_info); + info.dt = std::min(this->dataPtr->timeStep.value(), _info.dt); + info.simTime = this->dataPtr->currentTime.time_since_epoch(); + this->StepImpl(_info, _ecm); + this->dataPtr->currentTime += info.dt; + } + } +} + +////////////////////////////////////////////////// +void ICommsModel::StepImpl(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + // We lock while we manipulate data. + this->dataPtr->broker.Lock(); + + // Update the time in the broker. + this->dataPtr->broker.SetTime(_info.simTime); + + // Step the comms model. + const Registry ¤tRegistry = + this->dataPtr->broker.DataManager().DataConst(); + Registry newRegistry = this->dataPtr->broker.DataManager().Copy(); + this->Step(_info, currentRegistry, newRegistry, _ecm); + this->dataPtr->broker.DataManager().Set(newRegistry); + + this->dataPtr->broker.Unlock(); + + // Deliver the inbound messages. + this->dataPtr->broker.DeliverMsgs(); +} From e39c85ddf3dbf2aec8c60792b282602c9654b420 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 16:30:00 +0200 Subject: [PATCH 32/51] Use DblUniform() MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/rf_comms/RFComms.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 7b6adf6b83..becd5a5c06 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include "ignition/gazebo/comms/MsgManager.hh" #include "ignition/gazebo/components/Pose.hh" @@ -289,7 +290,7 @@ std::tuple RFComms::Implementation::AttemptSend( // "# Bytes: " << _numBytes << "\n" << // "PER: " << packetDropProb << std::endl; - double randDraw = (rand() % 1000) / 1000.0; + double randDraw = ignition::math::Rand::DblUniform(); bool packetReceived = randDraw > packetDropProb; if (!packetReceived) From 9789e663f7ab0151ea0d2dd4210d66eb44ad5b28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 16:53:43 +0200 Subject: [PATCH 33/51] Experimental message. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index be77331feb..45df9f4e5e 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -107,6 +107,9 @@ namespace comms /// \brief This method is called when there is a timestep in the simulator /// override this to update your data structures as needed. + /// + /// Note: this is an experimental interface and might change in the future. + /// /// \param[in] _info Simulator information about the current timestep. /// \param[in] _currentRegistry The current registry. /// \param[out] _newRegistry The new registry. When Step() is finished this From de15145081672359ca5420d35259ae43a86920ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 17:19:40 +0200 Subject: [PATCH 34/51] Use ByteSize(). MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/rf_comms/RFComms.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index becd5a5c06..22df7a00a0 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -437,7 +437,7 @@ void RFComms::Step( continue; auto [sendPacket, rssi] = this->dataPtr->AttemptSend( - itSrc->second, itDst->second, msg->ByteSizeLong()); + itSrc->second, itDst->second, msg->ByteSize()); if (sendPacket) _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); From cfb0b9987cee3ff3ded2e6e70420e18210041573 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 20:57:15 +0200 Subject: [PATCH 35/51] Explicit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/Broker.hh | 2 +- include/ignition/gazebo/comms/ICommsModel.hh | 2 +- include/ignition/gazebo/comms/MsgManager.hh | 2 +- src/systems/perfect_comms/PerfectComms.hh | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 7359924434..036067b994 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -84,7 +84,7 @@ namespace comms class Broker { /// \brief Constructor. - public: Broker(); + public: explicit Broker(); /// \brief Destructor. public: virtual ~Broker(); diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 45df9f4e5e..a839f8a2fd 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -70,7 +70,7 @@ namespace comms public ISystemPreUpdate { /// \brief Constructor. - public: ICommsModel(); + public: explicit ICommsModel(); /// \brief Destructor. public: ~ICommsModel() override = default; diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index 16022bfdcc..d4ee87a994 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -78,7 +78,7 @@ using Registry = std::unordered_map; class MsgManager { /// \brief Default constructor. - public: MsgManager(); + public: explicit MsgManager(); /// \brief Destructor. public: virtual ~MsgManager(); diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index 9e37697e8f..b51329a93f 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -41,7 +41,7 @@ namespace systems : public comms::ICommsModel { /// \brief Constructor. - public: PerfectComms(); + public: explicit PerfectComms(); /// \brief Destructor. public: ~PerfectComms(); From 2fc863dc727d6f5ec3734c0b8a9a4af8892f74cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 20:59:07 +0200 Subject: [PATCH 36/51] Explicit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/rf_comms/RFComms.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/rf_comms/RFComms.hh b/src/systems/rf_comms/RFComms.hh index 6c10fd601c..c36696b62a 100644 --- a/src/systems/rf_comms/RFComms.hh +++ b/src/systems/rf_comms/RFComms.hh @@ -83,7 +83,7 @@ namespace systems : public comms::ICommsModel { /// \brief Constructor. - public: RFComms(); + public: explicit RFComms(); /// \brief Destructor. public: ~RFComms() override = default; From 4be8dbebc67b06ae618c5de0faafd22032db3cd1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 11 Apr 2022 21:10:49 +0200 Subject: [PATCH 37/51] Windows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/comms/ICommsModel.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/comms/ICommsModel.cc b/src/comms/ICommsModel.cc index 7b0074821c..24eb5b90f1 100644 --- a/src/comms/ICommsModel.cc +++ b/src/comms/ICommsModel.cc @@ -48,7 +48,7 @@ class ignition::gazebo::comms::ICommsModel::Implementation ////////////////////////////////////////////////// ICommsModel::ICommsModel() - : dataPtr(ignition::utils::MakeUniqueImpl()) + : System(), dataPtr(ignition::utils::MakeUniqueImpl()) { } From fdf72451e9e363d9406de3ba7b69ca07a791f4db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 12 Apr 2022 16:42:33 +0200 Subject: [PATCH 38/51] Win MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index a839f8a2fd..61e049e228 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -73,7 +73,7 @@ namespace comms public: explicit ICommsModel(); /// \brief Destructor. - public: ~ICommsModel() override = default; + //public: ~ICommsModel() override = default; // Documentation inherited. public: void Configure(const Entity &_entity, From c8d3bd8ee1269ca0e08a76f1506bf64641e0db56 Mon Sep 17 00:00:00 2001 From: Carlos Aguero Date: Tue, 12 Apr 2022 17:15:33 +0200 Subject: [PATCH 39/51] Test Signed-off-by: Carlos Aguero --- include/ignition/gazebo/comms/ICommsModel.hh | 3 --- 1 file changed, 3 deletions(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 61e049e228..703d250cc4 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -72,9 +72,6 @@ namespace comms /// \brief Constructor. public: explicit ICommsModel(); - /// \brief Destructor. - //public: ~ICommsModel() override = default; - // Documentation inherited. public: void Configure(const Entity &_entity, const std::shared_ptr &_sdf, From fdae1abe60bf9cd1a378f555631e825cd9e0ca90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 12 Apr 2022 20:21:01 +0200 Subject: [PATCH 40/51] Win MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/Broker.hh | 3 --- include/ignition/gazebo/comms/MsgManager.hh | 3 --- src/comms/Broker.cc | 6 ------ src/comms/MsgManager.cc | 6 ------ src/systems/perfect_comms/PerfectComms.cc | 6 ------ src/systems/perfect_comms/PerfectComms.hh | 3 --- 6 files changed, 27 deletions(-) diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 036067b994..39941329ef 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -86,9 +86,6 @@ namespace comms /// \brief Constructor. public: explicit Broker(); - /// \brief Destructor. - public: virtual ~Broker(); - /// \brief Configure the broker via SDF. /// \param[in] _sdf The SDF Element associated with the broker parameters. public: void Load(std::shared_ptr _sdf); diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index d4ee87a994..b804173753 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -80,9 +80,6 @@ class MsgManager /// \brief Default constructor. public: explicit MsgManager(); - /// \brief Destructor. - public: virtual ~MsgManager(); - /// \brief Add a new subscriber. It's possible to associate multiple topics /// to the same address/model pair. However, the same address cannot be /// attached to multiple models. When all the subscribers are removed, it's diff --git a/src/comms/Broker.cc b/src/comms/Broker.cc index c06b6440a6..4dcd0fd923 100644 --- a/src/comms/Broker.cc +++ b/src/comms/Broker.cc @@ -64,12 +64,6 @@ Broker::Broker() { } -////////////////////////////////////////////////// -Broker::~Broker() -{ - // cannot use default destructor because of dataPtr -} - ////////////////////////////////////////////////// void Broker::Load(std::shared_ptr _sdf) { diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc index 2d406a8846..8682104f75 100644 --- a/src/comms/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -47,12 +47,6 @@ MsgManager::MsgManager() { } -////////////////////////////////////////////////// -MsgManager::~MsgManager() -{ - // cannot use default destructor because of dataPtr -} - ////////////////////////////////////////////////// bool MsgManager::AddSubscriber(const std::string &_address, const std::string &_modelName, diff --git a/src/systems/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc index e21d42c5f6..08f24cc033 100644 --- a/src/systems/perfect_comms/PerfectComms.cc +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -38,12 +38,6 @@ PerfectComms::PerfectComms() { } -////////////////////////////////////////////////// -PerfectComms::~PerfectComms() -{ - // cannot use default destructor because of dataPtr -} - ////////////////////////////////////////////////// void PerfectComms::Load(const Entity &/*_entity*/, std::shared_ptr /*_sdf*/, diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index b51329a93f..862b47ef1c 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -43,9 +43,6 @@ namespace systems /// \brief Constructor. public: explicit PerfectComms(); - /// \brief Destructor. - public: ~PerfectComms(); - // Documentation inherited. public: void Load(const Entity &_entity, std::shared_ptr _sdf, From 8636c29e7ee4af3e100f2dfd85df9ceca6c62a65 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 12 Apr 2022 23:31:03 +0200 Subject: [PATCH 41/51] Win MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/comms/Broker.cc | 15 ++++++++------- src/comms/MsgManager.cc | 5 +++-- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/comms/Broker.cc b/src/comms/Broker.cc index 4dcd0fd923..8c33c5fa21 100644 --- a/src/comms/Broker.cc +++ b/src/comms/Broker.cc @@ -51,7 +51,7 @@ class ignition::gazebo::comms::Broker::Implementation public: std::chrono::steady_clock::duration time{0}; /// \brief An Ignition Transport node for communications. - public: ignition::transport::Node node; + public: std::unique_ptr node; }; using namespace ignition; @@ -62,6 +62,7 @@ using namespace comms; Broker::Broker() : dataPtr(ignition::utils::MakeUniqueImpl()) { + this->dataPtr->node = std::make_unique(); } ////////////////////////////////////////////////// @@ -83,8 +84,8 @@ void Broker::Load(std::shared_ptr _sdf) void Broker::Start() { // Advertise the service for binding addresses. - if (!this->dataPtr->node.Advertise(this->dataPtr->bindSrv, - &Broker::OnBind, this)) + if (!this->dataPtr->node->Advertise(this->dataPtr->bindSrv, + &Broker::OnBind, this)) { ignerr << "Error advertising srv [" << this->dataPtr->bindSrv << "]" << std::endl; @@ -92,8 +93,8 @@ void Broker::Start() } // Advertise the service for unbinding addresses. - if (!this->dataPtr->node.Advertise(this->dataPtr->unbindSrv, - &Broker::OnUnbind, this)) + if (!this->dataPtr->node->Advertise(this->dataPtr->unbindSrv, + &Broker::OnUnbind, this)) { ignerr << "Error advertising srv [" << this->dataPtr->unbindSrv << "]" << std::endl; @@ -101,8 +102,8 @@ void Broker::Start() } // Advertise the topic for receiving data messages. - if (!this->dataPtr->node.Subscribe(this->dataPtr->msgTopic, - &Broker::OnMsg, this)) + if (!this->dataPtr->node->Subscribe(this->dataPtr->msgTopic, + &Broker::OnMsg, this)) { ignerr << "Error subscribing to topic [" << this->dataPtr->msgTopic << "]" << std::endl; diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc index 8682104f75..4219588099 100644 --- a/src/comms/MsgManager.cc +++ b/src/comms/MsgManager.cc @@ -34,7 +34,7 @@ class ignition::gazebo::comms::MsgManager::Implementation public: Registry data; /// \brief An Ignition Transport node for communications. - public: ignition::transport::Node node; + public: std::unique_ptr node; }; using namespace ignition; @@ -45,6 +45,7 @@ using namespace comms; MsgManager::MsgManager() : dataPtr(ignition::utils::MakeUniqueImpl()) { + this->dataPtr->node = std::make_unique(); } ////////////////////////////////////////////////// @@ -65,7 +66,7 @@ bool MsgManager::AddSubscriber(const std::string &_address, this->dataPtr->data[_address].modelName = _modelName; ignition::transport::Node::Publisher publisher = - this->dataPtr->node.Advertise(_topic); + this->dataPtr->node->Advertise(_topic); this->dataPtr->data[_address].subscriptions[_topic] = publisher; return true; From 3d4ea64341f27e8c1c149be8f393df1a171a2b4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 09:10:52 +0200 Subject: [PATCH 42/51] Include MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/Broker.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 39941329ef..04e68a2113 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -22,6 +22,7 @@ #include #include +#include "ignition/gazebo/comms/MsgManager.hh" #include "ignition/gazebo/config.hh" namespace ignition From d6aa6efc2c3edc2d6904833f7e024aef0c609998 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 10:06:20 +0200 Subject: [PATCH 43/51] Windows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/Broker.hh | 2 +- include/ignition/gazebo/comms/MsgManager.hh | 2 +- src/systems/comms_endpoint/CommsEndpoint.cc | 7 ++++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 04e68a2113..8936c50889 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -85,7 +85,7 @@ namespace comms class Broker { /// \brief Constructor. - public: explicit Broker(); + public: Broker(); /// \brief Configure the broker via SDF. /// \param[in] _sdf The SDF Element associated with the broker parameters. diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index b804173753..aefa48fe55 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -78,7 +78,7 @@ using Registry = std::unordered_map; class MsgManager { /// \brief Default constructor. - public: explicit MsgManager(); + public: MsgManager(); /// \brief Add a new subscriber. It's possible to associate multiple topics /// to the same address/model pair. However, the same address cannot be diff --git a/src/systems/comms_endpoint/CommsEndpoint.cc b/src/systems/comms_endpoint/CommsEndpoint.cc index 2756bea200..98c4522bac 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.cc +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -76,7 +76,7 @@ class ignition::gazebo::systems::CommsEndpoint::Implementation public: std::chrono::steady_clock::duration lastBindRequestTime{-2}; /// \brief The ignition transport node. - public: ignition::transport::Node node; + public: std::unique_ptr node; }; ////////////////////////////////////////////////// @@ -93,7 +93,7 @@ void CommsEndpoint::Implementation::BindCallback( ////////////////////////////////////////////////// void CommsEndpoint::Implementation::Bind() { - this->node.Request(this->bindSrv, this->bindReq, + this->node->Request(this->bindSrv, this->bindReq, &CommsEndpoint::Implementation::BindCallback, this); } @@ -101,6 +101,7 @@ void CommsEndpoint::Implementation::Bind() CommsEndpoint::CommsEndpoint() : dataPtr(ignition::utils::MakeUniqueImpl()) { + this->dataPtr->node = std::make_unique(); } ////////////////////////////////////////////////// @@ -112,7 +113,7 @@ CommsEndpoint::~CommsEndpoint() // Unbind. // We use a oneway request because we're not going // to be alive to check the result or retry. - this->dataPtr->node.Request( + this->dataPtr->node->Request( this->dataPtr->unbindSrv, this->dataPtr->unbindReq); } From 9bd38a1b75ff614b82c4299fa5fffc8fb9c35d39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 13:38:16 +0200 Subject: [PATCH 44/51] Visibility MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/Broker.hh | 2 +- include/ignition/gazebo/comms/ICommsModel.hh | 2 +- include/ignition/gazebo/comms/MsgManager.hh | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index 8936c50889..bf06cdf383 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -82,7 +82,7 @@ namespace comms /// /broker/unbind_address /// /// - class Broker + class IGNITION_GAZEBO_VISIBLE Broker { /// \brief Constructor. public: Broker(); diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 703d250cc4..3f83a33500 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -64,7 +64,7 @@ namespace comms /// name="ignition::gazebo::systems::PerfectComms"> /// 1 /// - class ICommsModel + class IGNITION_GAZEBO_VISIBLE ICommsModel : public System, public ISystemConfigure, public ISystemPreUpdate diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index aefa48fe55..aa5bde7f4d 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -75,7 +75,7 @@ struct AddressContent using Registry = std::unordered_map; /// \brief Class to handle messages and subscriptions. -class MsgManager +class IGNITION_GAZEBO_VISIBLE MsgManager { /// \brief Default constructor. public: MsgManager(); From e9ac2deef1f132f19dcd3291ea5def6a7ce6cbba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 14:58:55 +0200 Subject: [PATCH 45/51] Windows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 2 +- test/integration/perfect_comms.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 3f83a33500..703d250cc4 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -64,7 +64,7 @@ namespace comms /// name="ignition::gazebo::systems::PerfectComms"> /// 1 /// - class IGNITION_GAZEBO_VISIBLE ICommsModel + class ICommsModel : public System, public ISystemConfigure, public ISystemPreUpdate diff --git a/test/integration/perfect_comms.cc b/test/integration/perfect_comms.cc index 36d29acb84..56c77957c8 100644 --- a/test/integration/perfect_comms.cc +++ b/test/integration/perfect_comms.cc @@ -37,7 +37,7 @@ class PerfectCommsTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(PerfectCommsTest, PerfectComms) +TEST_F(PerfectCommsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PerfectComms)) { // Start server ServerConfig serverConfig; From 13c788f55650f1614e393d32864c95fbe8619267 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 16:34:25 +0200 Subject: [PATCH 46/51] Win MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- test/integration/rf_comms.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/rf_comms.cc b/test/integration/rf_comms.cc index 015e4b5380..a15cd222d4 100644 --- a/test/integration/rf_comms.cc +++ b/test/integration/rf_comms.cc @@ -38,7 +38,7 @@ class RFCommsTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(RFCommsTest, RFComms) +TEST_F(RFCommsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RFComms)) { // Start server ServerConfig serverConfig; From 088a68cf2f210223884d887961d862c64f47d06f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 16:34:41 +0200 Subject: [PATCH 47/51] Visibility MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 703d250cc4..3f83a33500 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -64,7 +64,7 @@ namespace comms /// name="ignition::gazebo::systems::PerfectComms"> /// 1 /// - class ICommsModel + class IGNITION_GAZEBO_VISIBLE ICommsModel : public System, public ISystemConfigure, public ISystemPreUpdate From 67a54f2d8cc0fc54a70987eb4d307e927a700200 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 19:20:42 +0200 Subject: [PATCH 48/51] Warning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- include/ignition/gazebo/comms/ICommsModel.hh | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index 3f83a33500..367cbe975b 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -64,8 +64,15 @@ namespace comms /// name="ignition::gazebo::systems::PerfectComms"> /// 1 /// - class IGNITION_GAZEBO_VISIBLE ICommsModel - : public System, + class IGNITION_GAZEBO_VISIBLE ICommsModel: +#ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable:4275) +#endif + public System, +#ifdef _MSC_VER + #pragma warning(pop) +#endif public ISystemConfigure, public ISystemPreUpdate { From 1803d74d0339ff91102504e8a04868061217e31f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 13 Apr 2022 21:02:39 +0200 Subject: [PATCH 49/51] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- examples/standalone/comms/publisher.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/standalone/comms/publisher.cc b/examples/standalone/comms/publisher.cc index 0579da04bf..d84d767df7 100644 --- a/examples/standalone/comms/publisher.cc +++ b/examples/standalone/comms/publisher.cc @@ -15,7 +15,7 @@ * */ -// Example: ./publisher addr1 +// Example: ./publisher addr2 #include #include From 59c021598f852e3e3dd33ed39967ef522574d72c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 13 Apr 2022 14:55:58 -0700 Subject: [PATCH 50/51] fix warnings on homebrew Signed-off-by: Ian Chen --- src/systems/perfect_comms/PerfectComms.hh | 2 +- src/systems/rf_comms/RFComms.cc | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/systems/perfect_comms/PerfectComms.hh b/src/systems/perfect_comms/PerfectComms.hh index 862b47ef1c..9ac93f07a7 100644 --- a/src/systems/perfect_comms/PerfectComms.hh +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -53,7 +53,7 @@ namespace systems public: void Step(const ignition::gazebo::UpdateInfo &_info, const comms::Registry &_currentRegistry, comms::Registry &_newRegistry, - EntityComponentManager &_ecm); + EntityComponentManager &_ecm) override; /// \brief Private data pointer. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 22df7a00a0..e74d26ce7a 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -199,8 +199,11 @@ class ignition::gazebo::systems::RFComms::Implementation /// \brief Duration of an epoch (seconds). public: double epochDuration = 1.0; + /// \brief Random device to seed random engine + public: std::random_device rd{}; + /// \brief Random number generator. - public: std::default_random_engine rndEngine; + public: std::default_random_engine rndEngine{rd()}; }; ///////////////////////////////////////////// @@ -437,7 +440,7 @@ void RFComms::Step( continue; auto [sendPacket, rssi] = this->dataPtr->AttemptSend( - itSrc->second, itDst->second, msg->ByteSize()); + itSrc->second, itDst->second, msg->ByteSizeLong()); if (sendPacket) _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); From 8757e98a4c41d441ccfbd4fd0c0687d19386a22a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 13 Apr 2022 16:06:18 -0700 Subject: [PATCH 51/51] add ifdef for ByteSizeLong Signed-off-by: Ian Chen --- src/systems/rf_comms/RFComms.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index e74d26ce7a..199d6f55ce 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -440,7 +440,11 @@ void RFComms::Step( continue; auto [sendPacket, rssi] = this->dataPtr->AttemptSend( +#if GOOGLE_PROTOBUF_VERSION < 3004001 + itSrc->second, itDst->second, msg->ByteSize()); +#else itSrc->second, itDst->second, msg->ByteSizeLong()); +#endif if (sendPacket) _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg);