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);
}
}