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
+/// \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;
 }

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)
 {