main.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 #include <boost/bind.hpp>
00003 #include <boost/function.hpp>
00004 #include <rtm/Manager.h>
00005 #include <rtm/CorbaNaming.h>
00006 #include <hrpModel/ModelLoaderUtil.h>
00007 #ifdef __APPLE__
00008 #include <GLUT/glut.h>
00009 #else
00010 #include <GL/glut.h>
00011 #endif
00012 #include <SDL_thread.h>
00013 #include "hrpsys/util/GLbodyRTC.h"
00014 #include "hrpsys/util/GLlink.h"
00015 #include "hrpsys/util/GLutil.h"
00016 #include "hrpsys/util/Project.h"
00017 #include "hrpsys/util/OpenRTMUtil.h"
00018 #include "hrpsys/util/SDLUtil.h"
00019 #include "hrpsys/util/BVutil.h"
00020 #include "Simulator.h"
00021 #include "GLscene.h"
00022 
00023 using namespace std;
00024 using namespace hrp;
00025 using namespace OpenHRP;
00026 
00027 hrp::BodyPtr createBody(const std::string& name, const ModelItem& mitem,
00028                         ModelLoader_ptr modelloader, GLscene *scene,
00029                         bool usebbox)
00030 {
00031     std::cout << "createBody(" << name << "," << mitem.url << ")" << std::endl;
00032     RTC::Manager& manager = RTC::Manager::instance();
00033     std::string args = "GLbodyRTC?instance_name="+name;
00034     GLbodyRTC *glbodyrtc = (GLbodyRTC *)manager.createComponent(args.c_str());
00035     hrp::BodyPtr body = hrp::BodyPtr(glbodyrtc);
00036     BodyInfo_var binfo;
00037     try{
00038         OpenHRP::ModelLoader::ModelLoadOption opt;
00039         opt.readImage = true;
00040         opt.AABBdata.length(0);
00041         opt.AABBtype = OpenHRP::ModelLoader::AABB_NUM;
00042         binfo = modelloader->getBodyInfoEx(mitem.url.c_str(), opt);
00043     }catch(OpenHRP::ModelLoader::ModelLoaderException ex){
00044         std::cerr << ex.description << std::endl;
00045         return hrp::BodyPtr();
00046     }
00047     if (!loadBodyFromBodyInfo(body, binfo, true, GLlinkFactory)){
00048         std::cerr << "failed to load model[" << mitem.url << "]" << std::endl;
00049         manager.deleteComponent(glbodyrtc);
00050         return hrp::BodyPtr();
00051     }else{
00052         for (std::map<std::string, JointItem>::const_iterator it2=mitem.joint.begin();
00053              it2 != mitem.joint.end(); it2++){
00054             hrp::Link *link = body->link(it2->first);
00055             if (!link) continue;
00056             if (link) link->isHighGainMode = it2->second.isHighGain;
00057             if (it2->second.collisionShape == ""){
00058                 // do nothing
00059             }else if (it2->second.collisionShape == "convex hull"){
00060                 convertToConvexHull(link);
00061             }else if (it2->second.collisionShape == "AABB"){
00062                 convertToAABB(link);
00063             }else{
00064                 std::cerr << "unknown value of collisionShape property:" 
00065                           << it2->second.collisionShape << std::endl;
00066             }
00067         }
00068         glbodyrtc->setup();
00069         if (usebbox) convertToAABB(body);
00070         for (size_t i=0; i<mitem.inports.size(); i++){
00071             glbodyrtc->createInPort(mitem.inports[i]);
00072         }
00073         for (size_t i=0; i<mitem.outports.size(); i++){
00074             glbodyrtc->createOutPort(mitem.outports[i]);
00075         }
00076         loadShapeFromBodyInfo(glbodyrtc, binfo);
00077         body->setName(name);
00078         scene->addBody(body);
00079         return body;
00080     }
00081 }
00082 
00083 void print_usage(char* progname)
00084 {
00085     std::cerr << "Usage:" << progname << " [project file] [options]" << std::endl;
00086     std::cerr << "Options:" << std::endl;
00087     std::cerr << " -nodisplay         : headless mode" << std::endl;
00088     std::cerr << " -realtime          : syncronize to real world time" << std::endl;
00089     std::cerr << " -usebbox           : use bounding box for collision detection" << std::endl;
00090     std::cerr << " -endless           : endless mode" << std::endl;
00091     std::cerr << " -showsensors       : visualize sensors" << std::endl;
00092     std::cerr << " -size [pixels]     : specify window size in pixels" << std::endl;
00093     std::cerr << " -no-default-lights : disable ambient light (simulation environment will be dark)" << std::endl;
00094     std::cerr << " -max-edge-length [value] : specify maximum length of polygon edge (if exceed, polygon will be divided to improve rendering quality)" << std::endl;
00095     std::cerr << " -max-log-length [value] : specify maximum size of the log" << std::endl;
00096     std::cerr << " -exit-on-finish    : exit the program when the simulation finish" << std::endl;
00097     std::cerr << " -record            : record the simulation as movie" << std::endl;
00098     std::cerr << " -bg [r] [g] [b]    : specify background color" << std::endl;
00099     std::cerr << " -h --help          : show this help message" << std::endl;
00100 }
00101 
00102 int main(int argc, char* argv[]) 
00103 {
00104     bool display = true, usebbox=false;
00105     bool showsensors = false;
00106     int wsize = 0;
00107     bool useDefaultLights = true;
00108     double maxEdgeLen = 0;
00109     bool exitOnFinish = false;
00110     bool record = false;
00111     double maxLogLen = 60;
00112     bool realtime = false;
00113     bool endless = false;
00114 
00115     if (argc <= 1){
00116         print_usage(argv[0]);
00117         return 1;
00118     }
00119 
00120     float bgColor[]={0,0,0};
00121     for (int i=1; i<argc; i++){
00122         if (strcmp("-nodisplay",argv[i])==0){
00123             display = false;
00124         }else if(strcmp("-realtime", argv[i])==0){
00125             realtime = true;
00126         }else if(strcmp("-usebbox", argv[i])==0){
00127             usebbox = true;
00128         }else if(strcmp("-endless", argv[i])==0){
00129             endless = true;
00130         }else if(strcmp("-showsensors", argv[i])==0){
00131             showsensors = true;
00132         }else if(strcmp("-size", argv[i])==0){
00133             wsize = atoi(argv[++i]);
00134         }else if(strcmp("-no-default-lights", argv[i])==0){
00135             useDefaultLights = false;
00136         }else if(strcmp("-max-edge-length", argv[i])==0){
00137             maxEdgeLen = atof(argv[++i]);
00138         }else if(strcmp("-max-log-length", argv[i])==0){
00139             maxLogLen = atof(argv[++i]);
00140         }else if(strcmp("-exit-on-finish", argv[i])==0){
00141             exitOnFinish = true;
00142         }else if(strcmp("-record", argv[i])==0){
00143             record = true;
00144             exitOnFinish = true;
00145         }else if(strcmp("-bg", argv[i])==0){
00146             bgColor[0] = atof(argv[++i]);
00147             bgColor[1] = atof(argv[++i]);
00148             bgColor[2] = atof(argv[++i]);
00149         }else if(strcmp("-h", argv[i])==0 || strcmp("--help", argv[i])==0){
00150             print_usage(argv[0]);
00151             return 1;
00152         }
00153     }
00154 
00155     Project prj;
00156     if (!prj.parse(argv[1])){
00157         std::cerr << "failed to parse " << argv[1] << std::endl;
00158         return 1;
00159     }
00160     if (realtime){
00161         prj.realTime(true);
00162     }
00163     if (endless){
00164         prj.totalTime(0);
00165     }
00166 
00167     //================= OpenRTM =========================
00168     RTC::Manager* manager;
00169     int rtmargc=0;
00170     std::vector<char *> rtmargv;
00171     for (int i=1; i<argc; i++){
00172         if (strcmp(argv[i], "-nodisplay") 
00173             && strcmp(argv[i], "-realtime")
00174             && strcmp(argv[i], "-usebbox")
00175             && strcmp(argv[i], "-endless")
00176             && strcmp(argv[i], "-showsensors")
00177             && strcmp(argv[i], "-size")
00178             && strcmp(argv[i], "-no-default-lights")
00179             && strcmp(argv[i], "-max-edge-length")
00180             && strcmp(argv[i], "-max-log-length")
00181             && strcmp(argv[i], "-exit-on-finish")
00182             && strcmp(argv[i], "-record")
00183             && strcmp(argv[i], "-bg")
00184             ){
00185             rtmargv.push_back(argv[i]);
00186             rtmargc++;
00187         }
00188     }
00189     manager = RTC::Manager::init(rtmargc, rtmargv.data());
00190     manager->init(rtmargc, rtmargv.data());
00191     GLbodyRTC::moduleInit(manager);
00192     manager->activateManager();
00193     manager->runManager(true);
00194 
00195     std::string nameServer = manager->getConfig()["corba.nameservers"];
00196     int comPos = nameServer.find(",");
00197     if (comPos < 0){
00198         comPos = nameServer.length();
00199     }
00200     nameServer = nameServer.substr(0, comPos);
00201     RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
00202 
00203     ModelLoader_var modelloader = getModelLoader(CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00204     if (CORBA::is_nil(modelloader)){
00205         std::cerr << "openhrp-model-loader is not running" << std::endl;
00206         return 1;
00207     }
00208     //==================== Viewer setup ===============
00209     LogManager<SceneState> log;
00210     GLscene scene(&log);
00211     scene.setBackGroundColor(bgColor);
00212     scene.showSensors(showsensors);
00213     scene.maxEdgeLen(maxEdgeLen);
00214     scene.showCollision(prj.view().showCollision);
00215     Simulator simulator(&log);
00216 
00217     SDLwindow window(&scene, &log, &simulator);
00218     if (display){
00219         window.init(wsize, wsize);
00220         if (!useDefaultLights) scene.defaultLights(false);
00221         window.setView(prj.view().T);
00222         scene.showFloorGrid(prj.view().showScale);
00223     }
00224 
00225     //================= setup Simulator ======================
00226     BodyFactory factory = boost::bind(createBody, _1, _2, modelloader, &scene, usebbox);
00227     simulator.init(prj, factory);
00228     if (!prj.totalTime()){
00229         log.enableRingBuffer(maxLogLen/prj.timeStep());
00230     }
00231 
00232     std::cout << "timestep = " << prj.timeStep() << ", total time = " 
00233               << prj.totalTime() << std::endl;
00234 
00235     if (display){
00236         simulator.start();
00237         while(window.oneStep()){
00238             if (exitOnFinish && !simulator.isRunning()) break;
00239         };
00240         simulator.stop();
00241         if (record){
00242             log.record(10);
00243             while(window.oneStep()){
00244                 if (!log.isRecording()) break;
00245             }
00246         }
00247     }else{
00248         while (simulator.oneStep());
00249     }
00250 
00251     manager->shutdown();
00252 
00253     return 0;
00254 }


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