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
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 #ifndef ROBOTMESHMODEL_H_
00045 #define ROBOTMESHMODEL_H_
00046
00047
00048 #include <boost/shared_ptr.hpp>
00049
00050 #include <ros/ros.h>
00051 #include <image_geometry/pinhole_camera_model.h>
00052 #include <tf/transform_listener.h>
00053 #include <urdf/model.h>
00054
00055 #define GL_GLEXT_PROTOTYPES
00056 #include <GL/gl.h>
00057 #include <GL/glext.h>
00058
00059 #include <assimp/assimp.hpp>
00060 #include <assimp/aiScene.h>
00061 #include <assimp/aiPostProcess.h>
00062 #include <assimp/IOStream.h>
00063 #include <assimp/IOSystem.h>
00064
00065 #include <resource_retriever/retriever.h>
00066
00067 class RobotMeshModel
00068 {
00069 public:
00070
00071
00072
00073 tf::TransformListener tf_;
00074 ros::NodeHandle nh_;
00075 ros::NodeHandle nh_priv_;
00076
00077 sensor_msgs::CameraInfoConstPtr cam_info_;
00078 ros::ServiceServer srver_;
00079 urdf::Model urdf_;
00080 std::string description_path;
00081
00082 std::string modelframe_;
00083 std::string cameraframe_;
00084 std::string camera_topic_;
00085 std::string camera_info_topic_;
00086
00087 tf::StampedTransform cameraPose;
00088 bool use_assimp_;
00089
00090 std::map<std::string, const aiScene *> as_meshes;
00091 std::map<std::string, tf::Transform > offsets_;
00092 std::map<std::string, tf::StampedTransform > robotLinks_;
00093 std::vector<boost::shared_ptr<urdf::Link> > links_with_meshes;
00094
00095 ros::Time current_time_stamp_;
00096
00097
00098 RobotMeshModel();
00099 ~RobotMeshModel();
00100
00101
00102 void setCamera();
00103 void setCameraInfo(sensor_msgs::CameraInfoConstPtr cam_info);
00104 bool setCameraPose();
00105 void initRobot();
00106 void updateRobotLinks(const ros::Time time_stamp);
00107 void paintRobot();
00108
00109
00110
00111 struct Vertex
00112 {
00113 float x,y,z;
00114 float nx,ny,nz;
00115 Vertex ()
00116 {}
00117 ~Vertex ()
00118 {}
00119 Vertex (float x, float y, float z, float nx, float ny, float nz)
00120 : x(x), y(y), z(z), nx(nx), ny(ny), nz(nz)
00121 {}
00122 };
00123
00124 struct SubMesh
00125 {
00126 enum {INVALID_VALUE = 0xFFFFFFFF};
00127 SubMesh ();
00128 ~SubMesh ();
00129
00130
00131
00132 unsigned int num_indices;
00133 std::vector<Vertex> vertices;
00134 std::vector<unsigned int> indices;
00135 };
00136
00137 void fromAssimpScene (const aiScene* scene);
00138 void initMesh (const aiMesh* mesh);
00139
00140 std::vector<SubMesh> meshes_vec;
00141
00142 };
00143
00144
00145
00146 #endif