Added user command to set multiple entities

src/systems/user_commands/UserCommands.cc

@@ -21,6 +21,7 @@
 #include <ignition/msgs/boolean.pb.h>
 #include <ignition/msgs/entity_factory.pb.h>
 #include <ignition/msgs/pose.pb.h>
+#include <ignition/msgs/pose_v.pb.h>
 #include <ignition/msgs/physics.pb.h>
 
 #include <string>
@@ -138,17 +139,19 @@ class PoseCommand : public UserCommandBase
 
   // Documentation inherited
   public: bool Execute() final;
+};
+
+/// \brief Command to update an entity's pose transform.
+class PoseVectorCommand : public UserCommandBase
+{
+  /// \brief Constructor
+  /// \param[in] _msg pose_v message.
+  /// \param[in] _iface Pointer to user commands interface.
+  public: PoseVectorCommand(msgs::Pose_V *_msg,
+      std::shared_ptr<UserCommandsInterface> &_iface);
 
-  /// \brief Pose3d equality comparison function.
-  public: std::function<bool(const math::Pose3d &, const math::Pose3d &)>
-          pose3Eql { [](const math::Pose3d &_a, const math::Pose3d &_b)
-                     {
-                       return _a.Pos().Equal(_b.Pos(), 1e-6) &&
-                         math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) &&
-                         math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) &&
-                         math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) &&
-                         math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
-                     }};
+  // Documentation inherited
+  public: bool Execute() final;
 };
 
 /// \brief Command to modify the physics parameters of a simulation.
@@ -202,6 +205,13 @@ class ignition::gazebo::systems::UserCommandsPrivate
   /// \return True if successful.
   public: bool PoseService(const msgs::Pose &_req, msgs::Boolean &_res);
 
+  /// \brief Callback for pose_v service
+  /// \param[in] _req Request containing pose update of several entities.
+  /// \param[out] _res True if message successfully received and queued.
+  /// It does not mean that the entity will be successfully moved.
+  /// \return True if successful.
+  public: bool PoseVectorService(const msgs::Pose_V &_req, msgs::Boolean &_res);
+
   /// \brief Callback for physics service
   /// \param[in] _req Request containing updates to the physics parameters.
   /// \param[in] _res True if message successfully received and queued.
@@ -222,6 +232,26 @@ class ignition::gazebo::systems::UserCommandsPrivate
   public: std::mutex pendingMutex;
 };
 
+/// \brief Pose3d equality comparison function.
+/// \param[in] _a A pose to compare
+/// \param[in] _b Another pose to compare
+bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b)
+{
+  return _a.Pos().Equal(_b.Pos(), 1e-6) &&
+    math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) &&
+    math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) &&
+    math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) &&
+    math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
+}
+
+/// \brief Update pose for a specific pose message
+/// \param[in] _req Message containing new pose
+/// \param[in] _iface Pointer to user commands interface.
+/// \return True if successful.
+bool updatePose(
+  const msgs::Pose &_req,
+  std::shared_ptr<UserCommandsInterface> _iface);
+
 //////////////////////////////////////////////////
 UserCommands::UserCommands() : System(),
     dataPtr(std::make_unique<UserCommandsPrivate>())
@@ -273,6 +303,14 @@ void UserCommands::Configure(const Entity &_entity,
 
   ignmsg << "Pose service on [" << poseService << "]" << std::endl;
 
+  // Pose vector service
+  std::string poseVectorService{
+    "/world/" + worldName + "/set_pose_vector"};
+  this->dataPtr->node.Advertise(poseVectorService,
+      &UserCommandsPrivate::PoseVectorService, this->dataPtr.get());
+
+  ignmsg << "Pose service on [" << poseVectorService << "]" << std::endl;
+
   // Physics service
   std::string physicsService{"/world/" + worldName + "/set_physics"};
   this->dataPtr->node.Advertise(physicsService,
@@ -390,6 +428,25 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req,
   return true;
 }
 
+//////////////////////////////////////////////////
+bool UserCommandsPrivate::PoseVectorService(const msgs::Pose_V &_req,
+    msgs::Boolean &_res)
+{
+  // Create command and push it to queue
+  auto msg = _req.New();
+  msg->CopyFrom(_req);
+  auto cmd = std::make_unique<PoseVectorCommand>(msg, this->iface);
+
+  // Push to pending
+  {
+    std::lock_guard<std::mutex> lock(this->pendingMutex);
+    this->pendingCmds.push_back(std::move(cmd));
+  }
+
+  _res.set_data(true);
+  return true;
+}
+
 //////////////////////////////////////////////////
 bool UserCommandsPrivate::PhysicsService(const msgs::Physics &_req,
     msgs::Boolean &_res)
@@ -679,60 +736,96 @@ bool RemoveCommand::Execute()
   return true;
 }
 
-//////////////////////////////////////////////////
-PoseCommand::PoseCommand(msgs::Pose *_msg,
-    std::shared_ptr<UserCommandsInterface> &_iface)
-    : UserCommandBase(_msg, _iface)
-{
-}
 
 //////////////////////////////////////////////////
-bool PoseCommand::Execute()
+bool updatePose(
+  const msgs::Pose &_poseMsg,
+  std::shared_ptr<UserCommandsInterface> _iface)
 {
-  auto poseMsg = dynamic_cast<const msgs::Pose *>(this->msg);
-  if (nullptr == poseMsg)
-  {
-    ignerr << "Internal error, null create message" << std::endl;
-    return false;
-  }
-
   // Check the name of the entity being spawned
-  std::string entityName = poseMsg->name();
+  std::string entityName = _poseMsg.name();
   Entity entity = kNullEntity;
   // TODO(anyone) Update pose message to use Entity, with default ID null
-  if (poseMsg->id() != kNullEntity && poseMsg->id() != 0)
+  if (_poseMsg.id() != kNullEntity && _poseMsg.id() != 0)
   {
-    entity = poseMsg->id();
+    entity = _poseMsg.id();
   }
   else if (!entityName.empty())
   {
-    entity = this->iface->ecm->EntityByComponents(components::Name(entityName),
-      components::ParentEntity(this->iface->worldEntity));
+    entity = _iface->ecm->EntityByComponents(components::Name(entityName),
+      components::ParentEntity(_iface->worldEntity));
   }
 
-  if (!this->iface->ecm->HasEntity(entity))
+  if (!_iface->ecm->HasEntity(entity))
   {
-    ignerr << "Unable to update the pose for entity id:[" << poseMsg->id()
+    ignerr << "Unable to update the pose for entity id:[" << _poseMsg.id()
            << "], name[" << entityName << "]" << std::endl;
     return false;
   }
 
   auto poseCmdComp =
-    this->iface->ecm->Component<components::WorldPoseCmd>(entity);
+    _iface->ecm->Component<components::WorldPoseCmd>(entity);
   if (!poseCmdComp)
   {
-    this->iface->ecm->CreateComponent(
-        entity, components::WorldPoseCmd(msgs::Convert(*poseMsg)));
+    _iface->ecm->CreateComponent(
+        entity, components::WorldPoseCmd(msgs::Convert(_poseMsg)));
   }
   else
   {
     /// \todo(anyone) Moving an object is not captured in a log file.
-    auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), this->pose3Eql) ?
+    auto state = poseCmdComp->SetData(msgs::Convert(_poseMsg), pose3Eql) ?
         ComponentState::OneTimeChange :
         ComponentState::NoChange;
-    this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId,
+    _iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId,
         state);
   }
+  return true;
+}
+
+//////////////////////////////////////////////////
+PoseCommand::PoseCommand(msgs::Pose *_msg,
+    std::shared_ptr<UserCommandsInterface> &_iface)
+    : UserCommandBase(_msg, _iface)
+{
+}
+
+//////////////////////////////////////////////////
+bool PoseCommand::Execute()
+{
+  auto poseMsg = dynamic_cast<const msgs::Pose *>(this->msg);
+  if (nullptr == poseMsg)
+  {
+    ignerr << "Internal error, null create message" << std::endl;
+    return false;
+  }
+
+  return updatePose(*poseMsg, this->iface);
+}
+
+//////////////////////////////////////////////////
+PoseVectorCommand::PoseVectorCommand(msgs::Pose_V *_msg,
+    std::shared_ptr<UserCommandsInterface> &_iface)
+    : UserCommandBase(_msg, _iface)
+{
+}
+
+//////////////////////////////////////////////////
+bool PoseVectorCommand::Execute()
+{
+  auto poseVectorMsg = dynamic_cast<const msgs::Pose_V *>(this->msg);
+  if (nullptr == poseVectorMsg)
+  {
+    ignerr << "Internal error, null create message" << std::endl;
+    return false;
+  }
+
+  for (int i = 0; i < poseVectorMsg->pose_size(); i++)
+  {
+    if (!updatePose(poseVectorMsg->pose(i), this->iface))
+    {
+      return false;
+    }
+  }
 
   return true;
 }

src/systems/user_commands/UserCommands.hh

@@ -53,6 +53,22 @@ namespace systems
   /// * **Request type*: ignition.msgs.EntityFactory_V
   /// * **Response type*: ignition.msgs.Boolean
   ///
+  /// # Set entity pose
+  ///
+  /// This service set the pose of entities
+  ///
+  /// * **Service**: `/world/<world name>/set_pose`
+  /// * **Request type*: ignition.msgs.Pose
+  /// * **Response type*: ignition.msgs.Boolean
+  ///
+  /// # Set multiple entity poses
+  ///
+  /// This service set the pose of multiple entities
+  ///
+  /// * **Service**: `/world/<world name>/set_pose_vector`
+  /// * **Request type*: ignition.msgs.Pose_V
+  /// * **Response type*: ignition.msgs.Boolean
+  ///
   /// Try some examples described on examples/worlds/empty.sdf
   class UserCommands:
     public System,

test/integration/user_commands.cc

@@ -669,6 +669,81 @@ TEST_F(UserCommandsTest, Pose)
   EXPECT_NEAR(500.0, poseComp->Data().Pos().Y(), 0.2);
 }
 
+
+/////////////////////////////////////////////////
+TEST_F(UserCommandsTest, PoseVector)
+{
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/shapes.sdf";
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Create a system just to get the ECM
+  EntityComponentManager *ecm{nullptr};
+  test::Relay testSystem;
+  testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &,
+                             gazebo::EntityComponentManager &_ecm)
+      {
+        ecm = &_ecm;
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+
+  // Run server and check we have the ECM
+  EXPECT_EQ(nullptr, ecm);
+  server.Run(true, 1, false);
+  EXPECT_NE(nullptr, ecm);
+
+  // Entity move by name
+  msgs::Pose_V req;
+
+  auto poseBoxMsg = req.add_pose();
+  poseBoxMsg->set_name("box");
+  poseBoxMsg->mutable_position()->set_y(123.0);
+
+  auto poseSphereMsg = req.add_pose();
+  poseSphereMsg->set_name("sphere");
+  poseSphereMsg->mutable_position()->set_y(456.0);
+
+  msgs::Boolean res;
+  bool result;
+  unsigned int timeout = 5000;
+  std::string service{"/world/default/set_pose_vector"};
+
+  transport::Node node;
+  EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(res.data());
+
+  // Box entity
+  auto boxEntity = ecm->EntityByComponents(components::Name("box"));
+  EXPECT_NE(kNullEntity, boxEntity);
+
+  // Check entity has not been moved yet
+  auto poseComp = ecm->Component<components::Pose>(boxEntity);
+  ASSERT_NE(nullptr, poseComp);
+  EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), poseComp->Data());
+
+  // Run an iteration and check it was moved
+  server.Run(true, 1, false);
+
+  poseComp = ecm->Component<components::Pose>(boxEntity);
+  ASSERT_NE(nullptr, poseComp);
+  EXPECT_NEAR(123.0, poseComp->Data().Pos().Y(), 0.2);
+
+  auto sphereEntity = ecm->EntityByComponents(components::Name("sphere"));
+  EXPECT_NE(kNullEntity, sphereEntity);
+
+  poseComp = ecm->Component<components::Pose>(sphereEntity);
+  ASSERT_NE(nullptr, poseComp);
+  EXPECT_NEAR(456, poseComp->Data().Pos().Y(), 0.2);
+}
+
 /////////////////////////////////////////////////
 TEST_F(UserCommandsTest, Physics)
 {