diff --git a/.github/ci/packages-bionic.apt b/.github/ci/packages-bionic.apt deleted file mode 100644 index a5d5a4f072..0000000000 --- a/.github/ci/packages-bionic.apt +++ /dev/null @@ -1,4 +0,0 @@ -dart6-data -libdart6-collision-ode-dev -libdart6-dev -libdart6-utils-urdf-dev diff --git a/.github/ci/packages-focal.apt b/.github/ci/packages-focal.apt index 7035d3e1d3..1e4c7143d8 100644 --- a/.github/ci/packages-focal.apt +++ b/.github/ci/packages-focal.apt @@ -3,4 +3,5 @@ libdart-dev libdart-external-ikfast-dev libdart-external-odelcpsolver-dev libdart-utils-urdf-dev +libogre-2.2-dev python3-ignition-math7 diff --git a/.github/ci/packages-jammy.apt b/.github/ci/packages-jammy.apt new file mode 100644 index 0000000000..9994c537f9 --- /dev/null +++ b/.github/ci/packages-jammy.apt @@ -0,0 +1,7 @@ +libdart-collision-ode-dev +libdart-dev +libdart-external-ikfast-dev +libdart-external-odelcpsolver-dev +libdart-utils-urdf-dev +libogre-next-dev +python3-ignition-math7 diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index d1b4b4cf27..e5d09e04b4 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -15,7 +15,6 @@ libignition-tools2-dev libignition-transport12-dev libignition-utils2-cli-dev libogre-1.9-dev -libogre-2.2-dev libprotobuf-dev libprotoc-dev libsdformat13-dev diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bf593694d2..130c5dfc58 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -16,3 +16,15 @@ jobs: codecov-enabled: true cppcheck-enabled: true cpplint-enabled: true + jammy-ci: + runs-on: ubuntu-latest + name: Ubuntu Jammy CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@jammy + with: + # per bug https://github.com/ignitionrobotics/ign-gazebo/issues/1409 + cmake-args: '-DBUILD_DOCS=OFF' diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e408d83fb..a5b6a23398 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -226,6 +226,8 @@ endif() #============================================================================ configure_file(${CMAKE_SOURCE_DIR}/api.md.in ${CMAKE_BINARY_DIR}/api.md) configure_file(${CMAKE_SOURCE_DIR}/tutorials.md.in ${CMAKE_BINARY_DIR}/tutorials.md) +configure_file(${CMAKE_SOURCE_DIR}/tools/desktop/ignition-gazebo.desktop.in ${CMAKE_BINARY_DIR}/ignition-gazebo${PROJECT_VERSION_MAJOR}.desktop) +configure_file(${CMAKE_SOURCE_DIR}/tools/desktop/ignition-gazebo.svg.in ${CMAKE_BINARY_DIR}/ignition-gazebo${PROJECT_VERSION_MAJOR}.svg) # disable doxygen on macOS due to issues with doxygen 1.9.0 # there is an unreleased fix; revert this when 1.9.1 is released diff --git a/Changelog.md b/Changelog.md index 6d013fda34..31221f0292 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,39 @@ ## Ignition Gazebo 6.x +### Ignition Gazebo 6.9.0 (2022-04-14) + +1. Add new `RFComms` system + * [Pull request #1428](https://github.com/ignitionrobotics/ign-gazebo/pull/1428) + +1. Add comms infrastructure + * [Pull request #1416](https://github.com/ignitionrobotics/ign-gazebo/pull/1416) + +1. Fix CMake version examples and bump plugin version + * [Pull request #1442](https://github.com/ignitionrobotics/ign-gazebo/pull/1442) + +1. Make sure pose publisher creates valid pose topics + * [Pull request #1433](https://github.com/ignitionrobotics/ign-gazebo/pull/1433) + +1. Add Ubuntu Jammy CI + * [Pull request #1418](https://github.com/ignitionrobotics/ign-gazebo/pull/1418) + +1. Removed `screenToPlane` method and use `rendering::screenToPlane` + * [Pull request #1432](https://github.com/ignitionrobotics/ign-gazebo/pull/1432) + +1. Supply world frame orientation and heading to IMU sensor (#1427) + * [Pull request #1427](https://github.com/ignitionrobotics/ign-gazebo/pull/1427) + +1. Add desktop entry and SVG logo + * [Pull request #1411](https://github.com/ignitionrobotics/ign-gazebo/pull/1411) + * [Pull request #1430](https://github.com/ignitionrobotics/ign-gazebo/pull/1430) + +1. Fix segfault at exit + * [Pull request #1317](https://github.com/ignitionrobotics/ign-gazebo/pull/1317) + +1. Add Gaussian noise to Odometry Publisher + * [Pull request #1393](https://github.com/ignitionrobotics/ign-gazebo/pull/1393) + ### Ignition Gazebo 6.8.0 (2022-04-04) 1. ServerConfig accepts an sdf::Root DOM object diff --git a/examples/standalone/comms/CMakeLists.txt b/examples/standalone/comms/CMakeLists.txt new file mode 100644 index 0000000000..f293242c3a --- /dev/null +++ b/examples/standalone/comms/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(ign-gazebo-comms) + +find_package(ignition-transport12 QUIET REQUIRED) +set(IGN_TRANSPORT_VER ${ignition-transport12_VERSION_MAJOR}) + +add_executable(publisher publisher.cc) +target_link_libraries(publisher + 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..d84d767df7 --- /dev/null +++ b/examples/standalone/comms/publisher.cc @@ -0,0 +1,106 @@ +/* + * 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 addr2 + +#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 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 +/// and exit the program smoothly. +void signal_handler(int _signal) +{ + if (_signal == SIGINT || _signal == SIGTERM) + g_terminatePub = true; +} + +////////////////////////////////////////////////// +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/msgs"; + + 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::Dataframe msg; + msg.set_src_address("addr1"); + 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/gtest_setup/CMakeLists.txt b/examples/standalone/gtest_setup/CMakeLists.txt index 3bb366d7ac..de3f2051c0 100644 --- a/examples/standalone/gtest_setup/CMakeLists.txt +++ b/examples/standalone/gtest_setup/CMakeLists.txt @@ -3,8 +3,8 @@ cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) project(GTestSetup) # Find Gazebo -set(IGN_GAZEBO_VER 7) -find_package(ignition-gazebo${IGN_GAZEBO_VER} REQUIRED) +find_package(ignition-gazebo7 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo7_VERSION_MAJOR}) # Fetch and configure GTest include(FetchContent) diff --git a/examples/worlds/perfect_comms.sdf b/examples/worlds/perfect_comms.sdf new file mode 100644 index 0000000000..090d71aa45 --- /dev/null +++ b/examples/worlds/perfect_comms.sdf @@ -0,0 +1,177 @@ + + + + + + + 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 + + + + + +
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/examples/worlds/rf_comms.sdf b/examples/worlds/rf_comms.sdf new file mode 100644 index 0000000000..4338b56601 --- /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 + + + + + + + diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh new file mode 100644 index 0000000000..bf06cdf383 --- /dev/null +++ b/include/ignition/gazebo/comms/Broker.hh @@ -0,0 +1,154 @@ +/* + * 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_BROKER_HH_ +#define IGNITION_GAZEBO_BROKER_HH_ + +#include + +#include +#include +#include "ignition/gazebo/comms/MsgManager.hh" +#include "ignition/gazebo/config.hh" + +namespace ignition +{ +namespace msgs +{ + // Forward declarations. + class Boolean; + class Dataframe; + class StringMsg_V; +} +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace comms +{ + // 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, 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. + /// + /// 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 IGNITION_GAZEBO_VISIBLE Broker + { + /// \brief Constructor. + public: 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); + + /// \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 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); + + /// \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. + /// \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. + /// \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 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 Process all the messages in the inbound queue and deliver them + /// to the destination clients. + 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. + public: void Lock(); + + /// \brief Unlock the mutex to access the message manager. + public: void Unlock(); + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh new file mode 100644 index 0000000000..367cbe975b --- /dev/null +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -0,0 +1,135 @@ +/* + * 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_ICOMMSMODEL_HH_ +#define IGNITION_GAZEBO_ICOMMSMODEL_HH_ + +#include + +#include +#include +#include "ignition/gazebo/comms/MsgManager.hh" +#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 { + + // Forward declarations + class EntityComponentManager; + class EventManager; + +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. + /// + /// Here's an example: + /// + /// 2 + /// 1.0 + /// + /// + /// 1 + /// + 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 + { + /// \brief Constructor. + public: explicit ICommsModel(); + + // 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 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); + + /// \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, + 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. + /// + /// 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 + /// will become the new registry. + /// \param[in] _ecm - Ignition's ECM. + public: virtual void Step(const UpdateInfo &_info, + const Registry &_currentRegistry, + Registry &_newRegistry, + EntityComponentManager &_ecm) = 0; + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh new file mode 100644 index 0000000000..aa5bde7f4d --- /dev/null +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -0,0 +1,159 @@ +/* + * 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_MSGMANAGER_HH_ +#define IGNITION_GAZEBO_MSGMANAGER_HH_ + +#include +#include +#include +#include + +#include +#include +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/Entity.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 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::unordered_map; + +/// \brief All the information associated to an address. +struct AddressContent +{ + /// \brief Queue of inbound messages. + public: DataQueue inboundMsgs; + + /// \brief Queue of outbound messages. + public: DataQueue outboundMsgs; + + /// \brief Subscribers. + public: SubscriptionHandler subscriptions; + + /// \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 +/// information associated to each address (subscribers, queues, ...). +using Registry = std::unordered_map; + +/// \brief Class to handle messages and subscriptions. +class IGNITION_GAZEBO_VISIBLE MsgManager +{ + /// \brief Default constructor. + 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 + /// 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. + /// \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); + + /// \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 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 msgs::DataframeSharedPtr &_msg); + + /// \brief Remove an existing subscriber. + /// \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); + + /// \brief Remove a message from the inbound queue. + /// \param[in] _address The destination address. + /// \param[in] _Msg Message pointer to remove. + /// \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. + /// \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 + /// the appropriate subscribers. This function also clears the inbound queue. + public: void DeliverMsgs(); + + /// \brief Get an inmutable reference to the data containing subscriptions and + /// data queues. + /// \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. + /// \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: Registry Copy() const; + + /// \brief Set the data structure containing subscriptions and data queues. + /// \param[in] _newContent New content to be set. + public: void Set(const Registry &_newContent); + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) +}; +} +} +} +} + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ee640b08fa..09769418ef 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -26,6 +26,12 @@ set(network_sources network/PeerTracker.cc ) +set(comms_sources + comms/Broker.cc + comms/ICommsModel.cc + comms/MsgManager.cc +) + set(gui_sources ${gui_sources} PARENT_SCOPE @@ -69,6 +75,7 @@ set (sources cmd/ModelCommandAPI.cc ${PROTO_PRIVATE_SRC} ${network_sources} + ${comms_sources} ) set (gtest_sources @@ -95,6 +102,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.cc b/src/comms/Broker.cc new file mode 100644 index 0000000000..8c33c5fa21 --- /dev/null +++ b/src/comms/Broker.cc @@ -0,0 +1,219 @@ +/* + * 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/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 The message manager. + public: MsgManager data; + + /// \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"; + + /// \brief The current time. + public: std::chrono::steady_clock::duration time{0}; + + /// \brief An Ignition Transport node for communications. + public: std::unique_ptr node; +}; + +using namespace ignition; +using namespace gazebo; +using namespace comms; + +////////////////////////////////////////////////// +Broker::Broker() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ + this->dataPtr->node = std::make_unique(); +} + +////////////////////////////////////////////////// +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(this->dataPtr->bindSrv, + &Broker::OnBind, this)) + { + ignerr << "Error advertising srv [" << this->dataPtr->bindSrv << "]" + << std::endl; + return; + } + + // Advertise the service for unbinding addresses. + if (!this->dataPtr->node->Advertise(this->dataPtr->unbindSrv, + &Broker::OnUnbind, this)) + { + ignerr << "Error advertising srv [" << this->dataPtr->unbindSrv << "]" + << std::endl; + return; + } + + // Advertise the topic for receiving data messages. + if (!this->dataPtr->node->Subscribe(this->dataPtr->msgTopic, + &Broker::OnMsg, this)) + { + 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; +} + +////////////////////////////////////////////////// +std::chrono::steady_clock::duration Broker::Time() const +{ + return this->dataPtr->time; +} + +////////////////////////////////////////////////// +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*/) +{ + 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); + std::string topic = _req.data(2); + + std::lock_guard lock(this->dataPtr->mutex); + + if (!this->DataManager().AddSubscriber(address, model, topic)) + return false; + + ignmsg << "Address [" << address << "] bound to model [" << model + << "] on topic [" << topic << "]" << std::endl; + + return true; +} + +////////////////////////////////////////////////// +void Broker::OnUnbind(const ignition::msgs::StringMsg_V &_req) +{ + auto count = _req.data_size(); + if (count != 2) + { + 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 lock(this->dataPtr->mutex); + this->DataManager().RemoveSubscriber(address, topic); + + ignmsg << "Address [" << address << "] unbound on topic [" + << topic << "]" << std::endl; +} + +////////////////////////////////////////////////// +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 lock(this->dataPtr->mutex); + + // Stamp the time. + msgPtr->mutable_header()->mutable_stamp()->CopyFrom( + gazebo::convert(this->dataPtr->time)); + + this->DataManager().AddOutbound(_msg.src_address(), msgPtr); +} + +////////////////////////////////////////////////// +void Broker::DeliverMsgs() +{ + std::lock_guard lock(this->dataPtr->mutex); + this->DataManager().DeliverMsgs(); +} + +////////////////////////////////////////////////// +MsgManager &Broker::DataManager() +{ + return this->dataPtr->data; +} + +////////////////////////////////////////////////// +void Broker::Lock() +{ + this->dataPtr->mutex.lock(); +} + +////////////////////////////////////////////////// +void Broker::Unlock() +{ + this->dataPtr->mutex.unlock(); +} diff --git a/src/comms/Broker_TEST.cc b/src/comms/Broker_TEST.cc new file mode 100644 index 0000000000..66e263fa9b --- /dev/null +++ b/src/comms/Broker_TEST.cc @@ -0,0 +1,103 @@ +/* + * 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/comms/Broker.hh" +#include "ignition/gazebo/comms/MsgManager.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.DataManager().Data(); + 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"); + EXPECT_TRUE(broker.OnBind(reqBind, unused)); + 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::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 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"); + 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()); + + // 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/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(); +} diff --git a/src/comms/MsgManager.cc b/src/comms/MsgManager.cc new file mode 100644 index 0000000000..4219588099 --- /dev/null +++ b/src/comms/MsgManager.cc @@ -0,0 +1,187 @@ +/* + * 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/config.hh" +#include "ignition/gazebo/comms/MsgManager.hh" + +/// \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: Registry data; + + /// \brief An Ignition Transport node for communications. + public: std::unique_ptr node; +}; + +using namespace ignition; +using namespace gazebo; +using namespace comms; + +////////////////////////////////////////////////// +MsgManager::MsgManager() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ + this->dataPtr->node = std::make_unique(); +} + +////////////////////////////////////////////////// +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; +} + +////////////////////////////////////////////////// +void MsgManager::AddInbound(const std::string &_address, + const msgs::DataframeSharedPtr &_msg) +{ + this->dataPtr->data[_address].inboundMsgs.push_back(_msg); +} + +////////////////////////////////////////////////// +void MsgManager::AddOutbound(const std::string &_address, + const msgs::DataframeSharedPtr &_msg) +{ + this->dataPtr->data[_address].outboundMsgs.push_back(_msg); +} + +////////////////////////////////////////////////// +bool MsgManager::RemoveSubscriber(const std::string &_address, + const std::string &_topic) +{ + 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; + + // 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; +} + +////////////////////////////////////////////////// +bool MsgManager::RemoveInbound(const std::string &_address, + const msgs::DataframeSharedPtr &_msg) +{ + 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 = it->second.inboundMsgs; + q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); + return true; +} + +////////////////////////////////////////////////// +bool MsgManager::RemoveOutbound(const std::string &_address, + const msgs::DataframeSharedPtr &_msg) +{ + 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 = it->second.outboundMsgs; + q.erase(std::remove(q.begin(), q.end(), _msg), q.end()); + return true; +} + +////////////////////////////////////////////////// +void MsgManager::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(); + } +} + +////////////////////////////////////////////////// +const Registry &MsgManager::DataConst() const +{ + return this->dataPtr->data; +} + +////////////////////////////////////////////////// +Registry &MsgManager::Data() +{ + return this->dataPtr->data; +} + +////////////////////////////////////////////////// +Registry MsgManager::Copy() const +{ + return this->dataPtr->data; +} + +////////////////////////////////////////////////// +void MsgManager::Set(const Registry &_newContent) +{ + this->dataPtr->data = _newContent; +} diff --git a/src/comms/MsgManager_TEST.cc b/src/comms/MsgManager_TEST.cc new file mode 100644 index 0000000000..b0e0989405 --- /dev/null +++ b/src/comms/MsgManager_TEST.cc @@ -0,0 +1,146 @@ +/* + * 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/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. + 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); + + // 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); + 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. + EXPECT_FALSE(msgManager.RemoveInbound("not_found", msgIn)); + EXPECT_TRUE(msgManager.RemoveInbound("addr2", msgIn)); + EXPECT_TRUE(msgManager.Data()["addr2"].inboundMsgs.empty()); + EXPECT_FALSE(msgManager.RemoveOutbound("not_found", msgOut)); + EXPECT_TRUE(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_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()); + + // 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::unordered_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]); + + // Test DataConst. + auto it = msgManager.DataConst().find("addr6"); + EXPECT_TRUE(it->second.subscriptions.empty()); + +} diff --git a/src/gui/plugins/spawn/Spawn.cc b/src/gui/plugins/spawn/Spawn.cc index 6dfb2c9d58..be60287e52 100644 --- a/src/gui/plugins/spawn/Spawn.cc +++ b/src/gui/plugins/spawn/Spawn.cc @@ -80,19 +80,6 @@ namespace ignition::gazebo /// \brief Handle placement requests public: void HandlePlacement(); - /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a - /// ray cast from the given 2D screen coordinates. - /// \param[in] _screenPos 2D coordinates on the screen, in pixels. - /// \param[in] _camera User camera - /// \param[in] _rayQuery Ray query for mouse clicks - /// \param[in] _offset Offset along the plane normal - /// \return 3D coordinates of a point in the 3D scene. - math::Vector3d ScreenToPlane( - const math::Vector2i &_screenPos, - const rendering::CameraPtr &_camera, - const rendering::RayQueryPtr &_rayQuery, - const float offset = 0.0); - /// \brief Ignition communication node. public: transport::Node node; @@ -201,34 +188,6 @@ void Spawn::LoadConfig(const tinyxml2::XMLElement *) ()->installEventFilter(this); } - -// TODO(ahcorde): Replace this when this function is on ign-rendering6 -///////////////////////////////////////////////// -math::Vector3d SpawnPrivate::ScreenToPlane( - const math::Vector2i &_screenPos, - const rendering::CameraPtr &_camera, - const rendering::RayQueryPtr &_rayQuery, - const float offset) -{ - // Normalize point on the image - double width = _camera->ImageWidth(); - double height = _camera->ImageHeight(); - - double nx = 2.0 * _screenPos.X() / width - 1.0; - double ny = 1.0 - 2.0 * _screenPos.Y() / height; - - // Make a ray query - _rayQuery->SetFromCamera( - _camera, math::Vector2d(nx, ny)); - - math::Planed plane(math::Vector3d(0, 0, 1), offset); - - math::Vector3d origin = _rayQuery->Origin(); - math::Vector3d direction = _rayQuery->Direction(); - double distance = plane.Distance(origin, direction); - return origin + direction * distance; -} - ///////////////////////////////////////////////// void SpawnPrivate::HandlePlacement() { @@ -237,7 +196,7 @@ void SpawnPrivate::HandlePlacement() if (this->spawnPreview && this->hoverDirty) { - math::Vector3d pos = this->ScreenToPlane( + math::Vector3d pos = ignition::rendering::screenToPlane( this->mouseHoverPos, this->camera, this->rayQuery); pos.Z(this->spawnPreview->WorldPosition().Z()); this->spawnPreview->SetWorldPosition(pos); @@ -257,7 +216,7 @@ void SpawnPrivate::HandlePlacement() if (!_result) ignerr << "Error creating entity" << std::endl; }; - math::Vector3d pos = this->ScreenToPlane( + math::Vector3d pos = ignition::rendering::screenToPlane( this->mouseEvent.Pos(), this->camera, this->rayQuery); pos.Z(pose.Pos().Z()); msgs::EntityFactory req; diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 3983736a68..043ffea86e 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) @@ -136,8 +137,10 @@ add_subdirectory(optical_tactile_plugin) add_subdirectory(particle_emitter) add_subdirectory(particle_emitter2) 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) 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..98c4522bac --- /dev/null +++ b/src/systems/comms_endpoint/CommsEndpoint.cc @@ -0,0 +1,193 @@ +/* + * 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/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 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; + + /// \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: std::atomic_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 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: std::unique_ptr 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() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ + this->dataPtr->node = std::make_unique(); +} + +////////////////////////////////////////////////// +CommsEndpoint::~CommsEndpoint() +{ + if (!this->dataPtr->bound) + return; + + // 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->unbindSrv, this->dataPtr->unbindReq); +} + +////////////////////////////////////////////////// +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"); + + // Parse . + 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); + + // 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*/) +{ + IGN_PROFILE("CommsEndpoint::PreUpdate"); + + if (this->dataPtr->bound) + return; + + 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, + 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..a21e9b3089 --- /dev/null +++ b/src/systems/comms_endpoint/CommsEndpoint.hh @@ -0,0 +1,95 @@ +/* + * 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 +{ + /// \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, + 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/imu/Imu.cc b/src/systems/imu/Imu.cc index a49d4ba583..bc1d504ebc 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -31,6 +31,7 @@ #include #include +#include "ignition/gazebo/World.hh" #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/Imu.hh" #include "ignition/gazebo/components/Gravity.hh" @@ -206,6 +207,28 @@ void ImuPrivate::AddSensor( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetOrientationReference(p.Rot()); + // Get world frame orientation and heading. + // If includes a named + // frame like NED, that must be supplied to the IMU sensor, + // otherwise orientations are reported w.r.t to the initial + // orientation. + if (data.Element()->HasElement("imu")) { + auto imuElementPtr = data.Element()->GetElement("imu"); + if (imuElementPtr->HasElement("orientation_reference_frame")) { + double heading = 0.0; + + ignition::gazebo::World world(worldEntity); + if (world.SphericalCoordinates(_ecm)) + { + auto sphericalCoordinates = world.SphericalCoordinates(_ecm).value(); + heading = sphericalCoordinates.HeadingOffset().Radian(); + } + + sensor->SetWorldFrameOrientation(math::Quaterniond(0, 0, heading), + ignition::sensors::WorldFrameEnumType::ENU); + } + } + // Set whether orientation is enabled if (data.ImuSensor()) { 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/perfect_comms/PerfectComms.cc b/src/systems/perfect_comms/PerfectComms.cc new file mode 100644 index 0000000000..08f24cc033 --- /dev/null +++ b/src/systems/perfect_comms/PerfectComms.cc @@ -0,0 +1,115 @@ +/* + * 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/Util.hh" +#include "PerfectComms.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::PerfectComms::Implementation +{ +}; + +////////////////////////////////////////////////// +PerfectComms::PerfectComms() + : dataPtr(ignition::utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +void PerfectComms::Load(const Entity &/*_entity*/, + std::shared_ptr /*_sdf*/, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ +} + +////////////////////////////////////////////////// +void PerfectComms::Step( + const UpdateInfo &/*_info*/, + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + 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; + + // 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(); + } +} + +IGNITION_ADD_PLUGIN(PerfectComms, + ignition::gazebo::System, + 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 new file mode 100644 index 0000000000..9ac93f07a7 --- /dev/null +++ b/src/systems/perfect_comms/PerfectComms.hh @@ -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. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PERFECTCOMMS_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 PerfectComms + : public comms::ICommsModel + { + /// \brief Constructor. + public: explicit PerfectComms(); + + // 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) override; + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + } +} +} +} + +#endif diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 6152c7d2a0..010c88b10b 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -251,6 +251,13 @@ void PosePublisher::Configure(const Entity &_entity, _sdf->Get("use_pose_vector_msg", this->dataPtr->usePoseV).first; std::string poseTopic = scopedName(_entity, _ecm) + "/pose"; + poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); + if (poseTopic.empty()) + { + poseTopic = "/pose"; + ignerr << "Empty pose topic generated for pose_publisher system. " + << "Setting to " << poseTopic << std::endl; + } std::string staticPoseTopic = poseTopic + "_static"; if (this->dataPtr->usePoseV) diff --git a/src/systems/rf_comms/CMakeLists.txt b/src/systems/rf_comms/CMakeLists.txt new file mode 100644 index 0000000000..e5d151aaa6 --- /dev/null +++ b/src/systems/rf_comms/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(rf-comms + SOURCES + 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..199d6f55ce --- /dev/null +++ b/src/systems/rf_comms/RFComms.cc @@ -0,0 +1,465 @@ +/* + * 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 +#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; + + /// \brief 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() const + { + 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[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 its radio state. + public: std::unordered_map radioStates; + + /// \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{rd()}; +}; + +///////////////////////////////////////////// +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 +{ + const double kRange = _txState.pose.Pos().Distance(_rxState.pose.Pos()); + + if (this->rangeConfig.maxRange > 0.0 && kRange > this->rangeConfig.maxRange) + return {-std::numeric_limits::infinity(), 0.0}; + + const double kPL = this->rangeConfig.l0 + + 10 * this->rangeConfig.fadingExponent * log10(kRange); + + return {_txPower - kPL, 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 = ignition::math::Rand::DblUniform(); + 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()) +{ +} + +////////////////////////////////////////////////// +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) +{ + // Update ratio states. + for (auto & [address, content] : _currentRegistry) + { + // Associate entity if needed. + 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; + + enableComponent(_ecm, entityId); + } + 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) + { + // Reference to the outbound queue for this address. + auto &outbound = content.outboundMsgs; + + // The source address needs to be attached to a robot. + auto itSrc = this->dataPtr->radioStates.find(address); + if (itSrc != this->dataPtr->radioStates.end()) + { + // 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( +#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); + } + } + + // 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..c36696b62a --- /dev/null +++ b/src/systems/rf_comms/RFComms.hh @@ -0,0 +1,111 @@ +/* + * 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 A comms model that simulates communication using radio frequency + /// (RF) devices. The model uses a log-distance path loss function. + /// + /// 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 + /// 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: explicit RFComms(); + + /// \brief Destructor. + public: ~RFComms() override = default; + + // 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) override; + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 7d38a0f889..2f8ec87d78 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -45,10 +45,12 @@ set(tests network_handshake.cc odometry_publisher.cc particle_emitter.cc + perfect_comms.cc performer_detector.cc 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/imu_system.cc b/test/integration/imu_system.cc index f5dc724304..6094bf76bb 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -52,6 +52,51 @@ class ImuTest : public InternalFixture<::testing::Test> std::mutex mutex; std::vector imuMsgs; +msgs::IMU lastImuMsgENU; +msgs::IMU lastImuMsgNED; +msgs::IMU lastImuMsgNWU; +msgs::IMU lastImuMsgCUSTOM; +msgs::IMU lastImuMsgDEFAULT; + +///////////////////////////////////////////////// +void imuENUCb(const msgs::IMU &_msg) +{ + lastImuMsgENU = _msg; +} + +///////////////////////////////////////////////// +void imuNEDCb(const msgs::IMU &_msg) +{ + lastImuMsgNED = _msg; +} + +///////////////////////////////////////////////// +void imuNWUCb(const msgs::IMU &_msg) +{ + lastImuMsgNWU = _msg; +} + +///////////////////////////////////////////////// +void imuCUSTOMCb(const msgs::IMU &_msg) +{ + lastImuMsgCUSTOM = _msg; +} + +///////////////////////////////////////////////// +void imuDEFULTCb(const msgs::IMU &_msg) +{ + lastImuMsgDEFAULT = _msg; +} + +///////////////////////////////////////////////// +void clearLastImuMsgs() +{ + lastImuMsgCUSTOM.Clear(); + lastImuMsgENU.Clear(); + lastImuMsgNED.Clear(); + lastImuMsgNWU.Clear(); + lastImuMsgDEFAULT.Clear(); +} ///////////////////////////////////////////////// void imuCb(const msgs::IMU &_msg) @@ -214,7 +259,7 @@ TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelFalling)) } ///////////////////////////////////////////////// -// The test checks to make sure orientation is not published if it is deabled +// The test checks to make sure orientation is not published if it is disabled TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OrientationDisabled)) { imuMsgs.clear(); @@ -250,3 +295,285 @@ TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OrientationDisabled)) } mutex.unlock(); } + +///////////////////////////////////////////////// +// The test checks if the orientation is published according to the +// localization tag +TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(NamedFrames)) +{ + imuMsgs.clear(); + clearLastImuMsgs(); + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "imu_named_frame.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + auto topicENU = "/imu_test_ENU"; + auto topicNED = "/imu_test_NED"; + auto topicNWU = "/imu_test_NWU"; + auto topicCUSTOM = "/imu_test_CUSTOM"; + auto topicDEFAULT = "/imu_test_DEFAULT"; + + // subscribe to imu topic + transport::Node node; + node.Subscribe(topicENU, &imuENUCb); + node.Subscribe(topicNED, &imuNEDCb); + node.Subscribe(topicNWU, &imuNWUCb); + node.Subscribe(topicCUSTOM, &imuCUSTOMCb); + node.Subscribe(topicDEFAULT, &imuDEFULTCb); + + // Run server + server.Run(true, 200u, false); + + // Check we received messages + EXPECT_TRUE(lastImuMsgENU.has_orientation()); + EXPECT_TRUE(lastImuMsgNED.has_orientation()); + EXPECT_TRUE(lastImuMsgNWU.has_orientation()); + EXPECT_TRUE(lastImuMsgCUSTOM.has_orientation()); + EXPECT_TRUE(lastImuMsgDEFAULT.has_orientation()); + + // For the DEFAULT msg, orientation is reported relative + // to the original pose, which should be identity quaternion. + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().x(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().y(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().z(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().w(), 1, 1e-2); + + // For the ENU msg + EXPECT_NEAR(lastImuMsgENU.orientation().x(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgENU.orientation().y(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgENU.orientation().z(), 1, 1e-2); + EXPECT_NEAR(lastImuMsgENU.orientation().w(), 0, 1e-2); + + // For the NED msg + EXPECT_NEAR(lastImuMsgNED.orientation().x(), -0.707, 1e-2); + EXPECT_NEAR(lastImuMsgNED.orientation().y(), 0.707, 1e-2); + EXPECT_NEAR(lastImuMsgNED.orientation().z(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNED.orientation().w(), 0, 1e-2); + + // For the NWU msg + EXPECT_NEAR(lastImuMsgNWU.orientation().x(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNWU.orientation().y(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNWU.orientation().z(), 0.707, 1e-2); + EXPECT_NEAR(lastImuMsgNWU.orientation().w(), 0.707, 1e-2); + + // For the CUSTOM msg + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().x(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().y(), 0.707, 1e-2); + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().z(), 0.707, 1e-2); + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().w(), 0, 1e-2); +} + +///////////////////////////////////////////////// +// The test checks if the orientation is published according to the +// localization tag, with heading_deg also accounted for +TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(NamedFramesWithHeading)) +{ + imuMsgs.clear(); + clearLastImuMsgs(); + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "imu_heading_deg.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + auto topicENU = "/imu_test_ENU"; + auto topicNED = "/imu_test_NED"; + auto topicNWU = "/imu_test_NWU"; + auto topicCUSTOM = "/imu_test_CUSTOM"; + auto topicDEFAULT = "/imu_test_DEFAULT"; + + // subscribe to imu topic + transport::Node node; + node.Subscribe(topicENU, &imuENUCb); + node.Subscribe(topicNED, &imuNEDCb); + node.Subscribe(topicNWU, &imuNWUCb); + node.Subscribe(topicCUSTOM, &imuCUSTOMCb); + node.Subscribe(topicDEFAULT, &imuDEFULTCb); + + // Run server + server.Run(true, 200u, false); + + // Check we received messages + EXPECT_TRUE(lastImuMsgENU.has_orientation()); + EXPECT_TRUE(lastImuMsgNED.has_orientation()); + EXPECT_TRUE(lastImuMsgNWU.has_orientation()); + EXPECT_TRUE(lastImuMsgCUSTOM.has_orientation()); + EXPECT_TRUE(lastImuMsgDEFAULT.has_orientation()); + + // For the DEFAULT msg, orientation is reported relative + // to the original pose, which should be identity quaternion. + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().x(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().y(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().z(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgDEFAULT.orientation().w(), 1, 1e-2); + + // For the ENU msg + EXPECT_NEAR(lastImuMsgENU.orientation().x(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgENU.orientation().y(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgENU.orientation().z(), 0.707, 1e-2); + EXPECT_NEAR(lastImuMsgENU.orientation().w(), 0.707, 1e-2); + + // For the NED msg + EXPECT_NEAR(lastImuMsgNED.orientation().x(), -1, 1e-2); + EXPECT_NEAR(lastImuMsgNED.orientation().y(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNED.orientation().z(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNED.orientation().w(), 0, 1e-2); + + // For the NWU msg + EXPECT_NEAR(lastImuMsgNWU.orientation().x(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNWU.orientation().y(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNWU.orientation().z(), 0, 1e-2); + EXPECT_NEAR(lastImuMsgNWU.orientation().w(), 1, 1e-2); + + // For the CUSTOM msg + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().x(), -0.5, 1e-2); + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().y(), 0.5, 1e-2); + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().z(), 0.5, 1e-2); + EXPECT_NEAR(lastImuMsgCUSTOM.orientation().w(), 0.5, 1e-2); +} + +///////////////////////////////////////////////// +// The test checks if orientations are reported correctly for a rotating body. +// The world includes a sphere rolling down a plane, with axis of rotation +// as the "west" direction vector, using the right hand rule. +TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RotatingBody)) +{ + imuMsgs.clear(); + clearLastImuMsgs(); + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "imu_rotating_demo.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + msgs::IMU currentImuMsgDefault_1; + msgs::IMU currentImuMsgDefault_2; + msgs::IMU currentImuMsgDefault_3; + + std::function defaultCb1 = + [&](const msgs::IMU &_msg) + { + currentImuMsgDefault_1 = _msg; + }; + std::function defaultCb2 = + [&](const msgs::IMU &_msg) + { + currentImuMsgDefault_2 = _msg; + }; + std::function defaultCb3 = + [&](const msgs::IMU &_msg) + { + currentImuMsgDefault_3 = _msg; + }; + + // subscribe to imu topic + transport::Node node; + node.Subscribe("/imu_test_ENU", &imuENUCb); + node.Subscribe("/imu_test_NED", &imuNEDCb); + node.Subscribe("/imu_test_DEFAULT_1", defaultCb1); + node.Subscribe("/imu_test_DEFAULT_2", defaultCb2); + node.Subscribe("/imu_test_DEFAULT_3", defaultCb3); + + // Run server + server.Run(true, 50u, false); + + // Store initial orientations reported by the IMUs + auto initialOrientationDEFAULT_1 = msgs::Convert( + currentImuMsgDefault_1.orientation()); + auto initialOrientationDEFAULT_2 = msgs::Convert( + currentImuMsgDefault_2.orientation()); + auto initialOrientationDEFAULT_3 = msgs::Convert( + currentImuMsgDefault_3.orientation()); + auto initialOrientationENU = msgs::Convert( + lastImuMsgENU.orientation()); + auto initialOrientationNED = msgs::Convert( + lastImuMsgNED.orientation()); + + server.Run(true, 1500u, false); + + // Store final orientations reported by the IMUs + auto finalOrientationDEFAULT_1 = msgs::Convert( + currentImuMsgDefault_1.orientation()); + auto finalOrientationDEFAULT_2 = msgs::Convert( + currentImuMsgDefault_2.orientation()); + auto finalOrientationDEFAULT_3 = msgs::Convert( + currentImuMsgDefault_3.orientation()); + auto finalOrientationENU = msgs::Convert( + lastImuMsgENU.orientation()); + auto finalOrientationNED = msgs::Convert( + lastImuMsgNED.orientation()); + + auto differenceOrientationDEFAULT_1 = finalOrientationDEFAULT_1 * + initialOrientationDEFAULT_1.Inverse(); + auto differenceOrientationDEFAULT_2 = finalOrientationDEFAULT_2 * + initialOrientationDEFAULT_2.Inverse(); + auto differenceOrientationDEFAULT_3 = finalOrientationDEFAULT_3 * + initialOrientationDEFAULT_3.Inverse(); + + auto differenceOrientationENU = finalOrientationENU * + initialOrientationENU.Inverse(); + auto differenceOrientationNED = finalOrientationNED * + initialOrientationNED.Inverse(); + + // Since the sphere has rotated along the west direction, + // pitch and yaw change for ENU reporting IMU should be zero, + // and roll should have some non trivial negative value. + EXPECT_TRUE((differenceOrientationENU.Roll() < -0.04)); + EXPECT_NEAR(differenceOrientationENU.Pitch(), 0, 1e-2); + EXPECT_NEAR(differenceOrientationENU.Yaw(), 0, 1e-2); + + // Similarly, roll and yaw for NED reporting IMU should be zero, + // and pitch should be some non trivial negative value. + EXPECT_NEAR(differenceOrientationNED.Roll(), 0, 1e-2); + EXPECT_TRUE((differenceOrientationNED.Pitch() < -0.04)); + EXPECT_NEAR(differenceOrientationNED.Yaw(), 0, 1e-2); + + // In the sdf world, the IMU model & link have a yaw pose of PI/2, + // which means the initial orientation of DEFAULT IMU is + // effectively WND (by rotating ENU by PI about N). Therefore, + // pitch and yaw for DEFAULT case should be zero, and roll + // should be nontrivial positive value. + EXPECT_TRUE((differenceOrientationDEFAULT_1.Roll() > 0.04)); + EXPECT_NEAR(differenceOrientationDEFAULT_1.Pitch(), 0, 1e-2); + EXPECT_NEAR(differenceOrientationDEFAULT_1.Yaw(), 0, 1e-2); + + // For DEFAULT_2, model has a pose PI/2 0 0 & link has a pose of + // 0 PI/2 0, which makes the frame NUE. + EXPECT_NEAR(differenceOrientationDEFAULT_2.Roll(), 0, 1e-2); + EXPECT_NEAR(differenceOrientationDEFAULT_2.Pitch(), 0, 1e-2); + EXPECT_TRUE((differenceOrientationDEFAULT_2.Yaw() < -0.04)); + + // For DEFAULT_3, model has a pose PI/2 0 0 & link has a pose of + // 0 0 PI/2, which makes the frame UWS. + EXPECT_NEAR(differenceOrientationDEFAULT_3.Roll(), 0, 1e-2); + EXPECT_TRUE((differenceOrientationDEFAULT_3.Pitch() > 0.04)); + EXPECT_NEAR(differenceOrientationDEFAULT_3.Yaw(), 0, 1e-2); + + // Those nontrivial values should match for all sensors. + EXPECT_NEAR(differenceOrientationENU.Roll(), + differenceOrientationNED.Pitch(), 1e-4); + + EXPECT_NEAR(differenceOrientationENU.Roll(), + -differenceOrientationDEFAULT_1.Roll(), 1e-4); + EXPECT_NEAR(differenceOrientationENU.Roll(), + differenceOrientationDEFAULT_2.Yaw(), 1e-4); + EXPECT_NEAR(differenceOrientationENU.Roll(), + -differenceOrientationDEFAULT_3.Pitch(), 1e-4); +} diff --git a/test/integration/perfect_comms.cc b/test/integration/perfect_comms.cc new file mode 100644 index 0000000000..36a107b8cf --- /dev/null +++ b/test/integration/perfect_comms.cc @@ -0,0 +1,115 @@ +/* + * 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 PerfectCommsTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(PerfectCommsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PerfectComms)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = + ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "perfect_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); +} diff --git a/test/integration/rf_comms.cc b/test/integration/rf_comms.cc new file mode 100644 index 0000000000..aba7b05092 --- /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, IGN_UTILS_TEST_DISABLED_ON_WIN32(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); +} diff --git a/test/worlds/imu_heading_deg.sdf b/test/worlds/imu_heading_deg.sdf new file mode 100644 index 0000000000..b93b2a3f0f --- /dev/null +++ b/test/worlds/imu_heading_deg.sdf @@ -0,0 +1,130 @@ + + + + 0 0 -5 + + 0.001 + 0 + + + + + + + + 90 + + + + 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 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + imu_test_DEFAULT + 1 + 1 + true + + + imu_test_ENU + 1 + + + ENU + + + 1 + true + + + imu_test_NED + 1 + + + NED + + + 1 + true + + + imu_test_NWU + 1 + + + NWU + + + 1 + true + + + imu_test_CUSTOM + 1 + + + CUSTOM + 1.570795 0 0 + + + 1 + true + + + + + + diff --git a/test/worlds/imu_named_frame.sdf b/test/worlds/imu_named_frame.sdf new file mode 100644 index 0000000000..e7256ec196 --- /dev/null +++ b/test/worlds/imu_named_frame.sdf @@ -0,0 +1,126 @@ + + + + 0 0 -5 + + 0.001 + 0 + + + + + + + + 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 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + imu_test_DEFAULT + 1 + 1 + true + + + imu_test_ENU + 1 + + + ENU + + + 1 + true + + + imu_test_NED + 1 + + + NED + + + 1 + true + + + imu_test_NWU + 1 + + + NWU + + + 1 + true + + + imu_test_CUSTOM + 1 + + + CUSTOM + 1.570795 0 0 + + + 1 + true + + + + + + diff --git a/test/worlds/imu_rotating_demo.sdf b/test/worlds/imu_rotating_demo.sdf new file mode 100644 index 0000000000..da86f4bd13 --- /dev/null +++ b/test/worlds/imu_rotating_demo.sdf @@ -0,0 +1,165 @@ + + + + 0 0.1 -0.1 + + 0.001 + 0 + + + + + + + + + + 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 + + + + + + + 1 1 1 0 1.57 0 + + 0 0 0 0 1.57 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + + + + + + + + + imu_test_DEFAULT_1 + 1 + 1 + true + + + imu_test_ENU + 1 + + + ENU + + + 1 + true + + + imu_test_NED + 1 + + + NED + + + 1 + true + + + + + + 4 4 1 1.57 0 0 + + 0 0 0 0 1.57 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + + + + + + + + + imu_test_DEFAULT_2 + 1 + 1 + true + + + + + + 7 7 1 1.57 0 0 + + 0 0 0 0 0 1.57 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + + + + + + + + + imu_test_DEFAULT_3 + 1 + 1 + true + + + + + + diff --git a/tools/desktop/ignition-gazebo.desktop.in b/tools/desktop/ignition-gazebo.desktop.in new file mode 100644 index 0000000000..30c7186def --- /dev/null +++ b/tools/desktop/ignition-gazebo.desktop.in @@ -0,0 +1,9 @@ +[Desktop Entry] +Version=1.0 +Type=Application +Name=Igniton Gazebo @PROJECT_VERSION_MAJOR@ +Comment=3D Robot Simulator +Exec=/usr/bin/ign gazebo --force-version @PROJECT_VERSION_MAJOR@ +Icon=ignition-gazebo@PROJECT_VERSION_MAJOR@ +Categories=Education;Science;Robotics;Development; +StartupNotify=true diff --git a/tools/desktop/ignition-gazebo.svg.in b/tools/desktop/ignition-gazebo.svg.in new file mode 100644 index 0000000000..ba0e150a2e --- /dev/null +++ b/tools/desktop/ignition-gazebo.svg.in @@ -0,0 +1,95 @@ + + + +image/svg+xml + + + + + + + + + + + \ No newline at end of file diff --git a/tutorials/create_system_plugins.md b/tutorials/create_system_plugins.md index 2f03b685ac..1a9eeecf07 100644 --- a/tutorials/create_system_plugins.md +++ b/tutorials/create_system_plugins.md @@ -82,8 +82,8 @@ Implement the system class as usual, for example: In your `CMakeLists.txt` add the following ``` -set(IGN_PLUGIN_VER 0) -ign_find_package(ignition-plugin0 REQUIRED COMPONENTS register) +ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) # Add sources for each plugin to be registered. add_library(SampleSystem SampleSystem.cc SampleSystem2.cc)