Commits

"Nat...@gmail.com>"  committed f48da77

Fix world to handle the case where a road is inserted.

  • Participants
  • Parent commits 716a6df
  • Branches vrc_worlds

Comments (0)

Files changed (1)

File gazebo/physics/World.cc

 //////////////////////////////////////////////////
 unsigned int World::GetModelCount() const
 {
-  return this->rootElement->GetChildCount();
+  // Not all children of the root element are models. We have to iterate
+  // through the children and count only the models.
+  unsigned int result = 0;
+  for (unsigned int i = 0; i < this->rootElement->GetChildCount(); i++)
+    result += this->rootElement->GetChild(i)->HasType(Base::MODEL) ? 1 : 0;
+  return result;
 }
 
 //////////////////////////////////////////////////
 {
   ModelPtr model;
 
-  if (_index < this->rootElement->GetChildCount() &&
-      this->rootElement->GetChild(_index)->HasType(Base::MODEL))
+  if (_index < this->rootElement->GetChildCount())
   {
-    model =
-      boost::shared_static_cast<Model>(this->rootElement->GetChild(_index));
+    // Not all children of the root element are models. We have to iterate
+    // through the children and count only the models.
+    for (unsigned int i = 0, count = 0;
+         i < this->rootElement->GetChildCount(); i++)
+    {
+      if (count == _index)
+      {
+        model = boost::shared_static_cast<Model>(
+            this->rootElement->GetChild(count));
+        break;
+      }
+      count += this->rootElement->GetChild(i)->HasType(Base::MODEL) ? 1 : 0;
+    }
+
+    if (!model)
+      gzerr << "Unable to get model with index[" << _index << "]\n";
   }
   else
   {
-    gzerr << "Invalid model index\n";
+    gzerr << "Given model index[" << _index << "] is out of range[0.."
+          << this->rootElement->GetChildCount() << "]\n";
   }
 
   return model;