sensor_msgs::LaserScan转PointCloud
void LaserScanToPointCloud(sensor_msgs::LaserScan::ConstPtr _laser_scan, pcl::PointCloud<pcl::PointXYZI>& _pointcloud)
{
_pointcloud.clear();
pcl::PointXYZI newPoint;
newPoint.z = 0.0;
double newPointAngle;
int beamNum = _laser_scan->ranges.size();
for (int i = 0; i < beamNum; i++)
{
newPointAngle = _laser_scan->angle_min + _laser_scan->angle_increment * i;
newPoint.x = _laser_scan->ranges[i] * cos(newPointAngle);
newPoint.y = _laser_scan->ranges[i] * sin(newPointAngle);
newPoint.intensity = _laser_scan->intensities[i];
_pointcloud.push_back(newPoint);
}
}
PointCloud转sensor_msgs::LaserScan
sensor_msgs::LaserScan PointCloudToLaserscan(pcl::PointCloud<pcl::PointXYZI>& _pointcloud)
{
float angle_min, angle_max, range_min, range_max, angle_increment;
//需要自行调整的参数
angle_min = -3.14159;
angle_max = 3.14159;
range_min = 0.5;
range_max = 20;
//角度分辨率,分辨率越小,转换后的误差越小
angle_increment = 0.005
//计算扫描点个数
unsigned int beam_size = ceil((angle_max - angle_min) / angle_increment);
sensor_msgs::LaserScan output;
output.header.stamp = ros::Time::now();
output.header.frame_id = "laser";
output.angle_min = angle_min;
output.angle_max = angle_max;
output.range_min = range_min;
output.range_max = range_max;
output.angle_increment = angle_increment;
output.time_increment = 0.0;
output.scan_time = 0.0;
//先将所有数据用nan填充
output.ranges.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
output.intensities.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
for (auto point : _pointcloud.points)
{
float range = hypot(point.x, point.y);
float angle = atan2(point.y, point.x);
int index = (int)((angle - output.angle_min) / output.angle_increment);
if (index >= 0 && index < beam_size)
{
//如果当前内容为nan,则直接赋值
if (isnan(output.ranges[index]))
{
output.ranges[index] = range;
}
//否则,只有距离小于当前值时,才可以重新赋值
else
{
if (range < output.ranges[index])
{
output.ranges[index] = range;
}
}
output.intensities[index] = point.intensity;
}
}
return output;
}
PointCloud转sensor_msgs::PointCloud2
pcl::toROSMsg(pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl,sensor_msgs::PointCloud2 pointcloud_msg);
sensor_msgs::PointCloud2转PointCloud
pcl::fromROSMsg(sensor_msgs::PointCloud2 pointcloud_msg,pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl);