4.1 -> 5 (pull request #1585)

gazebosim · Apr 29, 2015 · 3b410fc · 3b410fc
2 parents cb1313d + 94b2571
commit 3b410fc
Show file tree

Hide file tree

Showing 4 changed files with 73 additions and 31 deletions.
diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc
@@ -784,6 +784,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)
@@ -1033,32 +1035,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);
 }
 
 /////////////////////////////////////////////////
@@ -1141,6 +1122,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<std::string>("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
 {

diff --git a/gazebo/physics/Link.hh b/gazebo/physics/Link.hh
@@ -557,6 +557,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
@@ -2448,8 +2448,6 @@ bool Scene::ProcessVisualMsg(ConstVisualPtr &_msg)
       visual->ShowJoints(this->dataPtr->showJoints);
       visual->SetTransparency(this->dataPtr->transparent ? 0.5 : 0.0);
       visual->SetWireframe(this->dataPtr->wireframe);
-
-      visual->UpdateFromMsg(_msg);
     }
   }
 

diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc
@@ -504,7 +504,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 =
@@ -532,10 +532,21 @@ void Visual::Load()
       this->dataPtr->scale =
           geomElem->GetElement("mesh")->Get<math::Vector3>("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"))