OGMap3DViewer.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/CorbaNaming.h>
00011 #include <GL/gl.h>
00012 #include "IrrModel.h"
00013 #include <math.h>
00014 #include <hrpModel/ModelLoaderUtil.h>
00015 #include "OGMap3DViewer.h"
00016 
00017 using namespace irr;
00018 using namespace core;
00019 using namespace scene;
00020 using namespace video;
00021 
00022 class CMapSceneNode : public irr::scene::ISceneNode
00023 {
00024 public:
00025     CMapSceneNode(ISceneNode *i_parent, ISceneManager *i_mgr, s32 i_id,
00026                   double i_origin[3], double i_size[3]) :
00027         ISceneNode(i_parent, i_mgr, i_id),
00028         m_map(NULL){
00029         // bounding box
00030         m_vertices[0] = vector3df(i_origin[0], -i_origin[1], i_origin[2]);
00031         m_vertices[1] = vector3df(i_origin[0]+i_size[0], -i_origin[1], i_origin[2]);
00032         m_vertices[2] = vector3df(i_origin[0]+i_size[0], -(i_origin[1]+i_size[1]), i_origin[2]);
00033         m_vertices[3] = vector3df(i_origin[0], -(i_origin[1]+i_size[1]), i_origin[2]);
00034 
00035         m_vertices[4] = m_vertices[0]; m_vertices[4].Z += i_size[2];
00036         m_vertices[5] = m_vertices[1]; m_vertices[5].Z += i_size[2];
00037         m_vertices[6] = m_vertices[2]; m_vertices[6].Z += i_size[2];
00038         m_vertices[7] = m_vertices[3]; m_vertices[7].Z += i_size[2];
00039 
00040         m_box.reset(m_vertices[0]);
00041         for (int i=1; i<8; i++) m_box.addInternalPoint(m_vertices[i]);
00042 
00043         // vertex indices of cube
00044         m_cubeIndices[ 0] = 0; m_cubeIndices[ 1] = 1; m_cubeIndices[ 2] = 2;
00045         m_cubeIndices[ 3] = 2; m_cubeIndices[ 4] = 3; m_cubeIndices[ 5] = 0;
00046 
00047         m_cubeIndices[ 6] = 4; m_cubeIndices[ 7] = 5; m_cubeIndices[ 8] = 6;
00048         m_cubeIndices[ 9] = 6; m_cubeIndices[10] = 7; m_cubeIndices[11] = 4;
00049 
00050         m_cubeIndices[12] = 8; m_cubeIndices[13] = 9; m_cubeIndices[14] = 10;
00051         m_cubeIndices[15] = 10; m_cubeIndices[16] = 11; m_cubeIndices[17] = 8;
00052 
00053         m_cubeIndices[18] = 12; m_cubeIndices[19] = 13; m_cubeIndices[20] = 14;
00054         m_cubeIndices[21] = 14; m_cubeIndices[22] = 15; m_cubeIndices[23] = 12;
00055 
00056         m_cubeIndices[24] = 16; m_cubeIndices[25] = 17; m_cubeIndices[26] = 18;
00057         m_cubeIndices[27] = 18; m_cubeIndices[28] = 19; m_cubeIndices[29] = 16;
00058 
00059         m_cubeIndices[30] = 20; m_cubeIndices[31] = 21; m_cubeIndices[32] = 22;
00060         m_cubeIndices[33] = 22; m_cubeIndices[34] = 23; m_cubeIndices[35] = 20;
00061     }
00062 
00063     void setMap(OpenHRP::OGMap3D *i_map) { m_map = i_map; }
00064 
00065     void setupCubeVertices(double i_res){
00066         SColor white(0xff, 0xff, 0xff, 0xff);
00067         // +z
00068         m_cubeVerts[0] = S3DVertex(-i_res/2, -i_res/2, i_res/2,
00069                                    0,0,1, white,0,0);
00070         m_cubeVerts[1] = S3DVertex( i_res/2, -i_res/2, i_res/2,
00071                                    0,0,1, white,0,0);
00072         m_cubeVerts[2] = S3DVertex( i_res/2,  i_res/2, i_res/2,
00073                                    0,0,1, white,0,0);
00074         m_cubeVerts[3] = S3DVertex(-i_res/2,  i_res/2, i_res/2,
00075                                    0,0,1, white,0,0);
00076         // -z
00077         m_cubeVerts[4] = S3DVertex(-i_res/2,  i_res/2, -i_res/2,
00078                                    0,0,-1, white,0,0);
00079         m_cubeVerts[5] = S3DVertex( i_res/2,  i_res/2, -i_res/2,
00080                                    0,0,-1, white,0,0);
00081         m_cubeVerts[6] = S3DVertex( i_res/2, -i_res/2, -i_res/2,
00082                                    0,0,-1, white,0,0);
00083         m_cubeVerts[7] = S3DVertex(-i_res/2, -i_res/2, -i_res/2,
00084                                    0,0,-1, white,0,0);
00085         // +x
00086         m_cubeVerts[8] = S3DVertex(i_res/2, -i_res/2,  i_res/2,
00087                                    1,0,0, white,0,0);
00088         m_cubeVerts[9] = S3DVertex(i_res/2, -i_res/2, -i_res/2,
00089                                    1,0,0, white,0,0);
00090         m_cubeVerts[10] = S3DVertex(i_res/2,  i_res/2, -i_res/2,
00091                                    1,0,0, white,0,0);
00092         m_cubeVerts[11] = S3DVertex(i_res/2,  i_res/2,  i_res/2,
00093                                    1,0,0, white,0,0);
00094         // -x
00095         m_cubeVerts[12] = S3DVertex(-i_res/2,  i_res/2,  i_res/2,
00096                                    -1,0,0, white,0,0);
00097         m_cubeVerts[13] = S3DVertex(-i_res/2,  i_res/2, -i_res/2,
00098                                    -1,0,0, white,0,0);
00099         m_cubeVerts[14] = S3DVertex(-i_res/2, -i_res/2, -i_res/2,
00100                                    -1,0,0, white,0,0);
00101         m_cubeVerts[15] = S3DVertex(-i_res/2, -i_res/2,  i_res/2,
00102                                     -1,0,0, white,0,0);
00103         // +y
00104         m_cubeVerts[16] = S3DVertex(i_res/2, i_res/2,  i_res/2,
00105                                    0,1,0, white,0,0);
00106         m_cubeVerts[17] = S3DVertex(i_res/2, i_res/2, -i_res/2,
00107                                    0,1,0, white,0,0);
00108         m_cubeVerts[18] = S3DVertex(-i_res/2,i_res/2, -i_res/2,
00109                                    0,1,0, white,0,0);
00110         m_cubeVerts[19] = S3DVertex(-i_res/2,i_res/2,  i_res/2,
00111                                    0,1,0, white,0,0);
00112         // -y
00113         m_cubeVerts[20] = S3DVertex(-i_res/2,-i_res/2,  i_res/2,
00114                                    0,-1,0, white,0,0);
00115         m_cubeVerts[21] = S3DVertex(-i_res/2,-i_res/2, -i_res/2,
00116                                    0,-1,0, white,0,0);
00117         m_cubeVerts[22] = S3DVertex(i_res/2, -i_res/2, -i_res/2,
00118                                    0,-1,0, white,0,0);
00119         m_cubeVerts[23] = S3DVertex(i_res/2, -i_res/2,  i_res/2,
00120                                    0,-1,0, white,0,0);
00121 
00122     }
00123     virtual void OnRegisterSceneNode(){
00124         if (IsVisible)
00125             SceneManager->registerNodeForRendering(this);
00126         
00127         ISceneNode::OnRegisterSceneNode();
00128     }
00129     virtual void render() {
00130         IVideoDriver *driver = SceneManager->getVideoDriver();
00131         matrix4 m;
00132         driver->setTransform(ETS_WORLD, m);
00133 
00134         // bottom 
00135         driver->draw3DLine(m_vertices[0], m_vertices[1]);
00136         driver->draw3DLine(m_vertices[1], m_vertices[2]);
00137         driver->draw3DLine(m_vertices[2], m_vertices[3]);
00138         driver->draw3DLine(m_vertices[3], m_vertices[0]);
00139         // top 
00140         driver->draw3DLine(m_vertices[4], m_vertices[5]);
00141         driver->draw3DLine(m_vertices[5], m_vertices[6]);
00142         driver->draw3DLine(m_vertices[6], m_vertices[7]);
00143         driver->draw3DLine(m_vertices[7], m_vertices[4]);
00144         // vertical lines
00145         driver->draw3DLine(m_vertices[0], m_vertices[4]);
00146         driver->draw3DLine(m_vertices[1], m_vertices[5]);
00147         driver->draw3DLine(m_vertices[2], m_vertices[6]);
00148         driver->draw3DLine(m_vertices[3], m_vertices[7]);
00149 
00150         if (!m_map) return;
00151         setupCubeVertices(m_map->resolution);
00152         double res = m_map->resolution;
00153         int rank=0;
00154         int nxny = m_map->nx*m_map->ny;
00155 
00156         for (int i=0; i<m_map->nx; i++){
00157             m[12] = m_map->pos.x + i*res;
00158             for (int j=0; j<m_map->ny; j++){
00159                 m[13] = -(m_map->pos.y + j*res);
00160                 for (int k=0; k<m_map->nz; k++){
00161                     m[14] = m_map->pos.z + k*res;
00162                     unsigned char p = m_map->cells[rank++]; 
00163                     if (p != OpenHRP::gridUnknown 
00164                         && p != OpenHRP::gridEmpty){
00165                         driver->setTransform(ETS_WORLD, m);
00166                         driver->drawIndexedTriangleList(m_cubeVerts, 24,
00167                                                         m_cubeIndices, 12);
00168                     }
00169                 }
00170 
00171             }
00172 
00173         }
00174     }
00175     virtual const aabbox3d<f32>& getBoundingBox() const { return m_box; }
00176 private:
00177     irr::core::aabbox3d<f32> m_box;
00178     vector3df m_vertices[8];
00179     S3DVertex m_tileVerts[4], m_cubeVerts[24];
00180     u16 m_tileIndices[4], m_cubeIndices[36];
00181     float m_scale[3], m_origin[3];
00182     OpenHRP::OGMap3D* m_map;
00183 };
00184 
00185 // Module specification
00186 // <rtc-template block="module_spec">
00187 static const char* nullcomponent_spec[] =
00188   {
00189     "implementation_id", "OGMap3DViewer",
00190     "type_name",         "OGMap3DViewer",
00191     "description",       "null component",
00192     "version",           HRPSYS_PACKAGE_VERSION,
00193     "vendor",            "AIST",
00194     "category",          "example",
00195     "activity_type",     "DataFlowComponent",
00196     "max_instance",      "10",
00197     "language",          "C++",
00198     "lang_type",         "compile",
00199     // Configuration variables
00200     "conf.default.generateImageSequence", "0",
00201     "conf.default.generateMovie", "0",
00202     "conf.default.xSize", "4",
00203     "conf.default.ySize", "4",
00204     "conf.default.zSize", "4",
00205     "conf.default.xOrigin", "0",
00206     "conf.default.yOrigin", "-2",
00207     "conf.default.zOrigin", "0",
00208 
00209     ""
00210   };
00211 // </rtc-template>
00212 
00213 OGMap3DViewer::OGMap3DViewer(RTC::Manager* manager)
00214   : RTC::DataFlowComponentBase(manager),
00215     // <rtc-template block="initializer">
00216     m_qIn("q", m_q),
00217     m_pIn("p", m_p),
00218     m_rpyIn("rpy", m_rpy),
00219     m_OGMap3DServicePort("OGMap3DService"),
00220     // </rtc-template>
00221     dummy(0),
00222     m_isopen(false),
00223     m_generateImageSequence(false),
00224     m_body(NULL),
00225     m_imageCount(0),
00226     m_ogmap(NULL),
00227     m_generateMovie(false),
00228     m_isGeneratingMovie(false)
00229 {
00230 }
00231 
00232 OGMap3DViewer::~OGMap3DViewer()
00233 {
00234 }
00235 
00236 
00237 
00238 RTC::ReturnCode_t OGMap3DViewer::onInitialize()
00239 {
00240   std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00241   // <rtc-template block="bind_config">
00242   // Bind variables and configuration variable
00243   bindParameter("generateImageSequence", m_generateImageSequence, "0");
00244   bindParameter("generateMovie",      m_generateMovie, "0");
00245   bindParameter("xSize",      m_xSize, "4");
00246   bindParameter("ySize",      m_ySize, "4");
00247   bindParameter("zSize",      m_zSize, "4");
00248   bindParameter("xOrigin",      m_xOrigin, "0");
00249   bindParameter("yOrigin",      m_yOrigin, "-2");
00250   bindParameter("zOrigin",      m_zOrigin, "0");
00251   
00252   // </rtc-template>
00253 
00254   // Registration: InPort/OutPort/Service
00255   // <rtc-template block="registration">
00256   // Set InPort buffers
00257   addInPort("q", m_qIn);
00258   addInPort("p", m_pIn);
00259   addInPort("rpy", m_rpyIn);
00260 
00261   // Set OutPort buffer
00262   
00263   // Set service provider to Ports
00264   
00265   // Set service consumers to Ports
00266   m_OGMap3DServicePort.registerConsumer("service1","OGMap3DService",m_OGMap3DService);
00267   
00268   // Set CORBA Service Ports
00269   addPort(m_OGMap3DServicePort);
00270   
00271   // </rtc-template>
00272 
00273   RTC::Properties& prop = getProperties();
00274 
00275   return RTC::RTC_OK;
00276 }
00277 
00278 
00279 
00280 /*
00281 RTC::ReturnCode_t OGMap3DViewer::onFinalize()
00282 {
00283   return RTC::RTC_OK;
00284 }
00285 */
00286 
00287 /*
00288 RTC::ReturnCode_t OGMap3DViewer::onStartup(RTC::UniqueId ec_id)
00289 {
00290   return RTC::RTC_OK;
00291 }
00292 */
00293 
00294 /*
00295 RTC::ReturnCode_t OGMap3DViewer::onShutdown(RTC::UniqueId ec_id)
00296 {
00297   return RTC::RTC_OK;
00298 }
00299 */
00300 
00301 RTC::ReturnCode_t OGMap3DViewer::onActivated(RTC::UniqueId ec_id)
00302 {
00303   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00304   return RTC::RTC_OK;
00305 }
00306 
00307 RTC::ReturnCode_t OGMap3DViewer::onDeactivated(RTC::UniqueId ec_id)
00308 {
00309   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00310   return RTC::RTC_OK;
00311 }
00312 
00313 void capture(int w, int h, unsigned char *o_buffer)
00314 {
00315     glReadBuffer(GL_BACK);
00316     glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
00317     for (int i=0; i<h; i++){
00318         glReadPixels(0,(h-1-i),w,1,GL_RGB,GL_UNSIGNED_BYTE,
00319                      o_buffer + i*3*w);
00320     }
00321 }
00322 
00323 void save(int w, int h, const char *i_fname)
00324 {
00325     unsigned char *buffer = new unsigned char[w*h*3];
00326 
00327     capture(w, h, buffer);
00328     std::ofstream ofs("test.ppm", std::ios::out | std::ios::trunc | std::ios::binary );
00329     char buf[10];
00330     sprintf(buf, "%d %d", w, h);
00331     ofs << "P6" << std::endl << buf << std::endl << "255" << std::endl;
00332     for (int i=h-1; i>=0; i--){
00333         ofs.write((char *)(buffer+i*w*3), w*3);
00334     }
00335     delete [] buffer;
00336 }
00337 
00338 
00339 RTC::ReturnCode_t OGMap3DViewer::onExecute(RTC::UniqueId ec_id)
00340 {
00341     if (m_qIn.isNew()) m_qIn.read();
00342     if (m_pIn.isNew()) m_pIn.read();
00343     if (m_rpyIn.isNew()) m_rpyIn.read();
00344 
00345     if (!m_isopen){
00346         GLscene *scene = GLscene::getInstance();
00347 
00348         scene->init();
00349 
00350         RTC::Properties& prop = getProperties();
00351         RTC::Manager& rtcManager = RTC::Manager::instance();
00352         std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00353         int comPos = nameServer.find(",");
00354         if (comPos < 0){
00355             comPos = nameServer.length();
00356         }
00357         nameServer = nameServer.substr(0, comPos);
00358         RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00359         if (prop["model"] != ""){
00360             std::cerr << "model = " << prop["model"] << std::endl;
00361             OpenHRP::BodyInfo_var binfo = hrp::loadBodyInfo(
00362                 prop["model"].c_str(), 
00363                 CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00364             m_body = scene->addBody(binfo);
00365         }
00366 
00367         double origin[] = {m_xOrigin, m_yOrigin, m_zOrigin};
00368         double size[] = {m_xSize, m_ySize, m_zSize};
00369         ISceneManager *smgr = scene->getSceneManager();
00370         m_mapNode = new CMapSceneNode(smgr->getRootSceneNode(), smgr, -1, origin, size);
00371         
00372         m_isopen = true;
00373     }
00374 
00375     OpenHRP::AABB region;
00376     region.pos.x = m_xOrigin;
00377     region.pos.y = m_yOrigin;
00378     region.pos.z = m_zOrigin;
00379     region.size.l = m_xSize;
00380     region.size.w = m_ySize;
00381     region.size.h = m_zSize;
00382 
00383     if (m_ogmap){
00384         delete m_ogmap;
00385         m_ogmap = NULL;
00386     }
00387     if (!CORBA::is_nil(m_OGMap3DService.getObject())){
00388         try{
00389             m_ogmap = m_OGMap3DService->getOGMap3D(region);
00390         }catch(CORBA::SystemException& ex){
00391             // provider is not activated
00392         }
00393     }
00394     m_mapNode->setMap(m_ogmap);
00395     
00396     GLscene *scene = GLscene::getInstance();
00397     GLcamera *camera=scene->getCamera();
00398 
00399     if (m_body && m_q.data.length() > 0){
00400         double pos[] = {m_p.data.x, m_p.data.y, m_p.data.z}; 
00401         double rpy[] = {m_rpy.data.r, m_rpy.data.p, m_rpy.data.y}; 
00402         m_body->setPosture(m_q.data.get_buffer(), pos, rpy);
00403     }
00404 
00405     scene->draw();
00406 
00407     if (m_generateImageSequence){
00408         char buf[30];
00409         sprintf(buf, "OGMap3DViewer%03d.ppm", m_imageCount++);
00410         save(camera->width(), camera->height(), buf);
00411     }
00412 
00413     if (m_generateMovie){
00414         if (!m_isGeneratingMovie){
00415           std::string fname(m_profile.instance_name);
00416           fname += ".avi";
00417             m_videoWriter = cvCreateVideoWriter(
00418                 fname.c_str(),
00419                 CV_FOURCC('D','I','V','X'),
00420                 10,
00421                 cvSize(camera->width(), camera->height()));
00422             m_cvImage = cvCreateImage(
00423                 cvSize(camera->width(), camera->height()),
00424                 IPL_DEPTH_8U, 3);
00425             m_isGeneratingMovie = true;
00426         }
00427         // RGB -> BGR
00428         unsigned char rgb[camera->width()*camera->height()*3];
00429         capture(camera->width(), camera->height(), rgb);
00430         char *bgr = m_cvImage->imageData;
00431         for (int i=0; i<camera->width()*camera->height(); i++){
00432             bgr[i*3  ] = rgb[i*3+2]; 
00433             bgr[i*3+1] = rgb[i*3+1]; 
00434             bgr[i*3+2] = rgb[i*3  ]; 
00435         }
00436         cvWriteFrame(m_videoWriter, m_cvImage);
00437     }else{
00438         if (m_isGeneratingMovie){
00439             cvReleaseVideoWriter(&m_videoWriter);
00440             cvReleaseImage(&m_cvImage);
00441             m_isGeneratingMovie = false;
00442         }
00443     }
00444 
00445     
00446 
00447     return RTC::RTC_OK;
00448 }
00449 
00450 /*
00451 RTC::ReturnCode_t OGMap3DViewer::onAborting(RTC::UniqueId ec_id)
00452 {
00453   return RTC::RTC_OK;
00454 }
00455 */
00456 
00457 /*
00458 RTC::ReturnCode_t OGMap3DViewer::onError(RTC::UniqueId ec_id)
00459 {
00460   return RTC::RTC_OK;
00461 }
00462 */
00463 
00464 /*
00465 RTC::ReturnCode_t OGMap3DViewer::onReset(RTC::UniqueId ec_id)
00466 {
00467   return RTC::RTC_OK;
00468 }
00469 */
00470 
00471 /*
00472 RTC::ReturnCode_t OGMap3DViewer::onStateUpdate(RTC::UniqueId ec_id)
00473 {
00474   return RTC::RTC_OK;
00475 }
00476 */
00477 
00478 /*
00479 RTC::ReturnCode_t OGMap3DViewer::onRateChanged(RTC::UniqueId ec_id)
00480 {
00481   return RTC::RTC_OK;
00482 }
00483 */
00484 
00485 
00486 
00487 extern "C"
00488 {
00489 
00490   void OGMap3DViewerInit(RTC::Manager* manager)
00491   {
00492     RTC::Properties profile(nullcomponent_spec);
00493     manager->registerFactory(profile,
00494                              RTC::Create<OGMap3DViewer>,
00495                              RTC::Delete<OGMap3DViewer>);
00496   }
00497 
00498 };
00499 
00500 


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