OccupancyGridMap3D.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "OccupancyGridMap3D.h"
00011 #include "hrpUtil/Eigen3d.h"
00012 #include <octomap/octomap.h>
00013 
00014 #define KDEBUG 0
00015 //#define KDEBUG 1 // 121022
00016 
00017 #ifdef __APPLE__
00018 inline bool isnan(double x)
00019 {
00020   return (x != x);
00021 }
00022 #endif
00023 
00024 typedef coil::Guard<coil::Mutex> Guard;
00025 
00026 using namespace octomap;
00027 
00028 // Module specification
00029 // <rtc-template block="module_spec">
00030 static const char* occupancygridmap3d_spec[] =
00031   {
00032     "implementation_id", "OccupancyGridMap3D",
00033     "type_name",         "OccupancyGridMap3D",
00034     "description",       "null component",
00035     "version",           HRPSYS_PACKAGE_VERSION,
00036     "vendor",            "AIST",
00037     "category",          "example",
00038     "activity_type",     "DataFlowComponent",
00039     "max_instance",      "10",
00040     "language",          "C++",
00041     "lang_type",         "compile",
00042     // Configuration variables
00043     "conf.default.occupiedThd", "0.5",
00044     "conf.default.resolution", "0.1",
00045     "conf.default.initialMap", "",
00046     "conf.default.knownMap", "",
00047     "conf.default.debugLevel", "0",
00048     ""
00049   };
00050 // </rtc-template>
00051 
00052 OccupancyGridMap3D::OccupancyGridMap3D(RTC::Manager* manager)
00053   : RTC::DataFlowComponentBase(manager),
00054     // <rtc-template block="initializer">
00055     m_rangeIn("range", m_range),
00056     m_cloudIn("cloud", m_cloud),
00057     m_poseIn("pose", m_pose),
00058     m_sensorPosIn("sensorPos", m_sensorPos),
00059     m_updateIn("update", m_update),
00060     m_updateOut("updateSignal", m_updateSignal),
00061     m_OGMap3DServicePort("OGMap3DService"),
00062     // </rtc-template>
00063     m_service0(this),
00064     m_map(NULL),
00065     m_knownMap(NULL),
00066     dummy(0)
00067 {
00068 }
00069 
00070 OccupancyGridMap3D::~OccupancyGridMap3D()
00071 {
00072 }
00073 
00074 
00075 
00076 RTC::ReturnCode_t OccupancyGridMap3D::onInitialize()
00077 {
00078   std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00079   // <rtc-template block="bind_config">
00080   // Bind variables and configuration variable
00081   bindParameter("occupiedThd", m_occupiedThd, "0.5");
00082   bindParameter("resolution", m_resolution, "0.1");
00083   bindParameter("initialMap", m_initialMap, "");
00084   bindParameter("knownMap", m_knownMapPath, "");
00085   bindParameter("debugLevel", m_debugLevel, "0");
00086   
00087   // </rtc-template>
00088 
00089   // Registration: InPort/OutPort/Service
00090   // <rtc-template block="registration">
00091   // Set InPort buffers
00092   addInPort("range", m_rangeIn);
00093   addInPort("cloud", m_cloudIn);
00094   addInPort("pose", m_poseIn);
00095   addInPort("sensorPos", m_sensorPosIn);
00096   addInPort("update", m_updateIn);
00097 
00098   // Set OutPort buffer
00099   addOutPort("updateSignal", m_updateOut);
00100   
00101   // Set service provider to Ports
00102   m_OGMap3DServicePort.registerProvider("service1", "OGMap3DService", m_service0);
00103   
00104   // Set service consumers to Ports
00105   
00106   // Set CORBA Service Ports
00107   addPort(m_OGMap3DServicePort);
00108   
00109   // </rtc-template>
00110 
00111   //RTC::Properties& prop = getProperties();
00112 
00113   char cwd[PATH_MAX];
00114   char *p = getcwd(cwd, PATH_MAX);
00115   m_cwd = p;
00116   m_cwd += "/";
00117 
00118   m_update.data = 1;
00119 
00120   m_pose.data.position.x = 0; 
00121   m_pose.data.position.y = 0; 
00122   m_pose.data.position.z = 0; 
00123   m_pose.data.orientation.r = 0;
00124   m_pose.data.orientation.p = 0;
00125   m_pose.data.orientation.y = 0;
00126 
00127   m_sensorPos.data.x = 0;
00128   m_sensorPos.data.y = 0;
00129   m_sensorPos.data.z = 0;
00130 
00131   m_updateSignal.data = 0;
00132  
00133   return RTC::RTC_OK;
00134 }
00135 
00136 
00137 /*
00138 RTC::ReturnCode_t OccupancyGridMap3D::onFinalize()
00139 {
00140   return RTC::RTC_OK;
00141 }
00142 */
00143 
00144 /*
00145 RTC::ReturnCode_t OccupancyGridMap3D::onStartup(RTC::UniqueId ec_id)
00146 {
00147   return RTC::RTC_OK;
00148 }
00149 */
00150 
00151 /*
00152 RTC::ReturnCode_t OccupancyGridMap3D::onShutdown(RTC::UniqueId ec_id)
00153 {
00154   return RTC::RTC_OK;
00155 }
00156 */
00157 
00158 RTC::ReturnCode_t OccupancyGridMap3D::onActivated(RTC::UniqueId ec_id)
00159 {
00160   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00161 
00162   Guard guard(m_mutex);
00163   if (m_knownMapPath != ""){
00164       m_knownMap = new OcTree(m_cwd+m_knownMapPath);
00165   }
00166 
00167   if (m_initialMap != ""){
00168     // Working directories of threads which calls onInitialize() and onActivate() are different on MacOS
00169     // Assume path of initial map is given by a relative path to working directory of the thread which calls onInitialize()
00170     m_map = new OcTree(m_cwd+m_initialMap);
00171   }else{
00172     m_map = new OcTree(m_resolution);
00173   }
00174   m_updateOut.write();
00175 
00176   if(KDEBUG){
00177     std::cout << m_profile.instance_name << ": initial tree depth = " << m_map->getTreeDepth() << std::endl;
00178     std::cout << m_profile.instance_name << ": initial tree size = " << m_map->size() << std::endl;
00179     point3d p;
00180     //    p.x() = 0.1; //0.05;
00181     //    p.y() = 2.2; //2.15;
00182     //    p.z() = 1.4; //1.45;
00183     p.x() = -0.5; //-0.55;
00184     p.y() = 3.0; //2.95;
00185     p.z() = 0.5; //0.55;
00186     OcTreeNode *result = m_map->search(p);
00187     if (result){
00188       double prob = result->getOccupancy();
00189       std::cout << m_profile.instance_name << ": initial " << p << " " << prob << std::endl;
00190     std::cout << m_profile.instance_name << ": second tree depth = " << m_map->getTreeDepth() << std::endl;
00191     std::cout << m_profile.instance_name << ": second tree size = " << m_map->size() << std::endl;
00192     }
00193   }
00194 
00195   return RTC::RTC_OK;
00196 }
00197 
00198 RTC::ReturnCode_t OccupancyGridMap3D::onDeactivated(RTC::UniqueId ec_id)
00199 {
00200   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00201   Guard guard(m_mutex);
00202   delete m_map;
00203   if (m_knownMap) delete m_knownMap;
00204   return RTC::RTC_OK;
00205 }
00206 
00207 RTC::ReturnCode_t OccupancyGridMap3D::onExecute(RTC::UniqueId ec_id)
00208 {
00209     //std::cout << "OccupancyGrid3D::onExecute(" << ec_id << ")" << std::endl;
00210     coil::TimeValue t1(coil::gettimeofday());
00211 
00212     if (m_updateIn.isNew())  m_updateIn.read();
00213 
00214     if (!m_update.data) {
00215         // suspend updating map
00216         while (m_poseIn.isNew()) m_poseIn.read();
00217         while (m_cloudIn.isNew()) m_cloudIn.read();
00218         return RTC::RTC_OK;
00219     }
00220 
00221     while (m_rangeIn.isNew()){
00222         m_rangeIn.read();
00223         Guard guard(m_mutex);
00224         Pointcloud cloud;
00225         for (unsigned int i=0; i<m_range.ranges.length(); i++){
00226             double th = m_range.config.minAngle + i*m_range.config.angularRes;
00227             double d = m_range.ranges[i];
00228             if (d==0) continue;
00229             cloud.push_back(point3d(-d*sin(th), 0, -d*cos(th)));
00230         }
00231         point3d sensor(0,0,0);
00232         Pose3D &pose = m_range.geometry.geometry.pose;
00233         pose6d frame(pose.position.x,
00234                      pose.position.y,
00235                      pose.position.z, 
00236                      pose.orientation.r,
00237                      pose.orientation.p,
00238                      pose.orientation.y);
00239         m_map->insertPointCloud(cloud, sensor, frame);
00240     }
00241 
00242     if (m_cloudIn.isNew()){
00243         while (m_cloudIn.isNew()) m_cloudIn.read();
00244         while (m_poseIn.isNew())  m_poseIn.read();
00245         while (m_sensorPosIn.isNew())  m_sensorPosIn.read();
00246         Guard guard(m_mutex);
00247         float *ptr = (float *)m_cloud.data.get_buffer();
00248         if (strcmp(m_cloud.type, "xyz")==0 
00249             || strcmp(m_cloud.type, "xyzrgb")==0){
00250             Pointcloud cloud;
00251             for (unsigned int i=0; i<m_cloud.data.length()/16; i++, ptr+=4){
00252                 if (isnan(ptr[0])) continue;
00253                 cloud.push_back(point3d(ptr[0],ptr[1],ptr[2]));
00254             }
00255             point3d sensor(m_sensorPos.data.x,
00256                            m_sensorPos.data.y,
00257                            m_sensorPos.data.z);
00258             pose6d frame(m_pose.data.position.x,
00259                          m_pose.data.position.y,
00260                          m_pose.data.position.z, 
00261                          m_pose.data.orientation.r,
00262                          m_pose.data.orientation.p,
00263                          m_pose.data.orientation.y);
00264             m_map->insertPointCloud(cloud, sensor, frame);
00265         }else if (strcmp(m_cloud.type, "xyzv")==0){
00266             hrp::Matrix33 R;
00267             hrp::Vector3 p;
00268             p[0] = m_pose.data.position.x; 
00269             p[1] = m_pose.data.position.y; 
00270             p[2] = m_pose.data.position.z; 
00271             R = hrp::rotFromRpy(m_pose.data.orientation.r,
00272                                 m_pose.data.orientation.p,
00273                                 m_pose.data.orientation.y);
00274             int ocnum = 0;
00275             int emnum = 0;
00276 #if KDEBUG
00277             point3d pp1,pp2,pp3,pp4,pp5;
00278             //      pp1.x() = 0.1; //0.05;
00279             //      pp1.y() = 2.2; //2.15;
00280             //      pp1.z() = 1.4; //1.45;
00281             pp1.x() = -0.35;
00282             pp1.y() = 2.9;
00283             pp1.z() = 0.5;
00284             pp2.x() = -0.45; //-0.45;
00285             pp2.y() = 2.9; //2.95;
00286             pp2.z() = 0.5; //0.55;
00287             pp3.x() = -0.55; //-0.55;
00288             pp3.y() = 2.9; //2.95;
00289             pp3.z() = 0.5; //0.55;
00290             pp4.x() = -0.65; //-0.45;
00291             pp4.y() = 2.9; //3.05;
00292             pp4.z() = 0.5; //0.55;
00293             pp5.x() = -0.75; //-0.55;
00294             pp5.y() = 2.9; //3.05;
00295             pp5.z() = 0.5; //0.55;
00296             OcTreeNode *result1 = m_map->search(pp1);
00297             OcTreeKey result1_key;
00298             m_map->genKey(pp1, result1_key);
00299             OcTreeNode *result2 = m_map->search(pp2);
00300             OcTreeKey result2_key;
00301             m_map->genKey(pp2, result2_key);
00302             OcTreeNode *result3 = m_map->search(pp3);
00303             OcTreeKey result3_key;
00304             m_map->genKey(pp3, result3_key);
00305             OcTreeNode *result4 = m_map->search(pp4);
00306             OcTreeKey result4_key;
00307             m_map->genKey(pp4, result4_key);
00308             OcTreeNode *result5 = m_map->search(pp5);
00309             OcTreeKey result5_key;
00310             m_map->genKey(pp5, result5_key);
00311             double prob1,prob2,prob3,prob4,prob5;
00312             if (result1){
00313               prob1 = result1->getOccupancy();
00314               std::cout << m_profile.instance_name << ": " << pp1 << " original prob = " << prob1 << std::endl;
00315               std::cout << m_profile.instance_name << ": " << pp1 << " key = " << result1_key[0] << " " << result1_key[1] << " " << result1_key[2] << std::endl;
00316             }
00317             else
00318               std::cout << m_profile.instance_name << ": " << pp1 << " can not be searched." << std::endl;
00319             if (result2){
00320               prob2 = result2->getOccupancy();
00321               std::cout << m_profile.instance_name << ": " << pp2 << " original prob = " << prob2 << std::endl;
00322               std::cout << m_profile.instance_name << ": " << pp2 << " key = " << result2_key[0] << " " << result2_key[1] << " " << result2_key[2] << std::endl;
00323             }
00324             else
00325               std::cout << m_profile.instance_name << ": " << pp2 << " can not be searched." << std::endl;
00326             if (result3){
00327               prob3 = result3->getOccupancy();
00328               std::cout << m_profile.instance_name << ": " << pp3 << " original prob = " << prob3 << std::endl;
00329               std::cout << m_profile.instance_name << ": " << pp3 << " key = " << result3_key[0] << " " << result3_key[1] << " " << result3_key[2] << std::endl;
00330             }
00331             else
00332               std::cout << m_profile.instance_name << ": " << pp3 << " can not be searched." << std::endl;
00333             if (result4){
00334               prob4 = result4->getOccupancy();
00335               std::cout << m_profile.instance_name << ": " << pp4 << " original prob = " << prob4 << std::endl;
00336               std::cout << m_profile.instance_name << ": " << pp4 << " key = " << result4_key[0] << " " << result4_key[1] << " " << result4_key[2] << std::endl;
00337             }
00338             else
00339               std::cout << m_profile.instance_name << ": " << pp4 << " can not be searched." << std::endl;
00340             if (result5){
00341               prob5 = result5->getOccupancy();
00342               std::cout << m_profile.instance_name << ": " << pp5 << " original prob = " << prob5 << std::endl;
00343               std::cout << m_profile.instance_name << ": " << pp5 << " key = " << result5_key[0] << " " << result5_key[1] << " " << result5_key[2] << std::endl;
00344             }
00345             else
00346               std::cout << m_profile.instance_name << ": " << pp5 << " can not be searched." << std::endl;
00347 
00348             OcTreeNode *result;
00349             OcTreeKey result_key;
00350             double prob;
00351             point3d pp;
00352             pp.z() = 0.5;
00353             for(int i=0; i<5; i++){
00354               pp.x() = -0.3 - 0.1*i;
00355               for(int j=0; j<5; j++){
00356                 pp.y() = 2.8 + 0.1*j;
00357                 result = m_map->search(pp);
00358                 m_map->genKey(pp, result_key);
00359                 if(result){
00360                   prob = result->getOccupancy();
00361                   std::cout << m_profile.instance_name << ": " << pp << " original prob = " << prob << std::endl;
00362                   std::cout << m_profile.instance_name << ": " << pp << " key = " << result_key[0] << " " << result_key[1] << " " << result_key[2] << std::endl;
00363                 }
00364                 else
00365                   std::cout << m_profile.instance_name << ": " << pp << " can not be searched." << std::endl;
00366               }
00367             }
00368 #endif
00369             for (unsigned int i=0; i<m_cloud.data.length()/16; i++, ptr+=4){
00370                 if (isnan(ptr[0])) continue;
00371                 hrp::Vector3 peye(ptr[0],ptr[1],ptr[2]);
00372                 hrp::Vector3 pworld(R*peye+p);
00373                 point3d pog(pworld[0],pworld[1],pworld[2]);
00374 #if KDEBUG
00375                 OcTreeNode *target_node; // 121023
00376                 OcTreeKey target_key; // 121023
00377                 if((pworld[0]>(pp1.x()-0.1)&&pworld[0]<pp1.x()&&pworld[1]>pp1.y()&&pworld[1]<(pp1.y()+0.1)&&pworld[2]>pp1.z()&&pworld[2]<(pp1.z()+0.1)) ||
00378                    (pworld[0]>(pp2.x()-0.1)&&pworld[0]<pp2.x()&&pworld[1]>pp2.y()&&pworld[1]<(pp2.y()+0.1)&&pworld[2]>pp2.z()&&pworld[2]<(pp2.z()+0.1)) ||
00379                    (pworld[0]>(pp3.x()-0.1)&&pworld[0]<pp3.x()&&pworld[1]>pp3.y()&&pworld[1]<(pp3.y()+0.1)&&pworld[2]>pp3.z()&&pworld[2]<(pp3.z()+0.1))){
00380                   target_node = m_map->search(pog); // 121023
00381                   m_map->genKey(pog, target_key); // 121023
00382                   if (target_node){
00383                     double beforeprob = target_node->getOccupancy();
00384                     std::cout << m_profile.instance_name << ": " << pog << " before prob = " << beforeprob << std::endl;
00385                     std::cout << m_profile.instance_name << ": " << pog << " key = " << target_key[0] << " " << target_key[1] << " " << target_key[2] << std::endl;
00386                   }
00387                   else
00388                     std::cout << m_profile.instance_name << ": " << pog << " can not be searched." << std::endl;
00389                 }
00390 #endif
00391                 //                m_map->updateNode(pog, ptr[3]>0.0?true:false, false);
00392                 OcTreeNode *updated_node = m_map->updateNode(pog, ptr[3]>0.0?true:false, false); // 121023
00393 #if KDEBUG
00394 #if 0
00395                 std::cout << m_profile.instance_name << ": tree depth = " << m_map->getTreeDepth() << std::endl;
00396                 std::cout << m_profile.instance_name << ": tree size = " << m_map->size() << std::endl;
00397 #endif
00398                 if((pworld[0]>(pp1.x()-0.1)&&pworld[0]<pp1.x()&&pworld[1]>pp1.y()&&pworld[1]<(pp1.y()+0.1)&&pworld[2]>pp1.z()&&pworld[2]<(pp1.z()+0.1)) ||
00399                    (pworld[0]>(pp2.x()-0.1)&&pworld[0]<pp2.x()&&pworld[1]>pp2.y()&&pworld[1]<(pp2.y()+0.1)&&pworld[2]>pp2.z()&&pworld[2]<(pp2.z()+0.1)) ||
00400                    (pworld[0]>(pp3.x()-0.1)&&pworld[0]<pp3.x()&&pworld[1]>pp3.y()&&pworld[1]<(pp3.y()+0.1)&&pworld[2]>pp3.z()&&pworld[2]<(pp3.z()+0.1))){
00401                   target_node = m_map->search(pog); // 121023
00402                   m_map->genKey(pog, target_key); // 121023
00403                   if (target_node){
00404                     double afterprob = target_node->getOccupancy();
00405                     std::cout << m_profile.instance_name << ": " << pog << " after prob = " << afterprob << std::endl;
00406                   }
00407                   if (updated_node){
00408                     double updatedprob = updated_node->getOccupancy();
00409                     std::cout << m_profile.instance_name << ": " << pog << " updated prob = " << updatedprob << std::endl;
00410                   }
00411                   //            }
00412                 result2 = m_map->search(pp2);
00413                 m_map->genKey(pp2, result2_key);
00414                 if (result2){
00415                   double newprob = result2->getOccupancy();
00416                   //              if(newprob != prob){ // wrong
00417                   //              if(!(abs(newprob - prob) < 0.0001)){ // wrong
00418                   //              if(!(fabs(newprob - prob2) < 0.0001)){ // correct 121023
00419                     std::cout << m_profile.instance_name << ": " << pp2 << " updated prob = " << newprob << " by " << pog << " prob = " << prob2 << std::endl;
00420                     std::cout << m_profile.instance_name << ": " << pp2 << " key = " << result2_key[0] << " " << result2_key[1] << " " << result2_key[2] << std::endl;
00421                     std::cout << m_profile.instance_name << ": " << pp2 << " hasChildren = " << result2->hasChildren() << std::endl;
00422                     prob2 = newprob;
00423                     //            }
00424                 }
00425                 else
00426                   std::cout << m_profile.instance_name << ": " << pp2 << " can not be searched." << std::endl;
00427                 }
00428 #endif
00429                 ptr[3]>0.0?ocnum++:emnum++;
00430             }
00431             if(KDEBUG) std::cout << m_profile.instance_name << ": " << ocnum << " " << emnum << " " << p << std::endl;
00432         }else{
00433             std::cout << "point type(" << m_cloud.type 
00434                       << ") is not supported" << std::endl;
00435             return RTC::RTC_ERROR;
00436         }
00437         m_updateOut.write();
00438     }
00439 
00440     coil::TimeValue t2(coil::gettimeofday());
00441     if (m_debugLevel > 0){
00442         coil::TimeValue dt = t2-t1;
00443         std::cout << "OccupancyGridMap3D::onExecute() : " 
00444                   << dt.sec()*1e3+dt.usec()/1e3 << "[ms]" << std::endl;
00445     }
00446 
00447     return RTC::RTC_OK;
00448 }
00449 
00450 /*
00451 RTC::ReturnCode_t OccupancyGridMap3D::onAborting(RTC::UniqueId ec_id)
00452 {
00453   return RTC::RTC_OK;
00454 }
00455 */
00456 
00457 /*
00458 RTC::ReturnCode_t OccupancyGridMap3D::onError(RTC::UniqueId ec_id)
00459 {
00460   return RTC::RTC_OK;
00461 }
00462 */
00463 
00464 /*
00465 RTC::ReturnCode_t OccupancyGridMap3D::onReset(RTC::UniqueId ec_id)
00466 {
00467   return RTC::RTC_OK;
00468 }
00469 */
00470 
00471 /*
00472 RTC::ReturnCode_t OccupancyGridMap3D::onStateUpdate(RTC::UniqueId ec_id)
00473 {
00474   return RTC::RTC_OK;
00475 }
00476 */
00477 
00478 /*
00479 RTC::ReturnCode_t OccupancyGridMap3D::onRateChanged(RTC::UniqueId ec_id)
00480 {
00481   return RTC::RTC_OK;
00482 }
00483 */
00484 
00485 OpenHRP::OGMap3D* OccupancyGridMap3D::getOGMap3D(const OpenHRP::AABB& region)
00486 {
00487     Guard guard(m_mutex);
00488     coil::TimeValue t1(coil::gettimeofday());
00489 
00490     OpenHRP::OGMap3D *map = new OpenHRP::OGMap3D;
00491     double size = m_map->getResolution();
00492     map->resolution = size;
00493 
00494     double min[3];
00495     m_map->getMetricMin(min[0],min[1],min[2]);
00496     double max[3];
00497     m_map->getMetricMax(max[0],max[1],max[2]);
00498     for (int i=0; i<3; i++){
00499         min[i] -= size; 
00500         max[i] += size; 
00501     }
00502 
00503     if (m_knownMap){
00504         double kmin[3];
00505         m_knownMap->getMetricMin(kmin[0],kmin[1],kmin[2]);
00506         double kmax[3];
00507         m_knownMap->getMetricMax(kmax[0],kmax[1],kmax[2]);
00508         for (int i=0; i<3; i++){
00509             kmin[i] -= size; 
00510             kmax[i] += size; 
00511             if (kmin[i] < min[i]) min[i] = kmin[i]; 
00512             if (kmax[i] > max[i]) max[i] = kmax[i]; 
00513         }
00514         
00515     }
00516 
00517     double s[3];
00518     s[0] = region.pos.x;
00519     s[1] = region.pos.y;
00520     s[2] = region.pos.z;
00521     double e[3];
00522     e[0] = region.pos.x + region.size.l;
00523     e[1] = region.pos.y + region.size.w;
00524     e[2] = region.pos.z + region.size.h;
00525     double l[3];
00526     
00527     for (int i=0; i<3; i++){
00528         if (e[i] < min[i] || s[i] > max[i]){ // no overlap
00529             s[i] = e[i] = 0;
00530         }else{
00531             if (s[i] < min[i]) s[i] = min[i];
00532             if (e[i] > max[i]) e[i] = max[i];
00533         } 
00534         l[i] = e[i] - s[i];
00535     }
00536 
00537 #ifdef USE_ONLY_GRIDS
00538     map->pos.x = ((int)(s[0]/size))*size;
00539     map->pos.y = ((int)(s[1]/size))*size;
00540     map->pos.z = ((int)(s[2]/size))*size;
00541 #else
00542 #if 0
00543     map->pos.x = s[0];
00544     map->pos.y = s[1];
00545     map->pos.z = s[2];
00546     if(KDEBUG){
00547       std::cout << m_profile.instance_name << ": pos = " << map->pos.x << " " << map->pos.y << " " << map->pos.z << " " << std::endl;
00548       map->pos.x += size/2.0;
00549       map->pos.y += size/2.0;
00550       map->pos.z += size/2.0;
00551     }
00552 #endif      
00553     map->pos.x = ((int)(s[0]/size)+0.5)*size; // 121024
00554     map->pos.y = ((int)(s[1]/size)+0.5)*size; // 121024
00555     map->pos.z = ((int)(s[2]/size)+0.5)*size; // 121024
00556 #endif
00557     map->nx = l[0]/size;
00558     map->ny = l[1]/size;
00559     map->nz = l[2]/size;
00560     int rank=0;
00561     point3d p;
00562     map->cells.length(map->nx*map->ny*map->nz);
00563     {
00564         int no=0, ne=0, nu=0;
00565         //Guard guard(m_mutex);
00566         for (int i=0; i<map->nx; i++){
00567             p.x() = map->pos.x + i*size;
00568             for (int j=0; j<map->ny; j++){
00569                 p.y() = map->pos.y + j*size;
00570                 for (int k=0; k<map->nz; k++){
00571                     p.z() = map->pos.z + k*size;
00572                     OcTreeNode *result = m_map->search(p);
00573                     //                    if (result){
00574                     if (result && !(result->hasChildren())){ // 121023
00575                         double prob = result->getOccupancy();
00576                         if (prob >= m_occupiedThd){
00577                             map->cells[rank] = prob*0xfe;
00578                             no++;
00579                             //                      if(KDEBUG) std::cout << m_profile.instance_name << ": output " << p << " " << prob << std::endl;
00580                         }else{
00581                             map->cells[rank] = OpenHRP::gridEmpty;
00582                             ne++;
00583                         }
00584                         //printf("%6.3f %6.3f %6.3f:%d\n", p.x(), p.y(), p.z(),map->cells[rank-1]);
00585                     }else{
00586                         map->cells[rank] = OpenHRP::gridUnknown;
00587                         nu++;
00588                     }
00589                     if (m_knownMap){
00590                         OcTreeNode *result = m_knownMap->search(p);
00591                         if (result){
00592                             double prob = result->getOccupancy();
00593                             if (prob >= m_occupiedThd){
00594                                 map->cells[rank] = prob*0xfe;
00595                             }
00596                         }
00597                     }
00598                     rank++;
00599                 }
00600             }
00601         }
00602 #if 0
00603         std::cout << "occupied/empty/unknown = " << no << "/" << ne << "/" 
00604                   << nu << std::endl;
00605 #endif
00606     }
00607     coil::TimeValue t2(coil::gettimeofday());
00608     if (m_debugLevel > 0){
00609         coil::TimeValue dt = t2-t1;
00610         std::cout << "OccupancyGridMap3D::getOGMap3D() : " 
00611                   << dt.sec()*1e3+dt.usec()/1e3 << "[ms]" << std::endl;
00612     }
00613 
00614     return map;
00615 }
00616 
00617 void OccupancyGridMap3D::save(const char *filename)
00618 {
00619     Guard guard(m_mutex);
00620     m_map->writeBinary(filename);
00621 }
00622 
00623 void OccupancyGridMap3D::clear()
00624 {
00625     Guard guard(m_mutex);
00626     m_map->clear();
00627     m_updateOut.write();
00628 }
00629 
00630 extern "C"
00631 {
00632 
00633   void OccupancyGridMap3DInit(RTC::Manager* manager)
00634   {
00635     RTC::Properties profile(occupancygridmap3d_spec);
00636     manager->registerFactory(profile,
00637                              RTC::Create<OccupancyGridMap3D>,
00638                              RTC::Delete<OccupancyGridMap3D>);
00639   }
00640 
00641 };
00642 
00643 


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