void OctomapInterface::CreateFakeBody()
{
ROS_DEBUG("Creating fake body!");
OpenRAVE::KinBodyPtr kinbody = RaveCreateKinBody(GetEnv(), "");
kinbody->SetName("_OCTOMAP_MAP_");
GetEnv()->Add(kinbody);
}
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);
}
}
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("------");
}
void OSGViewer::RemoveKinBody(OpenRAVE::KinBodyPtr pbody) {
KinBodyGroup* node = (KinBodyGroup*)pbody->GetUserData("osg").get();
m_root->removeChild(node);
pbody->RemoveUserData("osg");
}
