Esempio n. 1
0
 void OctomapInterface::CreateFakeBody()
 {
     ROS_DEBUG("Creating fake body!");
     OpenRAVE::KinBodyPtr kinbody = RaveCreateKinBody(GetEnv(), "");
     kinbody->SetName("_OCTOMAP_MAP_");
     GetEnv()->Add(kinbody);
 }
Esempio n. 2
0
    void OctomapInterface::TestCollision()
    {
        float timey = 0.0f;
        bool lastCollisionState = false;
        while(!m_shouldExit)
        {
            timey += 0.01f;
            OpenRAVE::CollisionReportPtr report(new OpenRAVE::CollisionReport());
            OpenRAVE::KinBodyConstPtr fakeBody = GetEnv()->GetKinBody("_OCTOMAP_MAP_");
            OpenRAVE::KinBodyPtr fuze = GetEnv()->GetKinBody("fuze_bottle");
            if(fakeBody.get())
            {
                boost::mutex::scoped_lock(m_cloudQueueMutex);
                bool collides = GetEnv()->CheckCollision(fakeBody, report);

                ROS_INFO("Collides: %s", collides ? "true" : "false");

                OpenRAVE::Transform t;
                t.identity();

                float r = 0.1 * cos(timey * 0.1f);
                t.trans = OpenRAVE::Vector(r * sin(timey) + 0.5f, r * cos(timey), 0.0f);

                if(fuze.get())
                {
                    fuze->SetTransform(t);
                }

                lastCollisionState = collides;
            }

            usleep(10000);
        }
    }
Esempio n. 3
0
    void OctomapInterface::DestroyFakeBody()
    {
        OpenRAVE::KinBodyPtr kinBody = GetEnv()->GetKinBody("_OCTOMAP_MAP_");

        if(kinBody.get())
        {
            GetEnv()->Remove(kinBody);
        }
    }
void CollisionChecker::IgnoreZeroStateSelfCollisions(OpenRAVE::KinBodyPtr body) {
  LOG_DEBUG("IgnoreZeroStateSelfCollisions for %s", body->GetName().c_str());
  KinBody::KinBodyStateSaver saver(body);
  body->SetDOFValues(DblVec(body->GetDOF(), 0));
  body->SetTransform(Transform(Vector(1,0,0,0), (Vector(0,0,10))));


  vector<Collision> collisions;
  BodyVsAll(*body,  collisions);
  LOG_DEBUG("%li extra self collisions in zero state", collisions.size());
  for(int i=0; i < collisions.size(); ++i) {
    LOG_DEBUG("ignoring self-collision: %s %s", collisions[i].linkA->GetName().c_str(), collisions[i].linkB->GetName().c_str());
    ExcludeCollisionPair(*collisions[i].linkA, *collisions[i].linkB);
  }
  LOG_DEBUG("------");
}
Esempio n. 5
0
void OSGViewer::RemoveKinBody(OpenRAVE::KinBodyPtr pbody) {
  KinBodyGroup* node = (KinBodyGroup*)pbody->GetUserData("osg").get();
  m_root->removeChild(node);
  pbody->RemoveUserData("osg");
}