From 4b4b9e13e89e227d9abd1b454723e92d2c4aef37 Mon Sep 17 00:00:00 2001
From: Tyler Marr <41449746+marrts@users.noreply.github.com>
Date: Tue, 16 Jan 2024 08:51:16 -0600
Subject: [PATCH] Update URDF Robot version tag to 'tesseract_version' for ROS2
 compatibilty (#979)

---
 tesseract_urdf/src/urdf_parser.cpp | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/tesseract_urdf/src/urdf_parser.cpp b/tesseract_urdf/src/urdf_parser.cpp
index c02ff55b695..2d924f4da08 100644
--- a/tesseract_urdf/src/urdf_parser.cpp
+++ b/tesseract_urdf/src/urdf_parser.cpp
@@ -63,6 +63,18 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_
     std::throw_with_nested(
         std::runtime_error("URDF: Failed parsing attribute 'version' for robot '" + robot_name + "'!"));
 
+  if (urdf_version != 1)
+    std::throw_with_nested(
+        std::runtime_error("URDF: 'version' for robot '" + robot_name + "' is set to `" + std::to_string(urdf_version) +
+                           "', this is not supported, please set it to 1.0. If you want to use a different tesseract "
+                           "URDF parsing version use `tesseract_version=\"2\"`"));
+
+  int tesseract_urdf_version = 1;
+  auto tesseract_version_status = robot->QueryIntAttribute("tesseract_version", &tesseract_urdf_version);
+  if (tesseract_version_status != tinyxml2::XML_NO_ATTRIBUTE && tesseract_version_status != tinyxml2::XML_SUCCESS)
+    std::throw_with_nested(
+        std::runtime_error("URDF: Failed parsing attribute 'tesseract_version' for robot '" + robot_name + "'!"));
+
   auto sg = std::make_unique<tesseract_scene_graph::SceneGraph>();
   sg->setName(robot_name);
 
@@ -74,7 +86,7 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_
     std::unordered_map<std::string, tesseract_scene_graph::Material::Ptr> empty_material;
     try
     {
-      m = parseMaterial(material, empty_material, true, urdf_version);
+      m = parseMaterial(material, empty_material, true, tesseract_urdf_version);
     }
     catch (...)
     {
@@ -91,7 +103,7 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_
     tesseract_scene_graph::Link::Ptr l = nullptr;
     try
     {
-      l = parseLink(link, locator, available_materials, urdf_version);
+      l = parseLink(link, locator, available_materials, tesseract_urdf_version);
     }
     catch (...)
     {
@@ -119,7 +131,7 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_
     tesseract_scene_graph::Joint::Ptr j = nullptr;
     try
     {
-      j = parseJoint(joint, urdf_version);
+      j = parseJoint(joint, tesseract_urdf_version);
     }
     catch (...)
     {