ROS學習筆記——PCL激光雷達點雲處理

4.27更新

一、創建節點發布點雲數據並可視化

1、在ros工作空間的src目錄下新建包（包含依賴項）

catkin_create_pkg chapter10_tutorials pcl_conversions pcl_ros pcl_msgs sensor_msgs

3、在軟件包中新建src目錄

rospack profile
roscd chapter10_tutorials
mkdir src

4、在src目錄下新建pcl_create.cpp，該程序創建了100個隨機座標的點雲並以1Hz的頻率，topic爲“pcl_output"發佈。frame設爲odom。

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_create");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;

    // Fill in the cloud data
    cloud.width  = 100;
    cloud.height = 1;
    cloud.points.resize(cloud.width * cloud.height);

    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
        cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }

    //Convert the cloud to ROS message
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

5、修改cmakelist.txt內容

cmake_minimum_required(VERSION 2.8.3)
project(chapter10_tutorials)
find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
  rospy
)

find_package(PCL REQUIRED)
catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

link_directories(${PCL_LIBRARY_DIRS})


add_executable(pcl_create src/pcl_create.cpp)
target_link_libraries(pcl_create ${catkin_LIBRARIES} ${PCL_LIBRARIES})

6、進入工作空間編譯包

cd ~/catkin_ws
catkin_make --pkg chapter10_tutorials

7、若編譯成功，新窗口打開ros，新窗口運行pcl_create節點

roscore
rosrun chapter10_tutorials pcl_create

8、新窗口打開rviz，add topic"pcl_output"，Global options 設置Frame爲odom

rviz

 

 二、加載pcd文件、保存點云爲pcd文件

1、加載pcd文件併發布爲pcl_output點雲：在src下新建pcl_read.cpp，內容爲：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_read");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);

    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZ> cloud;

    pcl::io::loadPCDFile ("test_pcd.pcd", cloud);

    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

2、保存topic發佈的點云爲pcd文件，在src下新建pcl_write.cpp內容爲：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(input, cloud);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_write");
    ros::NodeHandle nh;
    ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);
    ros::spin();

    return 0;
}

 3、添加內容到cmakelist.txt

add_executable(pcl_read src/pcl_read.cpp)
add_executable(pcl_write src/pcl_write.cpp)

target_link_libraries(pcl_read ${catkin_LIBRARIES} ${PCL_LIBRARIES})
target_link_libraries(pcl_write ${catkin_LIBRARIES} ${PCL_LIBRARIES})

 4、在catkin_ws空間下編譯包（同上）

5、打開不同的窗口，在pcd文件夾下分別運行節點（因爲pcl_read要讀取pcd文件）

roscore
roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_read
roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_write

6、可視化同上

三、cloud_viewer可視化pcd文件的點雲

新建cpp文件，所有步驟同上。

#include <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::io::loadPCDFile ("0.pcd", cloud);
     viewer.showCloud(cloud.makeShared());
     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }

protected:
    ros::NodeHandle nh;
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    cloudHandler handler;
    ros::spin();
    return 0;
}

 編譯並在pcd數據文件夾下運行節點，可得下圖。可以按住ctrl鍵滑輪放大縮小。

 

三、點雲預處理——濾波和縮減採樣

濾波刪除離羣值pcl_filter.cpp，處理流程同上

#include <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>

class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
     pcl::io::loadPCDFile ("0.pcd", cloud);

     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.4);
        statFilter.filter(cloud_filtered);
     viewer.showCloud(cloud_filtered.makeShared());
     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }

protected:
    ros::NodeHandle nh;
    
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    cloudHandler handler;
    ros::spin();
    return 0;
}

濾波結果：

 

 濾波以後縮減採樣，只有public變化了：

public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
 pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;

     pcl::io::loadPCDFile ("0.pcd", cloud);

      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);

 	pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
        voxelSampler.setInputCloud(cloud_filtered.makeShared());
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
        voxelSampler.filter(cloud_downsampled);

     viewer.showCloud(cloud_downsampled.makeShared());

     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

縮減採樣結果：

---

4.24更新：播放bag並可視化激光雷達點雲

1、查看無人艇的視頻和雷達數據20200116.bag信息：

rosbag info 20200116.bag

發現時長約3分鐘，topic有fix(衛星導航數據)、heading(姿態四元數)、points_raw（激光雷達點雲）、rosout(節點圖)、camera_info(攝像機信息)、image_raw(攝像機圖片)、time_reference(時間)、vel(速度？角速度？)

2、回放bag文件：

rosbag play 20200116.bag

 

3、打開rviz:

rosrun rviz rviz

4、點擊add--->add topic--->Pointcloud2(或 image)

5、Global Options ：Fixed Frame 改成pandar

（剛開始沒有改fixed frame，add topic以後總是顯示錯誤，後來在src下的雷達的包裏看到readme文件裏面寫着：4. Change fixed frame to `pandar`。所以在vriz裏修改了，圖像和點雲都可以顯示了。

 

4.23更新：讓turtlebot3建圖並導航

turtlebot3教程 :https://www.ncnynl.com/archives/201702/1396.html

一、遠程連接步驟

1、連上同一個無線局域網

2、pc端： ifconfig查看ip,修改bashrc文件的ip

ifconfig
gedit ~/.bashrc
source ~/.bsahrc

3、pc端：  連接機器人

SSH登錄到遠程計算機：$ ssh username@ip_address

ssh [email protected] 

啓動機器人：

export TURTLEBOT3_MODEL=burger
cd ~/catkin_ws
source devel/setup.bash
roslaunch turtlebot3_bringup turtlebot3_robot.launch

修改ip(i 編輯  esc退出編輯 shift：wq 保存退出)

vi ~/.bashrc  

source ~/.bashrc

4、pc端運行以下命令才能進行節點其他操作：

cd ~/catkin_ws
source devel/setup.bash

 

二、運行以下啓動命令時報錯

ModuleNotFoundError: No module named 'rospkg'

roslaunch turtlebot3_bringup turtlebot3_robot.launch

解決方法：

1、命令行輸入Python，檢查Python版本是否爲2.7，如果不是2.7則需要修改系統Python版本

2、確認2.7版本，命令行輸入 pip install rospkg 即可

 

4.20更新

turtlebot3 安裝軟件git clone 報錯（因爲家裏網不行）：fatal:early EOF    fatal:index-pack failed

解決方法:

1、增加緩存（親測無效）

2、直接複製github網址，下載包到catkin_ws文件夾下，再重新執行命令即可

看過的教程：

https://www.bilibili.com/video/BV1mJ411R7Ni?p=17

https://blog.csdn.net/zhangrelay/article/details/51737074

 

VREP:

官方手冊https://www.coppeliarobotics.com/helpFiles/index.html

入門教程https://www.jianshu.com/p/eb3f38c0c5fa

基礎https://blog.csdn.net/danieldingshengli/category_7238158.html

泡泡機器人https://max.book118.com/html/2019/0430/6102210112002025.shtm