Tare_Planner小项目----＞2D雷达的数据接口(三)Wheeltec-brushless-senior-diff小车的2D雷达与深度相机在栅格地图上的建图

苏歆_sssuxin

已于 2023-02-01 11:02:50 修改

阅读量716

点赞数 5

分类专栏： Tare_Planner 文章标签：自动驾驶

于 2022-11-29 21:16:15 首次发布

本文链接：https://blog.csdn.net/skjfhsef/article/details/128103284

版权

Tare_Planner 专栏收录该内容

6 篇文章 4 订阅

订阅专栏

目标:让小车用激光雷达和深度相机完成边建图边导航的任务,用以代替tare运行时依赖的environment_simulator仿真环境

一.小车自身的配置和功能

小车能够边建图边导航(rp激光雷达),另外配有深度相机(这里建图还未使用到,不过后面融合会用到)

二.使用方法和融合代码以及实际效果

变数1.可能要注意一下膨胀后进行一个z轴上的滤波,这个可能需要按照实际的情况调节一下滤波范围(保留)

2.点云的预先旋转平移处理需要按照不同规格的相机作出变换,对着话题/astra_downsample_points查看是否在"map"坐标系下正常显示

ros节点

1.roslaunch turn_on_wheeltec_robot my_mapping.launch navigation:=true 打开建图和导航(并且订阅的雷达消息是laser_scan_pcl_fusion)
2.roslaunch astra_camera my_astra.launch
3.rosrun turn_on_wheeltec_robot astra_points_ds_2scan

在上位机
1.rosrun tare_planner wheeltec_2_tare
2.roslaunch tare_planner explore_garage.launch

pcl和laser融合代码

astra_points_ds_2scan.cpp
代码如下
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <tf/transform_listener.h>
#include <pcl/common/transforms.h>
#include <sensor_msgs/LaserScan.h>
#include <math.h>
#define PI (3.141592653589f)
///!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!也许后面还可以搞个点云积累效果?积累成一个图?      在rviz上感觉点云数据挺滞后的,这个不知道有没有影响
//本节点用于融合scan和点云信息=>最终输出laserscan   ,用于2D栅格建图和tare导航
class cloudHandler
{
public:
    pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;              //下采样专用的类
    pcl::PassThrough<pcl::PointXYZ> pass;// 创建滤波器对象
    tf::TransformListener listener;
    tf::StampedTransform transform;
    bool pcl_initial=false;    //用于判断是否点云数据已经出现
    double roll, pitch, yaw;
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
    pcl::PointCloud<pcl::PointXYZ> cloud_for_fusion;
    sensor_msgs::LaserScan laserscan_output;
    pcl::PointCloud<pcl::PointXYZ> copy_cloud;
    pcl::PointCloud<pcl::PointXYZ> copy_cloud_1;
    pcl::PointCloud<pcl::PointXYZ> copy_cloud_2;
    pcl::PointCloud<pcl::PointXYZ> copy_cloud_3;
    cloudHandler()
    {
        pcl_sub = nh.subscribe("/camera/depth/points", 1, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/astra_downsample_points", 1);
        pcl_test_pub = nh.advertise<sensor_msgs::PointCloud2>("/astra_downsample_points_laser", 1);
        scan_sub = nh.subscribe("/scan", 2, &cloudHandler::PCL_SCAN_fusion, this);
        scan_pub = nh.advertise<sensor_msgs::LaserScan>("/laser_scan_pcl_fusion", 1);
    }
    void cloudCB(const sensor_msgs::PointCloud2 &input)
    //@@4点云数据每次回调函数会更新到map,直到laser的回调函数里才被使用在生成新的laser上
    {
        pcl_initial=true;
        sensor_msgs::PointCloud2 output;
        pcl::fromROSMsg(input, cloud);
        // std::cout<<cloud.points.size()<<"    ";
        //@@1留下一个正方体内的点云,其余多余数据滤除,并进行体素降采样
        pass.setInputCloud (cloud.makeShared());//设置输入点云
        pass.setFilterFieldName ("x");//滤波字段名被设置为Z轴方向
        pass.setFilterLimits (-2, 2);//可接受的范围为（0.0，1.0） 
        pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
        pass.filter (cloud_downsampled); //执行滤波，保存过滤结果在cloud_filtered
        pass.setInputCloud (cloud_downsampled.makeShared());//设置输入点云
        pass.setFilterFieldName ("y");//滤波字段名被设置为Z轴方向
        pass.setFilterLimits (-2, 2);//可接受的范围为（0.0，1.0） 
        pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
        pass.filter (cloud_downsampled); //执行滤波，保存过滤结果在cloud_filtered
        pass.setInputCloud (cloud_downsampled.makeShared());//设置输入点云
	    pass.setFilterFieldName ("z");//滤波字段名被设置为Z轴方向
        pass.setFilterLimits (-2, 2);//可接受的范围为（0.0，1.0） 
        pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
        pass.filter (cloud_downsampled); //执行滤波，保存过滤结果在cloud_filtered
        voxelSampler.setInputCloud(cloud_downsampled.makeShared());
        voxelSampler.setLeafSize(0.1f, 0.1f, 0.1f);     //
        voxelSampler.filter(cloud_downsampled);
	// /*下采样*/
        try//这边是给
        {
        /*旋转平移*/                                                            //将直通滤波后的点云旋转平移到map坐标系下
        ros::Time now = input.header.stamp;
        listener.waitForTransform("/map", "/camera_depth_frame", now, ros::Duration(3.0));
        listener.lookupTransform("/map", "/camera_depth_frame", now, transform);                //
        tf::Quaternion q(transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ(),transform.getRotation().getW());
        tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
        /*旋转平移*/
	    /*@@2PREturn*///一个提前的补偿(旋转),让点云正常分布
        Eigen::Affine3f transform_me = Eigen::Affine3f::Identity();
        transform_me.rotate (Eigen::AngleAxisf (PI/2, Eigen::Vector3f::UnitY()));
        transform_me = Eigen::Affine3f::Identity();
        transform_me.rotate (Eigen::AngleAxisf (-PI/2, Eigen::Vector3f::UnitX()));
        pcl::transformPointCloud (cloud_downsampled, cloud_downsampled, transform_me);
        transform_me = Eigen::Affine3f::Identity();
        transform_me.rotate (Eigen::AngleAxisf (-PI/2, Eigen::Vector3f::UnitZ()));
        pcl::transformPointCloud (cloud_downsampled, cloud_downsampled, transform_me);
	   /*@@3PREturn*///坐标系转换(真正的旋转平移)
	    transform_me = Eigen::Affine3f::Identity();
        transform_me.translation() << transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z();
        transform_me.rotate (Eigen::AngleAxisf (yaw, Eigen::Vector3f::UnitZ()));
        pcl::transformPointCloud (cloud_downsampled, cloud_downsampled, transform_me);
	    // std::cout<<transform.getOrigin().x()<<"  "<<transform.getOrigin().y()<<"  "<< transform.getOrigin().z()<<"            "<<yaw<<" "<<roll<<" "<<pitch;
		// std::cout<<"      xuanzhunan---     ";            
            //发送
        // std::cout<<cloud_downsampled.points.size()<<std::endl;
        pcl::toROSMsg(cloud_downsampled, output);
        output.header.frame_id="map";
        output.header.stamp=input.header.stamp;
        pcl_pub.publish(output);
        }
        catch (tf::TransformException &ex) 
        {
            std::cout<<"pcl旋转map没有找到该时刻点坐标系"<<std::endl;
        }
    }
    void PCL_SCAN_fusion(const sensor_msgs::LaserScan &input)                      //替换laser的rang的数据           
    {
        if(!pcl_initial)
        {
            std::cout<<"pcl没有接准备好数据,quit"<<std::endl;
            return;
        }
        // std::vector<float> *ranges = &input.ranges;
        /*pcl数据旋转平移到laser坐标系下*/
        try
        {
            //@@5把上面旋转平移到map的pcl数据旋转平移到雷达坐标系下
            ros::Time now = input.header.stamp;
            listener.waitForTransform("/laser", "/map", now, ros::Duration(3.0));
            listener.lookupTransform("/laser", "/map", now, transform);          
            tf::Quaternion q(transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ(),transform.getRotation().getW());
            tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
            Eigen::Affine3f transform_me = Eigen::Affine3f::Identity();
            transform_me = Eigen::Affine3f::Identity();
            transform_me.translation() << transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z();
            transform_me.rotate (Eigen::AngleAxisf (yaw, Eigen::Vector3f::UnitZ()));
            pcl::transformPointCloud (cloud_downsampled, cloud_for_fusion, transform_me);

            /*@@6膨胀*///让点云看起来密集一点,这样才有助于栅格地图建图
            transform_me = Eigen::Affine3f::Identity();
            transform_me.translation() << -0.02, 0, 0;
            pcl::transformPointCloud (cloud_for_fusion, copy_cloud, transform_me);
            transform_me.translation() << 0, -0.02, 0;
            pcl::transformPointCloud (cloud_for_fusion, copy_cloud_1, transform_me);
            transform_me.translation() << 0.02, 0, 0;
            pcl::transformPointCloud (cloud_for_fusion, copy_cloud_2, transform_me);
            transform_me.translation() << 0, 0.02, 0;
            pcl::transformPointCloud (cloud_for_fusion, copy_cloud_3, transform_me);
            copy_cloud  =copy_cloud+copy_cloud_1;
            copy_cloud_2=copy_cloud_2+copy_cloud_3;
            copy_cloud  =copy_cloud+copy_cloud_2;
            cloud_for_fusion=cloud_for_fusion+ copy_cloud;
            //@@7用于覆盖laserscan原数据的点云过多,计算延迟过大,减少数据量
            pass.setInputCloud (cloud_for_fusion.makeShared());//设置输入点云
            pass.setFilterFieldName ("z");//滤波字段名被设置为Z轴方向
            pass.setFilterLimits (0, 1);//可接受的范围为（0.0，1.0） //膨胀后点云过多,去除一点
            pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
            pass.filter (cloud_for_fusion); //执行滤波，保存过滤结果在cloud_filtered
            /*测试点云旋转到laser效果*/
            sensor_msgs::PointCloud2 output;
            pcl::toROSMsg(cloud_for_fusion, output);
            output.header.frame_id="laser";
            output.header.stamp=input.header.stamp;
            pcl_test_pub.publish(output);
            /*遍历所有点云,将其替代range*/
            //@@8计算laser坐标系下点云xy坐标的角度,找到laser里相应的ranges序号,并判断它到原点的距离,如果距离比原数据小则替代原
            laserscan_output=input;
            for (int i=0;i<cloud_for_fusion.points.size();i++)
            {
                //求角度
                float x= cloud_for_fusion.points[i].x;
                float y= cloud_for_fusion.points[i].y;
                if(x==0)
                {
                    continue;
                }
                float angel=atan2(y,x);
                int seq_range = (angel-input.angle_min)/input.angle_increment;
                std::cout<<" x:"<<x<<" y:"<<y<<" angel:"<<angel<<std::endl;
                if(laserscan_output.ranges[seq_range]>sqrt(x*x+y*y))
                {
                    laserscan_output.ranges[seq_range]=sqrt(x*x+y*y);
                }
            }
            scan_pub.publish(laserscan_output);
            /*遍历所有点云,将其替代range*/

        }
        catch(tf::TransformException &ex) 
        {
            std::cout<<"pcl旋转到laser没有找到该时刻点坐标系"<<std::endl;
        }

    }
protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
    ros::Subscriber scan_sub;
    ros::Publisher scan_pub;
    ros::Publisher pcl_test_pub;
};
 
main(int argc, char **argv)
{
    ros::init(argc, argv, "Astra_pcl_downsampling");
 
    cloudHandler handler;
 
    ros::spin();
 
    return 0;
}

实现效果