From 9244c8284df8b20f601d9dc4590d96a762ea948e Mon Sep 17 00:00:00 2001 From: agentperfopt <295481948+agentperfopt@users.noreply.github.com> Date: Fri, 24 Jul 2026 00:49:19 -0400 Subject: [PATCH 1/3] Build the unpadded collision env lazily in allocateCollisionDetector allocateCollisionDetector runs on every diff()/clearDiffs(), which is frequent during planning, and builds two full collision environments every time: cenv_ (padded) and cenv_unpadded_. The unpadded one is only read by unpadded self-collision/distance checks (checkSelfCollision unpadded, distanceRobot), which most planning never calls, so on the common path it's built and never touched. Defer building cenv_unpadded_ to first use instead of building it eagerly in allocateCollisionDetector. getCollisionEnvUnpadded() now builds it on first call and caches the result, copy-constructing from the parent's unpadded env for child scenes (same as before, just later) or from the world/model directly for root scenes. Once built it's cached exactly like before; scenes that never query it just never pay for it. --- .../moveit/planning_scene/planning_scene.hpp | 15 +++++++++++++-- moveit_core/planning_scene/src/planning_scene.cpp | 8 ++++---- 2 files changed, 17 insertions(+), 6 deletions(-) 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..0480585368 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 @@ -950,8 +950,12 @@ 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_; + // Unpadded env is built LAZILY on first use (see getCollisionEnvUnpadded); many scenes never query it. + mutable collision_detection::CollisionEnvPtr cenv_unpadded_; + mutable collision_detection::CollisionEnvConstPtr cenv_unpadded_const_; + // Source for the deferred unpadded env: parent's unpadded env (child scenes) or null (build from model). + collision_detection::CollisionEnvConstPtr unpadded_parent_src_; + collision_detection::CollisionDetectorAllocatorPtr unpadded_alloc_; const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const { @@ -959,6 +963,13 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const { + if (!cenv_unpadded_const_) // lazy: build on first access, then cache + { + cenv_unpadded_ = unpadded_parent_src_ ? + unpadded_alloc_->allocateEnv(unpadded_parent_src_, cenv_->getWorld()) : + unpadded_alloc_->allocateEnv(cenv_->getWorld(), cenv_->getRobotModel()); + 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..894c7990bc 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -267,22 +267,22 @@ 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_); + // Defer the unpadded env; record how to build it on first use. + collision_detector_->unpadded_parent_src_ = parent_detector->cenv_unpadded_const_; } else { collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel()); - collision_detector_->cenv_unpadded_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel()); + // Defer the unpadded env (build from world+model on first use). // Copy padding to collision_detector_->cenv_ if (prev_coll_detector) collision_detector_->copyPadding(*prev_coll_detector); } + collision_detector_->unpadded_alloc_ = allocator; // Assign const pointers collision_detector_->cenv_const_ = collision_detector_->cenv_; - collision_detector_->cenv_unpadded_const_ = collision_detector_->cenv_unpadded_; } const collision_detection::CollisionEnvConstPtr& From cc8ff9ff5808d5abff40574d1959af5fee61e84d Mon Sep 17 00:00:00 2001 From: hugogo1998 <295481948+hugogo1998@users.noreply.github.com> Date: Sun, 16 Aug 2026 18:00:24 -0500 Subject: [PATCH 2/3] Preserve lazy unpadded collision environment creation --- .../moveit/planning_scene/planning_scene.hpp | 19 ++-- .../planning_scene/src/planning_scene.cpp | 6 +- .../test/test_planning_scene.cpp | 89 +++++++++++++++++++ 3 files changed, 99 insertions(+), 15 deletions(-) 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 0480585368..28ff46445e 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,12 +951,10 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro collision_detection::CollisionEnvPtr cenv_; // never nullptr collision_detection::CollisionEnvConstPtr cenv_const_; - // Unpadded env is built LAZILY on first use (see getCollisionEnvUnpadded); many scenes never query it. mutable collision_detection::CollisionEnvPtr cenv_unpadded_; mutable collision_detection::CollisionEnvConstPtr cenv_unpadded_const_; - // Source for the deferred unpadded env: parent's unpadded env (child scenes) or null (build from model). - collision_detection::CollisionEnvConstPtr unpadded_parent_src_; - collision_detection::CollisionDetectorAllocatorPtr unpadded_alloc_; + CollisionDetectorConstPtr unpadded_parent_; + mutable std::once_flag cenv_unpadded_once_; const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const { @@ -963,13 +962,13 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const { - if (!cenv_unpadded_const_) // lazy: build on first access, then cache - { - cenv_unpadded_ = unpadded_parent_src_ ? - unpadded_alloc_->allocateEnv(unpadded_parent_src_, cenv_->getWorld()) : - unpadded_alloc_->allocateEnv(cenv_->getWorld(), cenv_->getRobotModel()); + std::call_once(cenv_unpadded_once_, [this]() { + if (unpadded_parent_) + cenv_unpadded_ = alloc_->allocateEnv(unpadded_parent_->getCollisionEnvUnpadded(), cenv_->getWorld()); + else + cenv_unpadded_ = alloc_->allocateEnv(cenv_->getWorld(), cenv_->getRobotModel()); 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 894c7990bc..280fb2ff2f 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -267,20 +267,16 @@ void PlanningScene::allocateCollisionDetector(const collision_detection::Collisi if (parent_detector) { collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(parent_detector->cenv_, world_); - // Defer the unpadded env; record how to build it on first use. - collision_detector_->unpadded_parent_src_ = parent_detector->cenv_unpadded_const_; + collision_detector_->unpadded_parent_ = parent_detector; } else { collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel()); - // Defer the unpadded env (build from world+model on first use). // Copy padding to collision_detector_->cenv_ if (prev_coll_detector) collision_detector_->copyPadding(*prev_coll_detector); } - collision_detector_->unpadded_alloc_ = allocator; - // Assign const pointers collision_detector_->cenv_const_ = collision_detector_->cenv_; } diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 22cf5c8bc6..22e3272e19 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,44 @@ TEST(PlanningScene, LoadRestoreDiff) EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 2u); } +TEST(PlanningScene, UnpaddedCollisionEnvironmentIsLazyAndInheritedByNestedDiffs) +{ + 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(), 2u); + EXPECT_EQ(allocator->copyAllocations(), 4u); +} + +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(), 2u); + EXPECT_EQ(allocator->copyAllocations(), 0u); +} + TEST(PlanningScene, MakeAttachedDiff) { urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); From f75692599bdaa0ac087498507968c1989819b8cc Mon Sep 17 00:00:00 2001 From: hugogo1998 <295481948+hugogo1998@users.noreply.github.com> Date: Sun, 16 Aug 2026 18:20:23 -0500 Subject: [PATCH 3/3] Avoid retaining parent collision detectors --- .../moveit/planning_scene/planning_scene.hpp | 8 ++--- .../planning_scene/src/planning_scene.cpp | 1 - .../test/test_planning_scene.cpp | 36 ++++++++++++++++--- 3 files changed, 34 insertions(+), 11 deletions(-) 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 28ff46445e..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 @@ -953,7 +953,6 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro mutable collision_detection::CollisionEnvPtr cenv_unpadded_; mutable collision_detection::CollisionEnvConstPtr cenv_unpadded_const_; - CollisionDetectorConstPtr unpadded_parent_; mutable std::once_flag cenv_unpadded_once_; const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const @@ -963,10 +962,9 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const { std::call_once(cenv_unpadded_once_, [this]() { - if (unpadded_parent_) - cenv_unpadded_ = alloc_->allocateEnv(unpadded_parent_->getCollisionEnvUnpadded(), cenv_->getWorld()); - else - cenv_unpadded_ = alloc_->allocateEnv(cenv_->getWorld(), cenv_->getRobotModel()); + 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_; diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 280fb2ff2f..4df1720218 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -267,7 +267,6 @@ void PlanningScene::allocateCollisionDetector(const collision_detection::Collisi if (parent_detector) { collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(parent_detector->cenv_, world_); - collision_detector_->unpadded_parent_ = parent_detector; } else { diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 22e3272e19..94c2fe5501 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -256,7 +256,7 @@ TEST(PlanningScene, LoadRestoreDiff) EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 2u); } -TEST(PlanningScene, UnpaddedCollisionEnvironmentIsLazyAndInheritedByNestedDiffs) +TEST(PlanningScene, UnpaddedCollisionEnvironmentIsLazyAcrossNestedDiffs) { auto allocator = std::make_shared(); auto root = std::make_shared(moveit::core::loadTestingRobotModel("panda")); @@ -274,8 +274,8 @@ TEST(PlanningScene, UnpaddedCollisionEnvironmentIsLazyAndInheritedByNestedDiffs) EXPECT_EQ(allocator->copyAllocations(), 2u); grandchild->getCollisionEnvUnpadded(); - EXPECT_EQ(allocator->freshAllocations(), 2u); - EXPECT_EQ(allocator->copyAllocations(), 4u); + EXPECT_EQ(allocator->freshAllocations(), 1u); + EXPECT_EQ(allocator->copyAllocations(), 3u); } TEST(PlanningScene, UnpaddedCollisionEnvironmentIsInitializedOnceAcrossThreads) @@ -290,8 +290,34 @@ TEST(PlanningScene, UnpaddedCollisionEnvironmentIsInitializedOnceAcrossThreads) for (std::thread& thread : threads) thread.join(); - EXPECT_EQ(allocator->freshAllocations(), 2u); - EXPECT_EQ(allocator->copyAllocations(), 0u); + 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)