diff --git a/CMakeLists.txt b/CMakeLists.txt index 406ee1b308..35eefea33b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -110,7 +110,7 @@ set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-sensors -ign_find_package(ignition-sensors6 REQUIRED +ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1 # component order is important COMPONENTS # non-rendering diff --git a/include/ignition/gazebo/components/Recreate.hh b/include/ignition/gazebo/components/Recreate.hh new file mode 100644 index 0000000000..bb053fe5f9 --- /dev/null +++ b/include/ignition/gazebo/components/Recreate.hh @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2021 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. + * +*/ +#ifndef IGNITION_GAZEBO_COMPONENTS_RECREATE_HH_ +#define IGNITION_GAZEBO_COMPONENTS_RECREATE_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component that identifies an entity needs to be recreated. + /// Currently, only Models will be processed for recreation by the + /// SimulationRunner in the ProcessRecreateEntitiesRemove and + /// ProcessRecreateEntitiesCreate functions. + /// + /// The GUI ModelEditor contains example code on how to use this + /// component. For example, the ModelEditor allows a user to add a link to an + /// existing model. The existing model is tagged with this component so + /// that it can be recreated by the server. + using Recreate = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Recreate", Recreate) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/detail/View.hh b/include/ignition/gazebo/detail/View.hh index 93ea32c1b1..7b6ffd34b8 100644 --- a/include/ignition/gazebo/detail/View.hh +++ b/include/ignition/gazebo/detail/View.hh @@ -280,6 +280,10 @@ template bool View::NotifyComponentRemoval(const Entity _entity, const ComponentTypeId _typeId) { + // if entity is still marked as to add, remove from the view + if (this->RequiresComponent(_typeId)) + this->toAddEntities.erase(_entity); + // make sure that _typeId is a type required by the view and that _entity is // already a part of the view if (!this->RequiresComponent(_typeId) || diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 1b261294d0..5b0f482811 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -39,6 +39,8 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" +#include "ignition/gazebo/components/Recreate.hh" +#include "ignition/gazebo/components/World.hh" using namespace ignition; using namespace gazebo; @@ -420,7 +422,30 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, } else if (!_name.empty() && !_allowRename) { - if (kNullEntity != this->EntityByComponents(components::Name(_name))) + // Get the entity's original parent. This is used to make sure we get + // the correct entity. For example, two different models may have a + // child with the name "link". + auto origParentComp = + this->Component(_entity); + + // If there is an entity with the same name and user indicated renaming is + // not allowed then return null entity. + // If the entity or one of its ancestor has a Recreate component then carry + // on since the ECM is supposed to create a new entity with the same name. + Entity ent = this->EntityByComponents(components::Name(_name), + components::ParentEntity(origParentComp->Data())); + + bool hasRecreateComp = false; + Entity recreateEnt = ent; + while (recreateEnt != kNullEntity && !hasRecreateComp) + { + hasRecreateComp = this->Component(recreateEnt) != + nullptr; + auto parentComp = this->Component(recreateEnt); + recreateEnt = parentComp ? parentComp->Data() : kNullEntity; + } + + if (kNullEntity != ent && !hasRecreateComp) { ignerr << "Requested to clone entity [" << _entity << "] with a name of [" << _name << "], but another entity already " @@ -497,19 +522,35 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, Entity originalParentLink = kNullEntity; Entity originalChildLink = kNullEntity; + auto origParentComp = + this->Component(_entity); + const auto &parentName = this->Component(_entity); - if (parentName) + if (parentName && origParentComp) { - originalParentLink = this->EntityByComponents( - components::Name(parentName->Data())); + // CHangel the case where the parent link name is the world. + if (common::lowercase(parentName->Data()) == "world") + { + originalParentLink = this->Component( + origParentComp->Data())->Data(); + } + else + { + originalParentLink = + this->EntityByComponents( + components::Name(parentName->Data()), + components::ParentEntity(origParentComp->Data())); + } } const auto &childName = this->Component(_entity); - if (childName) + if (childName && origParentComp) { - originalChildLink = this->EntityByComponents( - components::Name(childName->Data())); + originalChildLink = + this->EntityByComponents( + components::Name(childName->Data()), + components::ParentEntity(origParentComp->Data())); } if (!originalParentLink || !originalChildLink) @@ -533,7 +574,14 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, for (const auto &childEntity : this->EntitiesByComponents(components::ParentEntity(_entity))) { - auto clonedChild = this->CloneImpl(childEntity, clonedEntity, "", true); + std::string name; + if (!_allowRename) + { + auto nameComp = this->Component(childEntity); + name = nameComp->Data(); + } + auto clonedChild = this->CloneImpl(childEntity, clonedEntity, name, + _allowRename); if (kNullEntity == clonedChild) { ignerr << "Cloning child entity [" << childEntity << "] failed.\n"; @@ -1038,6 +1086,14 @@ bool EntityComponentManager::CreateComponentImplementation( this->dataPtr->createdCompTypes.insert(_componentTypeId); + // If the component is a components::ParentEntity, then make sure to + // update the entities graph. + if (_componentTypeId == components::ParentEntity::typeId) + { + auto parentComp = this->Component(_entity); + this->SetParentEntity(_entity, parentComp->Data()); + } + return updateData; } @@ -1937,24 +1993,38 @@ bool EntityComponentManagerPrivate::ClonedJointLinkName(Entity _joint, return false; } - auto iter = this->originalToClonedLink.find(_originalLink); - if (iter == this->originalToClonedLink.end()) + Entity clonedLink = kNullEntity; + + + std::string name; + // Handle the case where the link coule have been the world. + if (_ecm->Component(_originalLink) != nullptr) { - ignerr << "Error: attempted to clone links, but link [" - << _originalLink << "] was never cloned.\n"; - return false; + // Use the special identifier "world". + name = "world"; } - auto clonedLink = iter->second; - - auto name = _ecm->Component(clonedLink); - if (!name) + else { - ignerr << "Link [" << _originalLink << "] was cloned, but its clone has no " - << "name.\n"; - return false; + auto iter = this->originalToClonedLink.find(_originalLink); + if (iter == this->originalToClonedLink.end()) + { + ignerr << "Error: attempted to clone links, but link [" + << _originalLink << "] was never cloned.\n"; + return false; + } + clonedLink = iter->second; + + auto nameComp = _ecm->Component(clonedLink); + if (!nameComp) + { + ignerr << "Link [" << _originalLink + << "] was cloned, but its clone has no name.\n"; + return false; + } + name = nameComp->Data(); } - _ecm->SetComponentData(_joint, name->Data()); + _ecm->SetComponentData(_joint, name); return true; } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index ed45452d66..ee5a5abf18 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2795,15 +2795,21 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) EXPECT_EQ(kNullEntity, failedClonedEntity); // create a joint with a parent and child link + const std::string parentModelEntityName = "parentModelEntity"; const std::string parentLinkEntityName = "parentLinkEntity"; const std::string childLinkEntityName = "childLinkEntity"; + Entity parentModelEntity = manager.CreateEntity(); + manager.CreateComponent(parentModelEntity, + components::Name(parentModelEntityName)); Entity parentLinkEntity = manager.CreateEntity(); manager.CreateComponent(parentLinkEntity, components::Name(parentLinkEntityName)); manager.CreateComponent(parentLinkEntity, components::CanonicalLink()); + manager.CreateComponent(parentLinkEntity, + components::ParentEntity(parentModelEntity)); Entity jointEntity = manager.CreateEntity(); manager.CreateComponent(jointEntity, - components::ParentEntity(parentLinkEntity)); + components::ParentEntity(parentModelEntity)); manager.CreateComponent(jointEntity, components::Name("jointEntity")); manager.CreateComponent(jointEntity, components::Joint()); manager.CreateComponent(jointEntity, @@ -2812,26 +2818,33 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) components::ChildLinkName(childLinkEntityName)); Entity childLinkEntity = manager.CreateEntity(); manager.CreateComponent(childLinkEntity, - components::ParentEntity(jointEntity)); + components::ParentEntity(parentModelEntity)); manager.CreateComponent(childLinkEntity, components::Name(childLinkEntityName)); manager.CreateComponent(childLinkEntity, components::Link()); - EXPECT_EQ(13u, manager.EntityCount()); + EXPECT_EQ(14u, manager.EntityCount()); // clone a joint that has a parent and child link. - auto clonedParentLinkEntity = manager.Clone(parentLinkEntity, kNullEntity, + auto clonedParentModelEntity = manager.Clone(parentModelEntity, kNullEntity, "", true); - ASSERT_NE(kNullEntity, clonedParentLinkEntity); - EXPECT_EQ(16u, manager.EntityCount()); - clonedEntities.insert(clonedParentLinkEntity); + ASSERT_NE(kNullEntity, clonedParentModelEntity); + // We just cloned a model with two links and a joint, a total of 4 new + // entities. + EXPECT_EQ(18u, manager.EntityCount()); + clonedEntities.insert(clonedParentModelEntity); auto clonedJoints = manager.EntitiesByComponents( - components::ParentEntity(clonedParentLinkEntity)); + components::ParentEntity(clonedParentModelEntity), components::Joint()); ASSERT_EQ(1u, clonedJoints.size()); clonedEntities.insert(clonedJoints[0]); auto clonedChildLinks = manager.EntitiesByComponents( - components::ParentEntity(clonedJoints[0])); + components::ParentEntity(clonedParentModelEntity), components::Link()); ASSERT_EQ(1u, clonedChildLinks.size()); clonedEntities.insert(clonedChildLinks[0]); + auto clonedChildCanonicalLinks = manager.EntitiesByComponents( + components::ParentEntity(clonedParentModelEntity), + components::CanonicalLink()); + ASSERT_EQ(1u, clonedChildCanonicalLinks.size()); + clonedEntities.insert(clonedChildCanonicalLinks[0]); // The cloned joint should have the cloned parent/child link names attached to // it, not the original parent/child link names @@ -2843,17 +2856,19 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) manager.Component(clonedJoints[0]); ASSERT_NE(nullptr, clonedJointChildLinkName); EXPECT_NE(clonedJointChildLinkName->Data(), childLinkEntityName); - auto clonedParentLinkName = - manager.Component(clonedParentLinkEntity); - ASSERT_NE(nullptr, clonedParentLinkName); - EXPECT_EQ(clonedParentLinkName->Data(), clonedJointParentLinkName->Data()); + auto clonedParentModelName = + manager.Component(clonedParentModelEntity); + ASSERT_NE(nullptr, clonedParentModelName); + auto clonedJointParentModelName = manager.Component( + manager.Component(clonedJoints[0])->Data()); + EXPECT_EQ(clonedParentModelName->Data(), clonedJointParentModelName->Data()); auto clonedChildLinkName = manager.Component(clonedChildLinks[0]); ASSERT_NE(nullptr, clonedChildLinkName); EXPECT_EQ(clonedJointChildLinkName->Data(), clonedChildLinkName->Data()); // make sure that the name given to each cloned entity is unique - EXPECT_EQ(8u, clonedEntities.size()); + EXPECT_EQ(9u, clonedEntities.size()); for (const auto &entity : clonedEntities) { auto nameComp = manager.Component(entity); @@ -2864,7 +2879,7 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) // try to clone an entity that does not exist EXPECT_EQ(kNullEntity, manager.Clone(kNullEntity, topLevelEntity, "", allowRename)); - EXPECT_EQ(16u, manager.EntityCount()); + EXPECT_EQ(18u, manager.EntityCount()); } ///////////////////////////////////////////////// diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 0a0ca96b33..ee7e06515e 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -27,8 +27,10 @@ #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/Events.hh" #include "ignition/gazebo/SdfEntityCreator.hh" #include "ignition/gazebo/Util.hh" @@ -820,6 +822,10 @@ void SimulationRunner::Step(const UpdateInfo &_info) IGN_PROFILE("SimulationRunner::Step"); this->currentInfo = _info; + // Process new ECM state information, typically sent from the GUI after + // a change was made to the GUI's ECM. + this->ProcessNewWorldControlState(); + // Publish info this->PublishStats(); @@ -831,6 +837,12 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Handle pending systems this->ProcessSystemQueue(); + // Handle entities that need to be recreated. + // Put in a request to mark them as removed so that in the UpdateSystem call + // the systems can remove them first before new ones are created. This is + // so that we can recreate entities with the same name. + this->ProcessRecreateEntitiesRemove(); + // Update all the systems. this->UpdateSystems(); @@ -861,6 +873,12 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Clear all new entities this->entityCompMgr.ClearNewlyCreatedEntities(); + // Recreate any entities that have the Recreate component + // The entities will have different Entity ids but keep the same name + // Make sure this happens after ClearNewlyCreatedEntities, otherwise the + // cloned entities will loose their "New" state. + this->ProcessRecreateEntitiesCreate(); + // Process entity removals. this->entityCompMgr.ProcessRemoveEntityRequests(); @@ -1136,11 +1154,13 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, { std::lock_guard lock(this->msgBufferMutex); - // update the server ECM if the request contains SerializedState information + // Copy the state information if it exists if (_req.has_state()) - this->entityCompMgr.SetState(_req.state()); - // TODO(anyone) notify server systems of changes made to the ECM, if there - // were any? + { + if (this->newWorldControlState == nullptr) + this->newWorldControlState = _req.New(); + this->newWorldControlState->CopyFrom(_req); + } WorldControl control; control.pause = _req.world_control().pause(); @@ -1170,7 +1190,7 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, { control.runToSimTime = std::chrono::seconds( _req.world_control().run_to_sim_time().sec()) + - std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); + std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); } this->worldControls.push_back(control); @@ -1179,6 +1199,22 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, return true; } +///////////////////////////////////////////////// +void SimulationRunner::ProcessNewWorldControlState() +{ + std::lock_guard lock(this->msgBufferMutex); + // update the server ECM if the request contains SerializedState information + if (this->newWorldControlState && this->newWorldControlState->has_state()) + { + this->entityCompMgr.SetState(this->newWorldControlState->state()); + + delete this->newWorldControlState; + this->newWorldControlState = nullptr; + } + // TODO(anyone) notify server systems of changes made to the ECM, if there + // were any? +} + ///////////////////////////////////////////////// bool SimulationRunner::OnPlaybackControl(const msgs::LogPlaybackControl &_req, msgs::Boolean &_res) @@ -1252,6 +1288,77 @@ void SimulationRunner::ProcessWorldControl() this->worldControls.clear(); } +///////////////////////////////////////////////// +void SimulationRunner::ProcessRecreateEntitiesRemove() +{ + IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesRemove"); + + // store the original entities to recreate and put in request to remove them + this->entityCompMgr.Each( + [&](const Entity &_entity, + const components::Model *, + const components::Recreate *)->bool + { + this->entitiesToRecreate.insert(_entity); + this->entityCompMgr.RequestRemoveEntity(_entity, true); + return true; + }); +} + +///////////////////////////////////////////////// +void SimulationRunner::ProcessRecreateEntitiesCreate() +{ + IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesCreate"); + + // clone the original entities + for (auto & ent : this->entitiesToRecreate) + { + auto nameComp = this->entityCompMgr.Component(ent); + auto parentComp = + this->entityCompMgr.Component(ent); + if (nameComp && parentComp) + { + // set allowRenaming to false so the entities keep their original name + Entity clonedEntity = this->entityCompMgr.Clone(ent, + parentComp->Data(), nameComp->Data(), false); + + // remove the Recreate component so they do not get recreated again in the + // next iteration + if (!this->entityCompMgr.RemoveComponent(ent)) + { + ignerr << "Failed to remove Recreate component from entity[" + << ent << "]" << std::endl; + } + + if (!this->entityCompMgr.RemoveComponent( + clonedEntity)) + { + ignerr << "Failed to remove Recreate component from entity[" + << clonedEntity << "]" << std::endl; + } + } + else + { + if (!nameComp) + { + ignerr << "Missing name component for entity[" << ent << "]. " + << "The entity will not be cloned during the recreation process." + << std::endl; + } + + if (!parentComp) + { + ignerr << "Missing parent component for entity[" << ent << "]. " + << "The entity will not be cloned during the recreation process." + << std::endl; + } + } + } + + this->entitiesToRecreate.clear(); +} + ///////////////////////////////////////////////// bool SimulationRunner::Paused() const { diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 9f17d59b6b..509e2b957a 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -466,6 +467,18 @@ namespace ignition std::optional _entity = std::nullopt, std::optional> _sdf = std::nullopt); + /// \brief Process entities with the components::Recreate component. + /// Put in a request to make them as removed + private: void ProcessRecreateEntitiesRemove(); + + /// \brief Process entities with the components::Recreate component. + /// Reccreate the entities by cloning from the original ones. + private: void ProcessRecreateEntitiesCreate(); + + /// \brief Process the new world state message, if it is present. + /// See the newWorldControlState variable below. + private: void ProcessNewWorldControlState(); + /// \brief This is used to indicate that a stop event has been received. private: std::atomic stopReceived{false}; @@ -627,6 +640,13 @@ namespace ignition /// WorldControl info (true) or not (false) private: bool stepping{false}; + /// \brief A set of entities that need to be recreated + private: std::set entitiesToRecreate; + + /// \brief Holds new world state information so that it can processed + /// at the appropriate time. + private: msgs::WorldControlState *newWorldControlState{nullptr}; + friend class LevelManager; }; } diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 6af7a70a90..c77cfaafc7 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -56,6 +56,7 @@ #include "ignition/gazebo/Events.hh" #include "ignition/gazebo/Util.hh" #include "ignition/gazebo/config.hh" + #include "../test/helpers/EnvTestFixture.hh" #include "SimulationRunner.hh" diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index d5e2a58df4..0632c355a6 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -18,6 +18,7 @@ #include #include #include + #include #include #include diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 1790c3b521..6661bce09e 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -61,7 +61,6 @@ namespace gazebo << _item->text().toStdString() << "]" << std::endl; } } - /// \brief Specialized to set string data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index d40cae6e1a..a40e8117da 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -32,6 +32,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" @@ -181,6 +182,9 @@ void ModelEditor::Update(const UpdateInfo &, linkSdf.SetName(linkName); auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); + // Make sure to mark the parent as needing recreation. This will + // tell the server to rebuild the model with the new link. + _ecm.CreateComponent(eta.parentEntity, components::Recreate()); // traverse the tree and add all new entities created by the entity // creator to the set diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 9407cc8d33..f2414936ba 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -122,6 +123,7 @@ #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/SlipComplianceCmd.hh" #include "ignition/gazebo/components/Static.hh" @@ -612,6 +614,13 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; + + /// \brief Set of links that were added to an existing model. This set + /// is used to track links that were added to an existing model, such as + /// through the GUI model editor, so that we can avoid premature creation + /// of links and collision elements. This also lets us suppress some + /// invalid error messages. + public: std::set linkAddedToModel; }; ////////////////////////////////////////////////// @@ -778,6 +787,9 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) ////////////////////////////////////////////////// void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) { + // Clear the set of links that were added to a model. + this->linkAddedToModel.clear(); + this->CreateWorldEntities(_ecm); this->CreateModelEntities(_ecm); this->CreateLinkEntities(_ecm); @@ -879,6 +891,9 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + if (_ecm.EntityHasComponentType(_entity, components::Recreate::typeId)) + return true; + // Check if model already exists if (this->entityModelMap.HasEntity(_entity)) { @@ -1011,6 +1026,18 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + // If the parent model is scheduled for recreation, then do not + // try to create a new link. This situation can occur when a link + // is added to a model from the GUI model editor. + if (_ecm.EntityHasComponentType(_parent->Data(), + components::Recreate::typeId)) + { + // Add this entity to the set of newly added links to existing + // models. + this->linkAddedToModel.insert(_entity); + return true; + } + // Check if link already exists if (this->entityLinkMap.HasEntity(_entity)) { @@ -1072,6 +1099,14 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) const components::CollisionElement *_collElement, const components::ParentEntity *_parent) -> bool { + // Check to see if this collision's parent is a link that was + // not created because the parent model is marked for recreation. + if (this->linkAddedToModel.find(_parent->Data()) != + this->linkAddedToModel.end()) + { + return true; + } + if (this->entityCollisionMap.HasEntity(_entity)) { ignwarn << "Collision entity [" << _entity @@ -2182,14 +2217,23 @@ std::map PhysicsPrivate::ChangedLinks( _ecm.Each( [&](const Entity &_entity, components::Link *) -> bool { - if (this->staticEntities.find(_entity) != this->staticEntities.end()) + if (this->staticEntities.find(_entity) != this->staticEntities.end() || + _ecm.EntityHasComponentType(_entity, components::Recreate::typeId)) + { return true; + } + // This `once` variable is here to aid in debugging, make sure to + // remove it. auto linkPhys = this->entityLinkMap.Get(_entity); if (nullptr == linkPhys) { - ignerr << "Internal error: link [" << _entity - << "] not in entity map" << std::endl; + if (this->linkAddedToModel.find(_entity) == + this->linkAddedToModel.end()) + { + ignerr << "Internal error: link [" << _entity + << "] not in entity map" << std::endl; + } return true; } @@ -2339,8 +2383,12 @@ bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, auto entityPhys = this->entityLinkMap.Get(_entity); if (nullptr == entityPhys) { - ignerr << "Internal error: entity [" << _entity - << "] not in entity map" << std::endl; + // Suppress error message if the link has just been added to the model. + if (this->linkAddedToModel.find(_entity) == this->linkAddedToModel.end()) + { + ignerr << "Internal error: entity [" << _entity + << "] not in entity map" << std::endl; + } return false; } @@ -2437,7 +2485,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, if (parentModelPoseIt == this->modelWorldPoses.end()) { ignerr << "Internal error: parent model [" << parentEntity - << "] does not have a world pose available" << std::endl; + << "] does not have a world pose available for child entity[" + << entity << "]" << std::endl; continue; } const math::Pose3d &parentWorldPose = parentModelPoseIt->second; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 9f5b0f434b..a2739240ca 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -91,7 +91,6 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Keep track of cameras, in case we need to handle stereo cameras. /// Key: Camera's parent scoped name /// Value: Pointer to camera - // TODO(anyone) Remove element when sensor is deleted public: std::map cameras; /// \brief Maps gazebo entity to its matching sensor ID @@ -361,6 +360,17 @@ void Sensors::RemoveSensor(const Entity &_entity) this->dataPtr->activeSensors.erase(activeSensorIt); } } + + // update cameras list + for (auto &it : this->dataPtr->cameras) + { + if (it.second->Id() == idIter->second) + { + this->dataPtr->cameras.erase(it.first); + break; + } + } + this->dataPtr->sensorIds.erase(idIter->second); this->dataPtr->sensorManager.Remove(idIter->second); this->dataPtr->entityToIdMap.erase(idIter); diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ca60386c8a..a2fec80165 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -47,6 +47,7 @@ set(tests physics_system.cc play_pause.cc pose_publisher_system.cc + recreate_entities.cc save_world.cc scene_broadcaster_system.cc sdf_frame_semantics.cc diff --git a/test/integration/recreate_entities.cc b/test/integration/recreate_entities.cc new file mode 100644 index 0000000000..74ca10950b --- /dev/null +++ b/test/integration/recreate_entities.cc @@ -0,0 +1,642 @@ +/* + * Copyright (C) 2021 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 + +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Recreate.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" // NOLINT(build/include) + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace components; + +using namespace std::chrono_literals; + +class RecreateEntitiesFixture : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(RecreateEntitiesFixture, RecreateEntities) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/shapes.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + EntityComponentManager *ecm = nullptr; + bool recreateEntities = false; + + // Create a system that adds a recreate component to models + test::Relay testSystem; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (!ecm) + { + ecm = &_ecm; + recreateEntities = true; + return; + } + + if (recreateEntities) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + // add a components::Recreate to indicate that this entity + // needs to be recreated + _ecm.CreateComponent(_entity, components::Recreate()); + return true; + }); + + // recreate entities only once so set it back to false + recreateEntities = false; + return; + } + }); + 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); + + // model entity ids + Entity boxModelEntity = kNullEntity; + Entity cylModelEntity = kNullEntity; + Entity sphModelEntity = kNullEntity; + Entity capModelEntity = kNullEntity; + Entity ellipModelEntity = kNullEntity; + + // link entity ids + Entity boxLinkEntity = kNullEntity; + Entity cylLinkEntity = kNullEntity; + Entity sphLinkEntity = kNullEntity; + Entity capLinkEntity = kNullEntity; + Entity ellipLinkEntity = kNullEntity; + + auto validateEntities = [&]() + { + boxModelEntity = kNullEntity; + cylModelEntity = kNullEntity; + sphModelEntity = kNullEntity; + capModelEntity = kNullEntity; + ellipModelEntity = kNullEntity; + + boxLinkEntity = kNullEntity; + cylLinkEntity = kNullEntity; + sphLinkEntity = kNullEntity; + capLinkEntity = kNullEntity; + ellipLinkEntity = kNullEntity; + + // Check entities + // 1 x world + 1 x (default) level + 1 x wind + 5 x model + 5 x link + 5 x + // collision + 5 x visual + 1 x light + EXPECT_EQ(24u, ecm->EntityCount()); + + Entity worldEntity = + ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + + // Check models + unsigned int modelCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Model *_model, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _model); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + modelCount++; + + EXPECT_EQ(worldEntity, _parent->Data()); + if (modelCount == 1) + { + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("box", _name->Data()); + boxModelEntity = _entity; + } + else if (modelCount == 2) + { + EXPECT_EQ(ignition::math::Pose3d(-1, -2, -3, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("cylinder", _name->Data()); + cylModelEntity = _entity; + } + else if (modelCount == 3) + { + EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("sphere", _name->Data()); + sphModelEntity = _entity; + } + else if (modelCount == 4) + { + EXPECT_EQ(ignition::math::Pose3d(-4, -5, -6, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("capsule", _name->Data()); + capModelEntity = _entity; + } + else if (modelCount == 5) + { + EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 1), + _pose->Data()); + EXPECT_EQ("ellipsoid", _name->Data()); + ellipModelEntity = _entity; + } + return true; + }); + + EXPECT_EQ(5u, modelCount); + EXPECT_NE(kNullEntity, boxModelEntity); + EXPECT_NE(kNullEntity, cylModelEntity); + EXPECT_NE(kNullEntity, sphModelEntity); + EXPECT_NE(kNullEntity, capModelEntity); + EXPECT_NE(kNullEntity, ellipModelEntity); + + // Check links + unsigned int linkCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Link *_link, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _link); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + linkCount++; + + if (linkCount == 1) + { + EXPECT_EQ(ignition::math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("box_link", _name->Data()); + EXPECT_EQ(boxModelEntity, _parent->Data()); + boxLinkEntity = _entity; + } + else if (linkCount == 2) + { + EXPECT_EQ(ignition::math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("cylinder_link", _name->Data()); + EXPECT_EQ(cylModelEntity, _parent->Data()); + cylLinkEntity = _entity; + } + else if (linkCount == 3) + { + EXPECT_EQ(ignition::math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("sphere_link", _name->Data()); + EXPECT_EQ(sphModelEntity, _parent->Data()); + sphLinkEntity = _entity; + } + else if (linkCount == 4) + { + EXPECT_EQ(ignition::math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("capsule_link", _name->Data()); + EXPECT_EQ(capModelEntity, _parent->Data()); + capLinkEntity = _entity; + } + else if (linkCount == 5) + { + EXPECT_EQ(ignition::math::Pose3d(0.8, 0.8, 0.8, 0, 0, 0), + _pose->Data()); + EXPECT_EQ("ellipsoid_link", _name->Data()); + EXPECT_EQ(ellipModelEntity, _parent->Data()); + ellipLinkEntity = _entity; + } + return true; + }); + + EXPECT_EQ(5u, linkCount); + EXPECT_NE(kNullEntity, boxLinkEntity); + EXPECT_NE(kNullEntity, cylLinkEntity); + EXPECT_NE(kNullEntity, sphLinkEntity); + EXPECT_NE(kNullEntity, capLinkEntity); + EXPECT_NE(kNullEntity, ellipLinkEntity); + }; + + // validate initial state + validateEntities(); + + // store the original entity ids + Entity originalboxModelEntity = boxModelEntity; + Entity originalcylModelEntity = cylModelEntity; + Entity originalsphModelEntity = sphModelEntity; + Entity originalcapModelEntity = capModelEntity; + Entity originalellipModelEntity = ellipModelEntity; + Entity originalboxLinkEntity = boxLinkEntity; + Entity originalcylLinkEntity = cylLinkEntity; + Entity originalsphLinkEntity = sphLinkEntity; + Entity originalcapLinkEntity = capLinkEntity; + Entity originalellipLinkEntity = ellipLinkEntity; + + // Run once to let the test relay system creates the components::Recreate + server.Run(true, 1, false); + + // Run again so that entities get recreated in the ECM + server.Run(true, 1, false); + + // validate that the entities are recreated + validateEntities(); + + // check that the newly recreated entities should have a different entity id + EXPECT_NE(originalboxModelEntity, boxModelEntity); + EXPECT_NE(originalcylModelEntity, cylModelEntity); + EXPECT_NE(originalsphModelEntity, sphModelEntity); + EXPECT_NE(originalcapModelEntity, capModelEntity); + EXPECT_NE(originalellipModelEntity, ellipModelEntity); + EXPECT_NE(originalboxLinkEntity, boxLinkEntity); + EXPECT_NE(originalcylLinkEntity, cylLinkEntity); + EXPECT_NE(originalsphLinkEntity, sphLinkEntity); + EXPECT_NE(originalcapLinkEntity, capLinkEntity); + EXPECT_NE(originalellipLinkEntity, ellipLinkEntity); + + // Run again to make sure the recreate components are removed and no + // entities to to be recreated + server.Run(true, 1, false); + + auto entities = ecm->EntitiesByComponents(components::Model(), + components::Recreate()); + EXPECT_TRUE(entities.empty()); +} + +///////////////////////////////////////////////// +TEST_F(RecreateEntitiesFixture, RecreateEntities_Joints) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/double_pendulum.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + EntityComponentManager *ecm = nullptr; + bool recreateEntities = false; + + // Create a system that adds a recreate component to models + test::Relay testSystem; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (!ecm) + { + ecm = &_ecm; + recreateEntities = true; + return; + } + + if (recreateEntities) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + // add a components::Recreate to indicate that this entity + // needs to be recreated + _ecm.CreateComponent(_entity, components::Recreate()); + return true; + }); + + // recreate entities only once so set it back to false + recreateEntities = false; + return; + } + }); + 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); + + auto validateEntities = [&]() + { + // Check entities + // 1 x world + 1 x (default) level + 1 x wind + 5 x model + 5 x link + 5 x + // collision + 5 x visual + 1 x light + EXPECT_EQ(48u, ecm->EntityCount()); + + Entity worldEntity = + ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + + // Check models + unsigned int modelCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Model *_model, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _model); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + modelCount++; + + EXPECT_EQ(worldEntity, _parent->Data()); + return true; + }); + + EXPECT_EQ(3u, modelCount); + + // Check links + unsigned int linkCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Link *_link, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _link); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + linkCount++; + + return true; + }); + + EXPECT_EQ(7u, linkCount); + + // Check links + unsigned int jointCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Joint *_joint, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _joint); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + jointCount++; + + return true; + }); + + EXPECT_EQ(4u, jointCount); + }; + + // validate initial state + validateEntities(); + + // Run once to let the test relay system creates the components::Recreate + server.Run(true, 1, false); + + // Run again so that entities get recreated in the ECM + server.Run(true, 1, false); + + // validate that the entities are recreated + validateEntities(); + + // Run again to make sure the recreate components are removed and no + // entities to to be recreated + server.Run(true, 1, false); + + auto entities = ecm->EntitiesByComponents(components::Model(), + components::Recreate()); + EXPECT_TRUE(entities.empty()); +} + +///////////////////////////////////////////////// +TEST_F(RecreateEntitiesFixture, RecreateEntities_WorldJoint) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/fixed_world_joint.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + EntityComponentManager *ecm = nullptr; + bool recreateEntities = false; + + // Create a system that adds a recreate component to models + test::Relay testSystem; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (!ecm) + { + ecm = &_ecm; + recreateEntities = true; + return; + } + + if (recreateEntities) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + // add a components::Recreate to indicate that this entity + // needs to be recreated + _ecm.CreateComponent(_entity, components::Recreate()); + return true; + }); + + // recreate entities only once so set it back to false + recreateEntities = false; + return; + } + }); + 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); + + auto validateEntities = [&]() + { + // Check entities + // 1 x world + 1 x (default) level + 1 x wind + 1 x model + 2 x link + 2 x + // collision + 2 x visual + 2 x joint + EXPECT_EQ(12u, ecm->EntityCount()); + + Entity worldEntity = + ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + + // Check models + unsigned int modelCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Model *_model, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _model); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + modelCount++; + + EXPECT_EQ(worldEntity, _parent->Data()); + return true; + }); + + EXPECT_EQ(1u, modelCount); + + // Check links + unsigned int linkCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Link *_link, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _link); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + linkCount++; + + return true; + }); + + EXPECT_EQ(2u, linkCount); + + // Check joint + unsigned int jointCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Joint *_joint, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name, + const components::ParentLinkName *_parentLinkName)->bool + { + EXPECT_NE(nullptr, _joint); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + EXPECT_NE(nullptr, _parentLinkName); + + std::cout << "JointName[" << _name->Data() << "]\n"; + if (_name->Data() == "world_fixed") + { + EXPECT_EQ("world", _parentLinkName->Data()); + } + jointCount++; + + return true; + }); + + EXPECT_EQ(2u, jointCount); + }; + + // validate initial state + validateEntities(); + + // Run once to let the test relay system creates the components::Recreate + server.Run(true, 1, false); + + // Run again so that entities get recreated in the ECM + server.Run(true, 1, false); + + // validate that the entities are recreated + validateEntities(); + + // Run again to make sure the recreate components are removed and no + // entities to to be recreated + server.Run(true, 1, false); + + auto entities = ecm->EntitiesByComponents(components::Model(), + components::Recreate()); + EXPECT_TRUE(entities.empty()); +} diff --git a/test/worlds/double_pendulum.sdf b/test/worlds/double_pendulum.sdf new file mode 100644 index 0000000000..10940e785f --- /dev/null +++ b/test/worlds/double_pendulum.sdf @@ -0,0 +1,451 @@ + + + + + 0.001 + 1.0 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + + 5 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + + diff --git a/test/worlds/fixed_world_joint.sdf b/test/worlds/fixed_world_joint.sdf new file mode 100644 index 0000000000..114675b885 --- /dev/null +++ b/test/worlds/fixed_world_joint.sdf @@ -0,0 +1,88 @@ + + + + + 0.001 + 1.0 + + + + 0 0 0 0 1.57 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 0.1 0 0 0 + + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.1 0.05 + + + + 0.2 0.8 0.2 1 + + + + + + 0.25 0.1 0.05 + + + + + + + world + base_link + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + +