From 762f4fe425d0662063f18e54bb20fb1392092a7b Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Fri, 3 Jun 2022 16:24:11 -0700
Subject: [PATCH] Fix build and tests with Humble and Rolling

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 sdformat_urdf/CMakeLists.txt                  |  6 ++---
 .../include/sdformat_urdf/sdformat_urdf.hpp   |  5 +++-
 sdformat_urdf/package.xml                     |  2 +-
 sdformat_urdf/src/sdformat_urdf.cpp           | 25 ++++++++++---------
 sdformat_urdf/test/geometry_tests.cpp         |  5 ++--
 sdformat_urdf/test/graph_tests.cpp            |  5 ++--
 sdformat_urdf/test/include/test_tools.hpp     | 12 +--------
 sdformat_urdf/test/joint_tests.cpp            | 17 +++++++------
 sdformat_urdf/test/link_tests.cpp             |  5 ++--
 sdformat_urdf/test/material_tests.cpp         |  5 ++--
 sdformat_urdf/test/model_tests.cpp            |  5 ++--
 sdformat_urdf/test/pose_tests.cpp             |  7 +++---
 12 files changed, 50 insertions(+), 49 deletions(-)

diff --git a/sdformat_urdf/CMakeLists.txt b/sdformat_urdf/CMakeLists.txt
index d12be155..c5ec1faa 100644
--- a/sdformat_urdf/CMakeLists.txt
+++ b/sdformat_urdf/CMakeLists.txt
@@ -14,7 +14,7 @@ find_package(ament_cmake_ros REQUIRED)
 
 find_package(pluginlib REQUIRED)
 find_package(rcutils REQUIRED)
-find_package(sdformat9 REQUIRED)
+find_package(sdformat12 REQUIRED)
 find_package(urdfdom_headers 1.0.6 REQUIRED)
 find_package(urdf_parser_plugin REQUIRED)
 find_package(tinyxml2_vendor REQUIRED)
@@ -25,7 +25,7 @@ add_library(sdformat_urdf SHARED
   src/sdformat_urdf.cpp
 )
 target_link_libraries(sdformat_urdf PUBLIC
-    sdformat9::sdformat9
+    sdformat12::sdformat12
   urdfdom_headers::urdfdom_headers
 )
 target_link_libraries(sdformat_urdf PRIVATE
@@ -51,7 +51,7 @@ target_link_libraries(sdformat_urdf_plugin PRIVATE
 
 ament_export_dependencies(pluginlib)
 ament_export_dependencies(rcutils)
-ament_export_dependencies(sdformat9)
+ament_export_dependencies(sdformat12)
 ament_export_dependencies(tinyxml2)
 ament_export_dependencies(urdf_parser_plugin)
 ament_export_dependencies(urdfdom_headers)
diff --git a/sdformat_urdf/include/sdformat_urdf/sdformat_urdf.hpp b/sdformat_urdf/include/sdformat_urdf/sdformat_urdf.hpp
index 9199e546..2daca0cb 100644
--- a/sdformat_urdf/include/sdformat_urdf/sdformat_urdf.hpp
+++ b/sdformat_urdf/include/sdformat_urdf/sdformat_urdf.hpp
@@ -15,12 +15,15 @@
 #ifndef SDFORMAT_URDF__SDFORMAT_URDF_HPP_
 #define SDFORMAT_URDF__SDFORMAT_URDF_HPP_
 
-#include <sdf/sdf.hh>
 #include <urdf_world/types.h>
 #include <urdf_model/types.h>
 
 #include <string>
 
+#include <sdf/Model.hh>
+#include <sdf/Root.hh>
+#include <sdf/Types.hh>
+
 #include "sdformat_urdf/visibility_control.hpp"
 
 namespace sdformat_urdf
diff --git a/sdformat_urdf/package.xml b/sdformat_urdf/package.xml
index ded3063f..cf10e307 100644
--- a/sdformat_urdf/package.xml
+++ b/sdformat_urdf/package.xml
@@ -17,7 +17,7 @@
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
   <buildtool_export_depend>ament_cmake_ros</buildtool_export_depend>
 
-  <depend>sdformat</depend>
+  <depend>sdformat12</depend>
   <depend>urdf</depend>
 
   <build_depend>liburdfdom-headers-dev</build_depend>
diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp
index 3dd95aa5..9abed32a 100644
--- a/sdformat_urdf/src/sdformat_urdf.cpp
+++ b/sdformat_urdf/src/sdformat_urdf.cpp
@@ -12,9 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <ignition/math/Pose3.hh>
 #include <rcutils/logging_macros.h>
-#include <sdf/sdf.hh>
 #include <urdf_world/types.h>
 #include <urdf_model/model.h>
 
@@ -23,6 +21,16 @@
 #include <string>
 #include <vector>
 
+#include <ignition/math/Pose3.hh>
+#include <sdf/Error.hh>
+#include <sdf/Collision.hh>
+#include <sdf/Geometry.hh>
+#include <sdf/Joint.hh>
+#include <sdf/JointAxis.hh>
+#include <sdf/Link.hh>
+#include <sdf/Mesh.hh>
+#include <sdf/Visual.hh>
+
 #include "sdformat_urdf/sdformat_urdf.hpp"
 
 namespace sdformat_urdf
@@ -66,20 +74,13 @@ sdformat_urdf::sdf_to_urdf(const sdf::Root & sdf_dom, sdf::Errors & errors)
       "SDFormat xml has a world; but only a single model is supported");
     return nullptr;
   }
-  if (0u == sdf_dom.ModelCount()) {
+  if (nullptr == sdf_dom.Model()) {
     errors.emplace_back(
-      sdf::ErrorCode::STRING_READ,
+      sdf::ErrorCode::ELEMENT_MISSING,
       "SDFormat xml has no models; need at least one");
     return nullptr;
   }
-  if (1u != sdf_dom.ModelCount()) {
-    errors.emplace_back(
-      sdf::ErrorCode::STRING_READ,
-      "SDFormat xml has multiple models; but only a single model is supported");
-    return nullptr;
-  }
-
-  return convert_model(*sdf_dom.ModelByIndex(0), errors);
+  return convert_model(*sdf_dom.Model(), errors);
 }
 
 urdf::ModelInterfaceSharedPtr
diff --git a/sdformat_urdf/test/geometry_tests.cpp b/sdformat_urdf/test/geometry_tests.cpp
index 4123688b..1e22cc78 100644
--- a/sdformat_urdf/test/geometry_tests.cpp
+++ b/sdformat_urdf/test/geometry_tests.cpp
@@ -14,10 +14,11 @@
 
 
 #include <gtest/gtest.h>
-#include <sdf/sdf.hh>
-#include <sdformat_urdf/sdformat_urdf.hpp>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
+#include <sdformat_urdf/sdformat_urdf.hpp>
+
+#include <sdf/Types.hh>
 
 #include "sdf_paths.hpp"
 #include "test_tools.hpp"
diff --git a/sdformat_urdf/test/graph_tests.cpp b/sdformat_urdf/test/graph_tests.cpp
index 23d8b319..fc69ba8f 100644
--- a/sdformat_urdf/test/graph_tests.cpp
+++ b/sdformat_urdf/test/graph_tests.cpp
@@ -14,10 +14,11 @@
 
 
 #include <gtest/gtest.h>
-#include <sdf/sdf.hh>
-#include <sdformat_urdf/sdformat_urdf.hpp>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
+#include <sdformat_urdf/sdformat_urdf.hpp>
+
+#include <sdf/Types.hh>
 
 #include "sdf_paths.hpp"
 #include "test_tools.hpp"
diff --git a/sdformat_urdf/test/include/test_tools.hpp b/sdformat_urdf/test/include/test_tools.hpp
index ef3459b1..f0269a68 100644
--- a/sdformat_urdf/test/include/test_tools.hpp
+++ b/sdformat_urdf/test/include/test_tools.hpp
@@ -15,8 +15,6 @@
 #ifndef TEST_TOOLS_HPP_
 #define TEST_TOOLS_HPP_
 
-#include <sdf/sdf.hh>
-
 #include <algorithm>
 #include <string>
 #include <fstream>
@@ -60,7 +58,7 @@ get_file(const char * path)
       bool name_is_expected = false; \
       auto expected_name_iter = expected_names.begin(); \
       while (expected_name_iter != expected_names.end()) { \
-        if (* expected_name_iter == child->name) { \
+        if (*expected_name_iter == child->name) { \
           name_is_expected = true; \
           expected_names.erase(expected_name_iter); \
           break; \
@@ -81,12 +79,4 @@ get_file(const char * path)
     } \
   } while (false)
 
-std::ostream & operator<<(std::ostream & os, const sdf::Errors & errors)
-{
-  for (const auto & error : errors) {
-    os << error;
-  }
-  return os;
-}
-
 #endif  // TEST_TOOLS_HPP_
diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp
index d38c622e..91e917f4 100644
--- a/sdformat_urdf/test/joint_tests.cpp
+++ b/sdformat_urdf/test/joint_tests.cpp
@@ -14,10 +14,11 @@
 
 
 #include <gtest/gtest.h>
-#include <sdf/sdf.hh>
-#include <sdformat_urdf/sdformat_urdf.hpp>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
+#include <sdformat_urdf/sdformat_urdf.hpp>
+
+#include <sdf/Types.hh>
 
 #include "sdf_paths.hpp"
 #include "test_tools.hpp"
@@ -112,8 +113,8 @@ TEST(Joint, joint_prismatic)
   ASSERT_NE(nullptr, joint->limits);
   EXPECT_DOUBLE_EQ(-0.2, joint->limits->lower);
   EXPECT_DOUBLE_EQ(0.2, joint->limits->upper);
-  EXPECT_DOUBLE_EQ(-1, joint->limits->effort);  // SDFormat default
-  EXPECT_DOUBLE_EQ(-1, joint->limits->velocity);  // SDFormat default
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), joint->limits->effort);
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), joint->limits->velocity);
   ASSERT_EQ(nullptr, joint->safety);
   ASSERT_EQ(nullptr, joint->calibration);
   ASSERT_EQ(nullptr, joint->mimic);
@@ -139,8 +140,8 @@ TEST(Joint, joint_revolute)
   ASSERT_NE(nullptr, joint->limits);
   EXPECT_DOUBLE_EQ(-1.5, joint->limits->lower);
   EXPECT_DOUBLE_EQ(1.5, joint->limits->upper);
-  EXPECT_DOUBLE_EQ(-1, joint->limits->effort);  // SDFormat default
-  EXPECT_DOUBLE_EQ(-1, joint->limits->velocity);  // SDFormat default
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), joint->limits->effort);
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), joint->limits->velocity);
   ASSERT_EQ(nullptr, joint->safety);
   ASSERT_EQ(nullptr, joint->calibration);
   ASSERT_EQ(nullptr, joint->mimic);
@@ -224,8 +225,8 @@ TEST(Joint, joint_revolute_default_limits)
   ASSERT_NE(nullptr, joint->limits);
   EXPECT_DOUBLE_EQ(-1e16, joint->limits->lower);  // SDFormat default
   EXPECT_DOUBLE_EQ(1e16, joint->limits->upper);  // SDFormat default
-  EXPECT_DOUBLE_EQ(-1, joint->limits->effort);  // SDFormat default
-  EXPECT_DOUBLE_EQ(-1, joint->limits->velocity);  // SDFormat default
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), joint->limits->effort);
+  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), joint->limits->velocity);
 }
 
 TEST(Joint, joint_revolute_two_joints_two_links)
diff --git a/sdformat_urdf/test/link_tests.cpp b/sdformat_urdf/test/link_tests.cpp
index d186a0de..fa0f809e 100644
--- a/sdformat_urdf/test/link_tests.cpp
+++ b/sdformat_urdf/test/link_tests.cpp
@@ -14,10 +14,11 @@
 
 
 #include <gtest/gtest.h>
-#include <sdf/sdf.hh>
-#include <sdformat_urdf/sdformat_urdf.hpp>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
+#include <sdformat_urdf/sdformat_urdf.hpp>
+
+#include <sdf/Types.hh>
 
 #include "sdf_paths.hpp"
 #include "test_tools.hpp"
diff --git a/sdformat_urdf/test/material_tests.cpp b/sdformat_urdf/test/material_tests.cpp
index 02a129f3..960313e0 100644
--- a/sdformat_urdf/test/material_tests.cpp
+++ b/sdformat_urdf/test/material_tests.cpp
@@ -14,10 +14,11 @@
 
 
 #include <gtest/gtest.h>
-#include <sdf/sdf.hh>
-#include <sdformat_urdf/sdformat_urdf.hpp>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
+#include <sdformat_urdf/sdformat_urdf.hpp>
+
+#include <sdf/Types.hh>
 
 #include "sdf_paths.hpp"
 #include "test_tools.hpp"
diff --git a/sdformat_urdf/test/model_tests.cpp b/sdformat_urdf/test/model_tests.cpp
index 7292fd31..8369b587 100644
--- a/sdformat_urdf/test/model_tests.cpp
+++ b/sdformat_urdf/test/model_tests.cpp
@@ -14,10 +14,11 @@
 
 
 #include <gtest/gtest.h>
-#include <sdf/sdf.hh>
-#include <sdformat_urdf/sdformat_urdf.hpp>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
+#include <sdformat_urdf/sdformat_urdf.hpp>
+
+#include <sdf/Types.hh>
 
 #include "sdf_paths.hpp"
 #include "test_tools.hpp"
diff --git a/sdformat_urdf/test/pose_tests.cpp b/sdformat_urdf/test/pose_tests.cpp
index 0f423a01..b92698e0 100644
--- a/sdformat_urdf/test/pose_tests.cpp
+++ b/sdformat_urdf/test/pose_tests.cpp
@@ -14,11 +14,12 @@
 
 
 #include <gtest/gtest.h>
-#include <ignition/math/Pose3.hh>
-#include <sdf/sdf.hh>
-#include <sdformat_urdf/sdformat_urdf.hpp>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
+#include <sdformat_urdf/sdformat_urdf.hpp>
+
+#include <ignition/math/Pose3.hh>
+#include <sdf/Types.hh>
 
 #include "sdf_paths.hpp"
 #include "test_tools.hpp"