示例#1
0
文件: sim_kinect.cpp 项目: scpgit/eih
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);
}