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
00056
00057 #include <wrap/io_trimesh/import.h>
00058 #include <wrap/io_trimesh/additionalinfo.h>
00059
00060 #include <vcg/complex/trimesh/update/bounding.h>
00061 #include <vcg/complex/trimesh/update/normal.h>
00062 #include <vcg/complex/trimesh/create/platonic.h>
00063
00064 #define GL_GLEXT_PROTOTYPES
00065 #include <GL/gl.h>
00066 #include <GL/glext.h>
00067 #include <wrap/gl/trimesh.h>
00068
00069 #include "MeshDefinition.h"
00070
00071
00072 class RobotMeshModel
00073 {
00074 public:
00075
00076
00077
00078 tf::TransformListener tf_;
00079 ros::NodeHandle nh_;
00080 ros::NodeHandle nh_priv_;
00081
00082 sensor_msgs::CameraInfoConstPtr cam_info_;
00083 ros::ServiceServer srver_;
00084 urdf::Model urdf_;
00085 std::string description_path;
00086
00087 std::string modelframe_;
00088 std::string cameraframe_;
00089 std::string camera_topic_;
00090
00091 tf::StampedTransform cameraPose;
00092
00093 std::map<std::string, boost::shared_ptr<CMeshO> > meshes;
00094 std::map<std::string, tf::Transform > offsets_;
00095 std::map<std::string, boost::shared_ptr<vcg::GlTrimesh<CMeshO> > > mesh_wrappers;
00096 std::map<std::string, btTransform > robotLinks_;
00097 std::vector<boost::shared_ptr<urdf::Link> > links_with_meshes;
00098
00099 ros::Time current_time_stamp_;
00100
00101
00102 RobotMeshModel();
00103 ~RobotMeshModel();
00104
00105
00106 void setCamera();
00107 void setCameraInfo(sensor_msgs::CameraInfoConstPtr cam_info);
00108 bool setCameraPose();
00109 void initRobot();
00110 void updateRobotLinks(const ros::Time time_stamp);
00111 void paintRobot();
00112
00113
00114 };
00115
00116
00117
00118 #endif