ROS 강의 - (8) Pointcloud, Pointcloud2에 대해 알아보기 (2)

Pointcloud란?

• ROS 에서 다루는 다양한 메시지 타입 중 점들의 집합에 대한 정보를 담고 있는 메시지
• 주로 Pointcloud2 를 통해 이 데이터를 얻어내고, 이것을 처리하기가 조금 어렵다는 게 지난 포스트 내용 
• 그걸 다루는 라이브러리에 대해 알아보자.

 

1) PCL library 설치하기 

• Pointcloud 정보를 다루기 위해서 필요한 라이브러리이다. 
• 설치 명령 (Ubuntu 20.04 / ROS noetic 기준)

sudo apt-get install libpcl-dev
sudo apt-get install ros-noetic-pcl-conversions ros-noetic-pcl-ros

 

2) PCL library 활용하기 

• 데이터를 받아서 활용할 방안은 무궁무진하지만, 단순한 예시를 보여주고자 한다.
• Pointcloud2 에서 얻어진 점의 위치를 알아내고, 이를 Down sampling 하고, Markersetarray 에 담아서 보여준다.

Code 설명 

1) pcl_conversion 을 통해 받은 msg (pointcloud2) 를 cloud 로 치환한다.

2) Voxel Grid Filter 를 통해 Pointcloud 데이터를 다운샘플링한다. (Leaf size : 0.1 --> 0.1 m^3 안에 점이 여러 개 있으면 그 것들의 평균지점을 하나만 남긴다)

3) 다시 Pointcloud2 output 으로 옮기고, 좌표를 알아내기 위해 Pointcloud output_dust 로 옮긴다.

4) 옮긴 데이터를 marker 형태로 표현하고, markersetarray 에 넣어준다.

5) 이전에 받은 데이터가 남아있지 않도록, 이전에 받은 cloud 정보량이 더 많으면 더 많은 만큼은 그 marker를 삭제해준다.

 

이 코드를 다른 package 의 src 폴더에 넣고, compile 해야 한다. 주의할 점은 아래와 같이 CMakeList 도 수정을 해줘야 한다. pcl_conversions, pcl_ros 를 find_package 에 넣어줘야 함. (add_executable 설명은 따로 하지 않았음)

#include <ros/ros.h>
#include <iostream>
#include <math.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>
#include <vector>
using namespace std;

pcl::PointCloud<pcl::PointXYZ> output_dust;
ros::Publisher marker_pub;
int former_sz = 0;

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& msg)
{
	const sensor_msgs::PointCloud2 cloud_in, cloud_out;
	tf::TransformListener listener;
	// Container for original & filtered data
	pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
	pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
	pcl::PCLPointCloud2 cloud_filtered;
	
	// Convert to PCL data type
	pcl_conversions::toPCL(*msg, *cloud);
	
	// Perform the actual filtering
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (cloudPtr);
	sor.setLeafSize (0.1, 0.1, 0.1); // unit : m
	sor.filter (cloud_filtered);
	
	// Convert to ROS data type
	sensor_msgs::PointCloud2 output;
	pcl_conversions::fromPCL(cloud_filtered, output);
	pcl::fromROSMsg(output, output_dust);


  // Publish Markersetarray
  
  int sz;
  visualization_msgs::MarkerArray markers_bbox;
  visualization_msgs::Marker marker;
	uint32_t shape = visualization_msgs::Marker::CUBE;
  if (former_sz > output_dust.size()){
    sz = former_sz;
  }
  else{
    sz = output_dust.size();
  }
  for (int i = 0; i < sz; i++){
    marker.ns = "basic_shapes";
    marker.id = i;
    marker.type = shape;
    if ( i > output_dust.size())
      marker.action = visualization_msgs::Marker::DELETE;
    else
      marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.x = output_dust[i].x;
    marker.pose.position.y = output_dust[i].y;
    marker.pose.position.z = output_dust[i].z;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.header.frame_id = "camera_depth_optical_center_link";
    markers_bbox.markers.push_back(marker);
  }
  marker_pub.publish(markers_bbox); 
  former_sz = output_dust.size();
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;
  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/firefly/vi_sensor/camera_depth/depth/points", 1, cloud_cb);
  marker_pub = nh.advertise<visualization_msgs::MarkerArray> ("out",1);
  // Spin
  ros::spin();
}

 

3) 실행 화면

총 4개의 코드를 실행해야 한다. 

roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch
rosrun beginner_tutorials marker_array_node // 위에서 설명한 새로 추가한 코드
rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 1 /firefly/vi_sensor/camera_depth_optical_center_link camera_depth_optical_center_link
rviz

code 실행화면

- RVIZ 에서 topic 을 추가하고, global fixed frame 은 world 로 수정한다.

 

- 아래와 같이 보인다. 복잡한 과정을 거친 것 같지만 그냥 Pointcloud 를 cube 모양으로 바꾼 것 뿐이다.

- 데이터를 활용해서 뭔가 하기 위한 준비 단계 수준~

끝~