ROS的二维LaserScan转为三维pcl::PointXYZ

最新推荐文章于 2023-06-03 18:39:34 发布

屁桃罐

最新推荐文章于 2023-06-03 18:39:34 发布

阅读量1.2k

点赞数 3

分类专栏：累了毕业真难文章标签： c++ linux

本文链接：https://blog.csdn.net/weixin_43137906/article/details/106387780

版权

累了毕业真难专栏收录该内容

3 篇文章 0 订阅

订阅专栏

ROS的二维LaserScan转为三维pcl::PointXYZ

做实验用的单线rplidar a1输出的是LaserScan，想转换成三维的PoinCloud2格式的，然后通过订阅串口发来的数据来确定Z轴的值，本来想在直接在驱动节点里转为PointCloud2的，但PCL库也太大了，买的弱鸡arm的开发板放不下，编译也太慢了，说到底还是我自己憨憨，应该有更好的办法哈哈。总之，就在上位机搞个单独的节点来转换。首先了解下数据格式

LaserScan
好像大概是这样

然后就
大概这样再参考下各位大佬的程序
主要是这个部分

 cloud_msg.header.frame_id = "laser";

    cloud_msg.height = 16;

    int count = scan_msg.scan_time / scan_msg.time_increment;
    cloud_msg.width  = count;
    cloud_msg.points.resize(cloud_msg.width * cloud_msg.height);
    for(int i = count; i < count+count; i++) {
        float degree = RAD2DEG(scan_msg.angle_min + scan_msg.angle_increment * (i-count));
        if(scan_msg.ranges[(i-count)])
        {
            cloud_msg.points[i].x = scan_msg.ranges[(i-count)]*cos(DEG2RAD(degree));
            cloud_msg.points[i].y = scan_msg.ranges[(i-count)]*sin(DEG2RAD(degree));
            cloud_msg.points[i].z = k.data;
        }
        }

整个程序是这样的


```cpp

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include "std_msgs/Float64.h"
ros::Publisher cloud;
std_msgs::Float64 height;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
pcl::PointCloud<pcl::PointXYZ> cloud_msg;
sensor_msgs::LaserScan scan;
void scan_pcl(sensor_msgs::LaserScan scan_msg, std_msgs::Float64 k){

    cloud_msg.header.frame_id = "laser";

    cloud_msg.height = 16;

    int count = scan_msg.scan_time / scan_msg.time_increment;
    cloud_msg.width  = count;
    cloud_msg.points.resize(cloud_msg.width * cloud_msg.height);
    for(int i = count; i < count+count; i++) {
        float degree = RAD2DEG(scan_msg.angle_min + scan_msg.angle_increment * (i-count));
        if(scan_msg.ranges[(i-count)])
        {
            cloud_msg.points[i].x = scan_msg.ranges[(i-count)]*cos(DEG2RAD(degree));
            cloud_msg.points[i].y = scan_msg.ranges[(i-count)]*sin(DEG2RAD(degree));
            cloud_msg.points[i].z = k.data;
        }
        else
        {
            cloud_msg.points[i].x = 6*cos(DEG2RAD(degree));
            cloud_msg.points[i].y = 6*sin(DEG2RAD(degree));
            cloud_msg.points[i].z = k.data;
        }
        ROS_INFO(": [%f, %f]", degree, scan_msg.ranges[(i-count)]);
    }
    cloud.publish(cloud_msg);

}
void myCallback(const std_msgs::Float64&msg){


   height=msg;
}
void callback(const sensor_msgs::LaserScan &msg){


    scan=msg;
    scan_pcl(scan,height);

}
int main(int argc,char * argv[])
{

    ros::init(argc, argv, "scan_pcl");
    ros::NodeHandle nh;
    ros::Rate r(5); //执行频率5Hz
    ros::Subscriber height_sub = nh.subscribe("/chatter", 1000, myCallback);
    ros::Subscriber scan_sub=nh.subscribe("/scan",1000,callback);
    cloud=nh.advertise<PointCloud>("/point_cloud",1000);
    while (ros::ok()) {

    std_msgs::Float64 height;
    height.data=10;
    //scan_pcl(scan,height);
    ros::spinOnce();
 r.sleep();
}
    return 0;

}