diff --git a/CMakeLists.txt b/CMakeLists.txt index a566322711..3e1dd1e979 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,7 +74,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs8 REQUIRED VERSION 8.3) +ign_find_package(ignition-msgs8 REQUIRED VERSION 8.5) set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index f4616bbcb4..3f817dd1c5 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -112,9 +112,28 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, static_cast(this->stepSize.count() / this->desiredRtf)); } + // World control + transport::NodeOptions opts; + std::string ns{"/world/" + this->worldName}; + if (this->networkMgr) + { + ns = this->networkMgr->Namespace() + ns; + } + + auto validNs = transport::TopicUtils::AsValidTopic(ns); + if (validNs.empty()) + { + ignerr << "Invalid namespace [" << ns + << "], not initializing runner transport." << std::endl; + return; + } + opts.SetNameSpace(validNs); + + this->node = std::make_unique(opts); + // Create the system manager this->systemMgr = std::make_unique(_systemLoader, - &this->entityCompMgr, &this->eventMgr); + &this->entityCompMgr, &this->eventMgr, validNs); this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -185,25 +204,6 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->LoadLoggingPlugins(this->serverConfig); - // World control - transport::NodeOptions opts; - std::string ns{"/world/" + this->worldName}; - if (this->networkMgr) - { - ns = this->networkMgr->Namespace() + ns; - } - - auto validNs = transport::TopicUtils::AsValidTopic(ns); - if (validNs.empty()) - { - ignerr << "Invalid namespace [" << ns - << "], not initializing runner transport." << std::endl; - return; - } - opts.SetNameSpace(validNs); - - this->node = std::make_unique(opts); - // TODO(louise) Combine both messages into one. this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); this->node->Advertise("control/state", &SimulationRunner::OnWorldControlState, @@ -807,6 +807,9 @@ void SimulationRunner::Step(const UpdateInfo &_info) // so that we can recreate entities with the same name. this->ProcessRecreateEntitiesRemove(); + // handle systems that need to be added + this->systemMgr->ProcessPendingEntitySystems(); + // Update all the systems. this->UpdateSystems(); diff --git a/src/SystemManager.cc b/src/SystemManager.cc index fbba44d71f..8440364fc8 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -25,11 +25,20 @@ using namespace gazebo; ////////////////////////////////////////////////// SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, EntityComponentManager *_entityCompMgr, - EventManager *_eventMgr) + EventManager *_eventMgr, + const std::string &_namespace) : systemLoader(_systemLoader), entityCompMgr(_entityCompMgr), eventMgr(_eventMgr) { + transport::NodeOptions opts; + opts.SetNameSpace(_namespace); + this->node = std::make_unique(opts); + std::string entitySystemService{"entity/system/add"}; + this->node->Advertise(entitySystemService, + &SystemManager::EntitySystemAddService, this); + ignmsg << "Serving entity system service on [" + << "/" << entitySystemService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -195,3 +204,40 @@ std::vector SystemManager::TotalByEntity(Entity _entity) std::back_inserter(result), checkEntity); return result; } + +////////////////////////////////////////////////// +bool SystemManager::EntitySystemAddService(const msgs::EntityPlugin_V &_req, + msgs::Boolean &_res) +{ + std::lock_guard lock(this->systemsMsgMutex); + this->systemsToAdd.push_back(_req); + _res.set_data(true); + return true; +} + +////////////////////////////////////////////////// +void SystemManager::ProcessPendingEntitySystems() +{ + std::lock_guard lock(this->systemsMsgMutex); + for (auto &req : this->systemsToAdd) + { + Entity entity = req.entity().id(); + + if (req.plugins().empty()) + { + ignwarn << "Unable to add plugins to Entity: '" << entity + << "'. No plugins specified." << std::endl; + continue; + } + + for (auto &pluginMsg : req.plugins()) + { + std::string fname = pluginMsg.filename(); + std::string name = pluginMsg.name(); + std::string innerxml = pluginMsg.innerxml(); + sdf::Plugin pluginSDF(fname, name, innerxml); + this->LoadPlugin(entity, pluginSDF); + } + } + this->systemsToAdd.clear(); +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh index e267c19c93..c23ee38cee 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -17,11 +17,14 @@ #ifndef IGNITION_GAZEBO_SYSTEMMANAGER_HH_ #define IGNITION_GAZEBO_SYSTEMMANAGER_HH_ +#include + #include #include #include #include +#include #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -48,9 +51,11 @@ namespace ignition /// be used when configuring new systems /// \param[in] _eventMgr Pointer to the event manager to be used when /// configuring new systems + /// \param[in] _namespace Namespace to use for the transport node public: explicit SystemManager(const SystemLoaderPtr &_systemLoader, EntityComponentManager *_entityCompMgr = nullptr, - EventManager *_eventMgr = nullptr); + EventManager *_eventMgr = nullptr, + const std::string &_namespace = std::string()); /// \brief Load system plugin for a given entity. /// \param[in] _entity Entity @@ -110,6 +115,9 @@ namespace ignition /// \return Vector of systems. public: std::vector TotalByEntity(Entity _entity); + /// \brief Process system messages and add systems to entities + public: void ProcessPendingEntitySystems(); + /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. /// \param[in] _system Generic representation of a system. @@ -125,6 +133,15 @@ namespace ignition private: void AddSystemImpl(SystemInternal _system, const sdf::Plugin &_sdf); + /// \brief Callback for entity add system service. + /// \param[in] _req Request message containing the entity id and plugins + /// to add to that entity + /// \param[out] _res Response containing a boolean value indicating if + /// service request is received or not + /// \return True if request received. + private: bool EntitySystemAddService(const msgs::EntityPlugin_V &_req, + msgs::Boolean &_res); + /// \brief All the systems. private: std::vector systems; @@ -157,6 +174,15 @@ namespace ignition /// \brief Pointer to associated event manager private: EventManager *eventMgr; + + /// \brief A list of entity systems to add + private: std::vector systemsToAdd; + + /// \brief Mutex to protect systemsToAdd list + private: std::mutex systemsMsgMutex; + + /// \brief Node for communication. + private: std::unique_ptr node{nullptr}; }; } } // namespace gazebo diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 5b234d4d4e..4b1e1e7358 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -16,6 +16,7 @@ set(tests diff_drive_system.cc each_new_removed.cc entity_erase.cc + entity_system.cc events.cc examples_build.cc follow_actor_system.cc diff --git a/test/integration/entity_system.cc b/test/integration/entity_system.cc new file mode 100644 index 0000000000..d4ea7ce57c --- /dev/null +++ b/test/integration/entity_system.cc @@ -0,0 +1,163 @@ +/* + * 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/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test DiffDrive system +class EntitySystemTest : public InternalFixture<::testing::TestWithParam> +{ + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + Entity vehicleEntity = kNullEntity; + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + vehicleEntity = id; + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + poses.clear(); + + // Publish command and check that vehicle still does not move + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + msgs::Twist msg; + const double desiredLinVel = 10.5; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, 0)); + pub.Publish(msg); + server.Run(true, 1000, false); + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + poses.clear(); + + // send request to add diff_drive system + EXPECT_NE(kNullEntity, vehicleEntity); + msgs::EntityPlugin_V req; + auto ent = req.mutable_entity(); + ent->set_id(vehicleEntity); + auto plugin = req.add_plugins(); + plugin->set_name("ignition::gazebo::systems::DiffDrive"); + plugin->set_filename("ignition-gazebo-diff-drive-system"); + std::stringstream innerxml; + innerxml + << "left_wheel_joint\n" + << "right_wheel_joint\n" + << "1.25\n" + << "0.3\n" + << "1\n" + << "-1\n" + << "2\n" + << "-2\n" + << "0.5\n" + << "-0.5\n" + << "1\n" + << "-1\n"; + plugin->set_innerxml(innerxml.str()); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/diff_drive/entity/system/add"}; + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // run once for the system to be added + server.Run(true, 1, false); + poses.clear(); + + // publish twist msg and verify that the vehicle now moves forward + pub.Publish(msg); + server.Run(true, 1000, false); + for (unsigned int i = 1; i < poses.size(); ++i) + { + EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()); + } + } +}; + +///////////////////////////////////////////////// +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_P(EntitySystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive_no_plugin.sdf", + "/model/vehicle/cmd_vel"); +} + +// Run multiple times +INSTANTIATE_TEST_SUITE_P(ServerRepeat, EntitySystemTest, + ::testing::Range(1, 2)); diff --git a/test/worlds/diff_drive_no_plugin.sdf b/test/worlds/diff_drive_no_plugin.sdf new file mode 100644 index 0000000000..1792199086 --- /dev/null +++ b/test/worlds/diff_drive_no_plugin.sdf @@ -0,0 +1,226 @@ + + + + + + 0.001 + 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 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + +