실행법!

1. 아래 파일들을 다운을 받는다

   obstacle_detector-20210210T044331Z-001.zip

   test_lidar3.py

2. obstacle detector를 catkin_ws에 넣고 cm 한다. ← 아르마딜로 깔아야 될수도 있음...!!

   (구글에 armadillo 다운받는법 쳐서 깔기!)

3. 앞서 VLP-16키는 법 참고해서 $roslaunch velodyne_pointcloud VLP16_points.launch

4. 이후 test_lidar3.py 있는 폴더에서 python3 test_lidar3.py

5. 다음으로 roslaunch obstacle_detector virtual ~ (tab 치면됨) 실행

6. rviz 창 나오면서 obstacle 확인 가능하고 rostopic에서 장애물 정보 확인 가능

이번 과정에서 바뀐부분

• lidar정보 받아와서 수정하는 부분

  1. DownSampling

  test_lidar3.py

  def do_voxel_grid_downssampling(pcl_data,leaf_size):
  '''
  Create a VoxelGrid filter object for a input point cloud
  :param pcl_data: point cloud data subscriber
  :param leaf_size: voxel(or leaf) size
  :return: Voxel grid downsampling on point cloud
  :[<https://github.com/fouliex/RoboticPerception>](<https://github.com/fouliex/RoboticPerception>)
  '''
  vox = pcl_data.make_voxel_grid_filter()
  vox.set_leaf_size(leaf_size, leaf_size, leaf_size) # The bigger the leaf size the less information retained
  return  vox.filter()
  
  def getMsg(msg):
  
      gen = point_cloud2.read_points(msg, skip_nans=True)   
      # print(type(gen))
      cnt = 0
      points_list = []
  
      for p in gen:
          points_list.append([p[0], p[1], p[2], p[3]])
  
      pcl_data = pcl.PointCloud_PointXYZRGB()
      #pcl_data = points_list
      pcl_data.from_list(points_list)
  
  # 실행 코드 부분
      LEAF_SIZE = 0.01
      cloud = do_voxel_grid_downssampling(pcl_data, LEAF_SIZE)
      print("Output :", cloud)
      print("")
  

  • 위의 코드에서 do_voxel_grid_downssampling 이 함수가 downsampling을 진행하는 부분이다.

  이 함수는 pcl 형식의 data를 받아와서 사용해야 한다. 맞춰주기 위해서 pcl_data = pcl.PointCloud_PointXYZRGB() 로 pcl 형식을 선언해주고 pcl_data.from_list(points_list)를 진행하여 점들을 모두 pcl_data에 넣어준다.

  이후, do_voxel_grid_downssampling 를 진행하게 되면 #실행 코드 부분에서 LEAF_SIZE가 voxel의 크기를 의미하고 이에 맞추어 downsampling이 진행되어 voxel의 크기가 클수록 points의 개수가 더 많이 감소하는것을 확인할 수 있다.

  2. pointcloud data에서 intensity 정보 송신

  test_lidar3.py

  test = PointCloud()
  get_in = ChannelFloat32()
  get_in.name = 'intensery'
  test.points = []
  for p in cloud:
  # if p[1] > 0:  
  	seo = Point32()
  	seo.x = p[0]
  	seo.y = p[1]
  	seo.z = p[2]
  	get_in.values.append(p[3])
  	test.points.append(seo)
  	cnt += 1
  print("Input :", cnt)
  test.channels.append(get_in)
  

  • 앞서 pointcloud 메세지 형식에서는 ChannelFloat32 형식을 사용하지 않아서 channel에 대한 메세지 정보가 없었다.

    이번에 get_in이라는 ChannelFloat32 형식을 선언하고 name을 intensery로 선언하면서 각 points들의 정보를 get_in.values 배열에 담았다. 이후, for문이 끝나고(모든 포인트들의 x, y, z ,intensity 정보를 배열에 담은 후) get_in 배열을 test.channels 에 담아서 메세지를 전송하게 된다.

    → 이 과정을 수행하면 rviz에서 포인트별 intensity를 rgb로 표현하여 확인할 수 있다.

    (당장 쓰일지는 모르지만 intensity 정보는 송신하면 좋으니까..!!)

• Obstacle_detection 수행하는 부분

ObstacleDetector::ObstacleDetector() : nh_(""), nh_local_("~") {
updateParams();if (p_use_scan_)
scan_sub_ = nh_.subscribe("scan", 10, &ObstacleDetector::scanCallback, this);
else if (p_use_pcl_)
pcl_sub_ = nh_.subscribe("lidar_pub", 10, &ObstacleDetector::pclCallback, this);obstacles_pub_ = nh_.advertise<obstacle_detector::Obstacles>("obstacles", 5);ROS_INFO("Obstacle Detector [OK]");
ros::spin();
}

저번 2020-2학기에 진행한 다학년 프로젝트에서 Lidar팀은 Obstacle detection을 수행했다.

points 들의 전체 집합을 모여있는 군집끼리 나누어 부분집합으로 나누었고 이를 별도의 과정을 거쳐 원으로 나타내는 작업이다.

tysik/obstacle_detector

자세한 내용은 위의 github를 참고하시면 됩니다.

이때, 저번 2D Lidar에서는 points들이 scan message로 들어와서 이를 이용해서 진행하였는데 지금은 VLP-16에서 들어온 정보를 가공하여 pointcloud 형식으로 바꿔서 송신했으므로 위의 pcl_sub에서 메세지 이름만 맞추어 주면 된다.