TeleoperationWorld(WorldPtr _world, SkeletonPtr _robot)
: osgDart::WorldNode(_world),
mAtlas(_robot),
iter(0),
l_foot(_robot->getEndEffector("l_foot")),
r_foot(_robot->getEndEffector("r_foot"))
{
mMoveComponents.resize(NUM_MOVE, false);
mAnyMovement = false;
}
void enableDragAndDrops(osgDart::Viewer& viewer, const SkeletonPtr& atlas)
{
// Turn on drag-and-drop for the whole Skeleton
for(size_t i=0; i < atlas->getNumBodyNodes(); ++i)
viewer.enableDragAndDrop(atlas->getBodyNode(i), false, false);
for(size_t i=0; i < atlas->getNumEndEffectors(); ++i)
{
EndEffector* ee = atlas->getEndEffector(i);
if(!ee->getIK())
continue;
// Check whether the target is an interactive frame, and add it if it is
if(const auto& frame = std::dynamic_pointer_cast<osgDart::InteractiveFrame>(
ee->getIK()->getTarget()))
viewer.enableDragAndDrop(frame.get());
}
}
TEST(Skeleton, NodePersistence)
{
SkeletonPtr skel = Skeleton::create();
skel->createJointAndBodyNodePair<FreeJoint>(nullptr);
{
EndEffector* manip =
skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip"));
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
WeakEndEffectorPtr weakManip = manip;
EXPECT_FALSE(weakManip.lock() == nullptr);
manip->remove();
// The Node has been removed, and no strong reference to it exists, so it
// should be gone from the Skeleton
EXPECT_TRUE(skel->getEndEffector("manip") == nullptr);
EXPECT_TRUE(skel->getNumEndEffectors() == 0);
EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0);
EXPECT_TRUE(weakManip.lock() == nullptr);
}
{
EndEffector* manip =
skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip"));
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
EndEffectorPtr strongManip = manip;
WeakEndEffectorPtr weakManip = strongManip;
EXPECT_FALSE(weakManip.lock() == nullptr);
manip->remove();
// The Node has been removed, but a strong reference to it still exists, so
// it will remain in the Skeleton for now
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
EXPECT_FALSE(weakManip.lock() == nullptr);
strongManip = nullptr;
// The Node has been removed, and no strong reference to it exists any
// longer, so it should be gone from the Skeleton
EXPECT_TRUE(skel->getEndEffector("manip") == nullptr);
EXPECT_TRUE(skel->getNumEndEffectors() == 0);
EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0);
EXPECT_TRUE(weakManip.lock() == nullptr);
}
}
