$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, Robert Bosch LLC. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Robert Bosch nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 *********************************************************************/ 00036 00037 /* 00038 * robotMeshModel.cpp 00039 * 00040 * Created on: Oct 7, 2010 00041 * Author: Christian Bersch 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 //Load robot description from parameter server 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>("robot_description_package_path", description_path, ".."); 00071 ROS_INFO("package_path %s", description_path.c_str()); 00072 nh_priv_.param<std::string>("camera_topic", camera_topic_, "/wide_stereo/right" ); 00073 nh_priv_.param<std::string>("camera_info_topic", camera_info_topic_, "/wide_stereo/right/camera_info" ); 00074 00075 00076 00077 //Load robot mesh for each link 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 //todo: this should really be done by resource retriever 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 //get camera intinsics 00123 00124 ROS_INFO("waiting for %s", camera_info_topic_.c_str()); 00125 cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic_); 00126 cameraframe_ = cam_info_->header.frame_id; 00127 00128 ROS_INFO("%s: robot model initialization done!", ros::this_node::getName().c_str()); 00129 00130 } 00131 00132 RobotMeshModel::~RobotMeshModel(){ 00133 00134 } 00135 00136 void RobotMeshModel::initRobot(){ 00137 std::vector<boost::shared_ptr<urdf::Link> > links ; 00138 00139 for (int i=0; i< links_with_meshes.size(); i++){ 00140 00141 //draw 00142 // update bounding box 00143 boost::shared_ptr<CMeshO> mesh_ptr = meshes[links_with_meshes[i]->name]; 00144 vcg::tri::UpdateBounding<CMeshO>::Box(*mesh_ptr); 00145 00146 // update Normals 00147 vcg::tri::UpdateNormals<CMeshO>::PerVertexNormalizedPerFace(*mesh_ptr); 00148 vcg::tri::UpdateNormals<CMeshO>::PerFaceNormalized(*mesh_ptr); 00149 00150 // Initialize the opengl wrapper 00151 boost::shared_ptr<vcg::GlTrimesh<CMeshO> > wrapper_ptr (new vcg::GlTrimesh<CMeshO>); 00152 wrapper_ptr->m = mesh_ptr.get(); 00153 wrapper_ptr->Update(); 00154 00155 mesh_wrappers[links_with_meshes[i]->name] = wrapper_ptr; 00156 00157 00158 } 00159 00160 00161 00162 00163 00164 }; 00165 00166 00167 void RobotMeshModel::updateRobotLinks(const ros::Time time_stamp){ 00168 00169 // get the current configuration of the robot links 00170 if (current_time_stamp_ != time_stamp){ 00171 current_time_stamp_ = time_stamp; 00172 tf::StampedTransform tf; 00173 for (int i=0; i < links_with_meshes.size(); ++i){ 00174 if (!tf_.waitForTransform(links_with_meshes[i]->name, modelframe_, current_time_stamp_, ros::Duration(0.5))){ 00175 ROS_ERROR("could not get transform from %s to %s", links_with_meshes[i]->name.c_str(),modelframe_.c_str() ); 00176 continue; 00177 } 00178 tf_.lookupTransform( modelframe_, links_with_meshes[i]->name, current_time_stamp_, tf); 00179 00180 tf *= offsets_[links_with_meshes[i]->name];; 00181 robotLinks_[links_with_meshes[i]->name] = tf; 00182 00183 } 00184 00185 } 00186 } 00187 00188 00189 void RobotMeshModel::paintRobot(){ 00190 00191 00192 // draw the configuration of the robot links as specified in robotLinks_ 00193 float d=1.0f; 00194 vcg::glScale(d); 00195 glDisable(GL_CULL_FACE); 00196 glMatrixMode(GL_MODELVIEW); 00197 00198 for (int i=0; i < links_with_meshes.size(); ++i){ 00199 boost::shared_ptr<vcg::GlTrimesh<CMeshO> > wrapper_ptr = mesh_wrappers[links_with_meshes[i]->name]; 00200 00201 btScalar glTf[16]; 00202 robotLinks_[links_with_meshes[i]->name].getOpenGLMatrix(glTf); 00203 00204 00205 00206 glPushMatrix(); 00207 00208 glMultMatrixd((GLdouble*)glTf); 00209 00210 wrapper_ptr->Draw<vcg::GLW::DMSmooth, vcg::GLW::CMNone,vcg::GLW::TMNone> (); 00211 00212 glPopMatrix(); 00213 } 00214 00215 00216 00217 00218 } 00219 00220 void RobotMeshModel::setCameraInfo(sensor_msgs::CameraInfoConstPtr cam_info){ 00221 cam_info_ = cam_info; 00222 } 00223 00224 00225 void RobotMeshModel::setCamera(){ 00226 00227 //calc field of view for OpenGL Camera; 00228 00229 double FulstrumWidth, FulstrumHeight, left, right, top, bottom; 00230 00231 double near_clip = 0.1; 00232 double farclip = 100; 00233 00234 00235 double cx= cam_info_->P[2]; 00236 double cy = cam_info_->P[6]; 00237 00238 double fx = cam_info_->P[0]; 00239 double fy = cam_info_->P[5]; 00240 00241 00242 FulstrumWidth = near_clip * cam_info_->width / fx; 00243 FulstrumHeight = near_clip * cam_info_->height / fy; 00244 00245 00246 left = FulstrumWidth * (- cx / cam_info_->width); 00247 right = FulstrumWidth * ( 1.0 - cx / cam_info_->width); 00248 top = FulstrumHeight * ( cy / cam_info_->height); 00249 bottom = FulstrumHeight * ( -1.0 + cy / cam_info_->height); 00250 00251 00252 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 00253 glMatrixMode(GL_PROJECTION); 00254 glLoadIdentity(); 00255 00256 glFrustum(left,right, bottom, top, near_clip, farclip); 00257 glMatrixMode(GL_MODELVIEW); 00258 00259 } 00260 00261 00262 bool RobotMeshModel::setCameraPose(){ 00263 00264 00265 //set camera pose relative to robot links 00266 if (!tf_.waitForTransform(modelframe_, cameraframe_, current_time_stamp_, ros::Duration(0.5))){ 00267 ROS_ERROR("setting cam pose: could not get transform from %s to %s", modelframe_.c_str(),cameraframe_.c_str() ); 00268 return false; 00269 } 00270 tf_.lookupTransform(modelframe_, cameraframe_, current_time_stamp_, cameraPose); 00271 00272 00273 //get offset for stereo 00274 double tx = -1.0 * (cam_info_->P[3] / cam_info_->P[0]); 00275 tf::Vector3 origin = cameraPose.getOrigin() + cameraPose.getBasis() * tf::Vector3(tx, 0.0, 0.0); 00276 00277 tf::Vector3 look_at_position = origin + cameraPose.getBasis().getColumn(2); 00278 glLoadIdentity(); 00279 tf::Vector3 up = -cameraPose.getBasis().getColumn(1); 00280 00281 gluLookAt(origin.getX() , origin.getY() , origin.getZ() , 00282 look_at_position.getX() , look_at_position.getY(), look_at_position.getZ(), 00283 up.getX(), up.getY(), up.getZ()); 00284 return true; 00285 00286 } 00287 00288