InventorViewer.cpp
Go to the documentation of this file.
00001 
00031 #include <urdf_viewer/InventorViewer.h>
00032 #include <urdf2inventor/Helpers.h>
00033 
00034 #include <Inventor/SoDB.h>  // for file reading
00035 #include <Inventor/SoInput.h>   // for file reading
00036 #include <Inventor/nodes/SoPerspectiveCamera.h>
00037 #include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
00038 #include <Inventor/events/SoMouseButtonEvent.h>
00039 #include <Inventor/SoPickedPoint.h>
00040 
00041 #include <Inventor/nodes/SoEventCallback.h>
00042 #include <Inventor/nodes/SoCone.h>
00043 #include <Inventor/nodes/SoSphere.h>
00044 #include <Inventor/nodes/SoCoordinate3.h>
00045 #include <Inventor/nodes/SoIndexedShape.h>
00046 #include <Inventor/nodes/SoVertexProperty.h>
00047 #include <Inventor/actions/SoSearchAction.h>
00048 #include <Inventor/details/SoFaceDetail.h>
00049 
00050 #include <vector>
00051 #include <map>
00052 #include <fstream>
00053 #include <iostream>
00054 #include <sstream>
00055 #include <string>
00056 #include <algorithm>
00057 
00058 #include <ros/ros.h>  // only needed for ROS prints, e.g. ROS_ERROR
00059 
00060 using urdf_viewer::InventorViewer;
00061 
00062 InventorViewer::InventorViewer(bool _faces_ccw):
00063     root(NULL), viewWindow(NULL), viewer(NULL),
00064     faces_ccw(_faces_ccw), initialized(false) {}
00065 
00066 
00067 InventorViewer::InventorViewer(const InventorViewer& o):
00068     root(o.root), viewWindow(o.viewWindow), viewer(o.viewer),
00069     faces_ccw(o.faces_ccw) {}
00070 InventorViewer::~InventorViewer()
00071 {
00072 //    SoQt::done();
00073     if (viewer)
00074     {
00075         delete viewer;
00076     }
00077 //    root->unref();
00078 }
00079 
00080 
00081 void InventorViewer::init(const char * windowName, float bck_r, float bck_g, float bck_b)
00082 {
00083     if (viewWindow)
00084     {
00085         ROS_ERROR("InventorViewer already initialized");
00086         return;
00087     }
00088     viewWindow = SoQt::init(windowName);
00089     viewer = new SoQtExaminerViewer(viewWindow);
00090     viewer->setBackgroundColor(SbColor(bck_r,bck_g,bck_b));
00091     root = new SoSelection();
00092     root->ref();
00093 
00094     SoEventCallback * ecb = new SoEventCallback();
00095     ecb->addEventCallback(SoMouseButtonEvent::getClassTypeId(), InventorViewer::mouseBtnCB, this);
00096     root->addChild(ecb);
00097     initialized = true;
00098 }
00099 
00100 void InventorViewer::loadModel(SoNode * model)
00101 {
00102     if (!initialized)
00103     {
00104       ROS_ERROR("InventorViewer not initialized.");
00105       return;
00106     }
00107     if (model) root->addChild(model);
00108 }
00109 
00110 bool InventorViewer::loadModel(const std::string& filename)
00111 {
00112     if (!initialized)
00113     {
00114       ROS_ERROR("InventorViewer not initialized.");
00115       return false;
00116     }
00117     SoInput in;
00118     SoNode  *model = NULL;
00119     if (!in.openFile(filename.c_str()))
00120         return false;
00121     if (!SoDB::read(&in, model) || model == NULL)
00122         /*model = SoDB::readAll(&in);
00123         if (!model)*/
00124         return false;
00125 
00126     root->addChild(model);
00127     in.closeFile();
00128     return true;
00129 }
00130 
00131 void InventorViewer::runViewer()
00132 {
00133     if (!initialized)
00134     {
00135       ROS_ERROR("InventorViewer not initialized.");
00136       return;
00137     }
00138     viewer->setSceneGraph(root);
00139     viewer->show();
00140 
00141     SoQt::show(viewWindow);
00142     SoQt::mainLoop();
00143 }
00144 
00145 
00146 void printPath(const SoPath* p)
00147 {
00148     for (int i = p->getLength() - 1; i >= 0;  --i)
00149     {
00150         SoNode * n = p->getNode(i);
00151         std::string name = n->getName().getString();
00152         ROS_INFO("Path[%i]: %s, type %s",i,name.c_str(),n->getTypeId().getName().getString());
00153     }
00154 }
00155 
00156 bool InventorViewer::computeCorrectFaceNormal(const SoPickedPoint * pick, bool ccw_face, Eigen::Vector3d& normal, int& shapeIdx)
00157 {
00158     const SoDetail *pickDetail = pick->getDetail();
00159     if ((pickDetail != NULL) && (pickDetail->getTypeId() == SoFaceDetail::getClassTypeId()))
00160     {
00161         // Picked object is a face
00162         const SoFaceDetail * fd = dynamic_cast<const SoFaceDetail*>(pickDetail);
00163         if (!fd)
00164         {
00165             ROS_ERROR("Could not cast to face detail");
00166             return false;
00167         }
00168 
00169         // face index is always 0 with triangle strips
00170        // ROS_INFO_STREAM("Face index: "<<fd->getFaceIndex());
00171 
00172         if (fd->getNumPoints() < 3)
00173         {
00174             ROS_ERROR("Clicked on degenerate face, can't compute normal");
00175             return false;
00176         }
00177         /*else
00178         {
00179             ROS_INFO_STREAM("Clicked on face with "<<fd->getNumPoints()<<" points.");
00180         }*/       
00181         
00182         //ROS_INFO("Pick path:");
00183         //printPath(pick->getPath());
00184 
00185         /*SbVec3f pickNormal = pick->getNormal();
00186         //SbVec3f _normalObj=pick->getObjectNormal();
00187         float _x, _y, _z;
00188         pickNormal.getValue(_x, _y, _z);
00189         Eigen::Vector3d normalDef = Eigen::Vector3d(_x, _y, _z);
00190         normal = normalDef;*/
00191 
00192         // ROS_INFO_STREAM("Clicked on face with "<<fd->getNumPoints()<<" points.");
00193 
00194         int p1 = fd->getPoint(0)->getCoordinateIndex();
00195         int p2 = fd->getPoint(1)->getCoordinateIndex();
00196         int p3 = fd->getPoint(2)->getCoordinateIndex();
00197 
00198         // ROS_INFO_STREAM("Face part index: "<<fd->getPartIndex());
00199 
00200         // ROS_INFO_STREAM("First 3 coord indices: "<<p1<<", "<<p2<<", "<<p3);
00201 
00202         // Find the coordinate node that is used for the faces.
00203         // Assume that it's the last SoCoordinate3 node traversed
00204         // before the picked shape.
00205         SoSearchAction  searchCoords;
00206         searchCoords.setType(SoCoordinate3::getClassTypeId());
00207         searchCoords.setInterest(SoSearchAction::LAST);
00208         searchCoords.apply(pick->getPath());
00209 
00210         SbVec3f coord1, coord2, coord3;
00211 
00212         shapeIdx=pick->getPath()->getLength()-1;
00213         //ROS_INFO_STREAM("Len of pick path: "<<shapeIdx);
00214 
00215         if (searchCoords.getPath() == NULL)
00216         {
00217             // try to find SoIndexedShape instead
00218             // ROS_INFO("No SoCoordinate3 node found, looking for SoIndexedShape...");
00219 
00220             searchCoords.setType(SoIndexedShape::getClassTypeId());
00221             searchCoords.setInterest(SoSearchAction::LAST);
00222             searchCoords.apply(pick->getPath());
00223 
00224             if (searchCoords.getPath() == NULL)
00225             {
00226                 ROS_ERROR("Failed to find coordinate node for the picked face. Returning default normal.");
00227                 return false;
00228             }
00229 
00230             shapeIdx=searchCoords.getPath()->getLength()-1;
00231             // ROS_INFO_STREAM("Coords at Idx: "<<shapeIdx);
00232 
00233             // ROS_INFO("SearchCoords path:");
00234             // printPath(searchCoords.getPath());
00235 
00236             SoIndexedShape * vShapeNode = dynamic_cast<SoIndexedShape*>(searchCoords.getPath()->getTail());
00237             if (!vShapeNode)
00238             {
00239                 ROS_ERROR("Could not cast SoIndexedShape");
00240                 return false;
00241             }
00242             SoVertexProperty * vProp = dynamic_cast<SoVertexProperty*>(vShapeNode->vertexProperty.getValue());
00243             if (!vProp)
00244             {
00245                 ROS_ERROR_STREAM("Could not cast SoVertexProperty.");
00246                 return false;
00247             }
00248             coord1 = vProp->vertex[p1];
00249             coord2 = vProp->vertex[p2];
00250             coord3 = vProp->vertex[p3];
00251         }
00252         else
00253         {
00254             shapeIdx=searchCoords.getPath()->getLength()-1;
00255             
00256             SoCoordinate3 * coordNode = dynamic_cast<SoCoordinate3*>(searchCoords.getPath()->getTail());
00257             if (!coordNode)
00258             {
00259                 ROS_ERROR("Could not cast SoCoordinate3");
00260                 return false;
00261             }
00262             coord1 = coordNode->point[p1];
00263             coord2 = coordNode->point[p2];
00264             coord3 = coordNode->point[p3];
00265         }
00266 
00267         if (fd->getNumPoints() > 3)
00268         {
00269             ROS_WARN_STREAM("Face with " << fd->getNumPoints() <<
00270                             " points is not a triangle and may lead to wrong normal calculations.");
00271         }
00272 
00273         /*ROS_INFO_STREAM("Coords "<<p1<<", "<<p2<<", "<<p3);
00274         float _x, _y, _z;
00275         coord1.getValue(_x, _y, _z);
00276         ROS_INFO_STREAM("val1 "<<_x<<", "<<_y<<", "<<_z);
00277         coord2.getValue(_x, _y, _z);
00278         ROS_INFO_STREAM("val2 "<<_x<<", "<<_y<<", "<<_z);
00279         coord3.getValue(_x, _y, _z);
00280         ROS_INFO_STREAM("val3 "<<_x<<", "<<_y<<", "<<_z);*/
00281 
00282         SbVec3f diff1(coord2.getValue());
00283         diff1 -= coord1;
00284         SbVec3f diff2(coord3.getValue());
00285         diff2 -= coord1;
00286         SbVec3f cross = diff1.cross(diff2);
00287         if (!ccw_face) cross = -cross;
00288 
00289         float x, y, z;
00290         cross.getValue(x, y, z);
00291         double len = sqrt(x * x + y * y + z * z);
00292         x /= len;
00293         y /= len;
00294         z /= len;
00295 
00296         normal = Eigen::Vector3d(x, y, z);
00297 
00298         return true;
00299     }
00300     return false;
00301 }
00302 
00303 
00304 SoNode * InventorViewer::getIntStr(const std::string& sscanfStr, const SoPath * path, std::string& extStr, int& extNum, int& pathIdx)
00305 {
00306     if (path->getLength()==0) return NULL;
00307     for (int i = path->getLength() - 1; i >= 0;  --i)
00308     {
00309         SoNode * n = path->getNode(i);
00310         std::string name = n->getName().getString();
00311         
00312         //ROS_INFO("Path[%i]: %s, type %s",i,name.c_str(),n->getTypeId().getName().getString());
00313 
00314         char ln[1000];
00315         int num;
00316         if (sscanf(name.c_str(), sscanfStr.c_str(), &num, ln) < 2) continue;
00317         // ROS_INFO("num: %i rest: %s\n",num,ln);
00318         extStr = ln; //urdf2inventor::helpers::getFilename(ln); // take only the name after the last '/'
00319         extNum = num;
00320         pathIdx = i;
00321         return n;
00322     }
00323     return NULL;
00324 }
00325 
00326 
00327 void InventorViewer::mouseBtnCB(void *userData, SoEventCallback *_pEvent)
00328 {
00329     InventorViewer * obj = static_cast<InventorViewer*>(userData);
00330     if (!obj)
00331     {
00332         ROS_ERROR("Invalid UseData passed into mouseBtnCB");
00333         return;
00334     }
00335 
00336     const SoEvent  *pEvent  = _pEvent->getEvent();
00337 
00338     // general callback:
00339     obj->onMouseBtnClick(_pEvent);
00340 
00341     // also see whether part of the model was clicked:
00342     const SoQtViewer *pViewer = obj->viewer;
00343 
00344     if (SoMouseButtonEvent::isButtonPressEvent(pEvent, SoMouseButtonEvent::BUTTON1))
00345     {
00346         SoRayPickAction rayPick(pViewer->getViewportRegion());
00347         rayPick.setPoint(pEvent->getPosition());
00348         rayPick.setPickAll(false);
00349         // rayPick.setRadius(1.0);
00350         rayPick.apply(pViewer->getSceneManager()->getSceneGraph());
00351         const SoPickedPoint *pPickedPt = rayPick.getPickedPoint();
00352         if (pPickedPt != NULL)
00353         {
00354             obj->onClickModel(pPickedPt);
00355 
00356             // see if a URDF link was clicked:
00357             /*SoPath *pPickPath = pPickedPt->getPath();
00358             std::string linkName;
00359             int visualNum;
00360             SoNode * linkNode = getLinkDesc(pPickPath, linkName, visualNum);
00361             if (!linkNode)
00362             {
00363                 ROS_INFO("Clicked on something other than a link");
00364                 return;
00365             }
00366             float x, y, z;
00367             pPickedPt->getObjectPoint(linkNode).getValue(x, y, z);
00368             ROS_INFO_STREAM("Clicked on " << linkName << ", at pos " << x << ", " << y << ", " << z);*/
00369         }
00370     }
00371 }


urdf_viewer
Author(s): Jennifer Buehler
autogenerated on Fri Mar 1 2019 03:38:13