void SimKinect::initializeOpenRave(const std::string& filename)
{
RaveInitialize(true, OpenRAVE::Level_Info);
OpenRAVE::EnvironmentBasePtr env = OpenRAVE::RaveCreateEnvironment();
bool success = env->Load(filename);
ALWAYS_ASSERT(success);
std::vector<OpenRAVE::RobotBasePtr> robots;
env->GetRobots(robots);
OpenRAVE::RobotBasePtr robot = robots[0];
std::vector<OpenRAVE::KinBodyPtr> bodies;
env->GetBodies(bodies);
for (int i=0; i < bodies.size(); ++i) {
OpenRAVE::KinBody& body = *bodies[i];
LOG_INFO("Parsing kinbody %s", body.GetName().c_str());
// TODO: Generalize to all objects in the scene except for the robot (PR2)
if (body.GetName().compare("mug")==0)
{
const std::vector<OpenRAVE::KinBody::LinkPtr>& links = body.GetLinks();
for (int i=0; i < links.size(); ++i) {
LOG_INFO("\t\tParsing kinbody link %s", links[i]->GetName().c_str());
if (links[i]->GetGeometries().size() > 0) {
extractGeomFromLink(*links[i]);
}
}
}
}
}
/**
* mongoc_stream_destroy:
* @stream: A mongoc_stream_t.
*
* Frees any resources referenced by @stream, including the memory allocation
* for @stream.
*/
void
mongoc_stream_destroy (mongoc_stream_t *stream)
{
ENTRY;
bson_return_if_fail(stream);
ALWAYS_ASSERT(stream->destroy);
stream->destroy(stream);
EXIT;
}
/**
* mongoc_stream_close:
* @stream: A mongoc_stream_t.
*
* Closes the underlying file-descriptor used by @stream.
*
* Returns: 0 on success, -1 on failure.
*/
int
mongoc_stream_close (mongoc_stream_t *stream)
{
int ret;
ENTRY;
bson_return_val_if_fail(stream, -1);
ALWAYS_ASSERT(stream->close);
ret = stream->close(stream);
RETURN (ret);
}