From 71fe377d8272c812c359284827755424b91ccc4d Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Tue, 7 May 2024 10:54:20 +0200
Subject: [PATCH] subt_ign: Correctly migrate to FCL 0.6

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 subt_ign/include/subt_ign/VisibilityTable.hh | 15 +++++++++++
 subt_ign/package.xml                         |  5 ++--
 subt_ign/src/VisibilityPlugin.cc             |  4 +++
 subt_ign/src/VisibilityTable.cc              | 22 +++++++++++++++-
 subt_ign/src/ign_to_fcl.cc                   | 27 +++++++++++++++++++-
 subt_ign/src/ign_to_fcl.hh                   | 15 +++++++++++
 6 files changed, 84 insertions(+), 4 deletions(-)

diff --git a/subt_ign/include/subt_ign/VisibilityTable.hh b/subt_ign/include/subt_ign/VisibilityTable.hh
index da918440..7ba030a3 100644
--- a/subt_ign/include/subt_ign/VisibilityTable.hh
+++ b/subt_ign/include/subt_ign/VisibilityTable.hh
@@ -28,9 +28,16 @@
 #include <ignition/math/graph/Vertex.hh>
 #include <subt_ign/VisibilityTypes.hh>
 
+#include <fcl/config.h>
+
 namespace fcl
 {
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
   class CollisionObject;
+#else
+  template<typename> class CollisionObject;
+  using CollisionObjectd = CollisionObject<double>;
+#endif
 }
 
 namespace subt
@@ -95,7 +102,11 @@ namespace subt
     /// \param[in] _collObjs Collision Objects of models.
     /// \sa Generate
     public: void SetModelCollisionObjects(
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
         const std::map<std::string, std::shared_ptr<fcl::CollisionObject>> &_collObjs);
+#else
+        const std::map<std::string, std::shared_ptr<fcl::CollisionObjectd>> &_collObjs);
+#endif
 
     /// \brief Get the collection of sampled 3D points and their associated
     /// vertex id.
@@ -218,7 +229,11 @@ namespace subt
 
     /// \brief A map of model name to its bounding box. Used for generating LUT
     private: std::map<std::string,
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
              std::shared_ptr<fcl::CollisionObject>> collisionObjs;
+#else
+             std::shared_ptr<fcl::CollisionObjectd>> collisionObjs;
+#endif
 
     /// \brief A map that stores 3D points an the vertex id in which are located
     private: std::map<std::tuple<int32_t, int32_t, int32_t>, uint64_t> vertices;
diff --git a/subt_ign/package.xml b/subt_ign/package.xml
index 306bf2dc..9cfdff73 100644
--- a/subt_ign/package.xml
+++ b/subt_ign/package.xml
@@ -1,4 +1,4 @@
-<package format="2">
+<package format="3">
   <name>subt_ign</name>
   <version>0.1.0</version>
   <description>The ROS package for the DARPA SubT challenge.</description>
@@ -20,7 +20,8 @@
   <depend>subt_ros</depend>
 
   <depend>libccd-dev</depend>
-  <depend>libfcl-dev</depend>
+  <depend condition="$ROS_DISTRO != noetic">libfcl-dev</depend>
+  <depend condition="$ROS_DISTRO == noetic">fcl</depend>
   <depend>rosbag</depend>
   <depend>roslib</depend>
 
diff --git a/subt_ign/src/VisibilityPlugin.cc b/subt_ign/src/VisibilityPlugin.cc
index d2dba2f5..ba24061a 100644
--- a/subt_ign/src/VisibilityPlugin.cc
+++ b/subt_ign/src/VisibilityPlugin.cc
@@ -61,7 +61,11 @@ class subt::VisibilityPluginPrivate
   /// \brief A map of model name ot its bounding box
   public: std::map<std::string, ignition::math::AxisAlignedBox> bboxes;
 
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
   public: std::map<std::string, std::shared_ptr<fcl::CollisionObject>> fclObjs;
+#else
+  public: std::map<std::string, std::shared_ptr<fcl::CollisionObjectd>> fclObjs;
+#endif
 
   /// \brief Name of the world
   public: std::string worldName;
diff --git a/subt_ign/src/VisibilityTable.cc b/subt_ign/src/VisibilityTable.cc
index c52cf9cd..60d9ad7d 100644
--- a/subt_ign/src/VisibilityTable.cc
+++ b/subt_ign/src/VisibilityTable.cc
@@ -33,10 +33,14 @@
 #include <subt_ign/VisibilityTable.hh>
 
 #include <fcl/config.h>
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
 #include <fcl/data_types.h>
 #include <fcl/shape/geometric_shapes.h>
-
 #include <fcl/distance.h>
+#else
+#include <fcl/common/types.h>
+#include <fcl/narrowphase/distance.h>
+#endif
 
 using namespace ignition;
 using namespace subt;
@@ -167,7 +171,11 @@ void VisibilityTable::SetModelBoundingBoxes(
 
 //////////////////////////////////////////////////
 void VisibilityTable::SetModelCollisionObjects(
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
     const std::map<std::string, std::shared_ptr<fcl::CollisionObject>> &_collObjs)
+#else
+    const std::map<std::string, std::shared_ptr<fcl::CollisionObjectd>> &_collObjs)
+#endif
 {
   this->collisionObjs= _collObjs;
 }
@@ -209,10 +217,17 @@ uint64_t VisibilityTable::Index(const ignition::math::Vector3d &_position) const
   else
   {
     // Fall back to using FCL to find the closest mesh.
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
     auto box = std::make_shared<fcl::Box>(0.01, 0.01, 0.01);
     auto boxObj = std::make_shared<fcl::CollisionObject>(box,
       fcl::Matrix3f::getIdentity(),
       fcl::Vec3f(_position.X(), _position.Y(), _position.Z()));
+#else
+    auto box = std::make_shared<fcl::Boxd>(0.01, 0.01, 0.01);
+    auto boxObj = std::make_shared<fcl::CollisionObjectd>(box,
+      fcl::Matrix3d::Identity(),
+      fcl::Vector3d(_position.X(), _position.Y(), _position.Z()));
+#endif
 
     uint64_t closestIdx = 0;
     float closestDist = 1e9;
@@ -230,8 +245,13 @@ uint64_t VisibilityTable::Index(const ignition::math::Vector3d &_position) const
       if (meshIt == this->collisionObjs.end())
         continue;
 
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
       fcl::DistanceRequest request(true, true);
       fcl::DistanceResult fclResult;
+#else
+      fcl::DistanceRequestd request(true, true);
+      fcl::DistanceResultd fclResult;
+#endif
       fcl::distance(meshIt->second.get(), boxObj.get(), request, fclResult);
 
       if (fclResult.min_distance < closestDist)
diff --git a/subt_ign/src/ign_to_fcl.cc b/subt_ign/src/ign_to_fcl.cc
index 0628e9cf..7283c679 100644
--- a/subt_ign/src/ign_to_fcl.cc
+++ b/subt_ign/src/ign_to_fcl.cc
@@ -1,9 +1,13 @@
 #include "ign_to_fcl.hh"
 
 #include <fcl/config.h>
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
 #include <fcl/data_types.h>
 #include <fcl/math/matrix_3f.h>
 #include <fcl/math/vec_3f.h>
+#else
+#include <fcl/common/types.h>
+#endif
 
 #include <ignition/common/Mesh.hh>
 #include <ignition/common/SubMesh.hh>
@@ -25,7 +29,11 @@ convert_to_fcl(const ignition::common::Mesh &_mesh)
   for (auto ii = 0u; ii < _mesh.SubMeshCount(); ++ii)
   {
     auto submesh = _mesh.SubMeshByIndex(ii).lock();
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
     std::vector<fcl::Vec3f> vertices;
+#else
+    std::vector<fcl::Vector3d> vertices;
+#endif
     std::vector<fcl::Triangle> triangles;
 
     vertices.reserve(submesh->VertexCount());
@@ -33,7 +41,11 @@ convert_to_fcl(const ignition::common::Mesh &_mesh)
 
     for(size_t jj = 0; jj < submesh->VertexCount(); ++jj)
     {
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
       vertices.push_back(fcl::Vec3f(
+#else
+      vertices.push_back(fcl::Vector3d(
+#endif
             submesh->Vertex(jj).X(),
             submesh->Vertex(jj).Y(),
             submesh->Vertex(jj).Z()));
@@ -55,13 +67,21 @@ convert_to_fcl(const ignition::common::Mesh &_mesh)
   return ret;
 }
 
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
 std::shared_ptr<fcl::CollisionObject>
+#else
+std::shared_ptr<fcl::CollisionObjectd>
+#endif
 convert_to_fcl(const ignition::common::Mesh &_mesh,
                const ignition::math::Pose3d &_pose)
 {
   auto model = convert_to_fcl(_mesh);
 
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
   fcl::Matrix3f rot;
+#else
+  fcl::Matrix3d rot;
+#endif
   for(size_t ii = 0; ii < 3; ++ii)
   {
     for(size_t jj = 0; jj < 3; ++jj)
@@ -70,8 +90,13 @@ convert_to_fcl(const ignition::common::Mesh &_mesh,
     }
   }
 
-  auto obj = std::make_shared<fcl::CollisionObject>(model, rot, 
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
+  auto obj = std::make_shared<fcl::CollisionObject>(model, rot,
       fcl::Vec3f(_pose.Pos().X(), _pose.Pos().Y(), _pose.Pos().Z()));
+#else
+  auto obj = std::make_shared<fcl::CollisionObjectd>(model, rot,
+      fcl::Vector3d(_pose.Pos().X(), _pose.Pos().Y(), _pose.Pos().Z()));
+#endif
   return obj;
 }
 
diff --git a/subt_ign/src/ign_to_fcl.hh b/subt_ign/src/ign_to_fcl.hh
index 4b001a8f..87b2e1b2 100644
--- a/subt_ign/src/ign_to_fcl.hh
+++ b/subt_ign/src/ign_to_fcl.hh
@@ -1,9 +1,16 @@
 #ifndef IGN_TO_FCL_HH_
 #define IGN_TO_FCL_HH_
 
+#include <fcl/config.h>
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
 #include <fcl/collision_object.h>
 #include <fcl/BVH/BVH_model.h>
 #include <fcl/BV/OBBRSS.h>
+#else
+#include <fcl/narrowphase/collision_object.h>
+#include <fcl/geometry/bvh/BVH_model.h>
+#include <fcl/math/bv/OBBRSS.h>
+#endif
 
 #include <ignition/common/Mesh.hh>
 #include <ignition/math/Pose3.hh>
@@ -18,7 +25,11 @@ namespace subt
 /// BVH = Bounding Volume Heirarchy
 /// OBB = Oriented Bounding Box
 /// RSS = Rectange Swept Sphere
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
 using Model = fcl::BVHModel<fcl::OBBRSS>;
+#else
+using Model = fcl::BVHModel<fcl::OBBRSSd>;
+#endif
 
 /// \brief Create an fcl model from an ignition mesh.
 ///
@@ -36,7 +47,11 @@ convert_to_fcl(const ignition::common::Mesh &_mesh);
 /// \param[in] _mesh mesh to convert to fcl model.
 /// \param[in] _pose world pose of the model.
 /// \return collision object when successfully converted otherwise nullptr
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
 std::shared_ptr<fcl::CollisionObject>
+#else
+std::shared_ptr<fcl::CollisionObjectd>
+#endif
 convert_to_fcl(const ignition::common::Mesh &_mesh,
                const ignition::math::Pose3d &_pose);