backport collide bitmask changes

Signed-off-by: Ian Chen <[email protected]>

CMakeLists.txt

@@ -87,7 +87,7 @@ ign_find_package (Qt5
 
 #--------------------------------------
 # Find ignition-physics
-ign_find_package(ignition-physics2
+ign_find_package(ignition-physics2 VERSION 2.1
   COMPONENTS
     mesh
     sdf

Changelog.md

@@ -1,5 +1,10 @@
 ## Ignition Gazebo 3.x
 
+### Ignition Gazebo 3.x.x (20xx-xx-xx)
+
+1. Backport collision bitmask changes
+    * [pull request 223](https://github.com/ignitionrobotics/ign-gazebo/pull/223)
+
 ### Ignition Gazebo 3.2.0 (2020-05-20)
 
 1. Merge ign-gazebo2 to ign-gazebo3

src/systems/physics/Physics.cc

@@ -66,6 +66,7 @@
 #include <sdf/Link.hh>
 #include <sdf/Mesh.hh>
 #include <sdf/Model.hh>
+#include <sdf/Surface.hh>
 #include <sdf/World.hh>
 
 #include "ignition/gazebo/EntityComponentManager.hh"
@@ -348,6 +349,24 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// that here they've been casted for `CollisionFeatureList`.
   public: std::unordered_map<Entity, WorldShapePtrType> entityWorldCollisionMap;
 
+  //////////////////////////////////////////////////
+  // Collision filtering with bitmasks
+
+  /// \brief Feature list to filter collisions with bitmasks.
+  public: using CollisionMaskFeatureList = ignition::physics::FeatureList<
+          CollisionFeatureList,
+          ignition::physics::CollisionFilterMaskFeature>;
+
+  /// \brief Collision type with collision filtering features.
+  public: using ShapeFilterMaskPtrType = ignition::physics::ShapePtr<
+            ignition::physics::FeaturePolicy3d, CollisionMaskFeatureList>;
+
+  /// \brief A map between collision entity ids in the ECM to Shape Entities in
+  /// ign-physics, with collision filtering feature.
+  /// All links on this map are also in `entityCollisionMap`. The difference is
+  /// that here they've been casted for `CollisionMaskFeatureList`.
+  public: std::unordered_map<Entity, ShapeFilterMaskPtrType> entityShapeMaskMap;
+
   //////////////////////////////////////////////////
   // Link force
 
@@ -743,6 +762,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         sdf::Collision collision = _collElement->Data();
         collision.SetRawPose(_pose->Data());
         collision.SetPoseRelativeTo("");
+        auto collideBitmask = collision.Surface()->Contact()->CollideBitmask();
 
         ShapePtrType collisionPtrPhys;
         if (_geom->Data().Type() == sdf::GeometryType::MESH)
@@ -807,6 +827,25 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           collisionPtrPhys =
               linkCollisionFeature->ConstructCollision(collision);
         }
+        // Check that the physics engine has a filter mask feature
+        // Set the collide_bitmask if it does
+        auto filterMaskFeature = entityCast(_parent->Data(), collisionPtrPhys,
+            entityShapeMaskMap);
+        if (filterMaskFeature)
+        {
+          filterMaskFeature->SetCollisionFilterMask(collideBitmask);
+        }
+        else
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to set collision bitmasks, but the physics "
+                   << "engine doesn't support feature [CollisionFilterMask]. "
+                   << "Collision bitmasks will be ignored." << std::endl;
+            informed = true;
+          }
+        }
 
         this->entityCollisionMap.insert(
             std::make_pair(_entity, collisionPtrPhys));

src/systems/physics/Physics.hh

@@ -103,11 +103,7 @@ namespace systems
     castEntity =
         physics::RequestFeatures<ToFeatureList>::From(_minimumEntity);
 
-    if (!castEntity)
-    {
-      ignwarn << "Physics engine missing requested feature." << std::endl;
-    }
-    else
+    if (castEntity)
     {
       _castMap.insert(std::make_pair(_entity, castEntity));
     }