main.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 #include <hrpModel/ColdetLinkPair.h>
00006 #ifdef __APPLE__
00007 #include <GLUT/glut.h>
00008 #else
00009 #include <GL/glut.h>
00010 #endif
00011 #include "hrpsys/util/ProjectUtil.h"
00012 #include "hrpsys/util/GLbody.h"
00013 #include "hrpsys/util/GLlink.h"
00014 #include "hrpsys/util/GLutil.h"
00015 #include "hrpsys/util/SDLUtil.h"
00016 #include "GLscene.h"
00017 #include "Monitor.h"
00018 
00019 using namespace hrp;
00020 using namespace OpenHRP;
00021 
00022 hrp::BodyPtr createBody(const std::string& name, const ModelItem& mitem,
00023                         ModelLoader_ptr modelloader)
00024 {
00025     BodyInfo_var binfo;
00026     try{
00027         OpenHRP::ModelLoader::ModelLoadOption opt;
00028         opt.readImage = true;
00029         opt.AABBdata.length(0);
00030         opt.AABBtype = OpenHRP::ModelLoader::AABB_NUM;
00031         binfo = modelloader->getBodyInfoEx(mitem.url.c_str(), opt);
00032     }catch(OpenHRP::ModelLoader::ModelLoaderException ex){
00033         std::cerr << ex.description << std::endl;
00034         return hrp::BodyPtr();
00035     }
00036     GLbody *glbody = new GLbody();
00037     hrp::BodyPtr body(glbody);
00038     hrp::loadBodyFromBodyInfo(body, binfo, true, GLlinkFactory);
00039     loadShapeFromBodyInfo(glbody, binfo);
00040     body->setName(name);
00041     return body;
00042 }
00043 
00044 int main(int argc, char* argv[]) 
00045 {
00046     if (argc < 2){
00047         std::cerr << "Usage:" << argv[0] << " project.xml [-rh RobotHardwareComponent] [-sh StateHolder component] [-size size] [-bg r g b] [-host localhost] [-port 2809] [-interval 100] [-nogui]" << std::endl;
00048         return 1;
00049     }
00050 
00051     Project prj;
00052     if (!prj.parse(argv[1])){
00053         std::cerr << "failed to parse " << argv[1] << std::endl;
00054         return 1;
00055     }
00056 
00057     char *rhname = NULL, *shname = NULL, *hostname = NULL;
00058     int wsize = 0, port=0, interval=0;
00059     float bgColor[] = {0,0,0};
00060     bool orbinitref = false, gui = true;
00061     for (int i = 2; i<argc; i++){
00062         if (strcmp(argv[i], "-rh")==0){
00063             rhname = argv[++i];
00064         }else if(strcmp(argv[i], "-sh")==0){
00065             shname = argv[++i];
00066         }else if(strcmp(argv[i], "-size")==0){
00067             wsize = atoi(argv[++i]);
00068         }else if(strcmp(argv[i], "-bg")==0){
00069             bgColor[0] = atof(argv[++i]);
00070             bgColor[1] = atof(argv[++i]);
00071             bgColor[2] = atof(argv[++i]);
00072         }else if(strcmp(argv[i], "-host")==0){
00073             hostname = argv[++i];
00074         }else if(strcmp(argv[i], "-port")==0){
00075             port = atoi(argv[++i]);
00076         }else if(strcmp(argv[i], "-interval")==0){
00077             interval = atoi(argv[++i]);
00078         }else if(strcmp(argv[i], "-nogui")==0){
00079             gui = false;
00080         }else if(strcmp(argv[i], "-ORBInitRef")==0){
00081             orbinitref = true;
00082         }
00083     }
00084     char **nargv = argv;
00085     int nargc = argc;
00086     if(orbinitref == false){
00087         nargv = (char **)malloc((argc+2)*sizeof(char *));
00088         for(int i = 0; i < nargc; i++) nargv[i] = argv[i];
00089         char buf1[] = "-ORBInitRef";
00090         char buf2[256];
00091         sprintf(buf2, "NameService=corbaloc:iiop:%s:%d/NameService", hostname==NULL?"localhost":hostname, port==0?2809:port);
00092         nargv[nargc++] = buf1;
00093         nargv[nargc++] = buf2;
00094     }
00095 
00096     //================= CORBA =========================
00097     for(int i = 0; i < nargc-1; i++) {
00098         if(strcmp(nargv[i], "-ORBInitRef")==0) {
00099             std::cerr << "[monitor] Starting CORBA Client with " << nargv[i+1] << std::endl;
00100         }
00101     }
00102     CORBA::ORB_var orb;
00103     CosNaming::NamingContext_var namingContext;
00104     
00105     try {
00106         orb = CORBA::ORB_init(nargc, nargv);
00107         
00108         CORBA::Object_var obj;
00109         obj = orb->resolve_initial_references("RootPOA");
00110         PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);
00111         if(CORBA::is_nil(poa)){
00112             throw std::string("error: failed to narrow root POA.");
00113         }
00114         
00115         PortableServer::POAManager_var poaManager = poa->the_POAManager();
00116         if(CORBA::is_nil(poaManager)){
00117             throw std::string("error: failed to narrow root POA manager.");
00118         }
00119         
00120         obj = orb->resolve_initial_references("NameService");
00121         namingContext = CosNaming::NamingContext::_narrow(obj);
00122         if(CORBA::is_nil(namingContext)){
00123             throw std::string("error: failed to narrow naming context.");
00124         }
00125         
00126         poaManager->activate();
00127     }catch (CORBA::SystemException& ex) {
00128         std::cerr << "[monitor] Failed to initialize CORBA " << std::endl << ex._rep_id() << std::endl;
00129         return -1;
00130     }catch (const std::string& error){
00131         std::cerr << "[monitor] Failed to initialize CORBA " << std::endl << error << std::endl;
00132         return -1;
00133     }
00134 
00135     //================= logger ======================
00136     LogManager<TimedRobotState> log; 
00137     log.enableRingBuffer(5000);
00138     
00139     //================= monitor ======================
00140     RobotHardwareClientView rhview = prj.RobotHardwareClient();
00141     if ( hostname != NULL ) rhview.hostname = hostname;
00142     if ( port != 0 ) rhview.port = port;
00143     if ( interval != 0 ) rhview.interval = interval;
00144     std::cerr << "[monitor] Monitor service with " << rhview.hostname << "@" << rhview.port << ", " << 1000/rhview.interval << "Hz" << std::endl;
00145     Monitor monitor(orb, 
00146                     rhview.hostname, rhview.port, rhview.interval,
00147                     &log);
00148     if (rhname) {
00149         monitor.setRobotHardwareName(rhname);
00150     }else{
00151         monitor.setRobotHardwareName(rhview.RobotHardwareName.c_str());
00152     }
00153     if (shname) {
00154         monitor.setStateHolderName(shname);
00155     }else{
00156         monitor.setStateHolderName(rhview.StateHolderName.c_str());
00157     }
00158     //==================== viewer ===============
00159     if ( gui ) {
00160         GLscene scene(&log);
00161         scene.setBackGroundColor(bgColor);
00162         scene.showCoMonFloor(prj.view().showCoMonFloor);
00163     
00164         SDLwindow window(&scene, &log, &monitor);
00165         window.init(wsize, wsize);
00166 
00167         std::vector<hrp::ColdetLinkPairPtr> pairs;
00168         ModelLoader_var modelloader = getModelLoader(namingContext);
00169         if (CORBA::is_nil(modelloader)){
00170             std::cerr << "openhrp-model-loader is not running" << std::endl;
00171             return 1;
00172         }
00173         BodyFactory factory = boost::bind(createBody, _1, _2, modelloader);
00174 
00175         initWorld(prj, factory, scene, pairs);
00176         scene.setCollisionCheckPairs(pairs);
00177     
00178         monitor.start();
00179         int cnt=0;
00180         while(window.oneStep());
00181         monitor.stop();
00182     }
00183     else
00184     {
00185         std::vector<hrp::ColdetLinkPairPtr> pairs;
00186         ModelLoader_var modelloader = getModelLoader(namingContext);
00187         if (CORBA::is_nil(modelloader)){
00188             std::cerr << "openhrp-model-loader is not running" << std::endl;
00189             return 1;
00190         }
00191         BodyFactory factory = boost::bind(createBody, _1, _2, modelloader);
00192 
00193         // add bodies
00194         hrp::BodyPtr body;
00195         for (std::map<std::string, ModelItem>::iterator it=prj.models().begin();
00196              it != prj.models().end(); it++){
00197             const std::string name
00198                 = it->second.rtcName == "" ? it->first : it->second.rtcName;
00199             hrp::BodyPtr tmp_body = factory(name, it->second);
00200             if (tmp_body){
00201                 tmp_body->setName(name);
00202                 if(tmp_body->numJoints() > 0)
00203                     body = tmp_body;
00204                 else
00205                     std::cerr << "[monitor] Skipping non-robot model (" << name << ")" << std::endl;
00206             }
00207         }
00208         if (!body) {
00209             std::cerr << "[monitor]  Monitoring " << body->name() << std::endl;
00210             return -1;
00211         }
00212         monitor.start();
00213         while(monitor.oneStep()){
00214             log.updateIndex();
00215             monitor.showStatus(body);
00216         }
00217         monitor.stop();
00218     }
00219     try {
00220         orb->destroy();
00221     }
00222     catch(...){
00223         
00224     }
00225     
00226     return 0;
00227 }


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