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
00045 #include <GL/glew.h>
00046 #include <GL/glu.h>
00047 #include <GL/freeglut.h>
00048 #include "camera_self_filter/robotMeshModel.h"
00049 #include "LinearMath/btTransform.h"
00050
00051
00052
00053
00054 RobotMeshModel::RobotMeshModel():nh_priv_("~"){
00055
00056
00057 std::string robot_desc_string;
00058 if(!nh_.getParam("robot_description", robot_desc_string)){
00059 ROS_ERROR("Could not get urdf from param server");
00060
00061 }
00062
00063
00064 if (!urdf_.initString(robot_desc_string)){
00065 ROS_ERROR("Failed to parse urdf");
00066
00067 }
00068 modelframe_= "/torso_lift_link";
00069
00070 nh_priv_.param<std::string>("camera_frame", cameraframe_, "/wide_stereo_optical_frame");
00071 nh_priv_.param<std::string>("robot_description_package_path", description_path, "..");
00072 ROS_INFO("package_path %s", description_path.c_str());
00073 nh_priv_.param<std::string>("camera_topic", camera_topic_, "/wide_stereo/right" );
00074
00075
00076
00077
00078 std::vector<boost::shared_ptr<urdf::Link> > links ;
00079 urdf_.getLinks(links);
00080 for (int i=0; i< links.size(); i++){
00081 if (links[i]->visual.get() == NULL) continue;
00082 if (links[i]->visual->geometry.get() == NULL) continue;
00083 if (links[i]->visual->geometry->type == urdf::Geometry::MESH){
00084
00085
00086 boost::shared_ptr<urdf::Mesh> mesh = boost::dynamic_pointer_cast<urdf::Mesh> (links[i]->visual->geometry);
00087 std::string filename (mesh->filename);
00088
00089 if (filename.substr(filename.size() - 4 , 4) != ".stl" && filename.substr(filename.size() - 4 , 4) != ".dae") continue;
00090 if (filename.substr(filename.size() - 4 , 4) == ".dae")
00091 filename.replace(filename.size() - 4 , 4, ".stl");
00092 ROS_INFO("adding link %d %s",i,links[i]->name.c_str());
00093 filename.erase(0,25);
00094 filename = description_path + filename;
00095
00096 boost::shared_ptr<CMeshO> mesh_ptr(new CMeshO);
00097
00098 if(vcg::tri::io::ImporterSTL<CMeshO>::Open(*mesh_ptr,filename.c_str())){
00099 ROS_ERROR("could not load mesh %s", filename.c_str());
00100 continue;
00101 }
00102
00103 links_with_meshes.push_back(links[i]);
00104 meshes[links[i]->name] = mesh_ptr;
00105
00106 tf::Vector3 origin(links[i]->visual->origin.position.x, links[i]->visual->origin.position.y, links[i]->visual->origin.position.z);
00107 tf::Quaternion rotation(links[i]->visual->origin.rotation.x, links[i]->visual->origin.rotation.y, links[i]->visual->origin.rotation.z, links[i]->visual->origin.rotation.w);
00108
00109 offsets_[links[i]->name] = tf::Transform(rotation, origin);
00110
00111
00112
00113
00114
00115 }
00116
00117 }
00118
00119
00120 initRobot();
00121
00122
00123 cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_topic_ + "/camera_info");
00124
00125 }
00126
00127 RobotMeshModel::~RobotMeshModel(){
00128
00129 }
00130
00131 void RobotMeshModel::initRobot(){
00132 std::vector<boost::shared_ptr<urdf::Link> > links ;
00133
00134 for (int i=0; i< links_with_meshes.size(); i++){
00135
00136
00137
00138 boost::shared_ptr<CMeshO> mesh_ptr = meshes[links_with_meshes[i]->name];
00139 vcg::tri::UpdateBounding<CMeshO>::Box(*mesh_ptr);
00140
00141
00142 vcg::tri::UpdateNormals<CMeshO>::PerVertexNormalizedPerFace(*mesh_ptr);
00143 vcg::tri::UpdateNormals<CMeshO>::PerFaceNormalized(*mesh_ptr);
00144
00145
00146 boost::shared_ptr<vcg::GlTrimesh<CMeshO> > wrapper_ptr (new vcg::GlTrimesh<CMeshO>);
00147 wrapper_ptr->m = mesh_ptr.get();
00148 wrapper_ptr->Update();
00149
00150 mesh_wrappers[links_with_meshes[i]->name] = wrapper_ptr;
00151
00152
00153 }
00154 ROS_INFO("%s: initRobot done!", ros::this_node::getName().c_str());
00155
00156
00157
00158
00159 };
00160
00161
00162 void RobotMeshModel::updateRobotLinks(const ros::Time time_stamp){
00163
00164
00165 if (current_time_stamp_ != time_stamp){
00166 current_time_stamp_ = time_stamp;
00167 tf::StampedTransform tf;
00168 for (int i=0; i < links_with_meshes.size(); ++i){
00169 if (!tf_.waitForTransform(links_with_meshes[i]->name, modelframe_, current_time_stamp_, ros::Duration(0.5))){
00170 ROS_ERROR("could not get transform from %s to %s", links_with_meshes[i]->name.c_str(),modelframe_.c_str() );
00171 continue;
00172 }
00173 tf_.lookupTransform( modelframe_, links_with_meshes[i]->name, current_time_stamp_, tf);
00174
00175 tf *= offsets_[links_with_meshes[i]->name];;
00176 robotLinks_[links_with_meshes[i]->name] = tf;
00177
00178 }
00179
00180 }
00181 }
00182
00183
00184 void RobotMeshModel::paintRobot(){
00185
00186
00187
00188 float d=1.0f;
00189 vcg::glScale(d);
00190 glDisable(GL_CULL_FACE);
00191 glMatrixMode(GL_MODELVIEW);
00192
00193 for (int i=0; i < links_with_meshes.size(); ++i){
00194 boost::shared_ptr<vcg::GlTrimesh<CMeshO> > wrapper_ptr = mesh_wrappers[links_with_meshes[i]->name];
00195
00196 btScalar glTf[16];
00197 robotLinks_[links_with_meshes[i]->name].getOpenGLMatrix(glTf);
00198
00199
00200
00201 glPushMatrix();
00202
00203 glMultMatrixd((GLdouble*)glTf);
00204
00205 wrapper_ptr->Draw<vcg::GLW::DMSmooth, vcg::GLW::CMNone,vcg::GLW::TMNone> ();
00206
00207 glPopMatrix();
00208 }
00209
00210
00211
00212
00213 }
00214
00215 void RobotMeshModel::setCameraInfo(sensor_msgs::CameraInfoConstPtr cam_info){
00216 cam_info_ = cam_info;
00217 }
00218
00219
00220 void RobotMeshModel::setCamera(){
00221
00222
00223
00224 double FulstrumWidth, FulstrumHeight, left, right, top, bottom;
00225
00226 double near_clip = 0.1;
00227 double farclip = 100;
00228
00229
00230 double cx= cam_info_->P[2];
00231 double cy = cam_info_->P[6];
00232
00233 double fx = cam_info_->P[0];
00234 double fy = cam_info_->P[5];
00235
00236
00237 FulstrumWidth = near_clip * cam_info_->width / fx;
00238 FulstrumHeight = near_clip * cam_info_->height / fy;
00239
00240
00241 left = FulstrumWidth * (- cx / cam_info_->width);
00242 right = FulstrumWidth * ( 1.0 - cx / cam_info_->width);
00243 top = FulstrumHeight * ( cy / cam_info_->height);
00244 bottom = FulstrumHeight * ( -1.0 + cy / cam_info_->height);
00245
00246
00247 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00248 glMatrixMode(GL_PROJECTION);
00249 glLoadIdentity();
00250
00251 glFrustum(left,right, bottom, top, near_clip, farclip);
00252 glMatrixMode(GL_MODELVIEW);
00253
00254 }
00255
00256
00257 bool RobotMeshModel::setCameraPose(){
00258
00259
00260
00261 if (!tf_.waitForTransform(modelframe_, cameraframe_, current_time_stamp_, ros::Duration(0.5))){
00262 ROS_ERROR("setting cam pose: could not get transform from %s to %s", modelframe_.c_str(),cameraframe_.c_str() );
00263 return false;
00264 }
00265 tf_.lookupTransform(modelframe_, cameraframe_, current_time_stamp_, cameraPose);
00266
00267
00268
00269 double tx = -1.0 * (cam_info_->P[3] / cam_info_->P[0]);
00270 tf::Vector3 origin = cameraPose.getOrigin() + cameraPose.getBasis() * tf::Vector3(tx, 0.0, 0.0);
00271
00272 tf::Vector3 look_at_position = origin + cameraPose.getBasis().getColumn(2);
00273 glLoadIdentity();
00274 tf::Vector3 up = -cameraPose.getBasis().getColumn(1);
00275
00276 gluLookAt(origin.getX() , origin.getY() , origin.getZ() ,
00277 look_at_position.getX() , look_at_position.getY(), look_at_position.getZ(),
00278 up.getX(), up.getY(), up.getZ());
00279 return true;
00280
00281 }
00282
00283