OccupancyGridMap3D.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "OccupancyGridMap3D.h"
11 #include "hrpUtil/Eigen3d.h"
12 #include <octomap/octomap.h>
13 
14 #define KDEBUG 0
15 //#define KDEBUG 1 // 121022
16 
17 #ifdef __APPLE__
18 inline bool isnan(double x)
19 {
20  return (x != x);
21 }
22 #endif
23 
25 
26 using namespace octomap;
27 
28 // Module specification
29 // <rtc-template block="module_spec">
30 static const char* occupancygridmap3d_spec[] =
31  {
32  "implementation_id", "OccupancyGridMap3D",
33  "type_name", "OccupancyGridMap3D",
34  "description", "null component",
35  "version", HRPSYS_PACKAGE_VERSION,
36  "vendor", "AIST",
37  "category", "example",
38  "activity_type", "DataFlowComponent",
39  "max_instance", "10",
40  "language", "C++",
41  "lang_type", "compile",
42  // Configuration variables
43  "conf.default.occupiedThd", "0.5",
44  "conf.default.resolution", "0.1",
45  "conf.default.initialMap", "",
46  "conf.default.knownMap", "",
47  "conf.default.debugLevel", "0",
48  ""
49  };
50 // </rtc-template>
51 
53  : RTC::DataFlowComponentBase(manager),
54  // <rtc-template block="initializer">
55  m_rangeIn("range", m_range),
56  m_cloudIn("cloud", m_cloud),
57  m_poseIn("pose", m_pose),
58  m_sensorPosIn("sensorPos", m_sensorPos),
59  m_updateIn("update", m_update),
60  m_updateOut("updateSignal", m_updateSignal),
61  m_OGMap3DServicePort("OGMap3DService"),
62  // </rtc-template>
63  m_service0(this),
64  m_map(NULL),
65  m_knownMap(NULL),
66  dummy(0)
67 {
68 }
69 
71 {
72 }
73 
74 
75 
77 {
78  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
79  // <rtc-template block="bind_config">
80  // Bind variables and configuration variable
81  bindParameter("occupiedThd", m_occupiedThd, "0.5");
82  bindParameter("resolution", m_resolution, "0.1");
83  bindParameter("initialMap", m_initialMap, "");
84  bindParameter("knownMap", m_knownMapPath, "");
85  bindParameter("debugLevel", m_debugLevel, "0");
86 
87  // </rtc-template>
88 
89  // Registration: InPort/OutPort/Service
90  // <rtc-template block="registration">
91  // Set InPort buffers
92  addInPort("range", m_rangeIn);
93  addInPort("cloud", m_cloudIn);
94  addInPort("pose", m_poseIn);
95  addInPort("sensorPos", m_sensorPosIn);
96  addInPort("update", m_updateIn);
97 
98  // Set OutPort buffer
99  addOutPort("updateSignal", m_updateOut);
100 
101  // Set service provider to Ports
102  m_OGMap3DServicePort.registerProvider("service1", "OGMap3DService", m_service0);
103 
104  // Set service consumers to Ports
105 
106  // Set CORBA Service Ports
108 
109  // </rtc-template>
110 
111  //RTC::Properties& prop = getProperties();
112 
113  char cwd[PATH_MAX];
114  char *p = getcwd(cwd, PATH_MAX);
115  m_cwd = p;
116  m_cwd += "/";
117 
118  m_update.data = 1;
119 
120  m_pose.data.position.x = 0;
121  m_pose.data.position.y = 0;
122  m_pose.data.position.z = 0;
123  m_pose.data.orientation.r = 0;
124  m_pose.data.orientation.p = 0;
125  m_pose.data.orientation.y = 0;
126 
127  m_sensorPos.data.x = 0;
128  m_sensorPos.data.y = 0;
129  m_sensorPos.data.z = 0;
130 
131  m_updateSignal.data = 0;
132 
133  return RTC::RTC_OK;
134 }
135 
136 
137 /*
138 RTC::ReturnCode_t OccupancyGridMap3D::onFinalize()
139 {
140  return RTC::RTC_OK;
141 }
142 */
143 
144 /*
145 RTC::ReturnCode_t OccupancyGridMap3D::onStartup(RTC::UniqueId ec_id)
146 {
147  return RTC::RTC_OK;
148 }
149 */
150 
151 /*
152 RTC::ReturnCode_t OccupancyGridMap3D::onShutdown(RTC::UniqueId ec_id)
153 {
154  return RTC::RTC_OK;
155 }
156 */
157 
159 {
160  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
161 
162  Guard guard(m_mutex);
163  if (m_knownMapPath != ""){
164  m_knownMap = new OcTree(m_cwd+m_knownMapPath);
165  }
166 
167  if (m_initialMap != ""){
168  // Working directories of threads which calls onInitialize() and onActivate() are different on MacOS
169  // Assume path of initial map is given by a relative path to working directory of the thread which calls onInitialize()
170  m_map = new OcTree(m_cwd+m_initialMap);
171  }else{
172  m_map = new OcTree(m_resolution);
173  }
174  m_updateOut.write();
175 
176  if(KDEBUG){
177  std::cout << m_profile.instance_name << ": initial tree depth = " << m_map->getTreeDepth() << std::endl;
178  std::cout << m_profile.instance_name << ": initial tree size = " << m_map->size() << std::endl;
179  point3d p;
180  // p.x() = 0.1; //0.05;
181  // p.y() = 2.2; //2.15;
182  // p.z() = 1.4; //1.45;
183  p.x() = -0.5; //-0.55;
184  p.y() = 3.0; //2.95;
185  p.z() = 0.5; //0.55;
186  OcTreeNode *result = m_map->search(p);
187  if (result){
188  double prob = result->getOccupancy();
189  std::cout << m_profile.instance_name << ": initial " << p << " " << prob << std::endl;
190  std::cout << m_profile.instance_name << ": second tree depth = " << m_map->getTreeDepth() << std::endl;
191  std::cout << m_profile.instance_name << ": second tree size = " << m_map->size() << std::endl;
192  }
193  }
194 
195  return RTC::RTC_OK;
196 }
197 
199 {
200  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
201  Guard guard(m_mutex);
202  delete m_map;
203  if (m_knownMap) delete m_knownMap;
204  return RTC::RTC_OK;
205 }
206 
208 {
209  //std::cout << "OccupancyGrid3D::onExecute(" << ec_id << ")" << std::endl;
211 
212  if (m_updateIn.isNew()) m_updateIn.read();
213 
214  if (!m_update.data) {
215  // suspend updating map
216  while (m_poseIn.isNew()) m_poseIn.read();
217  while (m_cloudIn.isNew()) m_cloudIn.read();
218  return RTC::RTC_OK;
219  }
220 
221  while (m_rangeIn.isNew()){
222  m_rangeIn.read();
223  Guard guard(m_mutex);
224  Pointcloud cloud;
225  for (unsigned int i=0; i<m_range.ranges.length(); i++){
226  double th = m_range.config.minAngle + i*m_range.config.angularRes;
227  double d = m_range.ranges[i];
228  if (d==0) continue;
229  cloud.push_back(point3d(-d*sin(th), 0, -d*cos(th)));
230  }
231  point3d sensor(0,0,0);
232  Pose3D &pose = m_range.geometry.geometry.pose;
233  pose6d frame(pose.position.x,
234  pose.position.y,
235  pose.position.z,
236  pose.orientation.r,
237  pose.orientation.p,
238  pose.orientation.y);
239  m_map->insertPointCloud(cloud, sensor, frame);
240  }
241 
242  if (m_cloudIn.isNew()){
243  while (m_cloudIn.isNew()) m_cloudIn.read();
244  while (m_poseIn.isNew()) m_poseIn.read();
246  Guard guard(m_mutex);
247  float *ptr = (float *)m_cloud.data.get_buffer();
248  if (strcmp(m_cloud.type, "xyz")==0
249  || strcmp(m_cloud.type, "xyzrgb")==0){
250  Pointcloud cloud;
251  for (unsigned int i=0; i<m_cloud.data.length()/16; i++, ptr+=4){
252  if (isnan(ptr[0])) continue;
253  cloud.push_back(point3d(ptr[0],ptr[1],ptr[2]));
254  }
255  point3d sensor(m_sensorPos.data.x,
256  m_sensorPos.data.y,
257  m_sensorPos.data.z);
258  pose6d frame(m_pose.data.position.x,
259  m_pose.data.position.y,
260  m_pose.data.position.z,
261  m_pose.data.orientation.r,
262  m_pose.data.orientation.p,
263  m_pose.data.orientation.y);
264  m_map->insertPointCloud(cloud, sensor, frame);
265  }else if (strcmp(m_cloud.type, "xyzv")==0){
267  hrp::Vector3 p;
268  p[0] = m_pose.data.position.x;
269  p[1] = m_pose.data.position.y;
270  p[2] = m_pose.data.position.z;
271  R = hrp::rotFromRpy(m_pose.data.orientation.r,
272  m_pose.data.orientation.p,
273  m_pose.data.orientation.y);
274  int ocnum = 0;
275  int emnum = 0;
276 #if KDEBUG
277  point3d pp1,pp2,pp3,pp4,pp5;
278  // pp1.x() = 0.1; //0.05;
279  // pp1.y() = 2.2; //2.15;
280  // pp1.z() = 1.4; //1.45;
281  pp1.x() = -0.35;
282  pp1.y() = 2.9;
283  pp1.z() = 0.5;
284  pp2.x() = -0.45; //-0.45;
285  pp2.y() = 2.9; //2.95;
286  pp2.z() = 0.5; //0.55;
287  pp3.x() = -0.55; //-0.55;
288  pp3.y() = 2.9; //2.95;
289  pp3.z() = 0.5; //0.55;
290  pp4.x() = -0.65; //-0.45;
291  pp4.y() = 2.9; //3.05;
292  pp4.z() = 0.5; //0.55;
293  pp5.x() = -0.75; //-0.55;
294  pp5.y() = 2.9; //3.05;
295  pp5.z() = 0.5; //0.55;
296  OcTreeNode *result1 = m_map->search(pp1);
297  OcTreeKey result1_key;
298  m_map->genKey(pp1, result1_key);
299  OcTreeNode *result2 = m_map->search(pp2);
300  OcTreeKey result2_key;
301  m_map->genKey(pp2, result2_key);
302  OcTreeNode *result3 = m_map->search(pp3);
303  OcTreeKey result3_key;
304  m_map->genKey(pp3, result3_key);
305  OcTreeNode *result4 = m_map->search(pp4);
306  OcTreeKey result4_key;
307  m_map->genKey(pp4, result4_key);
308  OcTreeNode *result5 = m_map->search(pp5);
309  OcTreeKey result5_key;
310  m_map->genKey(pp5, result5_key);
311  double prob1,prob2,prob3,prob4,prob5;
312  if (result1){
313  prob1 = result1->getOccupancy();
314  std::cout << m_profile.instance_name << ": " << pp1 << " original prob = " << prob1 << std::endl;
315  std::cout << m_profile.instance_name << ": " << pp1 << " key = " << result1_key[0] << " " << result1_key[1] << " " << result1_key[2] << std::endl;
316  }
317  else
318  std::cout << m_profile.instance_name << ": " << pp1 << " can not be searched." << std::endl;
319  if (result2){
320  prob2 = result2->getOccupancy();
321  std::cout << m_profile.instance_name << ": " << pp2 << " original prob = " << prob2 << std::endl;
322  std::cout << m_profile.instance_name << ": " << pp2 << " key = " << result2_key[0] << " " << result2_key[1] << " " << result2_key[2] << std::endl;
323  }
324  else
325  std::cout << m_profile.instance_name << ": " << pp2 << " can not be searched." << std::endl;
326  if (result3){
327  prob3 = result3->getOccupancy();
328  std::cout << m_profile.instance_name << ": " << pp3 << " original prob = " << prob3 << std::endl;
329  std::cout << m_profile.instance_name << ": " << pp3 << " key = " << result3_key[0] << " " << result3_key[1] << " " << result3_key[2] << std::endl;
330  }
331  else
332  std::cout << m_profile.instance_name << ": " << pp3 << " can not be searched." << std::endl;
333  if (result4){
334  prob4 = result4->getOccupancy();
335  std::cout << m_profile.instance_name << ": " << pp4 << " original prob = " << prob4 << std::endl;
336  std::cout << m_profile.instance_name << ": " << pp4 << " key = " << result4_key[0] << " " << result4_key[1] << " " << result4_key[2] << std::endl;
337  }
338  else
339  std::cout << m_profile.instance_name << ": " << pp4 << " can not be searched." << std::endl;
340  if (result5){
341  prob5 = result5->getOccupancy();
342  std::cout << m_profile.instance_name << ": " << pp5 << " original prob = " << prob5 << std::endl;
343  std::cout << m_profile.instance_name << ": " << pp5 << " key = " << result5_key[0] << " " << result5_key[1] << " " << result5_key[2] << std::endl;
344  }
345  else
346  std::cout << m_profile.instance_name << ": " << pp5 << " can not be searched." << std::endl;
347 
348  OcTreeNode *result;
349  OcTreeKey result_key;
350  double prob;
351  point3d pp;
352  pp.z() = 0.5;
353  for(int i=0; i<5; i++){
354  pp.x() = -0.3 - 0.1*i;
355  for(int j=0; j<5; j++){
356  pp.y() = 2.8 + 0.1*j;
357  result = m_map->search(pp);
358  m_map->genKey(pp, result_key);
359  if(result){
360  prob = result->getOccupancy();
361  std::cout << m_profile.instance_name << ": " << pp << " original prob = " << prob << std::endl;
362  std::cout << m_profile.instance_name << ": " << pp << " key = " << result_key[0] << " " << result_key[1] << " " << result_key[2] << std::endl;
363  }
364  else
365  std::cout << m_profile.instance_name << ": " << pp << " can not be searched." << std::endl;
366  }
367  }
368 #endif
369  for (unsigned int i=0; i<m_cloud.data.length()/16; i++, ptr+=4){
370  if (isnan(ptr[0])) continue;
371  hrp::Vector3 peye(ptr[0],ptr[1],ptr[2]);
372  hrp::Vector3 pworld(R*peye+p);
373  point3d pog(pworld[0],pworld[1],pworld[2]);
374 #if KDEBUG
375  OcTreeNode *target_node; // 121023
376  OcTreeKey target_key; // 121023
377  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)) ||
378  (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)) ||
379  (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))){
380  target_node = m_map->search(pog); // 121023
381  m_map->genKey(pog, target_key); // 121023
382  if (target_node){
383  double beforeprob = target_node->getOccupancy();
384  std::cout << m_profile.instance_name << ": " << pog << " before prob = " << beforeprob << std::endl;
385  std::cout << m_profile.instance_name << ": " << pog << " key = " << target_key[0] << " " << target_key[1] << " " << target_key[2] << std::endl;
386  }
387  else
388  std::cout << m_profile.instance_name << ": " << pog << " can not be searched." << std::endl;
389  }
390 #endif
391  // m_map->updateNode(pog, ptr[3]>0.0?true:false, false);
392  OcTreeNode *updated_node = m_map->updateNode(pog, ptr[3]>0.0?true:false, false); // 121023
393 #if KDEBUG
394 #if 0
395  std::cout << m_profile.instance_name << ": tree depth = " << m_map->getTreeDepth() << std::endl;
396  std::cout << m_profile.instance_name << ": tree size = " << m_map->size() << std::endl;
397 #endif
398  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)) ||
399  (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)) ||
400  (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))){
401  target_node = m_map->search(pog); // 121023
402  m_map->genKey(pog, target_key); // 121023
403  if (target_node){
404  double afterprob = target_node->getOccupancy();
405  std::cout << m_profile.instance_name << ": " << pog << " after prob = " << afterprob << std::endl;
406  }
407  if (updated_node){
408  double updatedprob = updated_node->getOccupancy();
409  std::cout << m_profile.instance_name << ": " << pog << " updated prob = " << updatedprob << std::endl;
410  }
411  // }
412  result2 = m_map->search(pp2);
413  m_map->genKey(pp2, result2_key);
414  if (result2){
415  double newprob = result2->getOccupancy();
416  // if(newprob != prob){ // wrong
417  // if(!(abs(newprob - prob) < 0.0001)){ // wrong
418  // if(!(fabs(newprob - prob2) < 0.0001)){ // correct 121023
419  std::cout << m_profile.instance_name << ": " << pp2 << " updated prob = " << newprob << " by " << pog << " prob = " << prob2 << std::endl;
420  std::cout << m_profile.instance_name << ": " << pp2 << " key = " << result2_key[0] << " " << result2_key[1] << " " << result2_key[2] << std::endl;
421  std::cout << m_profile.instance_name << ": " << pp2 << " hasChildren = " << result2->hasChildren() << std::endl;
422  prob2 = newprob;
423  // }
424  }
425  else
426  std::cout << m_profile.instance_name << ": " << pp2 << " can not be searched." << std::endl;
427  }
428 #endif
429  ptr[3]>0.0?ocnum++:emnum++;
430  }
431  if(KDEBUG) std::cout << m_profile.instance_name << ": " << ocnum << " " << emnum << " " << p << std::endl;
432  }else{
433  std::cout << "point type(" << m_cloud.type
434  << ") is not supported" << std::endl;
435  return RTC::RTC_ERROR;
436  }
437  m_updateOut.write();
438  }
439 
441  if (m_debugLevel > 0){
442  coil::TimeValue dt = t2-t1;
443  std::cout << "OccupancyGridMap3D::onExecute() : "
444  << dt.sec()*1e3+dt.usec()/1e3 << "[ms]" << std::endl;
445  }
446 
447  return RTC::RTC_OK;
448 }
449 
450 /*
451 RTC::ReturnCode_t OccupancyGridMap3D::onAborting(RTC::UniqueId ec_id)
452 {
453  return RTC::RTC_OK;
454 }
455 */
456 
457 /*
458 RTC::ReturnCode_t OccupancyGridMap3D::onError(RTC::UniqueId ec_id)
459 {
460  return RTC::RTC_OK;
461 }
462 */
463 
464 /*
465 RTC::ReturnCode_t OccupancyGridMap3D::onReset(RTC::UniqueId ec_id)
466 {
467  return RTC::RTC_OK;
468 }
469 */
470 
471 /*
472 RTC::ReturnCode_t OccupancyGridMap3D::onStateUpdate(RTC::UniqueId ec_id)
473 {
474  return RTC::RTC_OK;
475 }
476 */
477 
478 /*
479 RTC::ReturnCode_t OccupancyGridMap3D::onRateChanged(RTC::UniqueId ec_id)
480 {
481  return RTC::RTC_OK;
482 }
483 */
484 
485 OpenHRP::OGMap3D* OccupancyGridMap3D::getOGMap3D(const OpenHRP::AABB& region)
486 {
487  Guard guard(m_mutex);
489 
490  OpenHRP::OGMap3D *map = new OpenHRP::OGMap3D;
491  double size = m_map->getResolution();
492  map->resolution = size;
493 
494  double min[3];
495  m_map->getMetricMin(min[0],min[1],min[2]);
496  double max[3];
497  m_map->getMetricMax(max[0],max[1],max[2]);
498  for (int i=0; i<3; i++){
499  min[i] -= size;
500  max[i] += size;
501  }
502 
503  if (m_knownMap){
504  double kmin[3];
505  m_knownMap->getMetricMin(kmin[0],kmin[1],kmin[2]);
506  double kmax[3];
507  m_knownMap->getMetricMax(kmax[0],kmax[1],kmax[2]);
508  for (int i=0; i<3; i++){
509  kmin[i] -= size;
510  kmax[i] += size;
511  if (kmin[i] < min[i]) min[i] = kmin[i];
512  if (kmax[i] > max[i]) max[i] = kmax[i];
513  }
514 
515  }
516 
517  double s[3];
518  s[0] = region.pos.x;
519  s[1] = region.pos.y;
520  s[2] = region.pos.z;
521  double e[3];
522  e[0] = region.pos.x + region.size.l;
523  e[1] = region.pos.y + region.size.w;
524  e[2] = region.pos.z + region.size.h;
525  double l[3];
526 
527  for (int i=0; i<3; i++){
528  if (e[i] < min[i] || s[i] > max[i]){ // no overlap
529  s[i] = e[i] = 0;
530  }else{
531  if (s[i] < min[i]) s[i] = min[i];
532  if (e[i] > max[i]) e[i] = max[i];
533  }
534  l[i] = e[i] - s[i];
535  }
536 
537 #ifdef USE_ONLY_GRIDS
538  map->pos.x = ((int)(s[0]/size))*size;
539  map->pos.y = ((int)(s[1]/size))*size;
540  map->pos.z = ((int)(s[2]/size))*size;
541 #else
542 #if 0
543  map->pos.x = s[0];
544  map->pos.y = s[1];
545  map->pos.z = s[2];
546  if(KDEBUG){
547  std::cout << m_profile.instance_name << ": pos = " << map->pos.x << " " << map->pos.y << " " << map->pos.z << " " << std::endl;
548  map->pos.x += size/2.0;
549  map->pos.y += size/2.0;
550  map->pos.z += size/2.0;
551  }
552 #endif
553  map->pos.x = ((int)(s[0]/size)+0.5)*size; // 121024
554  map->pos.y = ((int)(s[1]/size)+0.5)*size; // 121024
555  map->pos.z = ((int)(s[2]/size)+0.5)*size; // 121024
556 #endif
557  map->nx = l[0]/size;
558  map->ny = l[1]/size;
559  map->nz = l[2]/size;
560  int rank=0;
561  point3d p;
562  map->cells.length(map->nx*map->ny*map->nz);
563  {
564  int no=0, ne=0, nu=0;
565  //Guard guard(m_mutex);
566  for (int i=0; i<map->nx; i++){
567  p.x() = map->pos.x + i*size;
568  for (int j=0; j<map->ny; j++){
569  p.y() = map->pos.y + j*size;
570  for (int k=0; k<map->nz; k++){
571  p.z() = map->pos.z + k*size;
572  OcTreeNode *result = m_map->search(p);
573  // if (result){
574  if (result && !(result->hasChildren())){ // 121023
575  double prob = result->getOccupancy();
576  if (prob >= m_occupiedThd){
577  map->cells[rank] = prob*0xfe;
578  no++;
579  // if(KDEBUG) std::cout << m_profile.instance_name << ": output " << p << " " << prob << std::endl;
580  }else{
581  map->cells[rank] = OpenHRP::gridEmpty;
582  ne++;
583  }
584  //printf("%6.3f %6.3f %6.3f:%d\n", p.x(), p.y(), p.z(),map->cells[rank-1]);
585  }else{
586  map->cells[rank] = OpenHRP::gridUnknown;
587  nu++;
588  }
589  if (m_knownMap){
590  OcTreeNode *result = m_knownMap->search(p);
591  if (result){
592  double prob = result->getOccupancy();
593  if (prob >= m_occupiedThd){
594  map->cells[rank] = prob*0xfe;
595  }
596  }
597  }
598  rank++;
599  }
600  }
601  }
602 #if 0
603  std::cout << "occupied/empty/unknown = " << no << "/" << ne << "/"
604  << nu << std::endl;
605 #endif
606  }
608  if (m_debugLevel > 0){
609  coil::TimeValue dt = t2-t1;
610  std::cout << "OccupancyGridMap3D::getOGMap3D() : "
611  << dt.sec()*1e3+dt.usec()/1e3 << "[ms]" << std::endl;
612  }
613 
614  return map;
615 }
616 
617 void OccupancyGridMap3D::save(const char *filename)
618 {
619  Guard guard(m_mutex);
620  m_map->writeBinary(filename);
621 }
622 
624 {
625  Guard guard(m_mutex);
626  m_map->clear();
627  m_updateOut.write();
628 }
629 
630 extern "C"
631 {
632 
634  {
636  manager->registerFactory(profile,
637  RTC::Create<OccupancyGridMap3D>,
638  RTC::Delete<OccupancyGridMap3D>);
639  }
640 
641 };
642 
643 
ComponentProfile m_profile
d
png_infop png_charpp int png_charpp profile
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
null component
#define max(a, b)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
PointCloudTypes::PointCloud m_cloud
png_voidp ptr
RTC::CorbaPort m_OGMap3DServicePort
png_uint_32 size
#define KDEBUG
void save(const char *filename)
int rank
png_uint_32 i
coil::Guard< coil::Mutex > Guard
OccupancyGridMap3D(RTC::Manager *manager)
Constructor.
bool addOutPort(const char *name, OutPortBase &outport)
octomap::OcTree * m_map
Eigen::Vector3d Vector3
Matrix33 rotFromRpy(const Vector3 &rpy)
Eigen::Matrix3d Matrix33
#define min(a, b)
static int frame
Definition: iob.cpp:19
InPort< TimedLong > m_updateIn
int gettimeofday(struct timeval *tv, struct timezone *tz)
ExecutionContextHandle_t UniqueId
OpenHRP::OGMap3D * getOGMap3D(const OpenHRP::AABB &region)
InPort< RangeData > m_rangeIn
OutPort< TimedLong > m_updateOut
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
InPort< TimedPose3D > m_poseIn
octomap::OcTree * m_knownMap
void OccupancyGridMap3DInit(RTC::Manager *manager)
InPort< PointCloudTypes::PointCloud > m_cloudIn
typedef int
static const char * occupancygridmap3d_spec[]
long int sec() const
virtual bool isNew()
bool addPort(PortBase &port)
long int usec() const
virtual bool write(DataType &value)
bool addInPort(const char *name, InPortBase &inport)
virtual RTC::ReturnCode_t onInitialize()
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
InPort< TimedPoint3D > m_sensorPosIn
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
OGMap3DService_impl m_service0
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
virtual ~OccupancyGridMap3D()
Destructor.


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20