Fix bullet constructor not updating world objects

moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp

@@ -80,15 +80,16 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m
 CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world)
   : CollisionEnv(other, world)
 {
-  // TODO(j-petit): Verify this constructor
-
   // request notifications about changes to new world
   observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2));
 
   for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : other.robot_model_->getURDF()->links_)
   {
     addLinkAsCollisionObject(link.second);
   }
+
+  // get notifications any objects already in the new world
+  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
 }
 
 CollisionEnvBullet::~CollisionEnvBullet()

moveit_core/planning_scene/test/test_multi_threaded.cpp

@@ -42,8 +42,7 @@
 #include <thread>
 
 #include <moveit/collision_detection/collision_common.h>
-#include <moveit/collision_detection/collision_env.h>
-#include <moveit/collision_detection/collision_detector_allocator.h>
+#include <moveit/collision_detection/collision_plugin_cache.h>
 
 const int TRIALS = 1000;
 const int THREADS = 2;
@@ -67,7 +66,7 @@ void runCollisionDetectionAssert(unsigned int id, unsigned int trials, const pla
   ASSERT_EQ(expected_result, runCollisionDetection(id, trials, scene, state));
 }
 
-class CollisionDetectorThreadedTest : public testing::Test
+class CollisionDetectorTests : public testing::TestWithParam<const char*>
 {
 protected:
   void SetUp() override
@@ -96,13 +95,26 @@ class CollisionDetectorThreadedTest : public testing::Test
   planning_scene::PlanningScenePtr planning_scene_;
 };
 
-/** \brief Tests the FCL collision detector in multiple threads. */
-TEST_F(CollisionDetectorThreadedTest, FCLThreaded)
+/** \brief Tests the collision detector in multiple threads. */
+TEST_P(CollisionDetectorTests, Threaded)
 {
+  const std::string plugin_name = GetParam();
+  SCOPED_TRACE(plugin_name);
+
   std::vector<moveit::core::RobotStatePtr> states;
   std::vector<std::thread*> threads;
   std::vector<bool> collisions;
 
+  collision_detection::CollisionPluginCache loader;
+  if (!loader.activate(plugin_name, planning_scene_, true))
+  {
+#if defined(GTEST_SKIP_)
+    GTEST_SKIP_("Failed to load collision plugin");
+#else
+    return;
+#endif
+  }
+
   for (unsigned int i = 0; i < THREADS; ++i)
   {
     moveit::core::RobotState* state = new moveit::core::RobotState(planning_scene_->getRobotModel());
@@ -122,8 +134,17 @@ TEST_F(CollisionDetectorThreadedTest, FCLThreaded)
     threads[i]->join();
     delete threads[i];
   }
+
+  planning_scene_.reset();
 }
 
+#ifndef INSTANTIATE_TEST_SUITE_P  // prior to gtest 1.10
+#define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
+#endif
+
+// instantiate parameterized tests for common collision plugins
+INSTANTIATE_TEST_SUITE_P(PluginTests, CollisionDetectorTests, testing::Values("FCL", "Bullet"));
+
 int main(int argc, char** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

moveit_core/planning_scene/test/test_planning_scene.cpp

@@ -45,6 +45,9 @@
 #include <boost/filesystem/path.hpp>
 #include <ros/package.h>
 
+#include <moveit/collision_detection/collision_common.h>
+#include <moveit/collision_detection/collision_plugin_cache.h>
+
 TEST(PlanningScene, LoadRestore)
 {
   urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
@@ -218,6 +221,101 @@ TEST(PlanningScene, loadBadSceneGeometry)
   EXPECT_FALSE(ps->loadGeometryFromStream(malformed_scene_geometry));
 }
 
+class CollisionDetectorTests : public testing::TestWithParam<const char*>
+{
+};
+TEST_P(CollisionDetectorTests, ClearDiff)
+{
+  const std::string plugin_name = GetParam();
+  SCOPED_TRACE(plugin_name);
+
+  urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
+  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  // create parent scene
+  planning_scene::PlanningScenePtr parent = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
+
+  collision_detection::CollisionPluginCache loader;
+  if (!loader.activate(plugin_name, parent, true))
+  {
+#if defined(GTEST_SKIP_)
+    GTEST_SKIP_("Failed to load collision plugin");
+#else
+    return;
+#endif
+  }
+
+  // create child scene
+  planning_scene::PlanningScenePtr child = parent->diff();
+
+  // create collision request variables
+  collision_detection::CollisionRequest req;
+  collision_detection::CollisionResult res;
+  moveit::core::RobotState* state = new moveit::core::RobotState(child->getRobotModel());
+  state->setToDefaultValues();
+  state->update();
+
+  // there should be no collision with the environment
+  res.clear();
+  parent->getCollisionEnv()->checkRobotCollision(req, res, *state, parent->getAllowedCollisionMatrix());
+  EXPECT_FALSE(res.collision);
+  res.clear();
+  child->getCollisionEnv()->checkRobotCollision(req, res, *state, child->getAllowedCollisionMatrix());
+  EXPECT_FALSE(res.collision);
+
+  // create message to add a collision object at the world origin
+  moveit_msgs::PlanningScene ps_msg;
+  ps_msg.is_diff = false;
+  moveit_msgs::CollisionObject co;
+  co.header.frame_id = "base_link";
+  co.operation = moveit_msgs::CollisionObject::ADD;
+  co.id = "box";
+  co.pose.orientation.w = 1.0;
+  {
+    shape_msgs::SolidPrimitive sp;
+    sp.type = shape_msgs::SolidPrimitive::BOX;
+    sp.dimensions = { 1., 1., 1. };
+    co.primitives.push_back(sp);
+    geometry_msgs::Pose sp_pose;
+    sp_pose.orientation.w = 1.0;
+    co.primitive_poses.push_back(sp_pose);
+  }
+  ps_msg.world.collision_objects.push_back(co);
+
+  // add object to the parent planning scene
+  parent->usePlanningSceneMsg(ps_msg);
+
+  // the parent scene should be in collision
+  res.clear();
+  parent->getCollisionEnv()->checkRobotCollision(req, res, *state, parent->getAllowedCollisionMatrix());
+  EXPECT_TRUE(res.collision);
+
+  // the child scene was not updated yet, so no collision
+  res.clear();
+  child->getCollisionEnv()->checkRobotCollision(req, res, *state, child->getAllowedCollisionMatrix());
+  EXPECT_FALSE(res.collision);
+
+  // update the child scene
+  child->clearDiffs();
+
+  // child and parent scene should be in collision
+  res.clear();
+  parent->getCollisionEnv()->checkRobotCollision(req, res, *state, parent->getAllowedCollisionMatrix());
+  EXPECT_TRUE(res.collision);
+  res.clear();
+  child->getCollisionEnv()->checkRobotCollision(req, res, *state, child->getAllowedCollisionMatrix());
+  EXPECT_TRUE(res.collision);
+
+  child.reset();
+  parent.reset();
+}
+
+#ifndef INSTANTIATE_TEST_SUITE_P  // prior to gtest 1.10
+#define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
+#endif
+
+// instantiate parameterized tests for common collision plugins
+INSTANTIATE_TEST_SUITE_P(PluginTests, CollisionDetectorTests, testing::Values("FCL", "Bullet"));
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);