1. 编程号首页

902实验项目ros+qt界面[通俗易懂]

编程小号 • • 未分类

902实验项目ros+qt界面[通俗易懂]目录源码头文件pro配置界面搭建源码main.cpp#include”mainwindow.h”#includeQApplicationintmain(intargc,char*argv[]){QApplicat

目录

源码

main.cpp

#include "mainwindow.h"
#include <QApplication>

int main(int argc, char *argv[])
{
    QApplication a(argc, argv);

    MainWindow w;
    w.show();

    ros::init(argc, argv, "node_b");	//初始化ROS，节点命名为node_b，节点名必须唯一。

    return a.exec();
}

mainwindow.cpp

//××××××××××××××××××××××××××××××××××××××××××××××××××××××
//
//阶段性集成界面，含后期bag数据包三维点云及栅格地图处理程序，含实时激光雷达采集程序
//
//
//××××××××××××××××××××××××××××××××××××××××××××××××××××××

#include "mainwindow.h"
#include "ui_mainwindow.h"

//pcl::visualization::CloudViewer loam_viewer("LoamCloud Viewer");
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudPtr (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>& cloud = *cloudPtr;

pcl::PointCloud<pcl::PointXYZI>::Ptr  cloud_look (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>& cloudLook = *cloud_look;

void cloud_save(const sensor_msgs::PointCloud2::ConstPtr& msg_cloud);
void loam_play(const sensor_msgs::PointCloud2::ConstPtr& msg_cloud);
bool save = false;
bool mTp = false;
QString fileName;

//void msgToPcl(const sensor_msgs::PointCloud2::ConstPtr& msg_cloud)
//{

//    sensor_msgs::PointCloud2 output;  //ROS中点云的数据格式

//    output=*msg_cloud;

//    pcl::fromROSMsg(output,*cloudPtr);

//    //cout<<"Trans success!"<<endl;

//    //pcl::visualization::CloudViewer viewer("Cloud Viewer");

//    //viewer.showCloud(cloudPtr);

//}

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    //connect(timer,&QTimer::timeout,this,&MainWindow::readFarme);// 时间到，读取当前摄像头信息

    thread = new thread_gou(this);
    thread_st = new thread_stair(this);
    thread_gird = new Gird_trench(this);
}

MainWindow::~MainWindow()
{
    delete ui;
}


void cloud_save(const sensor_msgs::PointCloud2::ConstPtr& msg_cloud)
{
    sensor_msgs::PointCloud2 output;  //ROS中点云的数据格式

    output=*msg_cloud;

    pcl::fromROSMsg(output,*cloudPtr);

    ros::shutdown();
    pcl::PCDWriter writer;

    std::string path="/media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/";
    writer.write<pcl::PointXYZI>(path+"CLOUD.pcd", *cloudPtr);
    //cout<<"Save success!"<<endl;
    QMessageBox::information(NULL, "Information", "Save PCD success!");
}

void MainWindow::on_gen_pcd_clicked()
{
    save=true;
    if(save == true){
        //       ros::init("node_b");	//初始化ROS，节点命名为node_b，节点名必须唯一。
        ros::NodeHandle n_g;	//节点句柄实例化
        ros::Subscriber sub = n_g.subscribe("laser_cloud_surround", 1000, cloud_save);	//向话题“str_message”订阅，一旦发布节点（node_a）在该话题上发布消息，本节点就会调用chatterCallbck函数。
        // ros::Subscriber sub = n_g.subscribe("velodyne_points", 1000, cloud_save);	//向话题“str_message”订阅，一旦发布节点（node_a）在该话题上发布消息，本节点就会调用chatterCallbck函数。
        ros::spin();	//程序进入循环，直到ros::ok()返回false，进程结束。
        save = false;
    }
}

void MainWindow::on_loam_clicked()
{
    std::string s;
    QString file;
    file="gnome-terminal -x bash -c 'source /media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/devel/setup.bash; rosbag play '"+fileName+"' --clock --topic /velodyne_points '&";
    s=file.toStdString();
    cout<<"hellpo"<<endl;
    cout<<s;

    system(file.toLatin1().data());
    system("gnome-terminal -x bash -c 'source /media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/devel/setup.bash; roslaunch loam_velodyne loam_velodyne.launch'&");

    //mTp=true;
    //    if(mTp==true){
    //        cout<<"hello"<<endl;
    //        ros::NodeHandle n;	//节点句柄实例化
    //        ros::Subscriber sub = n.subscribe("/laser_cloud_surround", 1000, msgToPcl);	//向话题“str_message”订阅，一旦发布节点（node_a）在该话题上发布消息，本节点就会调用chatterCallbck函数。
    //        ros::spin();	//程序进入循环，直到ros::ok()返回false，进程结束
    //    }
}

void MainWindow::on_List_clicked()
{
    fileName = QFileDialog::getOpenFileName(
                this,
                tr("open a file."),
                "/media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/",
                tr("files(*.bag);;All files(*.*)"));
    if (fileName.isEmpty()) {
        QMessageBox::warning(this, "Warning!", "Failed to open the file!");
    }
    else {
        ui->lineEdit->setText(fileName);
    }

}

void MainWindow::on_Tomap_clicked()
{
    // 创建八叉树对象，参数为分辨率，这里设成了0.05
    octomap::OcTree tree( 0.05 );
    for (auto p:cloud.points)
    {
        // 将点云里的点插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }
    // 更新octomap
    tree.updateInnerOccupancy();
    // 存储octomap
    std::string path="/media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/";
    tree.writeBinary(path+"sample.bt");
    QMessageBox::information(NULL, "Information", "Save MAP success!");


    //声明advertise，octomap rviz plug in 默认接受topic为octomap_full的message
    //   ros::NodeHandle n_map;	//节点句柄实例化
    //   ros::Publisher pub_octomap = n_map.advertise<octomap_msgs::Octomap>("octomap_full", 1, true);

    //   //声明message
    //   octomap_msgs::Octomap map_msg;
    //   //设置header
    //   map_msg.header.frame_id = "world";
    //   map_msg.header.stamp = ros::Time::now();
    //   //fullMapToMsg负责转换成message
    //   if (octomap_msgs::fullMapToMsg(tree, map_msg)){
    //       //转换成功，可以发布了
    //       pub_octomap.publish(map_msg);
    //       ros::spin();
    //       cout<<"pub success!"<<endl;
    //    }
    //   else{
    //   ROS_ERROR("Error serializing OctoMap");
    //   cout<<"pub success!"<<endl;
    //   }
}

void MainWindow::on_pcdLook_clicked()
{
    QString filePcd = QFileDialog::getOpenFileName(
                this,
                tr("open a file."),
                "/media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/",
                tr("files(*.pcd);;All files(*.*)"));
    if (filePcd.isEmpty()) {
        QMessageBox::warning(this, "Warning!", "Failed to open the file!");
    }
    else {
        std::string s;
        s = filePcd.toStdString();
        cout<<s<<endl;
        //pcl::io::loadPCDFile<pcl::PointXYZI>(s, cloudLook);
        QString file;
        file="pcl_viewer "+filePcd;
        system(file.toLatin1().data());
    }
}

void MainWindow::on_leGo_clicked()
{
    if (fileName == NULL){
        system("gnome-terminal -x bash -c 'source /media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/devel/setup.bash; roslaunch lego_loam run_false.launch'&");
    }
    else{
        system("gnome-terminal -x bash -c 'source /media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/devel/setup.bash; roslaunch lego_loam run_true.launch'&");
    }

}

void MainWindow::on_MapLook_clicked()
{
    QString fileMap = QFileDialog::getOpenFileName(
                this,
                tr("open a file."),
                "/media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/",
                tr("files(*.bt);;All files(*.*)"));
    if (fileMap.isEmpty()) {
        QMessageBox::warning(this, "Warning!", "Failed to open the file!");
    }
    else {
        //std::string s;
        //s = fileMap.toStdString();
        //cout<<s<<endl;
        QString file;
        file="/media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/src/octomap-1.9.0/bin/octovis "+fileMap;
        system(file.toLatin1().data());
    }
}

void MainWindow::on_Map_clicked()
{
    system("gnome-terminal -x bash -c 'source /media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/devel/setup.bash; roslaunch loam_velodyne ocmap.launch'&");
}

void MainWindow::on_velodyne_clicked()
{
    system("gnome-terminal -x bash -c 'source /media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/devel/setup.bash; roslaunch velodyne_pointcloud VLP16_points.launch calibration:=/media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/src/velodyne/velodyne_pointcloud/params/VLP-16.yaml'&");
}

void MainWindow::on_bag_save_clicked()
{
    system("gnome-terminal -x bash -c 'source /media/root/03a34f8a-a684-4e83-9e9f-747ac90d5e7e/catkin_ws/devel/setup.bash; rosbag record /velodyne_points'&");
}


void MainWindow::on_Detect_gou_clicked()
{
    thread->start();//QThread 的对象通过start()函数调用线程文件中的run()函数
}

void MainWindow::on_Detect_stair_clicked()
{
    thread_st->start();
}

void MainWindow::on_gird_gou_clicked()
{
    thread_gird->start();
}

gird_trench.cpp

#include "gird_trench.h"
#include "mainwindow.h"

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1Ptr_o (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1Ptr_x (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1Ptr_y (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1Ptr_out (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointXYZ>);

pcl::ExtractIndices<pcl::PointXYZ> extract;//定义一个抽取器

double base_x, base_y, base_z, roll, pitch, yaw;

class Grid
    {
    public:
        typename pcl::PointCloud<pcl::PointXYZ>::Ptr grid_cloud_1 {new pcl::PointCloud<pcl::PointXYZ>()};
        pcl::PointIndices::Ptr grid_inliers {new pcl::PointIndices};
        bool low_emp=true;
        float min_height;
    };

std::vector<int>save_vec;
int column = 100;
int row = 100;
int grid_size = 10;

Gird_trench::Gird_trench (QObject* par) : QThread(par)
{
}

void getTf(double& base_x, double& base_y, double& base_z, double& roll, double& pitch, double& yaw){
    tf::TransformListener listener;
    tf::StampedTransform transform;
    tf::Quaternion quat;

    try{
        listener.waitForTransform("/map","/base_link", ros::Time(0), ros::Duration(3.0));
        listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
    }
    catch(tf::TransformException &ex){
        ROS_ERROR("%s", ex.what());
        ros::Duration(1,0).sleep();
    }

    base_x = transform.getOrigin().x();
    base_y = transform.getOrigin().y();
    base_z = transform.getOrigin().z();

    quat[0] = transform.getRotation().getX();
    quat[1] = transform.getRotation().getY();
    quat[2] = transform.getRotation().getZ();
    quat[3] = transform.getRotation().getW();

    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
}

void gird_gou(const sensor_msgs::PointCloud2::ConstPtr& msg_cloud)
{
    sensor_msgs::PointCloud2 output;  //ROS中点云的数据格式

    output=*msg_cloud;

    pcl::fromROSMsg(output,*cloud_1Ptr_o);

    getTf(base_x, base_y, base_z, roll, pitch, yaw);
    pcl::io::savePCDFileASCII("cloud_1_o.pcd",*cloud_1Ptr_o);

    pcl::PassThrough<pcl::PointXYZ> pass1;
    pass1.setInputCloud (cloud_1Ptr_o);
    pass1.setFilterFieldName ("z");
    pass1.setFilterLimits (base_x, base_x + 5);
    pass1.setFilterLimitsNegative (false);
    pass1.filter (*cloud_1Ptr_x);
    pcl::io::savePCDFileASCII("cloud_1_x.pcd",*cloud_1Ptr_x);

    pcl::PassThrough<pcl::PointXYZ> pass2;
    pass2.setInputCloud (cloud_1Ptr_x);
    pass2.setFilterFieldName ("x");
    pass2.setFilterLimits (base_y - 2, base_y + 2);
    pass2.setFilterLimitsNegative (false);
    pass2.filter (*cloud_1Ptr_y);
    pcl::io::savePCDFileASCII("cloud_1_y.pcd",*cloud_1Ptr_y);

    cloud_1 = cloud_1Ptr_o;
    Grid grid[column][row];

    for(int count=0;count<cloud_1->points.size();count++){
        for(int i=0;i<column;i++){
            if((cloud_1->points[count].x > i*grid_size)&&(cloud_1->points[count].x < (i+1)*grid_size))
            {
                for(int j=-row/2;j<row/2;j++)
                {
                    if((cloud_1->points[count].y > j*grid_size)&&(cloud_1->points[count].y < (j+1)*grid_size))
                    {
                        grid[i][j+row/2].grid_inliers->indices.push_back(count);
                        grid[i][j+row/2].grid_cloud_1->points.push_back(cloud_1->points[count]);
                        if(grid[i][j+row/2].low_emp)
                        {
                            grid[i][j+row/2].min_height = cloud_1->points[count].z;
                            grid[i][j+row/2].low_emp=false;
                        }
                        else{
                            if(cloud_1->points[count].z<grid[i][j+row/2].min_height)
                            {grid[i][j+row/2].min_height = cloud_1->points[count].z;}
                        }
                    }
                }
            }
        }

    }
    //--------GRID-------//
    for(int i=0;i<column;i++)
    {
        for(int j=0;j<row;j++)
        {
            int grid_num = grid[i][j].grid_cloud_1->points.size();
            if(grid[i][j].min_height<0)
            {
                for(int k=0;k<grid_num;k++)
                {
                    if( (grid[i][j].grid_cloud_1->points[k].z>(grid[i][j].min_height+0.14)) )
                    {      //0.20
                        if(grid[i][j].grid_cloud_1->points[k].z<(grid[i][j].min_height+1.6))
                        {     //2.2
//                            if((right+0.4< grid[i][j].grid_cloud_1->points[k].y )&&(grid[i][j].grid_cloud_1->points[k].y<left-0.4)){
                                save_vec.push_back(grid[i][j].grid_inliers->indices[k]);
//                            }
                        }
                    }
                }
            }
        }
    }
    std::cerr<<"dd"<<std::endl;
    typename pcl::PointCloud<pcl::PointXYZ>::Ptr all_piece(new pcl::PointCloud<pcl::PointXYZ>());

    pcl::copyPointCloud(*cloud_1, save_vec , *all_piece);

    pcl::io::savePCDFileASCII("cloud_1_out.pcd",*all_piece);

}

void Gird_trench::run(){
    std::cerr <<"thread_gou"<<endl;
    ros::NodeHandle n_gou;	//节点句柄实例化
    ros::Subscriber sub = n_gou.subscribe("/laser_cloud_1_surround", 1000, gird_gou);	//向话题“str_message”订阅，一旦发布节点（node_a）在该话题上发布消息，本节点就会调用chatterCallbck函数。
    //ros::spin();	//程序进入循环，直到ros::ok()返回false，进程结束。
    ros::Publisher pub = n_gou.advertise<std_msgs::Bool>("/obstacle/trench", 10);

    ros::Rate loop_rate(5);
    while(ros::ok()){
        std_msgs::Bool msg;
        std::stringstream ss;
        ss<<1;
        //msg.data=ss.str();
        msg.data=1;
        //ROS_INFO("检测到楼梯 ：%s", msg.data.str());
        //        if(gou_pub ==1){
        //            pub.publish(msg);
        //        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}

S_gou.cpp

#include "thread_gou.h"
#include "mainwindow.h"

pcl::PointCloud<pcl::PointXYZ>::Ptr gou_cloudPtr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>& gou_cloud = *gou_cloudPtr;
pcl::PointCloud<pcl::PointXYZ>::Ptr gou_cloudPtr_x (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr gou_cloudPtr_y (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr gou_cloudPtr_z (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr gou_cloudPtr_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr gou_cloudPtr_trn (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr c_line(new pcl::PointCloud<pcl::PointXYZ>);  //存储平面点云
//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));

int step = 1, gou_pub = 0;
double cor[10], corRobot[10];
double robotHigh = 0.22;
double depth1, depth2, cross_width=0.3;
double width, dis,angle;
double x_sum, y_sum, z_sum, x_mean, y_mean, z_mean;
using namespace std;

thread_gou::thread_gou (QObject* par) : QThread(par)
{

}

void getTfCoordinate(double& base_x, double& base_y, double& base_z, double& roll, double& pitch, double& yaw){
    tf::TransformListener listener;
    tf::StampedTransform transform;
    tf::Quaternion quat;

    try{
        listener.waitForTransform("/map","/base_link", ros::Time(0), ros::Duration(3.0));
        listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
    }
    catch(tf::TransformException &ex){
        ROS_ERROR("%s", ex.what());
        ros::Duration(1,0).sleep();
    }

    base_x = transform.getOrigin().x();
    base_y = transform.getOrigin().y();
    base_z = transform.getOrigin().z();

    quat[0] = transform.getRotation().getX();
    quat[1] = transform.getRotation().getY();
    quat[2] = transform.getRotation().getZ();
    quat[3] = transform.getRotation().getW();

    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

}

void detect_gou(const sensor_msgs::PointCloud2::ConstPtr& msg_cloud)
{
    sensor_msgs::PointCloud2 output;  //ROS中点云的数据格式

    output=*msg_cloud;

    pcl::fromROSMsg(output,*gou_cloudPtr_in);

    //坐标系变换
//    float theta = -(M_PI*68)/180; // 弧度角
//    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
//    transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
//    pcl::transformPointCloud (*gou_cloudPtr_in, *gou_cloudPtr_trn, transform);

    pcl::PassThrough<pcl::PointXYZ> pass1;
    pass1.setInputCloud (gou_cloudPtr_in);
    pass1.setFilterFieldName ("x");
    pass1.setFilterLimits (0, 5);
    pass1.setFilterLimitsNegative (false);
    pass1.filter (*gou_cloudPtr_x);

    pcl::io::savePCDFileASCII("cloud_x.pcd",*gou_cloudPtr_x);

    pcl::PassThrough<pcl::PointXYZ> pass2;
    pass2.setInputCloud (gou_cloudPtr_x);
    pass2.setFilterFieldName ("y");
    pass2.setFilterLimits (-1, 1);
    pass2.setFilterLimitsNegative (false);
    pass2.filter (*gou_cloudPtr_y);

    pcl::PassThrough<pcl::PointXYZ> pass3;
    pass2.setInputCloud (gou_cloudPtr_y);
    pass2.setFilterFieldName ("z");
    pass2.setFilterLimits (-0.5, -robotHigh);//重要！！！！此处截取z轴-0.5到-robotHigh的平面，该处截取效果影响分割提取沟渠直线的效果，具体调试请根据保存的cloud_z.pcd效果进行参数调整。
    pass2.setFilterLimitsNegative (false);
    pass2.filter (*gou_cloudPtr);

    if(gou_cloudPtr->points.size()>50){

        pcl::io::savePCDFileASCII("cloud_z.pcd",*gou_cloudPtr);

        //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color (gou_cloudPtr, 255, 255, 255);
        //   viewer->removeAllPointClouds();  // 移除当前所有点云
        //   viewer->addPointCloud(gou_cloudPtr_in, "cloud_z");
        //viewer->addPointCloud<pcl::PointXYZ> (gou_cloudPtr_in, cloud_color, "cloud_z");
        //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_z");
        //   viewer->updatePointCloud(gou_cloudPtr_in, "cloud_z");

        // segmentation
        //创建一个模型参数对象，用于记录结果
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);  //inliers表示误差能容忍的点 记录的是点云的序号
        pcl::SACSegmentation<pcl::PointXYZ> seg;     // 创建一个分割器
        seg.setOptimizeCoefficients(true);      // Optional，这个设置可以选定结果平面展示的点是分割掉的点还是分割剩下的点。
        seg.setModelType(pcl::SACMODEL_LINE);  // Mandatory-设置目标几何形状
        seg.setMethodType(pcl::SAC_RANSAC);     //分割方法：随机采样法
        seg.setDistanceThreshold(0.01);         //设置误差容忍范围，也就是阈值
        seg.setInputCloud(gou_cloudPtr);               //输入点云
        seg.segment(*inliers, *coefficients);   //分割点云，获得平面和法向量
        pcl::ExtractIndices<pcl::PointXYZ> extract;  //创建点云提取对象
        extract.setInputCloud(gou_cloudPtr);    //设置输入点云
        extract.setIndices(inliers);     //设置分割后的内点为需要提取的点集
        extract.setNegative(false);      //false提取内点, true提取外点
        extract.filter(*c_line);        //提取输出存储到c_plane2

        pcl::io::savePCDFileASCII("cloud_seg.pcd",*c_line);

        // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> p_color (c_line, 255, 0, 0);

        // viewer->addPointCloud<pcl::PointXYZ> (c_line, p_color, "cloud_seg");
        //  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_seg");
        //   viewer->updatePointCloud(c_line, "cloud_seg");
        //    viewer->spinOnce(0.0000000000001);

        x_sum = 0;
        y_sum = 0;
        z_sum = 0;
        for(size_t i=0; i<c_line->points.size(); ++i){
            z_sum = z_sum + c_line->points[i].z;
            z_mean = z_sum /i;

            x_sum = x_sum + c_line->points[i].x;
            x_mean = x_sum/i;
            y_sum = y_sum + c_line->points[i].y;
            y_mean = y_sum/i;
        }
        //std::cerr<<"z="<<z_mean<<std::endl;

        if((z_mean + robotHigh + 0.01 <0) & (c_line->points.size() > 50) & (x_mean > 1.5) & (step == 1)){ //0.01为低于地面水平面0.1处为沟渠有效点，（根据激光雷达高度即robotHigh进行调整）当有效点个数大于90时认为是沟渠。距离沟渠1.5m时不再输出沟渠相关信息，此时误差较大。
            //std::cerr<<"x="<<x_mean<<std::endl;

            cor[1] = x_mean;
            cor[2] = y_mean;
            cor[3] = z_mean;

            std::cerr << std::endl << "发现沟渠!"<< std::endl;
            std::cerr << "沟渠坐标为：" << cor[1] <<","<< cor[2] <<","<< cor[3] << std::endl;
            depth1 = z_mean + robotHigh;
            std::cerr << "初始深度为：" << depth1 << std::endl;

            pcl::PointXYZ min;//用于存放三个轴的最小值
            pcl::PointXYZ max;//用于存放三个轴的最大值
            pcl::getMinMax3D(*c_line,min,max);
            double length= fabs(max.y - min.y);
            std::cerr<<"沟渠长度为："<<length<<std::endl;

            dis =  cor[1] - width - 0.2;
            double base_x, base_y, base_z;
            double roll, pitch, yaw;
            getTfCoordinate(base_x, base_y, base_z, roll, pitch, yaw);
            angle=yaw;
            std::cerr<<"当前机器人位置坐标为："<< base_x <<"," <<base_y<<","<<base_z<<std::endl;

            gou_pub =1;
            step=2;//该程序为单次触发
            //ros::shutdown();
        }


    }
    
    
    
 
}

void thread_gou::run(){
    step=1;
    std::cerr <<"thread_gou"<<endl;
    ros::NodeHandle n_gou;	//节点句柄实例化
    ros::Subscriber sub = n_gou.subscribe("/velodyne_points", 1000, detect_gou);	//向话题“str_message”订阅，一旦发布节点（node_a）在该话题上发布消息，本节点就会调用chatterCallbck函数。
    //ros::spin();	//程序进入循环，直到ros::ok()返回false，进程结束。
    ros::Publisher pub_bool = n_gou.advertise<std_msgs::Bool>("/obstacle/trench", 10);
    ros::Publisher pub_width = n_gou.advertise<std_msgs::Float32>("/obstacle/trench/width", 100);
    ros::Publisher pub_dis = n_gou.advertise<std_msgs::Float32>("/obstacle/trench/distance", 100);
    ros::Publisher pub_angle = n_gou.advertise<std_msgs::Float32>("/obstacle/trench/angle", 100);


    ros::Rate loop_rate(5);
    while(ros::ok()){
        std_msgs::Bool msg_bool;
        std_msgs::Float32 msg_width;
        std_msgs::Float32 msg_dis;
        std_msgs::Float32 msg_ang;

        //ROS_INFO("检测到沟渠 ：%s", msg.data());
        if(gou_pub ==1){
            msg_bool.data=1;
            msg_width.data=width;
            msg_dis.data=dis;
            msg_ang.data=angle;
            pub_bool.publish(msg_bool);
            pub_width.publish(msg_width);
            pub_dis.publish(msg_dis);
            pub_angle.publish(msg_ang);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }

}

thread_stair.cpp

#include "thread_stair.h"
#include "mainwindow.h"

int cloud_id=0;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloudP (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> cloud_i = *cloudP;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_voxel(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_Removal(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_Removal_pass(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass_voxel (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass_voxel_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass_voxel_projected_voxel (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_blob (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered_blob (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ladder (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ladder_filtered (new pcl::PointCloud<pcl::PointXYZ>);

//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

int s=1, k=0,count=0, could_id=0;
int LADDER=0;//为0说明没找到楼梯，为1说明找到楼梯
int Point_Threshold = 10;//单次截取平面的最少点云数据个数
float ladder_depth_max=1;//用于判断截平面中是否有楼梯的重要参数，单个楼梯宽度的最大阈值
float ladder_depth_min=0.1;//用于判断截平面中是否有楼梯的重要参数，单个楼梯宽度的最小阈值
float Scan_width = 0.2;//单次截取点云面的宽度
float ladder_depth=0.0;//楼梯深度
float Ref_z=0;//在截取点云面之前先将z值小于Ref_z的点去除
float ladder_position_x=0;//楼梯x方向的初始位置
float ladder_position_y=0;//楼梯y方向的初始位置
float ladder_position_z=0;//楼梯z方向的初始w位置

typedef pcl::PointXYZ PointType;  //PointXYZRGBA数据结构

thread_stair::thread_stair (QObject* par) : QThread(par)
{

}

void ladder(pcl::PointCloud<pcl::PointXYZ>& cloud_input){
    cout<<"******************************************************"<<endl;
    cout<<"****                                              ****"<<endl;
    cout<<"****                     ";cout<<s;cout<<"                       ****"<<endl;
    cout<<"****                                              ****"<<endl;
    cout<<"******************************************************"<<endl;

    clock_t begin,finish;
    double totaltime;
    begin=clock();

    *cloud_in=cloud_input;

    std::vector <int> array;
    std::vector <int> array_start;
    std::vector <int> array_end;

    //可视化场景点云
    //viewer->removeAllPointClouds();  // 移除当前所有点云
    //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color (cloud, 255, 255, 45);
    //viewer->addPointCloud<pcl::PointXYZ> (cloud, cloud_color, "cloud_filtered_voxel");
    //viewer->addPointCloud(cloud_in, "scene");
    //viewer->updatePointCloud(cloud_in, "scene");
    //viewer->spinOnce(0.0000000000001);

    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud_in);
    sor.setLeafSize ( 0.05f, 0.05f, 0.05f);
    sor.filter (*cloud_filtered_voxel);
    //    std::cerr << "PointCloud after filtering: " << cloud_filtered_voxel->width * cloud_filtered_voxel->height
    //              << " data points (" << pcl::getFieldsList (*cloud_filtered_voxel) << ").";

    //创建滤波器对象，移除离群点
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor1;
    sor1.setInputCloud (cloud_filtered_voxel);
    sor1.setMeanK (10);
    sor1.setStddevMulThresh (1.0);
    sor1.filter (*cloud_filtered_Removal);

    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud (cloud_filtered_Removal);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (-10, Ref_z);
    pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_p);

    cloud_filtered=cloud_p;

    pcl::PointXYZ min;//用于存放三个轴的最小值
    pcl::PointXYZ max;//用于存放三个轴的最大值
    pcl::getMinMax3D(*cloud_filtered,min,max);

    cout<<"min.x = "<<min.x<<"\n"<<endl;
    cout<<"max.x = "<<max.x<<"\n"<<endl;
    cout<<"min.y = "<<min.y<<"\n"<<endl;
    cout<<"max.y = "<<max.y<<"\n"<<endl;
    cout<<"min.z = "<<min.z<<"\n"<<endl;
    cout<<"max.z = "<<max.z<<"\n"<<endl;

    cout<<"Width.x = "<<max.x-min.x<<"\n"<<endl;
    cout<<"Length.y = "<<max.y-min.y<<"\n"<<endl;
    cout<<"Height.z = "<<max.z-min.z<<"\n"<<endl;

    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
    coefficients->values.resize (4);
    coefficients->values[0] = coefficients->values[1] = 0;
    coefficients->values[2] = 1.0;
    coefficients->values[3] = 0;

    // 创建滤波器对象
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType (pcl::SACMODEL_PLANE);
    proj.setInputCloud (cloud_filtered);
    proj.setModelCoefficients (coefficients);
    proj.filter (*cloud_projected);

    std::cerr<<"Zong Jie Qu Ci Shu"<<int((max.y-min.y)/Scan_width)<<std::endl;

    for (size_t i=0; i<int((max.y-min.y)/Scan_width); i++)
    {

        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud (cloud_projected);
        pass.setFilterFieldName ("y");
        pass.setFilterLimits (min.y+Scan_width*i, min.y+Scan_width*(i+1));
        //pass.setFilterLimitsNegative (true);
        pass.filter (*cloud_pass);
        if(cloud_pass->points.size()<Point_Threshold)
        {
            continue;
        }

        pcl::VoxelGrid<pcl::PointXYZ> sor2;
        sor2.setInputCloud (cloud_pass);
        sor2.setLeafSize (0.05f, 0.1f, 0.05f);
        sor2.filter (*cloud_pass_voxel);

        pcl::ModelCoefficients::Ptr coefficients2 (new pcl::ModelCoefficients ());
        coefficients2->values.resize (4);
        coefficients2->values[0] = 0;
        coefficients2->values[1] = 1.0;
        coefficients2->values[2] = 0;
        coefficients2->values[3] = 0;
        // 创建滤波器对象

        pcl::ProjectInliers<pcl::PointXYZ> proj1;
        proj1.setModelType (pcl::SACMODEL_PLANE);
        proj1.setInputCloud (cloud_pass_voxel);
        proj1.setModelCoefficients (coefficients2);
        proj1.filter (*cloud_pass_voxel_projected);

        pcl::VoxelGrid<pcl::PointXYZ> sor3;
        sor3.setInputCloud (cloud_pass_voxel_projected);
        sor3.setLeafSize (0.1f, 0.05f, 0.05f);
        sor3.filter (*cloud_pass_voxel_projected_voxel);

        for (size_t n=0;n < (cloud_pass_voxel_projected_voxel->points.size()-1); ++n)
        {
            if (((cloud_pass_voxel_projected_voxel->points[n+1].x - cloud_pass_voxel_projected_voxel->points[n].x) <ladder_depth_max)&&((cloud_pass_voxel_projected_voxel->points[n+1].x - cloud_pass_voxel_projected_voxel->points[n].x)>ladder_depth_min))
                //                                if ((cloud_pass_voxel_projected_voxel->points[n+1].x - cloud_pass_voxel_projected_voxel->points[n].x)>0.15)
            {
                count++;
                ladder_depth+=cloud_pass_voxel_projected_voxel->points[n+1].x - cloud_pass_voxel_projected_voxel->points[n].x;
            }
            else
                continue;
        }
        if(count<=3)
        {
            count=0;
            ladder_depth=0;
        }

        if (count>3)
        {
            LADDER = 1;
            ladder_depth=ladder_depth/count;
            float y_mid = (min.y+Scan_width*i, min.y+Scan_width*(i+1))/2;
            count=0;
            k++;
            array.push_back(i);
            std::cerr<<"Found Ladder;"<<std::endl;
            std::cerr<<"Ladder depth is "<<ladder_depth<<" m;"<<std::endl;
            std::cerr<<"Ladder Position in y axis"<<y_mid<<";"<<std::endl;
        }
        else
        {
            std::cerr<<"Not Found Ladder;"<<std::endl;
        }

        std::cerr<<"Jie Qu Jie Mian Ci Shu"<<i<<std::endl;

        cloud_pass->clear();
        cloud_pass_voxel->clear();
        cloud_pass_voxel_projected->clear();
        cloud_pass_voxel_projected_voxel->clear();
    }

    std::cerr<<"array size = "<<array.size()<<"   k = "<<k<<std::endl;

    array_start.push_back(array[0]);

    for (size_t i=0; i<(array.size()-1);i++)
    {
        std::cerr<<"array ["<<i<<"]:    "<<array[i]<<std::endl;
        if(array[i+1]-array[i]>1)
        {
            array_end.push_back(array[i]);
            array_start.push_back(array[i+1]);
        }
    }

    array_end.push_back(array[(array.size()-1)]);

    std::cerr<<"array_start size"<<array_start.size()<<std::endl;
    std::cerr<<"array_end size"<<array_end.size()<<std::endl;

    int start=array_start[0];
    int end=array_end[0];
    for (size_t i=0; i<(array_end.size()-1);i++)
    {
        if((array_end[i+1]-array_start[i+1])>(end-start))
        {
            start=array_start[i+1];
            end=array_end[i+1];
        }
    }
    std::cerr<<"ladder start position"<<start<<std::endl;
    std::cerr<<"ladder end position"<<end<<std::endl;

    pcl::PassThrough<pcl::PointXYZ> pass_ladder;
    pass_ladder.setInputCloud (cloud_p);
    pass_ladder.setFilterFieldName ("y");
    pass_ladder.setFilterLimits (min.y+Scan_width*start, min.y+Scan_width*(end+1));
    //pass.setFilterLimitsNegative (true);
    pass_ladder.filter (*cloud_ladder);

    pass.setInputCloud (cloud_ladder);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (-10, Ref_z);
    pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_ladder_filtered);

    pcl::PointXYZ min_ladder;//用于存放三个轴的最小值
    pcl::PointXYZ max_ladder;//用于存放三个轴的最大值
    pcl::getMinMax3D(*cloud_ladder_filtered,min_ladder,max_ladder);

    //为机器人指定楼梯前坐标位置
    ladder_position_x=min_ladder.x;
    ladder_position_y=(max_ladder.y+min_ladder.y)*0.5;
    ladder_position_z=min_ladder.z;

    cout<<"ladder_position_x = "<<ladder_position_x<<"\n"<<endl;
    cout<<"ladder_position_y = "<<ladder_position_y<<"\n"<<endl;
    cout<<"ladder_position_z = "<<ladder_position_z<<"\n"<<endl;

    //    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ladder_color (cloud_ladder, 255, 255, 45);
    //    viewer->addPointCloud<pcl::PointXYZ> (cloud_ladder, ladder_color, "cloud_filtered_voxel");
    //    viewer->updatePointCloud(cloud_ladder, "test");
    //    viewer->spinOnce(0.0000000000001);


    finish=clock();
    totaltime=(double)(finish-begin)/CLOCKS_PER_SEC;
    cout<<"\n此程序的运行时间为"<<totaltime<<"秒！"<<endl;

    s=s+1;

    cloud_id=cloud_id+1;

}

void detect_stair(const sensor_msgs::PointCloud2& input)
{

    pcl::fromROSMsg(input, cloud_i); // sensor_msgs::PointCloud2 ----> pcl::PointCloud<T>
    ladder(cloud_i);

}

void thread_stair::run(){
    std::cerr <<"thread_gou"<<endl;
    ros::NodeHandle n_stair;	//节点句柄实例化
    ros::Subscriber sub = n_stair.subscribe("/velodyne_points", 1000, detect_stair);	//向话题“str_message”订阅，一旦发布节点（node_a）在该话题上发布消息，本节点就会调用chatterCallbck函数。
    //ros::spin();	//程序进入循环，直到ros::ok()返回false，进程结束。
    ros::Rate loop_rate(5);
    while(ros::ok()){
        ros::spinOnce();
        loop_rate.sleep();
    }
}

头文件

gird_trench.h

#ifndef GIRD_TRENCH_H
#define GIRD_TRENCH_H

#include <QThread>

class Gird_trench : public QThread
{
    Q_OBJECT
protected:
    void run();

signals:
    void over();
public:
    Gird_trench(QObject* par);
};

#endif // GIRD_TRENCH_H

mainwindow.h

#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <stdio.h>
#include <iostream>
#include <QMainWindow>
#include <QFileDialog>
#include <QMessageBox>
#include <QTimer>   // 设置采集数据的间隔时间
#include "thread_gou.h"
#include "thread_stair.h"
#include "gird_trench.h"
#include <vector>
#include <time.h>
#include <sstream>

#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
 //#include<pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/ModelCoefficients.h>
#include <boost/thread/thread.hpp>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/common/transforms.h>

#include <sbg_driver/SbgImuData.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>
#include "ros/ros.h"
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap/math/Pose6D.h> // 可以用octomath::Pose6D来存储位姿
#include <octomap_msgs/GetOctomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap/octomap.h>
#include <octovis/OcTreeDrawer.h>
#include <octovis/ColorOcTreeDrawer.h>
#include <octovis/OcTreeRecord.h>

namespace Ui {
class MainWindow;
}

class MainWindow : public QMainWindow
{
    Q_OBJECT

public:
    explicit MainWindow(QWidget *parent = 0);
    ~MainWindow();


private slots:
    //void on_pushButton_clicked();

    void on_gen_pcd_clicked();


    void on_loam_clicked();

    void on_List_clicked();

    void on_Tomap_clicked();

    void on_pcdLook_clicked();

    void on_leGo_clicked();

    void on_MapLook_clicked();

    void on_Map_clicked();

    void on_velodyne_clicked();

    void on_bag_save_clicked();

    void on_Detect_gou_clicked();

    void on_Detect_stair_clicked();

    void on_gird_gou_clicked();

private:
    Ui::MainWindow *ui;

    QTimer    *timer;

    thread_gou *thread;
    thread_stair *thread_st;
    Gird_trench *thread_gird;
};

#endif // MAINWINDOW_H

thread_gou.h

#ifndef THREAD_GOU_H
#define THREAD_GOU_H

#include <QThread>

class thread_gou : public QThread
{

    Q_OBJECT
public:
    thread_gou(QObject* par);
protected:
    void run();

signals:
    void over();

};

#endif // THREAD_GOU_H

thread_stair.h

#ifndef THREAD_STAIR_H
#define THREAD_STAIR_H

#include <QThread>

class thread_stair : public QThread
{
    Q_OBJECT
public:
    thread_stair(QObject* par);
protected:
    void run();

signals:
    void over();
};

#endif // THREAD_STAIR_H

pro配置

#-------------------------------------------------

QT       += core gui

greaterThan(QT_MAJOR_VERSION, 4): QT += widgets

TARGET = ROS_Com
TEMPLATE = app


SOURCES += main.cpp\
        mainwindow.cpp \
    thread_stair.cpp \
    S_gou.cpp \
    gird_trench.cpp

HEADERS  += mainwindow.h \
    thread_gou.h \
    thread_stair.h \
    gird_trench.h

FORMS    += mainwindow.ui

CONFIG += console c++14

INCLUDEPATH += /opt/ros/kinetic/include
DEPENDPATH += /opt/ros/kinetic/include
LIBS += /opt/ros/kinetic/lib/libtf*.so
LIBS += -L/opt/ros/kinetic/lib -lroscpp -lroslib -lrosconsole -lroscpp_serialization -lrostime\


#Eigen
INCLUDEPATH += /usr/include/eigen3

#Vtk
INCLUDEPATH += /usr/include/vtk-6.2

#LIBS += /usr/lib/libvtk*.so
LIBS += /usr/lib/x86_64-linux-gnu/libvtk*.so

#Boost
INCLUDEPATH += /usr/include/boost

LIBS += /usr/lib/x86_64-linux-gnu/libboost_*.so

#PCL Header
#INCLUDEPATH += /usr/local/include/pcl-1.9
INCLUDEPATH += /usr/include/pcl-1.7

#PCL Lib
#LIBS        += /usr/local/lib/libpcl_*.so \
LIBS        += /usr/lib/x86_64-linux-gnu/libpcl_*.so

#Octomap
INCLUDEPATH += /usr/local/include/octomap
LIBS        += /usr/local/lib/libocto*.so

INCLUDEPATH += /usr/local/include/octovis
#LIBS        += /usr/local/lib/liboctovis.so

界面搭建

在这里插入图片描述

项目下载：https://download.csdn.net/download/xx970829/14044013

今天的文章902实验项目ros+qt界面[通俗易懂]分享到此就结束了，感谢您的阅读。

版权声明：本文内容由互联网用户自发贡献，该文观点仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 举报，一经查实，本站将立刻删除。
如需转载请保留出处：http://bianchenghao.cn/64332.html

赞 (0)
编程小号编程小号
0

相关推荐

发表回复

您的电子邮箱地址不会被公开。 必填项已用 * 标注