From fb4a177d13230fa2ebbcf1baa6d14f812770e18a Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Thu, 1 Apr 2021 17:04:47 -0400 Subject: [PATCH] Add a "ReferenceModels" component Signed-off-by: Ashton Larkin --- .../gazebo/components/ReferenceModels.hh | 122 +++++++ src/SdfEntityCreator.cc | 16 + test/integration/components.cc | 317 +++++++++++------- 3 files changed, 335 insertions(+), 120 deletions(-) create mode 100644 include/ignition/gazebo/components/ReferenceModels.hh diff --git a/include/ignition/gazebo/components/ReferenceModels.hh b/include/ignition/gazebo/components/ReferenceModels.hh new file mode 100644 index 00000000000..cfc2b95ec43 --- /dev/null +++ b/include/ignition/gazebo/components/ReferenceModels.hh @@ -0,0 +1,122 @@ +/* + * 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_REFERENCEMODELS_HH_ +#define IGNITION_GAZEBO_COMPONENTS_REFERENCEMODELS_HH_ + +#include +#include + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief Data structure that holds information about which models for + /// a given link view this link as the model's canonical link. An example + /// of multiple models having the same canonical link could be nested models. + struct ReferenceModelsInfo + { + /// \brief The models which this link serves as a canonical link for. + std::set models; + + /// \brief Add a model that references this link as its canonical link. + /// This should be called when a canonical link is assigned to a model. + /// \param[in] _model The model to be added. + public: void AddModel(const Entity &_model) + { + this->models.insert(_model); + } + + /// \brief Remove a model that no longer references this link as its + /// canonical link. This should be called when a model is removed/deleted. + /// \param[in] _model The model to be removed. + public: void RemoveModel(const Entity &_model) + { + this->models.erase(_model); + } + + public: bool operator==(const ReferenceModelsInfo &_info) const + { + return this->models == _info.models; + } + + public: bool operator!=(const ReferenceModelsInfo &_info) const + { + return !(*this == _info); + } + }; +} + +namespace serializers +{ + /// \brief Serializer for ReferenceModelsInfo object + class ReferenceModelsInfoSerializer + { + /// \brief Serialization for ReferenceModelsInfo. + /// \param[out] _out Output stream. + /// \param[in] _info ReferenceModelsInfo object to stream. + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const components::ReferenceModelsInfo &_info) + { + for (const auto &model : _info.models) + _out << model << " "; + + return _out; + } + + /// \brief Deserialization for ReferenceModelsInfo. + /// \param[in] _in Input stream. + /// \param[out] _info ReferenceModelsInfo object to populate. + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + components::ReferenceModelsInfo &_info) + { + _info.models.clear(); + Entity model; + while (_in >> model) + _info.AddModel(model); + + return _in; + }; + }; +} + +namespace components +{ + /// \brief A component that gives a mapping between a link and all of the + /// models this link serves as a canonical link for. This component should + /// only be applied to links. + using ReferenceModels = + Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ReferenceModels", + ReferenceModels) +} +} +} +} + +#endif diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 91a8aed6453..e94858f381a 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -59,6 +59,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/ReferenceModels.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" #include "ignition/gazebo/components/SelfCollide.hh" @@ -405,6 +406,21 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model, { this->dataPtr->ecm->CreateComponent( modelEntity, components::ModelCanonicalLink(canonicalLinkEntity)); + + // Map this canonical link to the model. Since a link may be the + // canonical link for multiple models (example: nested models), create + // a component that completes this mapping if it doesn't already exist. + if (!this->dataPtr->ecm->Component( + canonicalLinkEntity)) + { + components::ReferenceModelsInfo compInfo; + this->dataPtr->ecm->CreateComponent(canonicalLinkEntity, + components::ReferenceModels(compInfo)); + } + auto refModelComp = + this->dataPtr->ecm->Component( + canonicalLinkEntity); + refModelComp->Data().AddModel(modelEntity); } else { diff --git a/test/integration/components.cc b/test/integration/components.cc index 6143c1e0afc..c692ae675a0 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -70,6 +70,7 @@ #include "ignition/gazebo/components/PerformerLevels.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/ReferenceModels.hh" #include "ignition/gazebo/components/Scene.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" @@ -1192,6 +1193,98 @@ TEST_F(ComponentsTest, ParentLinkName) EXPECT_EQ("comp3", comp3.Data()); } +////////////////////////////////////////////////// +TEST_F(ComponentsTest, ParticleEmitter) +{ + msgs::ParticleEmitter emitter1; + emitter1.set_name("emitter1"); + emitter1.set_id(0); + emitter1.set_type(ignition::msgs::ParticleEmitter_EmitterType_BOX); + emitter1.mutable_size()->set_x(1); + emitter1.mutable_size()->set_y(2); + emitter1.mutable_size()->set_z(3); + emitter1.mutable_rate()->set_data(4.0); + emitter1.mutable_duration()->set_data(5.0); + emitter1.mutable_emitting()->set_data(false); + emitter1.mutable_particle_size()->set_x(0.1); + emitter1.mutable_particle_size()->set_y(0.2); + emitter1.mutable_particle_size()->set_z(0.3); + emitter1.mutable_lifetime()->set_data(6.0); + emitter1.mutable_min_velocity()->set_data(7.0); + emitter1.mutable_max_velocity()->set_data(8.0); + emitter1.mutable_color_start()->set_r(1.0); + emitter1.mutable_color_start()->set_g(0); + emitter1.mutable_color_start()->set_b(0); + emitter1.mutable_color_start()->set_a(0); + emitter1.mutable_color_end()->set_r(1.0); + emitter1.mutable_color_end()->set_g(1.0); + emitter1.mutable_color_end()->set_b(1.0); + emitter1.mutable_color_end()->set_a(0); + emitter1.mutable_scale_rate()->set_data(9.0); + emitter1.mutable_color_range_image()->set_data("path_to_texture"); + + msgs::ParticleEmitter emitter2; + emitter2.set_name("emitter2"); + emitter2.set_id(1); + emitter2.set_type(ignition::msgs::ParticleEmitter_EmitterType_BOX); + emitter2.mutable_size()->set_x(1); + emitter2.mutable_size()->set_y(2); + emitter2.mutable_size()->set_z(3); + emitter2.mutable_rate()->set_data(4.0); + emitter2.mutable_duration()->set_data(5.0); + emitter2.mutable_emitting()->set_data(false); + emitter2.mutable_particle_size()->set_x(0.1); + emitter2.mutable_particle_size()->set_y(0.2); + emitter2.mutable_particle_size()->set_z(0.3); + emitter2.mutable_lifetime()->set_data(6.0); + emitter2.mutable_min_velocity()->set_data(7.0); + emitter2.mutable_max_velocity()->set_data(8.0); + emitter2.mutable_color_start()->set_r(1.0); + emitter2.mutable_color_start()->set_g(0); + emitter2.mutable_color_start()->set_b(0); + emitter2.mutable_color_start()->set_a(0); + emitter2.mutable_color_end()->set_r(1.0); + emitter2.mutable_color_end()->set_g(1.0); + emitter2.mutable_color_end()->set_b(1.0); + emitter2.mutable_color_end()->set_a(0); + emitter2.mutable_scale_rate()->set_data(9.0); + emitter2.mutable_color_range_image()->set_data("path_to_texture"); + + // Create components. + auto comp1 = components::ParticleEmitter(emitter1); + auto comp2 = components::ParticleEmitter(emitter2); + + // Stream operators. + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + components::ParticleEmitter comp3; + comp3.Deserialize(istr); + EXPECT_EQ(comp1.Data().id(), comp3.Data().id()); +} + +////////////////////////////////////////////////// +TEST_F(ComponentsTest, ParticleEmitterCmd) +{ + msgs::ParticleEmitter emitter1; + emitter1.set_name("emitter1"); + emitter1.mutable_emitting()->set_data(true); + + // Create components. + auto comp1 = components::ParticleEmitterCmd(emitter1); + + // Stream operators. + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + components::ParticleEmitter comp3; + comp3.Deserialize(istr); + EXPECT_EQ(comp1.Data().emitting().data(), comp3.Data().emitting().data()); + EXPECT_EQ(comp1.Data().name(), comp3.Data().name()); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, Performer) { @@ -1315,6 +1408,110 @@ TEST_F(ComponentsTest, Pose) EXPECT_EQ(math::Pose3d(3, 2, 1, 0.3, 0.2, 0.1), comp3.Data()); } +////////////////////////////////////////////////// +TEST_F(ComponentsTest, ReferenceModels) +{ + components::ReferenceModelsInfo linkInfo1; + linkInfo1.AddModel(1); + linkInfo1.AddModel(2); + linkInfo1.AddModel(3); + EXPECT_EQ(3u, linkInfo1.models.size()); + EXPECT_TRUE(linkInfo1.models.find(1) != linkInfo1.models.end()); + EXPECT_TRUE(linkInfo1.models.find(2) != linkInfo1.models.end()); + EXPECT_TRUE(linkInfo1.models.find(3) != linkInfo1.models.end()); + + // test adding a model that already exists + linkInfo1.AddModel(1); + EXPECT_EQ(3u, linkInfo1.models.size()); + EXPECT_TRUE(linkInfo1.models.find(1) != linkInfo1.models.end()); + + linkInfo1.RemoveModel(3); + EXPECT_EQ(2u, linkInfo1.models.size()); + EXPECT_TRUE(linkInfo1.models.find(3) == linkInfo1.models.end()); + // test removing a model that doesn't exist + linkInfo1.RemoveModel(5); + EXPECT_EQ(2u, linkInfo1.models.size()); + EXPECT_TRUE(linkInfo1.models.find(5) == linkInfo1.models.end()); + + // linkInfo1 should now only reference models 1 and 2 + EXPECT_EQ(2u, linkInfo1.models.size()); + EXPECT_TRUE(linkInfo1.models.find(1) != linkInfo1.models.end()); + EXPECT_TRUE(linkInfo1.models.find(2) != linkInfo1.models.end()); + + components::ReferenceModelsInfo linkInfo2; + linkInfo2.AddModel(1); + linkInfo2.AddModel(2); + linkInfo2.AddModel(3); + EXPECT_EQ(3u, linkInfo2.models.size()); + + components::ReferenceModelsInfo linkInfo3; + linkInfo3.AddModel(1); + linkInfo3.AddModel(2); + EXPECT_EQ(2u, linkInfo3.models.size()); + + // create components + auto comp1 = components::ReferenceModels(linkInfo1); + auto comp2 = components::ReferenceModels(linkInfo2); + auto comp3 = components::ReferenceModels(linkInfo3); + + // equality operators + EXPECT_NE(comp1, comp2); + EXPECT_FALSE(comp1 == comp2); + EXPECT_TRUE(comp1 != comp2); + EXPECT_NE(comp2, comp3); + EXPECT_FALSE(comp2 == comp3); + EXPECT_TRUE(comp2 != comp3); + EXPECT_EQ(comp1, comp3); + EXPECT_TRUE(comp1 == comp3); + EXPECT_FALSE(comp1 != comp3); + + // stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("1 2 ", ostr.str()); + + std::istringstream istr(ostr.str()); + components::ReferenceModels comp4; + comp4.Deserialize(istr); + EXPECT_EQ(comp1, comp4); + + std::ostringstream ostr2; + comp2.Serialize(ostr2); + EXPECT_EQ("1 2 3 ", ostr2.str()); + + std::istringstream istr2(ostr2.str()); + comp4.Deserialize(istr2); + EXPECT_EQ(comp2, comp4); +} + +///////////////////////////////////////////////// +TEST_F(ComponentsTest, Scene) +{ + auto data1 = sdf::Scene(); + data1.SetAmbient(math::Color(1, 0, 1, 1)); + data1.SetBackground(math::Color(1, 1, 0, 1)); + data1.SetShadows(true); + data1.SetGrid(false); + data1.SetOriginVisual(true); + + // Create components + auto comp11 = components::Scene(data1); + + // TODO(anyone) Equality operators + + // Stream operators + std::ostringstream ostr; + comp11.Serialize(ostr); + std::istringstream istr(ostr.str()); + components::Scene comp3; + comp3.Deserialize(istr); + EXPECT_EQ(math::Color(1, 0, 1, 1), comp3.Data().Ambient()); + EXPECT_EQ(math::Color(1, 1, 0, 1), comp3.Data().Background()); + EXPECT_TRUE(comp3.Data().Shadows()); + EXPECT_FALSE(comp3.Data().Grid()); + EXPECT_TRUE(comp3.Data().OriginVisual()); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, Sensor) { @@ -1482,123 +1679,3 @@ TEST_F(ComponentsTest, World) components::World comp3; comp3.Deserialize(istr); } - -///////////////////////////////////////////////// -TEST_F(ComponentsTest, Scene) -{ - auto data1 = sdf::Scene(); - data1.SetAmbient(math::Color(1, 0, 1, 1)); - data1.SetBackground(math::Color(1, 1, 0, 1)); - data1.SetShadows(true); - data1.SetGrid(false); - data1.SetOriginVisual(true); - - // Create components - auto comp11 = components::Scene(data1); - - // TODO(anyone) Equality operators - - // Stream operators - std::ostringstream ostr; - comp11.Serialize(ostr); - std::istringstream istr(ostr.str()); - components::Scene comp3; - comp3.Deserialize(istr); - EXPECT_EQ(math::Color(1, 0, 1, 1), comp3.Data().Ambient()); - EXPECT_EQ(math::Color(1, 1, 0, 1), comp3.Data().Background()); - EXPECT_TRUE(comp3.Data().Shadows()); - EXPECT_FALSE(comp3.Data().Grid()); - EXPECT_TRUE(comp3.Data().OriginVisual()); -} - -////////////////////////////////////////////////// -TEST_F(ComponentsTest, ParticleEmitter) -{ - msgs::ParticleEmitter emitter1; - emitter1.set_name("emitter1"); - emitter1.set_id(0); - emitter1.set_type(ignition::msgs::ParticleEmitter_EmitterType_BOX); - emitter1.mutable_size()->set_x(1); - emitter1.mutable_size()->set_y(2); - emitter1.mutable_size()->set_z(3); - emitter1.mutable_rate()->set_data(4.0); - emitter1.mutable_duration()->set_data(5.0); - emitter1.mutable_emitting()->set_data(false); - emitter1.mutable_particle_size()->set_x(0.1); - emitter1.mutable_particle_size()->set_y(0.2); - emitter1.mutable_particle_size()->set_z(0.3); - emitter1.mutable_lifetime()->set_data(6.0); - emitter1.mutable_min_velocity()->set_data(7.0); - emitter1.mutable_max_velocity()->set_data(8.0); - emitter1.mutable_color_start()->set_r(1.0); - emitter1.mutable_color_start()->set_g(0); - emitter1.mutable_color_start()->set_b(0); - emitter1.mutable_color_start()->set_a(0); - emitter1.mutable_color_end()->set_r(1.0); - emitter1.mutable_color_end()->set_g(1.0); - emitter1.mutable_color_end()->set_b(1.0); - emitter1.mutable_color_end()->set_a(0); - emitter1.mutable_scale_rate()->set_data(9.0); - emitter1.mutable_color_range_image()->set_data("path_to_texture"); - - msgs::ParticleEmitter emitter2; - emitter2.set_name("emitter2"); - emitter2.set_id(1); - emitter2.set_type(ignition::msgs::ParticleEmitter_EmitterType_BOX); - emitter2.mutable_size()->set_x(1); - emitter2.mutable_size()->set_y(2); - emitter2.mutable_size()->set_z(3); - emitter2.mutable_rate()->set_data(4.0); - emitter2.mutable_duration()->set_data(5.0); - emitter2.mutable_emitting()->set_data(false); - emitter2.mutable_particle_size()->set_x(0.1); - emitter2.mutable_particle_size()->set_y(0.2); - emitter2.mutable_particle_size()->set_z(0.3); - emitter2.mutable_lifetime()->set_data(6.0); - emitter2.mutable_min_velocity()->set_data(7.0); - emitter2.mutable_max_velocity()->set_data(8.0); - emitter2.mutable_color_start()->set_r(1.0); - emitter2.mutable_color_start()->set_g(0); - emitter2.mutable_color_start()->set_b(0); - emitter2.mutable_color_start()->set_a(0); - emitter2.mutable_color_end()->set_r(1.0); - emitter2.mutable_color_end()->set_g(1.0); - emitter2.mutable_color_end()->set_b(1.0); - emitter2.mutable_color_end()->set_a(0); - emitter2.mutable_scale_rate()->set_data(9.0); - emitter2.mutable_color_range_image()->set_data("path_to_texture"); - - // Create components. - auto comp1 = components::ParticleEmitter(emitter1); - auto comp2 = components::ParticleEmitter(emitter2); - - // Stream operators. - std::ostringstream ostr; - comp1.Serialize(ostr); - - std::istringstream istr(ostr.str()); - components::ParticleEmitter comp3; - comp3.Deserialize(istr); - EXPECT_EQ(comp1.Data().id(), comp3.Data().id()); -} - -////////////////////////////////////////////////// -TEST_F(ComponentsTest, ParticleEmitterCmd) -{ - msgs::ParticleEmitter emitter1; - emitter1.set_name("emitter1"); - emitter1.mutable_emitting()->set_data(true); - - // Create components. - auto comp1 = components::ParticleEmitterCmd(emitter1); - - // Stream operators. - std::ostringstream ostr; - comp1.Serialize(ostr); - - std::istringstream istr(ostr.str()); - components::ParticleEmitter comp3; - comp3.Deserialize(istr); - EXPECT_EQ(comp1.Data().emitting().data(), comp3.Data().emitting().data()); - EXPECT_EQ(comp1.Data().name(), comp3.Data().name()); -}