diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 0280a0c078..65d72ec60f 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -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 &_iface); - /// \brief Pose3d equality comparison function. - public: std::function - 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 _iface); + ////////////////////////////////////////////////// UserCommands::UserCommands() : System(), dataPtr(std::make_unique()) @@ -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(msg, this->iface); + + // Push to pending + { + std::lock_guard 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 &_iface) - : UserCommandBase(_msg, _iface) -{ -} ////////////////////////////////////////////////// -bool PoseCommand::Execute() +bool updatePose( + const msgs::Pose &_poseMsg, + std::shared_ptr _iface) { - auto poseMsg = dynamic_cast(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(entity); + _iface->ecm->Component(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 &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool PoseCommand::Execute() +{ + auto poseMsg = dynamic_cast(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 &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool PoseVectorCommand::Execute() +{ + auto poseVectorMsg = dynamic_cast(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; } diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh index 3db285aad6..95af084c96 100644 --- a/src/systems/user_commands/UserCommands.hh +++ b/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//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//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, diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index d06ce9a2cb..63dee21695 100644 --- a/test/integration/user_commands.cc +++ b/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(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(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(sphereEntity); + ASSERT_NE(nullptr, poseComp); + EXPECT_NEAR(456, poseComp->Data().Pos().Y(), 0.2); +} + ///////////////////////////////////////////////// TEST_F(UserCommandsTest, Physics) {