PCL三维避障

最新推荐文章于 2024-03-17 15:02:49 发布

kuizhao8951

最新推荐文章于 2024-03-17 15:02:49 发布

阅读量1.6k

点赞数 1

分类专栏： Robot

本文链接：https://blog.csdn.net/kuizhao8951/article/details/104241979

版权

Robot 专栏收录该内容

43 篇文章 6 订阅

订阅专栏

kartoslam(simulation):
#建图
rosrun slam_karto slam_karto
roslaunch turtlebot_gazebo turtlebot_world.launch
roslaunch turtlebot_rviz_launchers view_navigation.launch
roslaunch turtlebot_teleop keyboard_teleop.launch
rosrun map_server map_saver -f /home/asber/turtlebot_custom_maps/tutorial
#定位导航
roslaunch turtlebot_gazebo turtlebot_world.launch
roslaunch turtlebot_gazebo amcl_demo.launch map_file:=/home/asber/turtlebot_custom_maps/tutorial.yaml
roslaunch turtlebot_rviz_launchers view_navigation.launch

/camera/depth/image_raw 深度图
/camera/rgb/image_raw RGB图像
/camera/depth/points 点云数据

把所有点云全部传入costmap
在这里插入图片描述

ubuntu16下配置搭建PCL开发环境

有两种方式：1.aptget的方式获取二进制库

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
sudo apt-get update
sudo apt-get install libpcl-dev

2.源码安装¹²
官方地址：http://pointclouds.org/downloads

下载github之后
cmake -DCMAKE_BUILD_TYPE=Release …
apt-get install cmake g++ libboost1.58-all-dev libeigen3-dev libflann-dev python libusb-1.0-0-dev libudev-dev freeglut3-dev doxygen graphviz libpng12-dev libgtest-dev libxmu-dev libxi-dev libpcap-dev libqhull-dev libvtk5-qt4-dev python-vtk libvtk-java（这一步可以先省略，看到哪里报错再安装相关的包）
最后make -j2
make -j2 install或sudo make -j2 install

在sudo apt-get install python-vtk libvtk-java的时候发现要删除，一不小心按了Y 千万不要按不然会各种报错

The following packages will be REMOVED:
  libpcl-dev libvtk6-dev libvtk6-qt-dev python-vtk6 ros-kinetic-astra-launch ros-kinetic-base-local-planner ros-kinetic-camera-calibration ros-kinetic-carrot-planner ros-kinetic-clear-costmap-recovery  ros-kinetic-compressed-depth-image-transport ros-kinetic-compressed-image-transport ros-kinetic-costmap-2d ros-kinetic-cv-bridge ros-kinetic-depth-image-proc ros-kinetic-depthimage-to-laserscan ros-kinetic-desktop ros-kinetic-desktop-full ros-kinetic-dwa-local-planner ros-kinetic-freenect-launch ros-kinetic-gazebo-plugins ros-kinetic-gazebo-ros-pkgs ros-kinetic-global-planner ros-kinetic-image-geometry ros-kinetic-image-pipeline  ros-kinetic-image-proc ros-kinetic-image-publisher ros-kinetic-image-rotate ros-kinetic-image-transport-plugins ros-kinetic-image-view ros-kinetic-kobuki-gazebo-plugins ros-kinetic-move-base ros-kinetic-move-slow-and-clear  ros-kinetic-nav-core ros-kinetic-navfn ros-kinetic-navigation ros-kinetic-opencv3 ros-kinetic-openni-launch ros-kinetic-openni2-launch ros-kinetic-pcl-conversions ros-kinetic-pcl-ros ros-kinetic-perception  ros-kinetic-perception-pcl ros-kinetic-realsense-camera ros-kinetic-rgbd-launch ros-kinetic-rotate-recovery ros-kinetic-rqt-common-plugins ros-kinetic-rqt-image-view ros-kinetic-simulators ros-kinetic-stereo-image-proc  ros-kinetic-theora-image-transport ros-kinetic-turtlebot ros-kinetic-turtlebot-actions ros-kinetic-turtlebot-apps ros-kinetic-turtlebot-bringup ros-kinetic-turtlebot-calibration ros-kinetic-turtlebot-follower
  ros-kinetic-turtlebot-gazebo ros-kinetic-turtlebot-interactions ros-kinetic-turtlebot-interactive-markers ros-kinetic-turtlebot-navigation ros-kinetic-turtlebot-rapps ros-kinetic-turtlebot-rviz-launchers
  ros-kinetic-turtlebot-simulator ros-kinetic-turtlebot-stage ros-kinetic-turtlebot-stdr ros-kinetic-turtlebot-teleop ros-kinetic-vision-opencv ros-kinetic-viz

在编译过程中遇到的问题

/home/asber/catkin_ws/src/pcl/gpu/utils/src/repacks.cu(84): error: identifier "copy_fields_t" is undefined
/home/asber/catkin_ws/src/pcl/gpu/utils/src/repacks.cu(84): error: expected a ";"
/home/asber/catkin_ws/src/pcl/gpu/utils/src/repacks.cu(96): error: namespace "pcl::device" has no member "copy_fields_t"
3 errors detected in the compilation of "/tmp/tmpxft_00007763_00000000-13_repacks.compute_70.cpp1.ii".
CMake Error at pcl_gpu_utils_generated_repacks.cu.o.cmake:268 (message):
  Error generating file
  /home/asber/catkin_ws/src/pcl/build/gpu/utils/CMakeFiles/pcl_gpu_utils.dir/src/./pcl_gpu_utils_generated_repacks.cu.o
gpu/utils/CMakeFiles/pcl_gpu_utils.dir/build.make:241: recipe for target 'gpu/utils/CMakeFiles/pcl_gpu_utils.dir/src/pcl_gpu_utils_generated_repacks.cu.o' failed
make[2]: *** [gpu/utils/CMakeFiles/pcl_gpu_utils.dir/src/pcl_gpu_utils_generated_repacks.cu.o] Error 1
CMakeFiles/Makefile2:5933: recipe for target 'gpu/utils/CMakeFiles/pcl_gpu_utils.dir/all' failed
make[1]: *** [gpu/utils/CMakeFiles/pcl_gpu_utils.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 20%] Building CXX object io/CMakeFiles/pcl_io.dir/src/openni2/openni2_device_info.cpp.o
[ 20%] Building CXX object io/CMakeFiles/pcl_io.dir/src/openni2/openni2_device_manager.cpp.o
[ 20%] Building CXX object io/CMakeFiles/pcl_io.dir/src/dinast_grabber.cpp.o
[ 20%] Building CXX object io/CMakeFiles/pcl_io.dir/src/libpng_wrapper.cpp.o
[ 20%] Linking CXX shared library ../lib/libpcl_io.so
[ 20%] Built target pcl_io
Makefile:149: recipe for target 'all' failed
make: *** [all] Error 2

使用cmake -DCMAKE_BUILD_TYPE=Release …构建就OK了

测试是否安装成功

https://blog.csdn.net/fsencen/article/details/79386570
测试成功

学习资源

官方资源
提高编译速度Using CCache to speed up compilation、Using DistCC to speed up compilation、Single compilation units、Compiler optimizations

SLAM三维视觉的点云处理概率机器人多视图几何
 点云库PCL从入门到精通-csdn
随笔分类 - PCL学习

去除地面点云

去除地面点云的时候最好知道自己需要进行地面拟合的点云集合，大概的位置范围，否则算法很容易错误拟合
在gezabo仿真中，我写了一个根据z轴直接分割点云数据发布的程序，然后发现原来点云z轴并不是高度，而是前后。y轴才是上下，x轴是左右。而且y轴是朝下的，以下是我写的程序，参数H 0.28表明了 y<0.28的点云部分是非地面点点云，如果要处理的话需要处理y>=0.28的点云部分

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//关于平面分割的头文件
#include <pcl/sample_consensus/model_types.h>   
#include <pcl/sample_consensus/method_types.h>   
#include <pcl/segmentation/sac_segmentation.h>  
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/extract_indices.h>
ros::Publisher pub;
double H = 0.28;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
   std::string function_name = "cloud_cut";
    ROS_INFO("[%s]:cloud_cutCallback function!",function_name.c_str());
  // 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::fromROSMsg (*input,*cloud);

 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_gound(new pcl::PointCloud<pcl::PointXYZ>);
 
 for (int i = 0; i < cloud->points.size(); i++)
 {
   if (cloud->points[i].y < H)
   {
     cloud_no_gound->points.push_back(cloud->points[i]);
   }
 }
  //再rviz上显示所以要转换回PointCloud2
  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg(*cloud_no_gound,output);
  output.header = input->header;
  pub.publish (output);
}
int main (int argc, char** argv)
{
  ros::init (argc, argv, "yyf");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe ("/camera/depth/points", 1, cloud_cb);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("cloud_no_gound", 100);
  ros::spin ();
}

ros回调函数传参
参考：表示感谢
有提供工程代码，我下了
 python手撸的，建议有python需要的时候再用
 windows的实现，不过备注很不错，而且提供网盘代码，很不错
—全场最佳
和新哥的代码一样了，估计就是这个了
 核心代码的讲解，把那几句仅有的代码看懂
 同上
 开源的完整代码！比第一个更（有开源精神）
最简单的完整代码介绍

下面给出我的代码

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//关于平面分割的头文件
#include <pcl/sample_consensus/model_types.h>   
#include <pcl/sample_consensus/method_types.h>   
#include <pcl/segmentation/sac_segmentation.h>  
 
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/extract_indices.h>
 
 
 
ros::Publisher groundPoint_pub;//输出地面的点云信息
ros::Publisher nogroundPoint_pub;//输出非地面的点云信息
double L = 0.27;
double H = -0.4;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  std::string function_name = "my_pcl_ransac_ground_filter";
  ROS_INFO("[%s]:Callback function!",function_name.c_str());
  // 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input,*cloud);
  //首先对大概的地面点云进行y轴提取
#pragma omp for
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_gound(new pcl::PointCloud<pcl::PointXYZ>);
  for (int i = 0; i < cloud->points.size(); i++)
  {
    if (cloud->points[i].y >= L)
    {
      cloud_no_gound->points.push_back(cloud->points[i]);
    }
  }
  //开始ransac算法
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);//申明模型的参数
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);//申明存储模型的内点的索引
  pcl::SACSegmentation<pcl::PointXYZ> seg;// 创建一个分割方法
  seg.setOptimizeCoefficients (true);// 这一句可以选择最优化参数的因子
  seg.setModelType (pcl::SACMODEL_PLANE);   //平面模型
  seg.setMethodType (pcl::SAC_RANSAC);    //分割平面模型所使用的分割方法
  seg.setDistanceThreshold (0.01);        //设置最小的阀值距离
  seg.setInputCloud (cloud_no_gound);   //设置输入的点云
  seg.segment (*inliers,*coefficients);  // 执行ransac算法

  pcl::PointCloud<pcl::PointXYZ>::Ptr GroundPoint_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr NoGroundPoint_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  
  double A = coefficients->values[0];
  double B = coefficients->values[1];
  double C = coefficients->values[2];
  double D = coefficients->values[2];

#pragma omp for
  double threshold = 0.01;
  for (int i = 0; i < cloud->points.size(); i++)
  {
      double X = cloud->points[i].x;
      double Y = cloud->points[i].y;
      double Z = cloud->points[i].z;
      double height = abs(A*X + B*Y + C*Z + D) / sqrt(A*A + B*B + C*C); 
      //如果垂直距离小于0.01 那么就认为是地面
      //&& (cloud->points[i].z < -0.2) )并且z小于-0.2
      if ((height <= threshold) && (cloud->points[i].y >= L) )
      {
          GroundPoint_ptr->points.push_back(cloud->points[i]);
      }
      else
      {
          NoGroundPoint_ptr->points.push_back(cloud->points[i]);
      }
  }

  //publish ground ptr and noground ptr
  sensor_msgs::PointCloud2 groundPoint_cloud_msg;
  sensor_msgs::PointCloud2 nogroundPoint_cloud_msg;
  pcl::toROSMsg(*GroundPoint_ptr, groundPoint_cloud_msg);
  pcl::toROSMsg(*NoGroundPoint_ptr, nogroundPoint_cloud_msg);
  groundPoint_cloud_msg.header = input->header;
  nogroundPoint_cloud_msg.header = input->header;
  groundPoint_pub.publish(groundPoint_cloud_msg);
  nogroundPoint_pub.publish(nogroundPoint_cloud_msg);
}
 
int main (int argc, char** argv)
{
 
  ros::init (argc, argv, "yyf");
  ros::NodeHandle nh;
 
  ros::Subscriber sub = nh.subscribe ("/camera/depth/points", 1, cloud_cb);
  groundPoint_pub = nh.advertise<sensor_msgs::PointCloud2>("/ransac_groundPoint",1000);
  nogroundPoint_pub = nh.advertise<sensor_msgs::PointCloud2>("/ransac_nogroundPoint",1000);
  ros::spin ();
}

可以去我的下载my_ground_filter查看整个pkg文件

roslaunch turtlebot_gazebo turtlebot_world.launch
rosrun my_ground_filter my_pcl_ransac_ground_filter 
roslaunch turtlebot_rviz_launchers view_navigation.launch
roslaunch turtlebot_teleop keyboard_teleop.launch 


记得navigation的param中costmap common的参数需要改为
  observation_sources:  scan bump pcl_no_ground
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    min_obstacle_height: 0.25
    max_obstacle_height: 0.35
  bump:
    data_type: PointCloud2
    topic: mobile_base/sensors/bumper_pointcloud
    marking: true
    clearing: false
    min_obstacle_height: 0.0
    max_obstacle_height: 0.15
  pcl_no_ground:
    data_type: PointCloud2
    topic: /ransac_nogroundPoint
    clearing: true
    marking: true
    max_obstacle_height: 2.0
    obstacle_range: 6.0
    raytrace_range: 8.0

点云下采样

暂时不实现，等到需要

点云平面投影

不需要

reference:

kuizhao8951

关注

1
点赞
踩
15

收藏

觉得还不错? 一键收藏
5
评论
PCL三维避障

kartoslam(simulation):#建图rosrun slam_karto slam_kartoroslaunch turtlebot_gazebo turtlebot_world.launchroslaunch turtlebot_rviz_launchers view_navigation.launchroslaunch turtlebot_teleop keyboard_...
复制链接

扫一扫

专栏目录