Fix calculation of vertex IDs for relays

subt_ign/src/VisibilityTable.cc

@@ -254,7 +254,16 @@ void VisibilityTable::PopulateVisibilityInfo(
   // Convert poses to vertices.
   std::set<ignition::math::graph::VertexId> relays;
   for (const auto pose : _relayPoses)
-    relays.insert(this->Index(pose));
+  {
+    int32_t x = std::round(pose.X());
+    int32_t y = std::round(pose.Y());
+    int32_t z = std::round(pose.Z());
+    auto vertexId = std::make_tuple(x, y, z);
+
+    auto it = this->vertices.find(vertexId);
+    if (it != this->vertices.end())
+      relays.insert(it->second);
+  }
 
   // Compute the cost of all routes without considering relays.
   this->PopulateVisibilityInfo();