Build unpadded collision environments lazily

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp

@@ -54,6 +54,7 @@
 #include <memory>
 #include <functional>
 #include <optional>
+#include <mutex>
 #include <thread>
 #include <variant>
 #include <optional>
@@ -950,15 +951,22 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
     collision_detection::CollisionEnvPtr cenv_;  // never nullptr
     collision_detection::CollisionEnvConstPtr cenv_const_;
 
-    collision_detection::CollisionEnvPtr cenv_unpadded_;
-    collision_detection::CollisionEnvConstPtr cenv_unpadded_const_;
+    mutable collision_detection::CollisionEnvPtr cenv_unpadded_;
+    mutable collision_detection::CollisionEnvConstPtr cenv_unpadded_const_;
+    mutable std::once_flag cenv_unpadded_once_;
 
     const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const
     {
       return cenv_const_;
     }
     const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const
     {
+      std::call_once(cenv_unpadded_once_, [this]() {
+        cenv_unpadded_ = alloc_->allocateEnv(cenv_, cenv_->getWorld());
+        cenv_unpadded_->setPadding(0.0);
+        cenv_unpadded_->setScale(1.0);
+        cenv_unpadded_const_ = cenv_unpadded_;
+      });
       return cenv_unpadded_const_;
     }
     void copyPadding(const CollisionDetector& src);

moveit_core/planning_scene/src/planning_scene.cpp

@@ -267,22 +267,17 @@ void PlanningScene::allocateCollisionDetector(const collision_detection::Collisi
   if (parent_detector)
   {
     collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(parent_detector->cenv_, world_);
-    collision_detector_->cenv_unpadded_ =
-        collision_detector_->alloc_->allocateEnv(parent_detector->cenv_unpadded_, world_);
   }
   else
   {
     collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel());
-    collision_detector_->cenv_unpadded_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel());
 
     // Copy padding to collision_detector_->cenv_
     if (prev_coll_detector)
       collision_detector_->copyPadding(*prev_coll_detector);
   }
-
   // Assign const pointers
   collision_detector_->cenv_const_ = collision_detector_->cenv_;
-  collision_detector_->cenv_unpadded_const_ = collision_detector_->cenv_unpadded_;
 }
 
 const collision_detection::CollisionEnvConstPtr&

moveit_core/planning_scene/test/test_planning_scene.cpp

@@ -34,6 +34,7 @@
 
 /* Author: Ioan Sucan */
 
+#include <atomic>
 #include <cstdint>
 #include <gtest/gtest.h>
 #include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.hpp>
@@ -44,13 +45,63 @@
 #include <fstream>
 #include <sstream>
 #include <string>
+#include <thread>
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <octomap_msgs/conversions.h>
 #include <octomap/octomap.h>
 
 #include <moveit/collision_detection/collision_common.hpp>
 #include <moveit/collision_detection/collision_plugin_cache.hpp>
 
+namespace
+{
+class CountingCollisionDetectorAllocator : public collision_detection::CollisionDetectorAllocator
+{
+public:
+  const std::string& getName() const override
+  {
+    static const std::string name = "CountingFCL";
+    return name;
+  }
+
+  collision_detection::CollisionEnvPtr allocateEnv(const collision_detection::WorldPtr& world,
+                                                   const moveit::core::RobotModelConstPtr& robot_model) const override
+  {
+    ++fresh_allocations_;
+    return delegate_->allocateEnv(world, robot_model);
+  }
+
+  collision_detection::CollisionEnvPtr allocateEnv(const collision_detection::CollisionEnvConstPtr& original,
+                                                   const collision_detection::WorldPtr& world) const override
+  {
+    ++copy_allocations_;
+    return delegate_->allocateEnv(original, world);
+  }
+
+  collision_detection::CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const override
+  {
+    ++fresh_allocations_;
+    return delegate_->allocateEnv(robot_model);
+  }
+
+  std::size_t freshAllocations() const
+  {
+    return fresh_allocations_;
+  }
+
+  std::size_t copyAllocations() const
+  {
+    return copy_allocations_;
+  }
+
+private:
+  collision_detection::CollisionDetectorAllocatorPtr delegate_ =
+      collision_detection::CollisionDetectorAllocatorFCL::create();
+  mutable std::atomic<std::size_t> fresh_allocations_{ 0 };
+  mutable std::atomic<std::size_t> copy_allocations_{ 0 };
+};
+}  // namespace
+
 // Test not setting the object's pose should use the shape pose as the object pose
 TEST(PlanningScene, TestOneShapeObjectPose)
 {
@@ -205,6 +256,70 @@ TEST(PlanningScene, LoadRestoreDiff)
   EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 2u);
 }
 
+TEST(PlanningScene, UnpaddedCollisionEnvironmentIsLazyAcrossNestedDiffs)
+{
+  auto allocator = std::make_shared<CountingCollisionDetectorAllocator>();
+  auto root = std::make_shared<planning_scene::PlanningScene>(moveit::core::loadTestingRobotModel("panda"));
+  root->allocateCollisionDetector(allocator);
+
+  EXPECT_EQ(allocator->freshAllocations(), 1u);
+  EXPECT_EQ(allocator->copyAllocations(), 0u);
+
+  planning_scene::PlanningScenePtr child = root->diff();
+  EXPECT_EQ(allocator->freshAllocations(), 1u);
+  EXPECT_EQ(allocator->copyAllocations(), 1u);
+
+  planning_scene::PlanningScenePtr grandchild = child->diff();
+  EXPECT_EQ(allocator->freshAllocations(), 1u);
+  EXPECT_EQ(allocator->copyAllocations(), 2u);
+
+  grandchild->getCollisionEnvUnpadded();
+  EXPECT_EQ(allocator->freshAllocations(), 1u);
+  EXPECT_EQ(allocator->copyAllocations(), 3u);
+}
+
+TEST(PlanningScene, UnpaddedCollisionEnvironmentIsInitializedOnceAcrossThreads)
+{
+  auto allocator = std::make_shared<CountingCollisionDetectorAllocator>();
+  auto scene = std::make_shared<planning_scene::PlanningScene>(moveit::core::loadTestingRobotModel("panda"));
+  scene->allocateCollisionDetector(allocator);
+
+  std::vector<std::thread> threads;
+  for (std::size_t i = 0; i < 8; ++i)
+    threads.emplace_back([scene]() { scene->getCollisionEnvUnpadded(); });
+  for (std::thread& thread : threads)
+    thread.join();
+
+  EXPECT_EQ(allocator->freshAllocations(), 1u);
+  EXPECT_EQ(allocator->copyAllocations(), 1u);
+}
+
+TEST(PlanningScene, DetachedSceneDoesNotRetainDeferredCollisionEnvironmentSource)
+{
+  auto allocator = std::make_shared<CountingCollisionDetectorAllocator>();
+  auto source = std::make_shared<planning_scene::PlanningScene>(moveit::core::loadTestingRobotModel("panda"));
+  source->allocateCollisionDetector(allocator);
+  source->getCollisionEnvNonConst()->setLinkPadding("panda_link0", 0.1);
+  source->getCollisionEnvNonConst()->setLinkScale("panda_link0", 1.1);
+  std::weak_ptr<const planning_scene::PlanningScene> source_scene = source;
+  std::weak_ptr<const collision_detection::CollisionEnv> source_collision_environment = source->getCollisionEnv();
+  std::weak_ptr<const collision_detection::World> source_world = source->getWorld();
+
+  planning_scene::PlanningScenePtr detached = planning_scene::PlanningScene::clone(source);
+  source.reset();
+
+  EXPECT_TRUE(source_scene.expired());
+  EXPECT_TRUE(source_collision_environment.expired());
+  EXPECT_TRUE(source_world.expired());
+  EXPECT_EQ(allocator->freshAllocations(), 1u);
+  EXPECT_EQ(allocator->copyAllocations(), 1u);
+
+  EXPECT_DOUBLE_EQ(detached->getCollisionEnvUnpadded()->getLinkPadding("panda_link0"), 0.0);
+  EXPECT_DOUBLE_EQ(detached->getCollisionEnvUnpadded()->getLinkScale("panda_link0"), 1.0);
+  EXPECT_EQ(allocator->freshAllocations(), 1u);
+  EXPECT_EQ(allocator->copyAllocations(), 2u);
+}
+
 TEST(PlanningScene, MakeAttachedDiff)
 {
   urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");