Merge 2ba4994 into ccbda48

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})
 
 #--------------------------------------

src/SimulationRunner.cc

@@ -112,9 +112,28 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
         static_cast<int>(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<transport::Node>(opts);
+
   // Create the system manager
   this->systemMgr = std::make_unique<SystemManager>(_systemLoader,
-      &this->entityCompMgr, &this->eventMgr);
+      &this->entityCompMgr, &this->eventMgr, validNs);
 
   this->pauseConn = this->eventMgr.Connect<events::Pause>(
       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<transport::Node>(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();
 

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<transport::Node>(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<SystemInternal> 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<std::mutex> lock(this->systemsMsgMutex);
+  this->systemsToAdd.push_back(_req);
+  _res.set_data(true);
+  return true;
+}
+
+//////////////////////////////////////////////////
+void SystemManager::ProcessPendingEntitySystems()
+{
+  std::lock_guard<std::mutex> 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();
+}

src/SystemManager.hh

@@ -17,11 +17,14 @@
 #ifndef IGNITION_GAZEBO_SYSTEMMANAGER_HH_
 #define IGNITION_GAZEBO_SYSTEMMANAGER_HH_
 
+#include <ignition/msgs/entity_plugin_v.pb.h>
+
 #include <memory>
 #include <string>
 #include <vector>
 
 #include <sdf/Plugin.hh>
+#include <ignition/transport/Node.hh>
 
 #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<SystemInternal> 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<SystemInternal> 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<msgs::EntityPlugin_V> systemsToAdd;
+
+      /// \brief Mutex to protect systemsToAdd list
+      private: std::mutex systemsMsgMutex;
+
+      /// \brief Node for communication.
+      private: std::unique_ptr<transport::Node> node{nullptr};
     };
     }
   }  // namespace gazebo

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

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 <gtest/gtest.h>
+#include <ignition/common/Console.hh>
+#include <ignition/common/Util.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/transport/Node.hh>
+#include <ignition/utilities/ExtraTestMacros.hh>
+
+#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<int>>
+{
+  /// \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<math::Pose3d> 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<components::Pose>(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<msgs::Twist>(_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_joint>left_wheel_joint</left_joint>\n"
+        << "<right_joint>right_wheel_joint</right_joint>\n"
+        << "<wheel_separation>1.25</wheel_separation>\n"
+        << "<wheel_radius>0.3</wheel_radius>\n"
+        << "<max_linear_acceleration>1</max_linear_acceleration>\n"
+        << "<min_linear_acceleration>-1</min_linear_acceleration>\n"
+        << "<max_angular_acceleration>2</max_angular_acceleration>\n"
+        << "<min_angular_acceleration>-2</min_angular_acceleration>\n"
+        << "<max_linear_velocity>0.5</max_linear_velocity>\n"
+        << "<min_linear_velocity>-0.5</min_linear_velocity>\n"
+        << "<max_angular_velocity>1</max_angular_velocity>\n"
+        << "<min_angular_velocity>-1</min_angular_velocity>\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));