Signed distance with halfspace unsupported - give good message

FCL currently doesn't support signed distance queries with halfspaces (and
other primitives). Rather than exploding in a segfault, we're catching the
failure condition in drake and throwing an exception with some guidance-
offering message.

geometry/proximity_engine.cc

@@ -465,6 +465,24 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd* a,
                                 fcl::DistanceResultd* result) {
   const fcl::CollisionGeometryd* a_geometry = a->collisionGeometry().get();
   const fcl::CollisionGeometryd* b_geometry = b->collisionGeometry().get();
+
+  // We use FCL's GJK/EPA fallback in those geometries we haven't explicitly
+  // supported. However, FCL doesn't support: half spaces, planes, triangles, or
+  // octtrees in that workflow. We need to give intelligent feedback rather than
+  // the segfault otherwise produced.
+  // NOTE: Currently this only tests for halfspace (because it is an otherwise
+  // supported geometry type in SceneGraph. When meshes, planes, and/or octrees
+  // are supported, this error would have to be modified.
+  // TODO(SeanCurtis-TRI): Remove this test when FCL supports signed distance
+  // queries for halfspaces (see issue #10905). Also see FCL issue
+  // https://github.com/flexible-collision-library/fcl/issues/383.
+  if (a_geometry->getNodeType() == fcl::GEOM_HALFSPACE ||
+      b_geometry->getNodeType() == fcl::GEOM_HALFSPACE) {
+    throw std::logic_error(
+        "Signed distance queries on halfspaces are not currently supported. "
+        "Try using a large box instead.");
+  }
+
   const RigidTransformd X_WA(a->getTransform());
   const RigidTransformd X_WB(b->getTransform());
   const auto id_A = EncodedData(*a).id(geometry_map);
@@ -479,17 +497,16 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd* a,
           auto sphere_A = static_cast<const fcl::Sphered*>(a_geometry);
           auto sphere_B = static_cast<const fcl::Sphered*>(b_geometry);
           distance_pair(sphere_A, sphere_B);
-          break;
+          return;
         }
         case fcl::GEOM_BOX: {
           auto sphere_A = static_cast<const fcl::Sphered*>(a_geometry);
           auto box_B = static_cast<const fcl::Boxd*>(b_geometry);
           distance_pair(sphere_A, box_B);
-          break;
-        }
-        default: {
-          fcl::distance(a, b, request, *result);
+          return;
         }
+        default:
+          break;
       }
       break;
     }
@@ -499,18 +516,17 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd* a,
           auto box_A = static_cast<const fcl::Boxd*>(a_geometry);
           auto sphere_B = static_cast<const fcl::Sphered*>(b_geometry);
           distance_pair(box_A, sphere_B);
-          break;
-        }
-        default: {
-          fcl::distance(a, b, request, *result);
+          return;
         }
+        default:
+          break;
       }
       break;
     }
-    default: {
-      fcl::distance(a, b, request, *result);
-    }
+    default:
+      break;
   }
+  fcl::distance(a, b, request, *result);
 }
 
 // The callback function in fcl::distance request. The final unnamed parameter

geometry/test/proximity_engine_test.cc

@@ -1715,6 +1715,32 @@ INSTANTIATE_TEST_CASE_P(SphereBoxBoundary, SignedDistancePairTest,
 INSTANTIATE_TEST_CASE_P(SphereBoxBoundaryTransform, SignedDistancePairTest,
     testing::ValuesIn(GenDistPairTestSphereBoxBoundaryTransform()));
 
+// This tests that signed distance queries against a halfspace throw an
+// intelligible exception rather than a segfault.
+GTEST_TEST(SignedDistancePairError, HalfspaceException) {
+  // Note: this doesn't fully test the condition for emitting the error; we
+  // have no *real* control over which geometry is the first geometry and
+  // second geometry in the pair being tested. However, this test isn't
+  // intended to be long-lasting; we want to correct FCL's short-coming soon
+  // so that the behavior (and this test) can be removed.
+  ProximityEngine<double> engine;
+  std::vector<GeometryId> geometry_map;
+
+  Sphere sphere{0.5};
+  engine.AddDynamicGeometry(sphere, GeometryIndex(0));
+  geometry_map.push_back(GeometryId::get_new_id());
+
+  HalfSpace halfspace;
+  engine.AddAnchoredGeometry(halfspace, Isometry3d::Identity(),
+                             GeometryIndex(1));
+  geometry_map.push_back(GeometryId::get_new_id());
+
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      engine.ComputeSignedDistancePairwiseClosestPoints(geometry_map),
+      std::logic_error,
+      "Signed distance .* halfspaces .* not .* supported.* Try .* box .*");
+}
+
 // Concentric geometries A, B do not have a unique pair of witness points
 // Na, Nb. We inherit another test fixture from SignedDistancePairTest, so we
 // can define another TEST_P that checks |Na-Nb| = -signed_distance but does