rosbag으로 라이다 데이터를 받아서, voxelization을 해보았다.
rosbag에서 sensor_msgs/PointCloud2 데이터를 받아서 처리해 PCL 홈페이지의 예제와는 데이터 형 처리하는데서 달라서 참고자료를 더 찾아서 코드를 짜 보았다.
윗 부분에는 데이터의 xyz 방향이 반대가 되어있어서 돌리는 부분도 추가하였다.
#include <iostream>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <string>
#include <pcl/visualization/pcl_visualizer.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
ros::Publisher vox_pub;
void voxel_callback(const sensor_msgs::PointCloud2ConstPtr& scan_msgs){
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>());
pcl::fromROSMsg(*scan_msgs, *cloud_in);
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.rotate(Eigen::AngleAxisf (M_PI, Eigen::Vector3f::UnitZ()));
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_turn(new pcl::PointCloud<pcl::PointXYZI>());
pcl::transformPointCloud(*cloud_in, *cloud_turn, transform_2);
pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_pc(new pcl::PointCloud<pcl::PointXYZI>());
double voxelsize = 0.5;
voxel_filter.setInputCloud(cloud_turn);
voxel_filter.setLeafSize(voxelsize,voxelsize,voxelsize);
voxel_filter.filter(*voxel_pc);
sensor_msgs::PointCloud2 cloud_out;
pcl::toROSMsg(*voxel_pc, cloud_out);
cloud_out.header.frame_id = "os1_lidar";
vox_pub.publish(cloud_out);
}
int main(int argc, char** argv){
ros::init(argc, argv, "main");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/lidar_data",1, voxel_callback);
vox_pub = nh.advertise<sensor_msgs::PointCloud2>("/voxel_lidar_data", 1);
ros::spin();
return 0;
}
끝