https://github.com/tjdalsckd/gpu-voxel-collision

gvl_ompl_planner를 이용한 충돌 파악

대략적인 코드진행은 다음과 같다.

  1. gvl_ompl_planner의 코드는 gvl_ompl_planner_helper.cpp 코드의 GvlOmplPlannerHelper 클래스에서 모션플래닝 관련 내용이 선언되고 동작한다.

  2. ros callback을 이용해 realsense pointcloud 데이터를 받고 이를 화면에 출력한다.

  3. ros callback을 이용해 현재 jointState를 받는다.

  4. pointcloud 데이터를 myEnvironmentMap으로 설정한다.

  5. ompl을 이용해 로봇이 움직일 경로인 solutionMap을 획득한다.

  6. solutionMap과 myEnvironmentMap 각각에서 현재 로봇과 교차하는 부분을 계산한다.

  7. 각각의 교차부분을 빼서 실제 물체와 solutionMap과의 교차 부분을 계산한다.

ezgif com-gif-maker (2)

collision 개수 파악

num_colls는 모션플래닝으로 획득한 solution map과 realsense를 이용한 countingVoxelList의 교차를 파악해 개수를 얻는다. num_colls2는 로봇과 countingVoxellist와의 교차를 파악하고 개수를 얻는다. 이 둘의 차가 증가했을 경우 충돌로 파악하고 프린트한다. ```javascript {.line-numbers} while(ros::ok()){ ros::spinOnce();

  if (new_data_received) 
  {
       new_data_received = false;
       iteration++;

       pbaDistanceVoxmap->clearMap();
  
       countingVoxelList->clearMap();
       countingVoxelListFiltered->clearMap();
       erodeTempVoxmap1->clearMap();
       erodeTempVoxmap2->clearMap();
       countingVoxelList->insertPointCloud(my_point_cloud, eBVM_OCCUPIED);
       gvl->visualizeMap("countingVoxelList");
       num_colls = gvl->getMap("countingVoxelList")->as<gpu_voxels::voxellist::CountingVoxelList>()->collideWith(gvl->getMap("mySolutionMap")->as<gpu_voxels::voxellist::BitVectorVoxelList>(), 1.0f);
       num_colls2 = gvl->getMap("countingVoxelList")->as<gpu_voxels::voxellist::CountingVoxelList>()->collideWith(gvl->getMap("myRobotMap")->as<gpu_voxels::voxellist::BitVectorVoxelList>(), 1.0f); 
       if(num_colls-num_colls2>=180)
             std::cout << "!!!!!!!!!!!!!!!Detected Collision!!!!!!!!! " << num_colls-num_colls2 << " collisions " << std::endl;
       
  }
  r.sleep();
} ```