目录
一、ROS中激光雷达数据类型传递转换；
二、点云数据解析;
三、自定义点云数据类型;
一、ROS中激光雷达数据类型传递转换；
ROS中涉及激光雷达传递的消息类型有两种，一种是针对2D雷达:sensor_msgs::LaserScan;一种是针对3D雷达的即点云数据的：sensor_msgs::PointCloud2.
(1) 2D激光雷达LaserScan;
ROS中LaserScan.msg定义类型如下：
File: sensor_msgs/LaserScan.msg
Raw Message Definition
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data
Header header # timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities # intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty.
发布：
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
int main(int argc, char** argv){
ros::init(argc, argv, "laser_scan_publisher");
ros::NodeHandle n;
ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
unsigned int num_readings = 1000;
double laser_frequency = 50;
double ranges[num_readings];
double intensities[num_readings];
int count = 0;
ros::Rate r(10.0);
while(n.ok()){
//generate some fake data for our laser scan
for(unsigned int i = 0; i < num_readings; ++i){
ranges[i] = count;
intensities[i] = 100 + count;
}
ros::Time scan_time = ros::Time::now();
//populate the LaserScan message
sensor_msgs::LaserScan scan;
scan.header.stamp = scan_time;
scan.header.frame_id = "laser_frame";
scan.angle_min = -1.57; //雷达扫描起始角度;
scan.angle_max = 1.57; //雷达扫描结束角度;
scan.angle_increment = 3.14 / num_readings; //水平角度分辨率;
scan.time_increment = (1 / laser_frequency) / (num_readings); //相邻点时间间隔
scan.range_min = 0.0; //雷达有效测量最小值;
scan.range_max = 100.0; //雷达有效测量最大值;
scan.ranges.resize(num_readings); // num_readings = 扫描角 / 角度分辨率;根据雷达参数计算;
scan.intensities.resize(num_readings);
//实际中，距离值根据激光雷达传感器测得;
for(unsigned int i = 0; i < num_readings; ++i){
scan.ranges[i] = ranges[i];
scan.intensities[i] = intensities[i];
}
scan_pub.publish(scan);
r.sleep();
}
}
接收：
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#define PI 3.1415926
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
std::vector<float> ranges = msg->ranges;
//转换到二维XY平面坐标系下;
for(int i=0; i< ranges.size(); i++)
{
double angle = msg.angle_min + i * msg.angle_increment;
double X = ranges[i] * cos(angle);
double Y = ranges[i] * sin(angle);
float intensity = msg.intensities[i];
std::cout << ranges[i] << " , " << std::endl;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "laser_listener");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/scan", 10, laserCallback);
ros::spin();
return 0;
}
(2) 3D点云数据PointCloud2;
接收：
#include <stdio.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types_conversion.h>
#include <sensor_msgs/PointCloud2.h>
//点云数据接收回调函数;
void PointCloud_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg)
{
const auto& stamp = points_msg->header.stamp;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>()); //
pcl::fromROSMsg(*points_msg, *cloud_in);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "point_cloud_subscriber");
ros::NodeHandle node;
ros::Subscriber points_sub = node.subscribe("/points_cloud", 5, PointCloud_callback);
ros::spin();
return 0;
}
发布：
#include <stdio.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types_conversion.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>
ros::Subscriber points_sub; //订阅者;
ros::Publisher Map_pub; //发布者;
//降采样函数;
void DownSample_Filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in,
pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_downsize,
double downsize){
pcl::VoxelGrid<pcl::PointXYZI> downsample_filter;
downsample_filter.setLeafSize(downsize, downsize, downsize);
downsample_filter.setDownsampleAllData(true); //对全字段进行下采样;
downsample_filter.setInputCloud(cloud_in);
downsample_filter.filter(*cloud_downsize);
}
//点云数据发布;
void publish_result(pcl::PointCloud<pcl::PointXYZI>::Ptr fliter_map){
if(Map_pub.getNumSubscribers() != 0){
sensor_msgs::PointCloud2 cloud_out;
pcl::toROSMsg(*fliter_map, cloud_out);
cloud_out.header.frame_id = "map";
cloud_out.header.stamp = stamp_now;
Map_pub.publish(cloud_out);
std::cout << " map size: " << fliter_map->points.size() << std::endl;
}
}
//点云数据接收回调函数;
void PointCloud_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg)
{
const auto& stamp = points_msg->header.stamp;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>()); //
pcl::fromROSMsg(*points_msg, *cloud_in);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>());
DownSample_Filter(cloud_in, cloud_filter, 0.15);
publish_result(cloud_filter);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "point_cloud_subscriber");
ros::NodeHandle node;
points_sub = node.subscribe("/points_cloud", 5, PointCloud_callback);
Map_pub= node.advertise<sensor_msgs::PointCloud2>("/filtered_map", 5, true);
ros::spin();
return 0;
}
（3）pointcloud to laserscan 转换;
pointcloud类型可以转化为 laserScan，github有专门的代码包：
https://github.com/ros-perception/pointcloud_to_laserscan
二、点云数据解析;
在进行ROS消息类型和PCL库点云类型之间转换时，除了可以利用PCL自带的转换函数，也可以自己解析。参考代码如下：
void velodyPointCloundCallback(const sensor_msgs::PointCloud2ConstPtr &point_clound)
{
std::string frame_id = point_clound->header.frame_id;
ros::Time received_time = point_clound->header.stamp;
std::vector<pcl::PointXYZI> *gridpoints;
gridpoints = new std::vector<pcl::PointXYZI>[laser_rings];
pcl::PointXYZI point;
//通过迭代器获取参数；
for (sensor_msgs::PointCloud2ConstIterator<float> its(*point_clound, "x"); its != its.end(); ++its)
{
const uint16_t r = *((const uint16_t*)(&its[5])); // ring
float x_p = its[0]; // x
float y_p = its[1]; // y
float z_p = its[2]; // z
float in_p = its[4]; // intensity
point.x = x_p;
point.y = y_p;
point.z = z_p;
point.intensity = in_p;
gridpoints[r].push_back(point);
}
}
三、自定义点云数据类型;
在PCL点云库中，提供了自定义模板扩展，用户可以自己定义点云的数据类型。PCL提供了常见的点云类型，包括：pcl::PointXYZ, pcl::PointXYZI,pcl::PointXYZL等。pcl提供的点云数据类型
如果需要自己重新添加自定义的点云类型，可参考如下：
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
struct PointXYZIRPYT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(float, roll, roll) (float, pitch, pitch)
(float, yaw, yaw)
(double, time, time)
)
typedef PointXYZIRPYT PointTypePose;
#include <stdint.h>
#define PCL_NO_PRECOMPILE
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/impl/instantiate.hpp>
struct _Kitti_PointExtended {
inline _Kitti_PointExtended(const _Kitti_PointExtended &p)
: data { p.x, p.y, p.z, 1.0f }, i(p.i){
}
inline _Kitti_PointExtended()
: data { 0.0f, 0.0f, 0.0f, 1.0f }, i(0.0f) {
}
friend std::ostream& operator << (std::ostream& os, const _Kitti_PointExtended& p) {
return os << "x: "<< p.x << ", y: " << p.y << ", z: " << p.z
<< ", intensity : " << p.i;
}
PCL_ADD_POINT4D;
union {
struct {
float i;
};
float data_c[4];
};
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}EIGEN_ALIGN16; // 强制SSE对齐
// Register the point type.
POINT_CLOUD_REGISTER_POINT_STRUCT (_Kitti_PointExtended,
(float, x, x)
(float, y, y)
(float, z, z)
(float, i, i)
)
typedef _Kitti_PointExtended PointKittiExtended;
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
