CollisionDetectorViewer.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 #include <iostream>
00003 #include <boost/bind.hpp>
00004 #include <hrpModel/ModelLoaderUtil.h>
00005 #ifdef __APPLE__
00006 #include <GLUT/glut.h>
00007 #else
00008 #include <GL/glut.h>
00009 #endif
00010 #include "hrpsys/util/ProjectUtil.h"
00011 #include "hrpsys/util/GLbody.h"
00012 #include "hrpsys/util/GLlink.h"
00013 #include "hrpsys/util/GLutil.h"
00014 #include "hrpsys/util/SDLUtil.h"
00015 #include "hrpsys/util/LogManager.h"
00016 #include "hrpsys/util/BVutil.h"
00017 #include "TimedPosture.h"
00018 #include "GLscene.h"
00019 #include "hrpsys/idl/CollisionDetectorService.hh"
00020 
00021 using namespace hrp;
00022 using namespace OpenHRP;
00023 
00024 hrp::BodyPtr createBody(const std::string& name, const ModelItem& mitem,
00025                         ModelLoader_ptr modelloader)
00026 {
00027     BodyInfo_var binfo;
00028     try{
00029         OpenHRP::ModelLoader::ModelLoadOption opt;
00030         opt.readImage = true;
00031         opt.AABBdata.length(0);
00032         opt.AABBtype = OpenHRP::ModelLoader::AABB_NUM;
00033         binfo = modelloader->getBodyInfoEx(mitem.url.c_str(), opt);
00034     }catch(OpenHRP::ModelLoader::ModelLoaderException ex){
00035         std::cerr << ex.description << std::endl;
00036         return hrp::BodyPtr();
00037     }
00038     GLbody *glbody = new GLbody();
00039     hrp::BodyPtr body(glbody);
00040     hrp::loadBodyFromBodyInfo(body, binfo, true, GLlinkFactory);
00041     loadShapeFromBodyInfo(glbody, binfo);
00042 
00043     convertToConvexHull(body);
00044 
00045     body->setName(name);
00046     return body;
00047 }
00048 
00049 int main(int argc, char* argv[])
00050 {
00051     if (argc < 2){
00052         std::cerr << "Usage:" << argv[0] << " project.xml [-co CollisionDetector Component] [-size size] [-bg r g b]" << std::endl;
00053         return 1;
00054     }
00055 
00056     Project prj;
00057     if (!prj.parse(argv[1])){
00058         std::cerr << "failed to parse " << argv[1] << std::endl;
00059         return 1;
00060     }
00061 
00062     const char *coname = "co";
00063     int wsize = 0;
00064     float bgColor[] = {0,0,0};
00065     for (int i = 2; i<argc; i++){
00066         if (strcmp(argv[i], "-co")==0){
00067             coname = argv[++i];
00068         }else if(strcmp(argv[i], "-size")==0){
00069             wsize = atoi(argv[++i]);
00070         }else if(strcmp(argv[i], "-bg")==0){
00071             bgColor[0] = atof(argv[++i]);
00072             bgColor[1] = atof(argv[++i]);
00073             bgColor[2] = atof(argv[++i]);
00074         }
00075     }
00076 
00077     //================= CORBA =========================
00078     CORBA::ORB_var orb;
00079     CosNaming::NamingContext_var namingContext;
00080  
00081     try {
00082         orb = CORBA::ORB_init(argc, argv);
00083 
00084         CORBA::Object_var obj;
00085         obj = orb->resolve_initial_references("RootPOA");
00086         PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);
00087         if(CORBA::is_nil(poa)){
00088             throw std::string("error: failed to narrow root POA.");
00089         }
00090         
00091         PortableServer::POAManager_var poaManager = poa->the_POAManager();
00092         if(CORBA::is_nil(poaManager)){
00093             throw std::string("error: failed to narrow root POA manager.");
00094         }
00095         
00096         obj = orb->resolve_initial_references("NameService");
00097         namingContext = CosNaming::NamingContext::_narrow(obj);
00098         if(CORBA::is_nil(namingContext)){
00099             throw std::string("error: failed to narrow naming context.");
00100         }
00101         
00102         poaManager->activate();
00103     }catch (CORBA::SystemException& ex) {
00104         std::cerr << ex._rep_id() << std::endl;
00105     }catch (const std::string& error){
00106         std::cerr << error << std::endl;
00107     }
00108 
00109     //==================== viewer ===============
00110     LogManager<OpenHRP::CollisionDetectorService::CollisionState> log;
00111     CollisionDetectorComponent::GLscene scene(&log);
00112     scene.setBackGroundColor(bgColor);
00113 
00114     SDLwindow window(&scene, &log);
00115     window.init(wsize, wsize);
00116 
00117     std::vector<hrp::ColdetLinkPairPtr> pairs;
00118     ModelLoader_var modelloader = getModelLoader(namingContext);
00119     if (CORBA::is_nil(modelloader)){
00120         std::cerr << "openhrp-model-loader is not running" << std::endl;
00121         return 1;
00122     }
00123     BodyFactory factory = boost::bind(createBody, _1, _2, modelloader);
00124     //hrp::BodyPtr m_robot = createBody("pa10", , modelloader);
00125     //scene.addBody(m_robot);
00126     initWorld(prj, factory, scene, pairs);
00127     GLlink::drawMode(GLlink::DM_COLLISION);
00128     scene.showFloorGrid(false);
00129 
00130     log.enableRingBuffer(1);
00131     OpenHRP::CollisionDetectorService_var coService;
00132 
00133     while(window.oneStep()) {
00134         //==================== collision detecter ===============
00135 
00136         if (CORBA::is_nil(coService)){
00137             try{
00138                 CosNaming::Name name;
00139                 name.length(1);
00140                 name[0].id = CORBA::string_dup(coname);
00141                 name[0].kind = CORBA::string_dup("rtc");
00142                 CORBA::Object_var obj = namingContext->resolve(name);
00143                 RTC::RTObject_var rtc = RTC::RTObject::_narrow(obj);
00144                 const char *ior = getServiceIOR(rtc, "CollisionDetectorService");
00145                 coService = OpenHRP::CollisionDetectorService::_narrow(orb->string_to_object(ior));
00146             }catch(...){
00147                 std::cerr << "could not found collision detector component " << coname << std::endl;
00148                 return 1;
00149             }
00150         }
00151         // get CollisionState
00152         OpenHRP::CollisionDetectorService::CollisionState co;
00153         bool stateUpdate = false;
00154         if (!CORBA::is_nil(coService)){
00155             try{
00156                 OpenHRP::CollisionDetectorService::CollisionState_var t_co;
00157                 coService->getCollisionStatus(t_co);
00158                 co = t_co;
00159                 stateUpdate = true;
00160             }catch(...){
00161                 std::cerr << "exception in getCollisionStatus()" << std::endl;
00162                 coService = NULL;
00163             }
00164         }
00165 
00166         if (stateUpdate) {
00167             try {
00168                 GLbody* glbody = dynamic_cast<GLbody *>(scene.body(0).get());
00169                 for (size_t i=0; i<co.collide.length(); i++) {
00170                     ((GLlink *)glbody->link(i))->highlight(co.collide[i]);
00171                 }
00172 #if 0
00173                 TimedPosture tp;
00174                 tp.time = co.time;
00175                 tp.posture.resize(co.angle.length());
00176                 for (size_t i=0; i<tp.posture.size(); i++) {
00177                     tp.posture[i] = co.angle[i];
00178                 }
00179                 for (size_t i=0; i<co.lines.length(); i++) {
00180                     hrp::Vector3 p0, p1;
00181                     p0[0] = co.lines[i][0][0];
00182                     p0[1] = co.lines[i][0][1];
00183                     p0[2] = co.lines[i][0][2];
00184                     p1[0] = co.lines[i][1][0];
00185                     p1[1] = co.lines[i][1][1];
00186                     p1[2] = co.lines[i][1][2];
00187                     tp.lines.push_back(std::make_pair(p0,p1));
00188                 }
00189 #endif
00190                 log.add(co);
00191            } catch (...) {
00192                std::cerr << "exceptoin in stateUpdate" << std::endl;
00193            }
00194         }
00195 
00196         // update Scene
00197     }
00198 
00199     try {
00200         orb->destroy();
00201     }
00202     catch(...){
00203 
00204     }
00205 
00206     return 0;
00207 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:54