Visibility table enhancements

subt_ign/CMakeLists.txt

@@ -26,7 +26,7 @@ set(IGN_GAZEBO_VER ${ignition-gazebo2_VERSION_MAJOR})
 
 file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include)
 
-find_package(ignition-common3 REQUIRED)
+find_package(ignition-common3 REQUIRED COMPONENTS graphics)
 find_package(ignition-math6 REQUIRED)
 find_package(ignition-msgs4 REQUIRED)
 find_package(ignition-transport7 REQUIRED)
@@ -35,6 +35,12 @@ find_package(ignition-launch1 REQUIRED)
 find_package(sdformat8 REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(LIBFCL_PC REQUIRED fcl)
+# find *absolute* paths to LIBFCL_INCLUDE_DIRS and LIBFCL_LIBRARIES
+find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS})
+find_library(LIBFCL_LIBRARIES fcl HINTS ${LIBFCL_PC_LIBRARY_DIRS})
+
 ###########
 ## Build ##
 ###########
@@ -46,6 +52,8 @@ include_directories(
   ${CATKIN_DEVEL_PREFIX}/include
 )
 
+message(STATUS ${LIBFCL_INCLUDE_DIRS})
+
 link_directories()
 
 catkin_package(
@@ -178,13 +186,33 @@ install(TARGETS validate_connections
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
 
-add_executable(validate_visibility_table
+add_library(Visibility
+  STATIC
+  src/ign_to_fcl.cc
+  src/SimpleDOTParser.cc
+  src/VisibilityRfModel.cc
   src/VisibilityTable.cc
-  src/SimpleDOTParser.cc)
+)
+target_include_directories(Visibility
+  PUBLIC
+  ${catkin_INCLUDE_DIRS}
+  ${LIBFCL_INCLUDE_DIRS}
+)
+target_link_libraries(
+  Visibility
+  PUBLIC
+  ignition-gazebo${IGN_GAZEBO_VER}::core
+  ignition-common3::ignition-common3
+  ignition-common3::graphics
+  ${LIBFCL_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(validate_visibility_table
+  src/validate_visibility_table.cc)
 target_link_libraries(validate_visibility_table
-  PRIVATE
-    ignition-gazebo${IGN_GAZEBO_VER}::core
-    ${catkin_LIBRARIES}
+  PRIVATE 
+  Visibility
 )
 install(TARGETS validate_visibility_table
   ARCHIVE DESTINATION lib
@@ -195,9 +223,6 @@ install(TARGETS validate_visibility_table
 set(comms_broker_plugin_name CommsBrokerPlugin)
 add_library(${comms_broker_plugin_name}
   src/CommsBrokerPlugin.cc
-  src/VisibilityRfModel.cc
-  src/VisibilityTable.cc
-  src/SimpleDOTParser.cc
 )
 target_include_directories(${comms_broker_plugin_name}
   PRIVATE ${CATKIN_DEVEL_PREFIX}/include)
@@ -211,7 +236,7 @@ target_link_libraries(${comms_broker_plugin_name}
   ignition-transport7::ignition-transport7
   sdformat8::sdformat8
   ${protobuf_lib_name}
-  ${catkin_LIBRARIES}
+  Visibility
 )
 install(TARGETS ${comms_broker_plugin_name}
   ARCHIVE DESTINATION lib
@@ -222,14 +247,9 @@ install(TARGETS ${comms_broker_plugin_name}
 set(visibility_plugin_name VisibilityPlugin)
 add_library(${visibility_plugin_name} SHARED
   src/VisibilityPlugin.cc
-  src/VisibilityTable.cc
-  src/SimpleDOTParser.cc
 )
-target_include_directories(${visibility_plugin_name}
-  PRIVATE ${CATKIN_DEVEL_PREFIX}/include)
 target_link_libraries(${visibility_plugin_name}
   PRIVATE
-  ignition-gazebo${IGN_GAZEBO_VER}::core
   ignition-common3::ignition-common3
   ignition-math6::ignition-math6
   ignition-msgs4::ignition-msgs4
@@ -238,7 +258,7 @@ target_link_libraries(${visibility_plugin_name}
   ignition-transport7::ignition-transport7
   sdformat8::sdformat8
   ${protobuf_lib_name}
-  ${catkin_LIBRARIES}
+  Visibility
 )
 install(TARGETS ${visibility_plugin_name}
   ARCHIVE DESTINATION lib

subt_ign/include/subt_ign/VisibilityTable.hh

@@ -27,6 +27,11 @@
 #include <ignition/math/graph/Vertex.hh>
 #include <subt_ign/VisibilityTypes.hh>
 
+namespace fcl
+{
+  class CollisionObject;
+}
+
 namespace subt
 {
   /// \brief This class stores the connectivity between pairs of sections of a
@@ -85,6 +90,12 @@ namespace subt
     public: void SetModelBoundingBoxes(
         const std::map<std::string, ignition::math::AxisAlignedBox> &_boxes);
 
+    /// \brief Set the collision objects of models. Used for generating LUT
+    /// \param[in] _collObjs Collision Objects of models.
+    /// \sa Generate
+    public: void SetModelCollisionObjects(
+        const std::map<std::string, std::shared_ptr<fcl::CollisionObject>> &_collObjs);
+
     /// \brief Get the collection of sampled 3D points and their associated
     /// vertex id.
     /// \return the collection.
@@ -197,9 +208,17 @@ namespace subt
     private: std::vector<
              std::pair<ignition::math::AxisAlignedBox, uint64_t>> worldSegments;
 
+    /// \brief A map between each segment's id and the corresponding name.
+    /// This is used for looking up the corresponding names in generating LUT.
+    private: std::map<uint64_t, std::string> worldSegmentNames;
+
     /// \brief A map of model name to its bounding box. Used for generating LUT
     private: std::map<std::string, ignition::math::AxisAlignedBox> bboxes;
 
+    /// \brief A map of model name to its bounding box. Used for generating LUT
+    private: std::map<std::string, 
+             std::shared_ptr<fcl::CollisionObject>> collisionObjs;
+
     /// \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;
 

subt_ign/package.xml

@@ -17,5 +17,9 @@
   <depend>subt_rf_interface</depend>
   <depend>subt_communication_model</depend>
   <depend>subt_communication_broker</depend>
+
+  <depend>libccd-dev</depend>
+  <depend>libfcl-dev</depend>
+
   <test_depend>rostest</test_depend>
 </package>

subt_ign/src/VisibilityPlugin.cc

@@ -20,19 +20,28 @@
 #include <ignition/math/AxisAlignedBox.hh>
 
 #include <ignition/gazebo/components/AxisAlignedBox.hh>
+#include <ignition/gazebo/components/Collision.hh>
 #include <ignition/gazebo/components/Model.hh>
 #include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/Geometry.hh> 
+#include <ignition/gazebo/components/ParentEntity.hh>
 #include <ignition/gazebo/components/Pose.hh>
 #include <ignition/gazebo/components/Static.hh>
 #include <ignition/gazebo/components/World.hh>
 #include <ignition/gazebo/Conversions.hh>
 #include <ignition/gazebo/EntityComponentManager.hh>
 #include <ignition/gazebo/Util.hh>
 
+#include <sdf/Mesh.hh>
+#include <ignition/common/MeshManager.hh>
+#include <ignition/common/Mesh.hh>
+#include <ignition/common/SubMesh.hh>
 
 #include "subt_ign/VisibilityTable.hh"
 #include "subt_ign/VisibilityPlugin.hh"
 
+#include "ign_to_fcl.hh"
+
 IGNITION_ADD_PLUGIN(
     subt::VisibilityPlugin,
     ignition::gazebo::System,
@@ -51,6 +60,8 @@ class subt::VisibilityPluginPrivate
   /// \brief A map of model name ot its bounding box
   public: std::map<std::string, ignition::math::AxisAlignedBox> bboxes;
 
+  public: std::map<std::string, std::shared_ptr<fcl::CollisionObject>> fclObjs;
+
   /// \brief Name of the world
   public: std::string worldName;
 };
@@ -132,6 +143,47 @@ void VisibilityPlugin::PostUpdate(
   if (s < 1)
     return;
 
+  _ecm.Each<gazebo::components::Collision,
+            gazebo::components::Name,
+            gazebo::components::Geometry,
+            gazebo::components::ParentEntity>(
+      [&](const Entity &,
+          const components::Collision *,
+          const components::Name *_name,
+          const components::Geometry *_geom,
+          const components::ParentEntity *_parent) -> bool
+      {
+        if (_geom->Data().Type() == sdf::GeometryType::MESH)
+        {
+          const sdf::Mesh *meshSdf = _geom->Data().MeshShape();
+          if (nullptr == meshSdf)
+          {
+            ignwarn << "Mesh geometry for collision [" << _name->Data()
+                    << "] missing mesh shape." << std::endl;
+            return true;
+          }
+
+          auto &meshManager = *ignition::common::MeshManager::Instance();
+          auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath());
+          auto *mesh = meshManager.Load(fullPath);
+          if (nullptr == mesh)
+          {
+            ignwarn << "Failed to load mesh from [" << fullPath
+                    << "]." << std::endl;
+            return true;
+          }
+
+          auto gP = _ecm.Component<gazebo::components::ParentEntity>(_parent->Data())->Data();
+          auto parentPose = _ecm.Component<gazebo::components::Pose>(gP)->Data();
+          auto parentName = _ecm.Component<gazebo::components::Name>(gP)->Data();
+          auto collisionObj = convert_to_fcl(*mesh, parentPose); 
+          collisionObj->computeAABB();
+          this->dataPtr->fclObjs[parentName] = collisionObj;
+        }
+
+        return true;
+      });
+
   // get all the bounding boxes
   _ecm.Each<gazebo::components::Model,
             gazebo::components::Name,
@@ -145,13 +197,30 @@ void VisibilityPlugin::PostUpdate(
       {
         // todo store bboxes instead
         this->dataPtr->bboxes[_nameComp->Data()] = _aabb->Data();
+
+        auto it = this->dataPtr->fclObjs.find(_nameComp->Data());
+        if (it == this->dataPtr->fclObjs.end())
+        {
+          std::cout << "Couldn't find fcl obj for: " << _nameComp->Data() << std::endl;
+          return true;
+        }
+
+        auto fclAABB = it->second->getAABB();
+
+        std::cout << _nameComp->Data() << std::endl;
+        std::cout << fclAABB.center()[0] << " " 
+                  << fclAABB.center()[1] << " " 
+                  << fclAABB.center()[2] << std::endl;
+        std::cout << _aabb->Data().Center().X() << " " << _aabb->Data().Center().Y()  << " " << _aabb->Data().Center().Z() << std::endl;
+
         return true;
       });
 
   // generate the LUT
   subt::VisibilityTable table;
   table.Load(this->dataPtr->worldName, false);
   table.SetModelBoundingBoxes(this->dataPtr->bboxes);
+  table.SetModelCollisionObjects(this->dataPtr->fclObjs);
   table.Generate();
 
   // Send SIGINT to terminate Gazebo.