From 022e1692d459332403813263d69749e877ae1cd9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 29 Jun 2022 21:00:59 -0700 Subject: [PATCH 1/3] Add World::ShininessByScopedName, some refactoring This adds a method to refactor access to the world shininess map, to ensure that map entries aren't added during access by using at() isntead of []. Also add a mutex to protect the shininess map and fix a doc-string. Signed-off-by: Steve Peters --- gazebo/physics/Link.cc | 5 +---- gazebo/physics/World.cc | 14 +++++++++++++- gazebo/physics/World.hh | 9 ++++++++- gazebo/physics/WorldPrivate.hh | 4 ++++ 4 files changed, 26 insertions(+), 6 deletions(-) diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc index 60291cf472..4a1bbd3a32 100644 --- a/gazebo/physics/Link.cc +++ b/gazebo/physics/Link.cc @@ -1545,13 +1545,11 @@ void Link::UpdateVisualMsg() common::resolveSdfPose(visualDom->SemanticPose())); } bool newVis = true; - std::string linkName = this->GetScopedName(); + std::string visName = this->GetScopedName() + "::" + msg.name(); // update visual msg if it exists for (auto &iter : this->visuals) { - std::string visName = linkName + "::" + - visualElem->Get("name"); if (iter.second.name() == visName) { iter.second.mutable_geometry()->CopyFrom(msg.geometry()); @@ -1563,7 +1561,6 @@ void Link::UpdateVisualMsg() // add to visual msgs if not found. if (newVis) { - std::string visName = this->GetScopedName() + "::" + msg.name(); msg.set_name(visName); msg.set_id(physics::getUniqueId()); msg.set_parent_name(this->GetScopedName()); diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index 4c469114e2..63ced84d51 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -3467,11 +3467,23 @@ bool World::ShadowCasterRenderBackFacesService(ignition::msgs::Boolean &_res) return true; } +////////////////////////////////////////////////// +double World::ShininessByScopedName(const std::string &_scopedName) const +{ + std::lock_guard lock(this->dataPtr->materialShininessMutex); + if (this->dataPtr->materialShininessMap.find(_scopedName) != + this->dataPtr->materialShininessMap.end()) + { + return this->dataPtr->materialShininessMap.at(_scopedName); + } + return 0.0; +} + ////////////////////////////////////////////////// bool World::MaterialShininessService( const ignition::msgs::StringMsg &_req, msgs::Any &_res) { _res.set_type(msgs::Any::DOUBLE); - _res.set_double_value(this->dataPtr->materialShininessMap[_req.data()]); + _res.set_double_value(this->ShininessByScopedName(_req.data())); return true; } diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index 3ed2ac4315..c9ccc1d1eb 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -461,7 +461,7 @@ namespace gazebo public: std::string UniqueModelName(const std::string &_name); /// \brief Set callback 'waitForSensors' - /// \param[in] function to be called + /// \param[in] _func function to be called public: void SetSensorWaitFunc(std::function _func); /// \cond @@ -678,6 +678,13 @@ namespace gazebo private: bool MaterialShininessService( const ignition::msgs::StringMsg &_request, msgs::Any &_response); + /// \brief Helper function for getting shininess values by scoped + /// visual name. + /// \param[in] _scopedName Scoped visual name. + /// \return Shininess value. + private: double ShininessByScopedName(const std::string &_scopedName) + const; + /// \internal /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/gazebo/physics/WorldPrivate.hh b/gazebo/physics/WorldPrivate.hh index d1bf78c42d..694813c61e 100644 --- a/gazebo/physics/WorldPrivate.hh +++ b/gazebo/physics/WorldPrivate.hh @@ -398,6 +398,10 @@ namespace gazebo /// \brief Shadow caster render back faces from scene SDF public: bool shadowCasterRenderBackFaces = true; + /// \brief This mutex is used to by the SetVisualShininess and + /// ShininessByScopedName methods to protect materialShininessMap. + public: std::mutex materialShininessMutex; + /// \brief Shininess values from scene SDF public: std::map materialShininessMap; }; From 58d94ddb1a1dbae7b8d73d9f3cbf49f7def91ac5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 29 Jun 2022 22:53:28 -0700 Subject: [PATCH 2/3] Store shininess values by scoped Visual name This moves the parsing of shininess values from World::LoadModel to Link::UpdateVisualMsg and stores the values by scoped Visual name rather than by Model name. The per-model gz services are also replaced by a single /shininess service. Signed-off-by: Steve Peters --- gazebo/physics/Link.cc | 10 +++++++ gazebo/physics/World.cc | 45 ++++++++++------------------ gazebo/physics/World.hh | 6 ++++ gazebo/rendering/Visual.cc | 5 ++-- test/integration/visual_shininess.cc | 16 +++++----- 5 files changed, 42 insertions(+), 40 deletions(-) diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc index 4a1bbd3a32..5b17b55a90 100644 --- a/gazebo/physics/Link.cc +++ b/gazebo/physics/Link.cc @@ -1547,6 +1547,16 @@ void Link::UpdateVisualMsg() bool newVis = true; std::string visName = this->GetScopedName() + "::" + msg.name(); + if (visualElem->HasElement("material")) + { + sdf::ElementPtr matElem = visualElem->GetElement("material"); + if (matElem->HasElement("shininess")) + { + this->world->SetVisualShininess( + visName, matElem->Get("shininess")); + } + } + // update visual msg if it exists for (auto &iter : this->visuals) { diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index 63ced84d51..b0f1ca6194 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -334,6 +334,14 @@ void World::Load(sdf::ElementPtr _sdf) shadowCasterRenderBackFacesService << "]" << std::endl; } + std::string materialShininessService("/shininess"); + if (!this->dataPtr->ignNode.Advertise(materialShininessService, + &World::MaterialShininessService, this)) + { + gzerr << "Error advertising service [" + << materialShininessService << "]" << std::endl; + } + // This should come before loading of entities sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics"); @@ -1259,35 +1267,6 @@ ModelPtr World::LoadModel(sdf::ElementPtr _sdf , BasePtr _parent) } } - if (_sdf->HasElement("link")) - { - sdf::ElementPtr linkElem = _sdf->GetElement("link"); - if (linkElem->HasElement("visual") && - linkElem->GetElement("visual")->HasElement("material")) - { - sdf::ElementPtr matElem = linkElem->GetElement("visual")-> - GetElement("material"); - - if (matElem->HasElement("shininess")) - { - this->dataPtr->materialShininessMap[modelName] = - matElem->Get("shininess"); - } - else - { - this->dataPtr->materialShininessMap[modelName] = 0; - } - - std::string materialShininessService("/" + modelName + "/shininess"); - if (!this->dataPtr->ignNode.Advertise(materialShininessService, - &World::MaterialShininessService, this)) - { - gzerr << "Error advertising service [" - << materialShininessService << "]" << std::endl; - } - } - } - model = this->dataPtr->physicsEngine->CreateModel(_parent); model->SetWorld(shared_from_this()); model->Load(_sdf); @@ -3467,6 +3446,14 @@ bool World::ShadowCasterRenderBackFacesService(ignition::msgs::Boolean &_res) return true; } +////////////////////////////////////////////////// +void World::SetVisualShininess(const std::string &_scopedName, + double _shininess) +{ + std::lock_guard lock(this->dataPtr->materialShininessMutex); + this->dataPtr->materialShininessMap[_scopedName] = _shininess; +} + ////////////////////////////////////////////////// double World::ShininessByScopedName(const std::string &_scopedName) const { diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index c9ccc1d1eb..5e709ce8d7 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -464,6 +464,12 @@ namespace gazebo /// \param[in] _func function to be called public: void SetSensorWaitFunc(std::function _func); + /// \brief Set Visual shininess value by scoped name + /// \param[in] _scopedName Scoped name of visual. + /// \param[in] _shininess Shininess value. + public: void SetVisualShininess(const std::string &_scopedName, + double _shininess); + /// \cond /// This is an internal function. /// \brief Get a model by id. diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index f4cb663329..c83f7f8c57 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -354,10 +354,9 @@ void Visual::Load() ignition::transport::Node node; msgs::Any rep; - const std::string visualName = - this->Name().substr(0, this->Name().find(":")); + const std::string visualName = this->Name(); - const std::string serviceName = "/" + visualName + "/shininess"; + const std::string serviceName = "/shininess"; const std::string validServiceName = ignition::transport::TopicUtils::AsValidTopic(serviceName); diff --git a/test/integration/visual_shininess.cc b/test/integration/visual_shininess.cc index 5f5de6970e..2ee39c1dda 100644 --- a/test/integration/visual_shininess.cc +++ b/test/integration/visual_shininess.cc @@ -28,10 +28,10 @@ class VisualShininess : public RenderingFixture }; ///////////////////////////////////////////////// -void CheckShininessService(const std::string &_modelName, +void CheckShininessService(const std::string &_scopedName, double _expectedShininess) { - std::string serviceName = '/' + _modelName + "/shininess"; + std::string serviceName = "/shininess"; ignition::transport::Node ignNode; { @@ -43,11 +43,11 @@ void CheckShininessService(const std::string &_modelName, gazebo::msgs::Any reply; const unsigned int timeout = 3000; bool result = false; - request.set_data(_modelName); + request.set_data(_scopedName); EXPECT_TRUE(ignNode.Request(serviceName, request, timeout, reply, result)); EXPECT_TRUE(result); EXPECT_EQ(msgs::Any_ValueType_DOUBLE, reply.type()); - EXPECT_DOUBLE_EQ(_expectedShininess, reply.double_value()) << _modelName; + EXPECT_DOUBLE_EQ(_expectedShininess, reply.double_value()) << _scopedName; } ///////////////////////////////////////////////// @@ -55,10 +55,10 @@ TEST_F(VisualShininess, ShapesShininessServices) { Load("worlds/shapes_shininess.world", true); - CheckShininessService("ground_plane", 0.0); - CheckShininessService("box", 1.0); - CheckShininessService("sphere", 5.0); - CheckShininessService("cylinder", 10.0); + CheckShininessService("ground_plane::link::visual", 0.0); + CheckShininessService("box::link::visual", 1.0); + CheckShininessService("sphere::link::visual", 5.0); + CheckShininessService("cylinder::link::visual", 10.0); } ///////////////////////////////////////////////// From 8de984a95c75654e7d43c053af3ec1e86bb04615 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 29 Jun 2022 22:58:18 -0700 Subject: [PATCH 3/3] Add extra box visual to shapes_shininess world Signed-off-by: Steve Peters --- test/integration/visual_shininess.cc | 2 ++ worlds/shapes_shininess.world | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/test/integration/visual_shininess.cc b/test/integration/visual_shininess.cc index 2ee39c1dda..1bd46d6310 100644 --- a/test/integration/visual_shininess.cc +++ b/test/integration/visual_shininess.cc @@ -57,6 +57,7 @@ TEST_F(VisualShininess, ShapesShininessServices) CheckShininessService("ground_plane::link::visual", 0.0); CheckShininessService("box::link::visual", 1.0); + CheckShininessService("box::link::visual2", 10.0); CheckShininessService("sphere::link::visual", 5.0); CheckShininessService("cylinder::link::visual", 10.0); } @@ -101,6 +102,7 @@ TEST_F(VisualShininess, ShapesShininess) std::unordered_map nameToShininess; nameToShininess["box::link::visual"] = 1.0; + nameToShininess["box::link::visual2"] = 10.0; nameToShininess["sphere::link::visual"] = 5.0; nameToShininess["cylinder::link::visual"] = 10.0; diff --git a/worlds/shapes_shininess.world b/worlds/shapes_shininess.world index 85c30333a9..1f48211428 100644 --- a/worlds/shapes_shininess.world +++ b/worlds/shapes_shininess.world @@ -28,6 +28,18 @@ + + 0 0 3 0 0 0 + + 1 0 0 1 + 10 + + + + 1 1 1 + + +