From 626ab52b54dd5ff8cd43d1390e54c30038d481ab Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 Apr 2015 14:18:54 -0700 Subject: [PATCH 1/2] port visual collision scale fix --- gazebo/physics/Link.cc | 83 +++++++++++++++++++++++++------------- gazebo/physics/Link.hh | 3 ++ gazebo/rendering/Scene.cc | 2 - gazebo/rendering/Visual.cc | 19 +++++++-- 4 files changed, 73 insertions(+), 34 deletions(-) diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc index 73c41217f1..1b18aa3106 100644 --- a/gazebo/physics/Link.cc +++ b/gazebo/physics/Link.cc @@ -778,6 +778,8 @@ void Link::FillMsg(msgs::Link &_msg) // Parse visuals from SDF if (this->visuals.empty()) this->ParseVisuals(); + else + this->UpdateVisualMsg(); for (Visuals_M::iterator iter = this->visuals.begin(); iter != this->visuals.end(); ++iter) @@ -1027,32 +1029,11 @@ void Link::OnCollision(ConstContactsPtr &_msg) ///////////////////////////////////////////////// void Link::ParseVisuals() { - // TODO: this shouldn't be in the physics sim - if (this->sdf->HasElement("visual")) - { - sdf::ElementPtr visualElem = this->sdf->GetElement("visual"); - while (visualElem) - { - msgs::Visual msg = msgs::VisualFromSDF(visualElem); - - std::string visName = this->GetScopedName() + "::" + msg.name(); - msg.set_name(visName); - msg.set_id(physics::getUniqueId()); - msg.set_parent_name(this->GetScopedName()); - msg.set_parent_id(this->GetId()); - msg.set_is_static(this->IsStatic()); + this->UpdateVisualMsg(); - this->visPub->Publish(msg); - - Visuals_M::iterator iter = this->visuals.find(msg.id()); - if (iter != this->visuals.end()) - gzthrow(std::string("Duplicate visual name[")+msg.name()+"]\n"); - - this->visuals[msg.id()] = msg; - - visualElem = visualElem->GetNextElement("visual"); - } - } + for (Visuals_M::iterator iter = this->visuals.begin(); + iter != this->visuals.end(); ++iter) + this->visPub->Publish(iter->second); } ///////////////////////////////////////////////// @@ -1135,6 +1116,55 @@ void Link::UpdateVisualSDF() } } +////////////////////////////////////////////////// +void Link::UpdateVisualMsg() +{ + // TODO: this shouldn't be in the physics sim + if (this->sdf->HasElement("visual")) + { + sdf::ElementPtr visualElem = this->sdf->GetElement("visual"); + while (visualElem) + { + msgs::Visual msg = msgs::VisualFromSDF(visualElem); + + bool newVis = true; + std::string linkName = this->GetScopedName(); + + // update visual msg if it exists + for (Visuals_M::iterator iter = this->visuals.begin(); + iter != this->visuals.end(); ++iter) + { + std::string visName = linkName + "::" + + visualElem->Get("name"); + if (iter->second.name() == visName) + { + iter->second.mutable_geometry()->CopyFrom(msg.geometry()); + newVis = false; + break; + } + } + + // 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()); + msg.set_parent_id(this->GetId()); + msg.set_is_static(this->IsStatic()); + + Visuals_M::iterator iter = this->visuals.find(msg.id()); + if (iter != this->visuals.end()) + gzthrow(std::string("Duplicate visual name[")+msg.name()+"]\n"); + this->visuals[msg.id()] = msg; + } + + visualElem = visualElem->GetNextElement("visual"); + } + } +} + ///////////////////////////////////////////////// double Link::GetWorldEnergyPotential() const { @@ -1326,6 +1356,3 @@ msgs::Visual Link::GetVisualMessage(const std::string &_name) const return result; } - - - diff --git a/gazebo/physics/Link.hh b/gazebo/physics/Link.hh index b9202973e4..09601c1bc8 100644 --- a/gazebo/physics/Link.hh +++ b/gazebo/physics/Link.hh @@ -550,6 +550,9 @@ namespace gazebo /// \brief Update visual SDFs. private: void UpdateVisualSDF(); + /// \brief Update visual msgs. + private: void UpdateVisualMsg(); + /// \brief Inertial properties. protected: InertialPtr inertial; diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index fb7eddb21f..09ecfa6d37 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -2397,8 +2397,6 @@ bool Scene::ProcessVisualMsg(ConstVisualPtr &_msg) visual->ShowJoints(this->showJoints); visual->SetTransparency(this->transparent ? 0.5 : 0.0); visual->SetWireframe(this->wireframe); - - visual->UpdateFromMsg(_msg); } } diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index b9856bfc1f..1da4c684bc 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -507,7 +507,7 @@ void Visual::Load() if (this->dataPtr->sdf->HasElement("geometry")) { sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry"); - + bool hasGeom = true; if (geomElem->HasElement("box")) { this->dataPtr->scale = @@ -535,10 +535,21 @@ void Visual::Load() this->dataPtr->scale = geomElem->GetElement("mesh")->Get("scale"); } - } + else + { + hasGeom = false; + } - this->dataPtr->sceneNode->setScale(this->dataPtr->scale.x, - this->dataPtr->scale.y, this->dataPtr->scale.z); + if (hasGeom) + { + // geom values give the absolute size so compute a scale that will + // be mulitiply by the current scale to get to the geom size. + math::Vector3 derivedScale = Conversions::Convert( + this->dataPtr->sceneNode->_getDerivedScale()); + math::Vector3 toScale = this->dataPtr->scale / derivedScale; + this->dataPtr->sceneNode->scale(toScale.x, toScale.y, toScale.z); + } + } // Set the material of the mesh if (this->dataPtr->sdf->HasElement("material")) From dfea63cdc227245631785670b004a32cfa51e278 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 28 Apr 2015 23:30:58 -0700 Subject: [PATCH 2/2] Close branch issue_1269_4.1