Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <string>
00019 #include <vector>
00020
00021 #include <boost/filesystem.hpp>
00022 #include <ros/ros.h>
00023 #include <fstream>
00024
00025 #include "cob_obstacle_distance/parsers/mesh_parser.hpp"
00026 #include "cob_obstacle_distance/helpers/helper_functions.hpp"
00027
00028 #define MAX_NUM_MESHES 1
00029
00036 int8_t MeshParser::read(std::vector<TriangleSupport>& tri_vec)
00037 {
00038 std::string file_path = this->file_path_;
00039 if (!boost::filesystem::exists(this->file_path_))
00040 {
00041 file_path = resolveURI(this->file_path_);
00042 }
00043
00044
00045 Assimp::Importer importer;
00046 const aiScene* scene = importer.ReadFile(file_path,
00047 0);
00048 if (!scene)
00049 {
00050 ROS_ERROR_STREAM("Assimp::Importer Error: " << importer.GetErrorString());
00051 return -1;
00052 }
00053
00054 if (0 >= scene->mNumMeshes)
00055 {
00056 ROS_ERROR("Found no meshes. Check mesh file. Aborting ...");
00057 return -2;
00058 }
00059
00060 if (MAX_NUM_MESHES < scene->mNumMeshes)
00061 {
00062 ROS_WARN("Found more than one mesh in mesh file. The current implementation can only process %d mesh!!!", static_cast<uint32_t>(MAX_NUM_MESHES));
00063 }
00064
00065 aiMesh** mesh = scene->mMeshes;
00066 aiVector3D* vertex = mesh[0]->mVertices;
00067
00068 ROS_DEBUG_STREAM("mesh[0]->mNumVertices: " << mesh[0]->mNumVertices);
00069 ROS_DEBUG_STREAM("mesh[0]->mNumFaces: " << mesh[0]->mNumFaces);
00070
00071
00072 for (uint32_t i = 0; i < mesh[0]->mNumFaces; ++i)
00073 {
00074
00075 TriangleSupport t;
00076 if (0 != this->toVec3f(i, vertex++, t.a))
00077 {
00078 break;
00079 }
00080
00081 if (0 != this->toVec3f(i, vertex++, t.b))
00082 {
00083 break;
00084 }
00085
00086 if (0 != this->toVec3f(i, vertex++, t.c))
00087 {
00088 break;
00089 }
00090
00091 tri_vec.push_back(t);
00092 }
00093
00094 return 0;
00095 }
00096
00104 int8_t MeshParser::toVec3f(uint32_t num_current_face, aiVector3D* vertex, fcl::Vec3f& out)
00105 {
00106 if (!vertex)
00107 {
00108 ROS_ERROR("No valid vertex found at face %d", num_current_face);
00109 return -3;
00110 }
00111
00112 out = fcl::Vec3f((*vertex)[0], (*vertex)[1], (*vertex)[2]);
00113 return 0;
00114 }
00115