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); } }
void OctomapInterface::DestroyFakeBody() { OpenRAVE::KinBodyPtr kinBody = GetEnv()->GetKinBody("_OCTOMAP_MAP_"); if(kinBody.get()) { GetEnv()->Remove(kinBody); } }