diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp index a141b9f7e5..9d4eeb4e32 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -950,8 +951,9 @@ 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 { @@ -959,6 +961,12 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } 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); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index cccc55d7a8..4df1720218 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/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& diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 22cf5c8bc6..94c2fe5501 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -34,6 +34,7 @@ /* Author: Ioan Sucan */ +#include #include #include #include @@ -44,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +53,55 @@ #include #include +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 fresh_allocations_{ 0 }; + mutable std::atomic 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(); + auto root = std::make_shared(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(); + auto scene = std::make_shared(moveit::core::loadTestingRobotModel("panda")); + scene->allocateCollisionDetector(allocator); + + std::vector 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(); + auto source = std::make_shared(moveit::core::loadTestingRobotModel("panda")); + source->allocateCollisionDetector(allocator); + source->getCollisionEnvNonConst()->setLinkPadding("panda_link0", 0.1); + source->getCollisionEnvNonConst()->setLinkScale("panda_link0", 1.1); + std::weak_ptr source_scene = source; + std::weak_ptr source_collision_environment = source->getCollisionEnv(); + std::weak_ptr 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");