From f6b4b0ac463019a8133a83a59d2d5a6be4099632 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 17 Sep 2020 14:54:00 -0700 Subject: [PATCH] Add more model APIs (#349) Signed-off-by: Louise Poubel Co-authored-by: John Shepherd --- include/ignition/gazebo/Model.hh | 64 ++++++ include/ignition/gazebo/Util.hh | 50 +++++ .../gazebo/components/AngularVelocityCmd.hh | 6 +- .../gazebo/components/LinearVelocityCmd.hh | 10 +- src/Model.cc | 102 +++++++++ src/Util.cc | 149 ++++++++++--- src/Util_TEST.cc | 123 ++++++++++- .../component_inspector/ComponentInspector.cc | 16 ++ .../plugins/component_inspector/String.qml | 19 +- src/systems/physics/Physics.cc | 18 ++ test/integration/model.cc | 132 ++++++++++- tutorials.md.in | 8 +- tutorials/migration_model_api.md | 205 ++++++++++++++++++ 13 files changed, 851 insertions(+), 51 deletions(-) create mode 100644 tutorials/migration_model_api.md diff --git a/include/ignition/gazebo/Model.hh b/include/ignition/gazebo/Model.hh index 0e3fcdf0b0..7fc107f09e 100644 --- a/include/ignition/gazebo/Model.hh +++ b/include/ignition/gazebo/Model.hh @@ -19,6 +19,9 @@ #include #include +#include + +#include #include #include @@ -95,11 +98,41 @@ namespace ignition /// \return Model's name. public: std::string Name(const EntityComponentManager &_ecm) const; + /// \brief Get the ID of the entity which is the immediate parent of this + /// model. + /// \param[in] _ecm Entity-component manager. + /// \return Parent entity ID. + public: gazebo::Entity Parent(const EntityComponentManager &_ecm) const; + + /// \brief Get whether this model is static. + /// \param[in] _ecm Entity-component manager. + /// \return True if static. + public: bool Static(const EntityComponentManager &_ecm) const; + + /// \brief Get whether this model has self-collide enabled. + /// \param[in] _ecm Entity-component manager. + /// \return True if self-colliding. + public: bool SelfCollide(const EntityComponentManager &_ecm) const; + + /// \brief Get whether this model has wind enabled. + /// \param[in] _ecm Entity-component manager. + /// \return True if wind mode is on. + public: bool WindMode(const EntityComponentManager &_ecm) const; + + /// \brief Get the source file where this model came from. If empty, + /// the model wasn't loaded directly from a file, probably from an SDF + /// string. + /// \param[in] _ecm Entity-component manager. + /// \return Path to the source SDF file. + public: std::string SourceFilePath(const EntityComponentManager &_ecm) + const; + /// \brief Get the ID of a joint entity which is an immediate child of /// this model. /// \param[in] _ecm Entity-component manager. /// \param[in] _name Joint name. /// \return Joint entity. + /// \todo(anyone) Make const public: gazebo::Entity JointByName(const EntityComponentManager &_ecm, const std::string &_name); @@ -108,9 +141,40 @@ namespace ignition /// \param[in] _ecm Entity-component manager. /// \param[in] _name Link name. /// \return Link entity. + /// \todo(anyone) Make const public: gazebo::Entity LinkByName(const EntityComponentManager &_ecm, const std::string &_name); + /// \brief Get all joints which are immediate children of this model. + /// \param[in] _ecm Entity-component manager. + /// \return All joints in this model. + public: std::vector Joints( + const EntityComponentManager &_ecm) const; + + /// \brief Get all links which are immediate children of this model. + /// \param[in] _ecm Entity-component manager. + /// \return All links in this model. + public: std::vector Links( + const EntityComponentManager &_ecm) const; + + /// \brief Get the number of joints which are immediate children of this + /// model. + /// \param[in] _ecm Entity-component manager. + /// \return Number of joints in this model. + public: uint64_t JointCount(const EntityComponentManager &_ecm) const; + + /// \brief Get the number of links which are immediate children of this + /// model. + /// \param[in] _ecm Entity-component manager. + /// \return Number of links in this model. + public: uint64_t LinkCount(const EntityComponentManager &_ecm) const; + + /// \brief Set a command to change the model's pose. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _pose New model pose. + public: void SetWorldPoseCmd(EntityComponentManager &_ecm, + const math::Pose3d &_pose); + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 87cc852779..3dd37d393f 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -51,6 +51,56 @@ namespace ignition const EntityComponentManager &_ecm, const std::string &_delim = "/", bool _includePrefix = true); + /// \brief Generally, each entity will be of some specific high-level type, + /// such as World, Sensor, Collision, etc, and one type only. + /// The entity type is usually marked by having some component that + /// represents that type, such as `ignition::gazebo::components::Visual`. + /// + /// This function returns the type ID of the given entity's type, which + /// can be checked against different types. For example, if the + /// entity is a model, this will be true: + /// + /// `gazebo::components::Model::typeId == entityTypeId(entity, ecm)` + /// + /// In case the entity isn't of any known type, this will return + /// `ignition::gazebo::kComponentTypeIdInvalid`. + /// + /// In case the entity has more than one type, only one of them will be + /// returned. This is not standard usage. + /// + /// \param[in] _entity Entity to get the type for. + /// \param[in] _ecm Immutable reference to ECM. + /// \return ID of entity's type-defining components. + ComponentTypeId IGNITION_GAZEBO_VISIBLE entityTypeId(const Entity &_entity, + const EntityComponentManager &_ecm); + + /// \brief Generally, each entity will be of some specific high-level type, + /// such as "world", "sensor", "collision", etc, and one type only. + /// + /// This function returns a lowercase string for each type. For example, + /// "light", "actor", etc. + /// + /// In case the entity isn't of any known type, this will return an empty + /// string. + /// + /// In case the entity has more than one type, only one of them will be + /// returned. This is not standard usage. + /// + /// Note that this is different from component type names. + /// + /// \param[in] _entity Entity to get the type for. + /// \param[in] _ecm Immutable reference to ECM. + /// \return ID of entity's type-defining components. + std::string IGNITION_GAZEBO_VISIBLE entityTypeStr(const Entity &_entity, + const EntityComponentManager &_ecm); + + /// \brief Get the world to which the given entity belongs. + /// \param[in] _entity Entity to get the world for. + /// \param[in] _ecm Immutable reference to ECM. + /// \return World entity ID. + Entity IGNITION_GAZEBO_VISIBLE worldEntity(const Entity &_entity, + const EntityComponentManager &_ecm); + /// \brief Helper function to remove a parent scope from a given name. /// This removes the first name found before the delimiter. /// \param[in] _name Input name possibly generated by scopedName. diff --git a/include/ignition/gazebo/components/AngularVelocityCmd.hh b/include/ignition/gazebo/components/AngularVelocityCmd.hh index 77edfd277a..1a22171456 100644 --- a/include/ignition/gazebo/components/AngularVelocityCmd.hh +++ b/include/ignition/gazebo/components/AngularVelocityCmd.hh @@ -32,14 +32,14 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component type that contains angular velocity cmd of an entity - /// represented by ignition::math::Vector3d. + /// \brief A component type that contains the commanded angular velocity of + /// an entity, in its own frame, represented by ignition::math::Vector3d. using AngularVelocityCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.AngularVelocityCmd", AngularVelocityCmd) - /// \brief A component type that contains angular velocity cmd + /// \brief A component type that contains the commanded angular velocity /// of an entity in the world frame represented by ignition::math::Vector3d. using WorldAngularVelocityCmd = Component; diff --git a/include/ignition/gazebo/components/LinearVelocityCmd.hh b/include/ignition/gazebo/components/LinearVelocityCmd.hh index e68eac2eeb..cf03bea0ce 100644 --- a/include/ignition/gazebo/components/LinearVelocityCmd.hh +++ b/include/ignition/gazebo/components/LinearVelocityCmd.hh @@ -32,15 +32,17 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component type that contains linear velocity of an entity - /// represented by ignition::math::Vector3d. + // \brief A component type that contains the commanded linear velocity of an + /// entity represented by ignition::math::Vector3d, expressed in the entity's + /// frame. using LinearVelocityCmd = Component< math::Vector3d, class LinearVelocityCmdTag>; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.LinearVelocityCmd", LinearVelocityCmd) - /// \brief A component type that contains linear velocity of an entity in the - /// world frame represented by ignition::math::Vector3d. + /// \brief A component type that contains the commanded linear velocity of an + /// entity represented by ignition::math::Vector3d, expressed in the world + /// frame. using WorldLinearVelocityCmd = Component; IGN_GAZEBO_REGISTER_COMPONENT( diff --git a/src/Model.cc b/src/Model.cc index 6af4d332b3..a3a445a6e0 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -20,6 +20,11 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/SelfCollide.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/Model.hh" class ignition::gazebo::ModelPrivate @@ -81,6 +86,56 @@ std::string Model::Name(const EntityComponentManager &_ecm) const return ""; } +////////////////////////////////////////////////// +Entity Model::Parent(const EntityComponentManager &_ecm) const +{ + auto comp = _ecm.Component(this->dataPtr->id); + if (comp) + return comp->Data(); + + return kNullEntity; +} + +////////////////////////////////////////////////// +bool Model::Static(const EntityComponentManager &_ecm) const +{ + auto comp = _ecm.Component(this->dataPtr->id); + if (comp) + return comp->Data(); + + return false; +} + +////////////////////////////////////////////////// +bool Model::SelfCollide(const EntityComponentManager &_ecm) const +{ + auto comp = _ecm.Component(this->dataPtr->id); + if (comp) + return comp->Data(); + + return false; +} + +////////////////////////////////////////////////// +bool Model::WindMode(const EntityComponentManager &_ecm) const +{ + auto comp = _ecm.Component(this->dataPtr->id); + if (comp) + return comp->Data(); + + return false; +} + +////////////////////////////////////////////////// +std::string Model::SourceFilePath(const EntityComponentManager &_ecm) const +{ + auto comp = _ecm.Component(this->dataPtr->id); + if (comp) + return comp->Data(); + + return ""; +} + ////////////////////////////////////////////////// Entity Model::JointByName(const EntityComponentManager &_ecm, const std::string &_name) @@ -101,3 +156,50 @@ Entity Model::LinkByName(const EntityComponentManager &_ecm, components::Link()); } +////////////////////////////////////////////////// +std::vector Model::Joints(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Joint()); +} + +////////////////////////////////////////////////// +std::vector Model::Links(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Link()); +} + +////////////////////////////////////////////////// +uint64_t Model::JointCount(const EntityComponentManager &_ecm) const +{ + return this->Joints(_ecm).size(); +} + +////////////////////////////////////////////////// +uint64_t Model::LinkCount(const EntityComponentManager &_ecm) const +{ + return this->Links(_ecm).size(); +} + +////////////////////////////////////////////////// +void Model::SetWorldPoseCmd(EntityComponentManager &_ecm, + const math::Pose3d &_pose) +{ + auto poseCmdComp = _ecm.Component( + this->dataPtr->id); + if (!poseCmdComp) + { + _ecm.CreateComponent(this->dataPtr->id, components::WorldPoseCmd(_pose)); + } + else + { + poseCmdComp->SetData(_pose, + [](const math::Pose3d &, const math::Pose3d &){return false;}); + _ecm.SetChanged(this->dataPtr->id, + components::WorldPoseCmd::typeId, ComponentState::OneTimeChange); + } +} + diff --git a/src/Util.cc b/src/Util.cc index 4e655ccf3f..1477af29a6 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -25,6 +25,7 @@ #include #include +#include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Light.hh" @@ -84,47 +85,14 @@ std::string scopedName(const Entity &_entity, auto name = nameComp->Data(); // Get entity type - std::string prefix; - if (_ecm.Component(entity)) - { - prefix = "world"; - } - else if (_ecm.Component(entity)) - { - prefix = "model"; - } - else if (_ecm.Component(entity)) - { - prefix = "light"; - } - else if (_ecm.Component(entity)) - { - prefix = "link"; - } - else if (_ecm.Component(entity)) - { - prefix = "collision"; - } - else if (_ecm.Component(entity)) - { - prefix = "visual"; - } - else if (_ecm.Component(entity)) - { - prefix = "joint"; - } - else if (_ecm.Component(entity)) - { - prefix = "sensor"; - } - else + std::string prefix = entityTypeStr(entity, _ecm); + if (prefix.empty()) { ignwarn << "Skipping entity [" << name << "] when generating scoped name, entity type not known." << std::endl; } - auto parentComp = _ecm.Component(entity); if (!prefix.empty()) { @@ -148,6 +116,117 @@ std::string scopedName(const Entity &_entity, return result; } +////////////////////////////////////////////////// +ComponentTypeId entityTypeId(const Entity &_entity, + const EntityComponentManager &_ecm) +{ + ComponentTypeId type{kComponentTypeIdInvalid}; + + if (_ecm.Component(_entity)) + { + type = components::World::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Model::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Light::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Link::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Collision::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Visual::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Joint::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Sensor::typeId; + } + else if (_ecm.Component(_entity)) + { + type = components::Actor::typeId; + } + + return type; +} + +////////////////////////////////////////////////// +std::string entityTypeStr(const Entity &_entity, + const EntityComponentManager &_ecm) +{ + std::string type; + + if (_ecm.Component(_entity)) + { + type = "world"; + } + else if (_ecm.Component(_entity)) + { + type = "model"; + } + else if (_ecm.Component(_entity)) + { + type = "light"; + } + else if (_ecm.Component(_entity)) + { + type = "link"; + } + else if (_ecm.Component(_entity)) + { + type = "collision"; + } + else if (_ecm.Component(_entity)) + { + type = "visual"; + } + else if (_ecm.Component(_entity)) + { + type = "joint"; + } + else if (_ecm.Component(_entity)) + { + type = "sensor"; + } + else if (_ecm.Component(_entity)) + { + type = "actor"; + } + + return type; +} + +////////////////////////////////////////////////// +Entity worldEntity(const Entity &_entity, + const EntityComponentManager &_ecm) +{ + auto entity = _entity; + while (nullptr == _ecm.Component(entity)) + { + // Keep going up the tree + auto parentComp = _ecm.Component(entity); + if (!parentComp) + { + entity = kNullEntity; + break; + } + entity = parentComp->Data(); + } + return entity; +} + ////////////////////////////////////////////////// std::string removeParentScope(const std::string &_name, const std::string &_delim) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index ebb6865706..525abe539c 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -16,8 +16,10 @@ */ #include +#include #include +#include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Light.hh" @@ -47,14 +49,16 @@ TEST(UtilTest, ScopedName) // - sensorB // - modelC // - linkC - // - collision + // - collisionC // - visualC // - jointC // - modelCC // - linkCC + // - actorD // World auto worldEntity = ecm.CreateEntity(); + EXPECT_EQ(kNullEntity, gazebo::worldEntity(worldEntity, ecm)); ecm.CreateComponent(worldEntity, components::World()); ecm.CreateComponent(worldEntity, components::Name("world_name")); @@ -130,6 +134,12 @@ TEST(UtilTest, ScopedName) ecm.CreateComponent(linkCCEntity, components::Name("linkCC_name")); ecm.CreateComponent(linkCCEntity, components::ParentEntity(modelCCEntity)); + // Actor D + auto actorDEntity = ecm.CreateEntity(); + ecm.CreateComponent(actorDEntity, components::Actor(sdf::Actor())); + ecm.CreateComponent(actorDEntity, components::Name("actorD_name")); + ecm.CreateComponent(actorDEntity, components::ParentEntity(worldEntity)); + // Check names EXPECT_EQ(scopedName(worldEntity, ecm), "world/world_name"); EXPECT_EQ(scopedName(lightAEntity, ecm, "::"), @@ -158,6 +168,8 @@ TEST(UtilTest, ScopedName) "world/world_name/model/modelC_name/model/modelCC_name"); EXPECT_EQ(scopedName(linkCCEntity, ecm), "world/world_name/model/modelC_name/model/modelCC_name/link/linkCC_name"); + EXPECT_EQ(scopedName(actorDEntity, ecm, "::"), + "world::world_name::actor::actorD_name"); // check name without prefix EXPECT_EQ(scopedName(worldEntity, ecm, "/", false), "world_name"); @@ -185,6 +197,115 @@ TEST(UtilTest, ScopedName) "world_name/modelC_name/modelCC_name"); EXPECT_EQ(scopedName(linkCCEntity, ecm, "/", false), "world_name/modelC_name/modelCC_name/linkCC_name"); + EXPECT_EQ(scopedName(actorDEntity, ecm, "::", false), + "world_name::actorD_name"); + + // World entity + EXPECT_EQ(worldEntity, gazebo::worldEntity(worldEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(lightAEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(modelBEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(linkBEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(lightBEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(sensorBEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(modelCEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(linkCEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(collisionCEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(visualCEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(jointCEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(modelCCEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(linkCCEntity, ecm)); + EXPECT_EQ(worldEntity, gazebo::worldEntity(actorDEntity, ecm)); + EXPECT_EQ(kNullEntity, gazebo::worldEntity(kNullEntity, ecm)); +} + +///////////////////////////////////////////////// +TEST(UtilTest, EntityTypeId) +{ + EntityComponentManager ecm; + + auto entity = ecm.CreateEntity(); + EXPECT_EQ(kComponentTypeIdInvalid, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::World()); + EXPECT_EQ(components::World::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Model()); + EXPECT_EQ(components::Model::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Light()); + EXPECT_EQ(components::Light::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Link()); + EXPECT_EQ(components::Link::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Visual()); + EXPECT_EQ(components::Visual::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Collision()); + EXPECT_EQ(components::Collision::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Joint()); + EXPECT_EQ(components::Joint::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Sensor()); + EXPECT_EQ(components::Sensor::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Actor()); + EXPECT_EQ(components::Actor::typeId, entityTypeId(entity, ecm)); +} + +///////////////////////////////////////////////// +TEST(UtilTest, EntityTypeStr) +{ + EntityComponentManager ecm; + + auto entity = ecm.CreateEntity(); + EXPECT_TRUE(entityTypeStr(entity, ecm).empty()); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::World()); + EXPECT_EQ("world", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Model()); + EXPECT_EQ("model", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Light()); + EXPECT_EQ("light", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Link()); + EXPECT_EQ("link", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Visual()); + EXPECT_EQ("visual", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Collision()); + EXPECT_EQ("collision", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Joint()); + EXPECT_EQ("joint", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Sensor()); + EXPECT_EQ("sensor", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Actor()); + EXPECT_EQ("actor", entityTypeStr(entity, ecm)); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index df66dda105..2f66e9903d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -48,7 +48,9 @@ #include "ignition/gazebo/components/PerformerAffinity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/WindMode.hh" @@ -506,6 +508,13 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::SelfCollide::typeId) + { + auto comp = + _ecm.Component(this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::SensorTopic::typeId) { auto comp = @@ -513,6 +522,13 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::SourceFilePath::typeId) + { + auto comp = + _ecm.Component(this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::WindMode::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); diff --git a/src/gui/plugins/component_inspector/String.qml b/src/gui/plugins/component_inspector/String.qml index 8a35f62b6a..974ad846d1 100644 --- a/src/gui/plugins/component_inspector/String.qml +++ b/src/gui/plugins/component_inspector/String.qml @@ -58,15 +58,28 @@ Rectangle { id: typeHeader } - TextInput { + // TODO(anyone) Support write mode + Text { id: content text: textFromModel(model) Layout.fillWidth: true horizontalAlignment: Text.AlignRight color: Material.theme == Material.Light ? "black" : "white" font.pointSize: 12 - selectByMouse: true // will only work when we enable it - enabled: false + elide: Text.ElideLeft + + ToolTip { + visible: ma.containsMouse + delay: Qt.styleHints.mousePressAndHoldInterval + text: content.text + enter: null + exit: null + } + MouseArea { + id: ma + anchors.fill: content + hoverEnabled: true + } } Item { diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index d5714c39cf..763026945c 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1409,6 +1409,15 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) this->entityWorldVelocityCommandMap); if (!worldAngularVelFeature) { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set model angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } return true; } @@ -1440,6 +1449,15 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) this->entityWorldVelocityCommandMap); if (!worldLinearVelFeature) { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set model linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } return true; } diff --git a/test/integration/model.cc b/test/integration/model.cc index bcd10db876..4c4fc3c21b 100644 --- a/test/integration/model.cc +++ b/test/integration/model.cc @@ -18,15 +18,23 @@ #include #include +#include + +#include +#include #include #include #include #include #include -#include -#include +#include +#include +#include +#include +#include -using namespace ignition::gazebo; +using namespace ignition; +using namespace gazebo; class ModelIntegrationTest : public ::testing::Test { @@ -82,6 +90,99 @@ TEST_F(ModelIntegrationTest, Name) EXPECT_EQ("model_name", model.Name(ecm)); } +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, Parent) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Model()); + + Model model(id); + + // No parent + EXPECT_EQ(kNullEntity, model.Parent(ecm)); + + // Add parent + ecm.CreateComponent(id, + components::ParentEntity(100u)); + EXPECT_EQ(100u, model.Parent(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, Static) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Model()); + + Model model(id); + + // Not static + EXPECT_FALSE(model.Static(ecm)); + + // Make static + ecm.CreateComponent(id, components::Static(true)); + EXPECT_TRUE(model.Static(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, SelfCollide) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Model()); + + Model model(id); + + // Not self collide + EXPECT_FALSE(model.SelfCollide(ecm)); + + // Make self collide + ecm.CreateComponent(id, + components::SelfCollide(true)); + EXPECT_TRUE(model.SelfCollide(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, WindMode) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Model()); + + Model model(id); + + // Not static + EXPECT_FALSE(model.WindMode(ecm)); + + // Make static + ecm.CreateComponent(id, components::WindMode(true)); + EXPECT_TRUE(model.WindMode(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, SourceFilePath) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Model()); + + Model model(id); + + // No path + EXPECT_TRUE(model.SourceFilePath(ecm).empty()); + + // Add path + ecm.CreateComponent(id, + components::SourceFilePath("/tmp/path")); + EXPECT_EQ("/tmp/path", model.SourceFilePath(ecm)); +} + ////////////////////////////////////////////////// TEST_F(ModelIntegrationTest, LinkByName) { @@ -91,6 +192,7 @@ TEST_F(ModelIntegrationTest, LinkByName) auto eModel = ecm.CreateEntity(); Model model(eModel); EXPECT_EQ(eModel, model.Entity()); + EXPECT_EQ(0u, model.LinkCount(ecm)); // Link auto eLink = ecm.CreateEntity(); @@ -102,6 +204,7 @@ TEST_F(ModelIntegrationTest, LinkByName) // Check model EXPECT_EQ(eLink, model.LinkByName(ecm, "link_name")); + EXPECT_EQ(1u, model.LinkCount(ecm)); } ////////////////////////////////////////////////// @@ -113,6 +216,7 @@ TEST_F(ModelIntegrationTest, JointByName) auto eModel = ecm.CreateEntity(); Model model(eModel); EXPECT_EQ(eModel, model.Entity()); + EXPECT_EQ(0u, model.JointCount(ecm)); // Joint auto eJoint = ecm.CreateEntity(); @@ -124,5 +228,27 @@ TEST_F(ModelIntegrationTest, JointByName) // Check model EXPECT_EQ(eJoint, model.JointByName(ecm, "joint_name")); + EXPECT_EQ(1u, model.JointCount(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, SetWorldPoseCmd) +{ + EntityComponentManager ecm; + + // Model + auto eModel = ecm.CreateEntity(); + Model model(eModel); + + auto worldPoseCmdComp = ecm.Component(eModel); + EXPECT_EQ(nullptr, worldPoseCmdComp); + EXPECT_FALSE(ecm.HasOneTimeComponentChanges()); + + model.SetWorldPoseCmd(ecm, math::Pose3d(1, 2, 3, 0, 0, 0)); + + worldPoseCmdComp = ecm.Component(eModel); + ASSERT_NE(nullptr, worldPoseCmdComp); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), worldPoseCmdComp->Data()); + EXPECT_TRUE(ecm.HasOneTimeComponentChanges()); } diff --git a/tutorials.md.in b/tutorials.md.in index fd462e2f86..d6ace40092 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -11,8 +11,6 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage createsystemplugins "Create System Plugins": Programmatically access simulation using C++ plugins * \subpage levels "Levels": Load entities on demand in large environments * \subpage distributedsimulation "Distributed Simulation": Spread simulation across several processes -* \subpage migrationplugins "Migration from Gazebo-classic: plugins": Walk through the differences between writing plugins for Gazebo-classic and Ignition Gazebo -* \subpage ardupilot "Migration Case Study": Migrating the ArduPilot ModelPlugin from Classic Gazebo to Ignition Gazebo. * \subpage resources "Finding resources": The different ways in which Ignition looks for files * \subpage log "Logging": Record and play back time series of world state. * \subpage physics "Physics engines": Loading different physics engines. @@ -24,6 +22,12 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage detachablejoints "Detachable Joints": Creating models that start off rigidly attached and then get detached during simulation * \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation +**Migration from Gazebo-classic** + +* \subpage migrationplugins "Plugins": Walk through the differences between writing plugins for Gazebo-classic and Ignition Gazebo +* \subpage migrationmodelapi "Model API": Guide on what Model C++ functions to call in Ignition Gazebo when migrating from Gazebo-classic +* \subpage ardupilot "Case Study": Migrating the ArduPilot ModelPlugin from Classic Gazebo to Ignition Gazebo. + ## License The code associated with this documentation is licensed under an [Apache 2.0 License](https://www.apache.org/licenses/LICENSE-2.0). diff --git a/tutorials/migration_model_api.md b/tutorials/migration_model_api.md new file mode 100644 index 0000000000..fced536cdd --- /dev/null +++ b/tutorials/migration_model_api.md @@ -0,0 +1,205 @@ +\page migrationmodelapi + +# Migration from Gazebo-classic: Model API + +When migrating plugins from Gazebo-classic to Ignition Gazebo, developers will +notice that the C++ APIs for both simulators are quite different. Be sure to +check the [plugin migration tutorial](migrationplugins.html) to get a high-level +view of the architecture differences before using this guide. + +This tutorial is meant to serve as a reference guide for developers migrating +functions from the +[gazebo::physics::Model](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Model.html) +class. + +If you're trying to use some API which doesn't have an equivalent on Ignition +yet, feel free to +[ticket an issue](https://github.com/ignitionrobotics/ign-gazebo/issues/). + +## Model API + +Gazebo-classic's `gazebo::physics::Model` provides lots of functionality, which +can be divided in these categories: + +* **Properties**: Setting / getting properties + * Example: [Model::GetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#a9a98946a64f3893b085f650932c9dfee) / [Model::SetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a5d74ac4d7a230aed1ab4b11933b16e92) +* **Read family**: Getting children and parent + * Example: [Model::GetLink](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Model.html#a7a923baddd29445b23740ca06e153c06) +* **Write family**: Adding children, changing parent + * Example: [Model::RemoveChildren](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#aa85d2386e6fb02bdbec060a74b63238a) +* **Lifecycle**: Functions to control the model's lifecycle + * Example: [Model::Init](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Model.html#ae048ef824aaf614707c1496a2aefd415) +* **Others**: Functions that don't fit any of the categories above + * Example: [Model::PlaceOnEntity](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a9ecbfeb56940cacd75f55bed6aa9fcb4) + +You'll find the Ignition APIs below on the following headers: + +* [ignition/gazebo/Model.hh](https://ignitionrobotics.org/api/gazebo/3.3/Model_8hh.html) +* [ignition/gazebo/Util.hh](https://ignitionrobotics.org/api/gazebo/3.3/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://ignitionrobotics.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) + +It's worth remembering that most of this functionality can be performed using +the +[EntityComponentManager](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +directly. The functions presented here exist for convenience and readability. + +### Properties + +Most of Gazebo-classic's Model API is related to setting and getting +properties. These functions are great candidates to have equivalents on Ignition +Gazebo, because the Entity-Component-System architecture is perfect for setting +components (properties) into entities such as models. + +Classic | Ignition +-- | -- +AddType | `ecm.CreateComponent(entity, Type())` +BoundingBox | +CollisionBoundingBox | +DirtyPose | Not supported +FillMsg | +GetAutoDisable | +GetId | `ignition::gazebo::Model::Entity` +GetName | `ignition::gazebo::Model::Name` +GetPluginCount | +GetSDF | +GetSDFDom | +GetSaveable | Not supported +GetScopedName | `ignition::gazebo::scopedName` +GetSelfCollide | `ignition::gazebo::Model::SelfCollide` +GetType | `ignition::gazebo::entityType` +GetWorldEnergy | +GetWorldEnergyKinetic | +GetWorldEnergyPotential | +HasType | `ignition::gazebo::isType` +InitialRelativePose | +IsCanonicalLink | See link API +IsSelected | Selection is client-specific, not porting +IsStatic | `ignition::gazebo::Model::Static` +PluginInfo | +Print | +ProcessMsg | +RelativeAngularAccel | +RelativeAngularVel | +RelativeLinearAccel | +RelativeLinearVel | +RelativePose | +SDFPoseRelativeToParent | +SDFSemanticPose | +Scale | +SensorScopedName | +SetAngularVel | +SetAnimation | +SetAutoDisable | +SetCollideMode | +SetEnabled | +SetGravityMode | +SetInitialRelativePose | +SetJointAnimation | +SetJointPosition | See joint API +SetJointPositions | See joint API +SetLaserRetro | +SetLinearVel | +SetLinkWorldPose | See link API +SetName | +SetRelativePose | +SetSaveable | Not supported +SetScale | +SetSelected | Selection is client-specific, not porting +SetSelfCollide | +SetState | +SetStatic | +SetWindMode | +SetWorldPose | `ignition::gazebo::Model::SetWorldPoseCmd` +SetWorldTwist | +StopAnimation | +TypeStr | `ignition::gazebo::entityTypeStr` +URI | +UnscaledSDF | +UpdateParameters | +WindMode | `ignition::gazebo::Model::WindMode` +WorldAngularAccel | +WorldAngularVel | +WorldLinearAccel | +WorldLinearVel | +WorldPose | `ignition::gazebo::worldPose` + + +## Read family + +These APIs deal with reading information related to child / parent +relationships. + +The main difference in these APIs across Gazebo generations is that +on classic, they deal with shared pointers to entities, while on Ignition, +they deal with entity IDs. + +Classic | Ignition +-- | -- +GetByName | Use type-specific `ignition::gazebo::Model::*ByName` +GetChild | Use type-specific `ignition::gazebo::Model::*ByName` +GetChildCollision | See link API +GetChildCount | Use type-specific `ignition::gazebo::Model::*Count` +GetChildLink | `ignition::gazebo::Model::LinkByName` +GetGripper | +GetGripperCount | +GetJoint | `ignition::gazebo::Model::JointByName` +GetJointCount | `ignition::gazebo::Model::JointCount` +GetJoints | `ignition::gazebo::Model::Joints` +GetLink | `ignition::gazebo::Model::LinkByName` +GetLinks | const `ignition::gazebo::Model::Links` +GetParent | `ignition::gazebo::Model::Parent` +GetParentId | `ignition::gazebo::Model::Parent` +GetParentModel | `ignition::gazebo::Model::Parent` +GetSensorCount | See link API +GetWorld | const `ignition::gazebo::Model::World` +NestedModel | `ignition::gazebo::Model::NestedModelByName` +NestedModels | const `ignition::gazebo::Model::NestedModels` + + +## Write family + +These functions deal with modifying the entity tree, attaching children to new +parents. + +Classic | Ignition +-- | -- +AddChild | +AttachStaticModel | +CreateJoint | +CreateLink | +DetachStaticModel | +RemoveChild | +RemoveChildren | +RemoveJoint | +SetCanonicalLink | +SetParent | +SetWorld | + +## Lifecycle + +These functions aren't related to the state of a model, but perform some +processing related to the model's lifecycle, like initializing, updating or +terminating it. + +Classic | Ignition +-- | -- +Fini | N/A +Init | N/A +Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` +LoadJoints | `ignition::gazebo::SdfEntityCreator::CreateEntities` +LoadPlugins | +Reset | +ResetPhysicsStates | +Update | Entities are updated by systems + +## Others + +Miscelaneous functions that don't fit the other categories. Most of them involve +logic that should be performed from within a system. + +Classic | Ignition +-- | -- +GetJointController | Use this system: `ignition::gazebo::systems::JointController` +GetNearestEntityBelow | Requires a system +PlaceOnEntity | Involves Requires a system +PlaceOnNearestEntityBelow | Requires a system