Skip to content

Commit

Permalink
merged with model_inspector
Browse files Browse the repository at this point in the history
Signed-off-by: Nate Koenig <[email protected]>
  • Loading branch information
Nate Koenig committed Nov 17, 2021
2 parents a4f8336 + 9d4cd24 commit d3cb2af
Show file tree
Hide file tree
Showing 20 changed files with 1,631 additions and 133 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
48 changes: 48 additions & 0 deletions include/ignition/gazebo/components/Recreate.hh
Original file line number Diff line number Diff line change
@@ -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 <ignition/gazebo/components/Factory.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/config.hh>

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<NoData, class RecreateTag>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Recreate", Recreate)
}
}
}
}

#endif
4 changes: 4 additions & 0 deletions include/ignition/gazebo/detail/View.hh
Original file line number Diff line number Diff line change
Expand Up @@ -280,6 +280,10 @@ template<typename ...ComponentTypeTs>
bool View<ComponentTypeTs...>::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) ||
Expand Down
112 changes: 91 additions & 21 deletions src/EntityComponentManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<components::ParentEntity>(_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<components::Recreate>(recreateEnt) !=
nullptr;
auto parentComp = this->Component<components::ParentEntity>(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 "
Expand Down Expand Up @@ -497,19 +522,35 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent,
Entity originalParentLink = kNullEntity;
Entity originalChildLink = kNullEntity;

auto origParentComp =
this->Component<components::ParentEntity>(_entity);

const auto &parentName =
this->Component<components::ParentLinkName>(_entity);
if (parentName)
if (parentName && origParentComp)
{
originalParentLink = this->EntityByComponents<components::Name>(
components::Name(parentName->Data()));
// CHangel the case where the parent link name is the world.
if (common::lowercase(parentName->Data()) == "world")
{
originalParentLink = this->Component<components::ParentEntity>(
origParentComp->Data())->Data();
}
else
{
originalParentLink =
this->EntityByComponents<components::Name, components::ParentEntity>(
components::Name(parentName->Data()),
components::ParentEntity(origParentComp->Data()));
}
}

const auto &childName = this->Component<components::ChildLinkName>(_entity);
if (childName)
if (childName && origParentComp)
{
originalChildLink = this->EntityByComponents<components::Name>(
components::Name(childName->Data()));
originalChildLink =
this->EntityByComponents<components::Name, components::ParentEntity>(
components::Name(childName->Data()),
components::ParentEntity(origParentComp->Data()));
}

if (!originalParentLink || !originalChildLink)
Expand All @@ -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<components::Name>(childEntity);
name = nameComp->Data();
}
auto clonedChild = this->CloneImpl(childEntity, clonedEntity, name,
_allowRename);
if (kNullEntity == clonedChild)
{
ignerr << "Cloning child entity [" << childEntity << "] failed.\n";
Expand Down Expand Up @@ -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<components::ParentEntity>(_entity);
this->SetParentEntity(_entity, parentComp->Data());
}

return updateData;
}

Expand Down Expand Up @@ -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<components::World>(_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<components::Name>(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<components::Name>(clonedLink);
if (!nameComp)
{
ignerr << "Link [" << _originalLink
<< "] was cloned, but its clone has no name.\n";
return false;
}
name = nameComp->Data();
}

_ecm->SetComponentData<ComponentTypeT>(_joint, name->Data());
_ecm->SetComponentData<ComponentTypeT>(_joint, name);
return true;
}

Expand Down
45 changes: 30 additions & 15 deletions src/EntityComponentManager_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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
Expand All @@ -2843,17 +2856,19 @@ TEST_P(EntityComponentManagerFixture, CloneEntities)
manager.Component<components::ChildLinkName>(clonedJoints[0]);
ASSERT_NE(nullptr, clonedJointChildLinkName);
EXPECT_NE(clonedJointChildLinkName->Data(), childLinkEntityName);
auto clonedParentLinkName =
manager.Component<components::Name>(clonedParentLinkEntity);
ASSERT_NE(nullptr, clonedParentLinkName);
EXPECT_EQ(clonedParentLinkName->Data(), clonedJointParentLinkName->Data());
auto clonedParentModelName =
manager.Component<components::Name>(clonedParentModelEntity);
ASSERT_NE(nullptr, clonedParentModelName);
auto clonedJointParentModelName = manager.Component<components::Name>(
manager.Component<components::ParentEntity>(clonedJoints[0])->Data());
EXPECT_EQ(clonedParentModelName->Data(), clonedJointParentModelName->Data());
auto clonedChildLinkName =
manager.Component<components::Name>(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<components::Name>(entity);
Expand All @@ -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());
}

/////////////////////////////////////////////////
Expand Down
Loading

0 comments on commit d3cb2af

Please sign in to comment.