New service for adding systems to an entity

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:/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));