diff --git a/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp index 2caee9406e..ed2e2acf84 100644 --- a/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp @@ -41,7 +41,7 @@ namespace collision_detection { bool CollisionDetectorBtPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const { - scene->setActiveCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive); + scene->allocateCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive); return true; } } // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp index 14b6376448..dd55132fd2 100644 --- a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp @@ -41,7 +41,7 @@ namespace collision_detection { bool CollisionDetectorFCLPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene) const { - scene->setCollisionDetectorType(CollisionDetectorAllocatorFCL::create()); + scene->allocateCollisionDetector(CollisionDetectorAllocatorFCL::create()); return true; } } // namespace collision_detection diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index d499a6e7be..7d0a532d0e 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -912,7 +912,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Clone a planning scene. Even if the scene \e scene depends on a parent, the cloned scene will not. */ static PlanningScenePtr clone(const PlanningSceneConstPtr& scene); - /** \brief Replace previous collision detector with a new collision detector type (or create new, if none previously). + /** \brief Allocate a new collision detector and replace the previous one if there was any. * * The collision detector type is specified with (a shared pointer to) an * allocator which is a subclass of CollisionDetectorAllocator. This @@ -922,10 +922,13 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from * A new PlanningScene uses an FCL collision detector by default. * * example: to add FCL collision detection (normally not necessary) call - * planning_scene->setCollisionDetectorType(collision_detection::CollisionDetectorAllocatorFCL::create()); + * planning_scene->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); * * */ - void setCollisionDetectorType(const collision_detection::CollisionDetectorAllocatorPtr& allocator); + void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator) + { + allocateCollisionDetector(allocator, nullptr /* no parent_detector */); + } private: /* Private constructor used by the diff() methods. */ @@ -949,6 +952,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from MOVEIT_STRUCT_FORWARD(CollisionDetector) + /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not null */ + void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator, + const CollisionDetectorPtr& parent_detector); + /* \brief A set of compatible collision detectors */ struct CollisionDetector { diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 2203e49d32..0720584c0a 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -161,7 +161,7 @@ void PlanningScene::initialize() for (const srdf::Model::DisabledCollision& it : dc) acm_->setEntry(it.link1_, it.link2_, true); - setCollisionDetectorType(collision_detection::CollisionDetectorAllocatorFCL::create()); + allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } /* return NULL on failure */ @@ -193,7 +193,7 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare // record changes to the world world_diff_.reset(new collision_detection::WorldDiff(world_)); - setCollisionDetectorType(parent_->collision_detector_->alloc_); + allocateCollisionDetector(parent_->collision_detector_->alloc_, parent_->collision_detector_); collision_detector_->copyPadding(*parent_->collision_detector_); } @@ -223,36 +223,37 @@ void PlanningScene::CollisionDetector::copyPadding(const PlanningScene::Collisio cenv_->setLinkScale(src.getCollisionEnv()->getLinkScale()); } -void PlanningScene::setCollisionDetectorType(const collision_detection::CollisionDetectorAllocatorPtr& allocator) +void PlanningScene::allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator, + const CollisionDetectorPtr& parent_detector) { - // Temporary copy of the previous (if any), to copy padding - CollisionDetector prev_coll_detector; - bool have_previous_coll_detector = false; - if (collision_detector_) - { - have_previous_coll_detector = true; - prev_coll_detector = *collision_detector_; - } - - // TODO(andyz): uncomment this for a small speed boost when another collision detector type is available in MoveIt2 - // For now, it is useful in the switchCollisionDetectorType() unit test - // const std::string& name = allocator->getName(); - // if (name == getCollisionDetectorName()) // already using this collision detector - // return; + // Temporarily keep pointer to the previous (if any) collision detector to copy padding from + CollisionDetectorPtr prev_coll_detector = collision_detector_; + // Construct a fresh CollisionDetector and store allocator collision_detector_.reset(new CollisionDetector()); - collision_detector_->alloc_ = allocator; - collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel()); - collision_detector_->cenv_const_ = collision_detector_->cenv_; - collision_detector_->cenv_unpadded_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel()); - collision_detector_->cenv_unpadded_const_ = collision_detector_->cenv_unpadded_; - // Copy padding from the previous collision detector - if (have_previous_coll_detector) + // If parent_detector is specified, copy-construct collision environments (copies link shapes and attached objects) + // Otherwise, construct new collision environment from world and robot model + if (parent_detector) { - collision_detector_->copyPadding(prev_coll_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& @@ -295,11 +296,8 @@ void PlanningScene::clearDiffs() if (current_world_object_update_callback_) current_world_object_update_observer_handle_ = world_->addObserver(current_world_object_update_callback_); - if (parent_) - { - collision_detector_->copyPadding(*parent_->collision_detector_); - } - collision_detector_->cenv_const_ = collision_detector_->cenv_; + // Reset collision detector to the the parent's version + allocateCollisionDetector(parent_->collision_detector_->alloc_, parent_->collision_detector_); scene_transforms_.reset(); robot_state_.reset(); diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 6fda481ee5..bca541db68 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -224,7 +224,7 @@ TEST(PlanningScene, switchCollisionDetectorType) EXPECT_FALSE(ps->isStateValid(current_state, "left_arm")); } - ps->setCollisionDetectorType(collision_detection::CollisionDetectorAllocatorFCL::create()); + ps->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); if (ps->isStateColliding(current_state, "left_arm")) { EXPECT_FALSE(ps->isStateValid(current_state, "left_arm"));