ROS学习记录(三)

1、基础

进入~/catkin_ws/src：cd ~/catkin_ws/src
创建一个ROS包：catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs
进入my_pcl_tutorial：cd my_pcl_tutorial
打开package.xml增加

<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>

在src文件下新建文件example.cpp
输入以下代码：(建立代码框架)

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

ros::Publisher pub;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Create a container for the data.
  sensor_msgs::PointCloud2 output;

  // Do data processing here...
  output = *input;

  // Publish the data.
  pub.publish (output);
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

  // Spin
  ros::spin ();
}

打开my_pcl_tutorial包下的CMakeLists.txt文件，增加:
add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES})

PCL主要有四种点云结构：
sensor_msgs::PointCloud — ROS message (deprecated)
sensor_msgs::PointCloud2 — ROS message
pcl::PCLPointCloud2 — PCL data structure mostly for compatibility with ROS (I think)
pcl::PointCloud — standard PCL data structure
我实验等等的只关注标准的点云结构(PS:使用过PCL)
复制粘贴以下代码到example.cpp中：

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/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>

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);

  pcl::ModelCoefficients coefficients;
  pcl::PointIndices inliers;
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (cloud.makeShared ());
  seg.segment (inliers, coefficients); 
  
  // Publish the model coefficients
  pcl_msgs::ModelCoefficients ros_coefficients;
  pcl_conversions::fromPCL(coefficients, ros_coefficients);
  pub.publish (ros_coefficients);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  // Create a ROS publisher for the output model coefficients
  pub = nh.advertise<pcl_msgs::ModelCoefficients> ("output", 1);

  // Spin
  ros::spin ();
}

突然发现没有连接激光雷达。
想个办法测试下这段时间学的东西。

2、想法

1、设计一个节点输入激光雷达数据(从pcd文件中输入)。
2、设计一个节点作为可视化。
3、设计一个节点分割地面点。

3、点云输入节点

早已安装了pcl-conversions，但是没有pcl/ros/conversions.h文件？？？

经过一系列的踩坑、查资料、摸索，一个简单的读取PCD数据、并发布的节点完成。

CMakeList.txt:

cmake_minimum_required(VERSION 3.0.2)
project(my_pcl_tutorial)
find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
)
find_package(PCL REQUIRED)
catkin_package(
CATKIN_DEPENDS pcl_ros roscpp sensor_msgs
)
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)
add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES} ${PCL_LIBRARIES})

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

删除example.cpp中的

#include<pcl/ros/conversions,h>

新建一个pcl_pcd.cpp文件
复制粘贴(用绝对路径，相对路径搞不懂)：

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

ros::Publisher pub;

int
main (int argc, char** argv)
{
  ROS_INFO("Start!!!!!!!");

  ros::init (argc, argv, "pcl_input");

  ros::NodeHandle nh;
  
  ros::Publisher pcl_pub;

  pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcd_data", 1);

  pcl::PointCloud<pcl::PointXYZ> testcloud;

  sensor_msgs::PointCloud2 output;
  ROS_INFO("Loading data.......");
  pcl::io::loadPCDFile ("/home/cj/catkin_ws/src/my_pcl_tutorial/src/000000.pcd", testcloud);
  pcl::toROSMsg(testcloud, output);
  int count=0;
  output.header.frame_id = "point_cloud";
  ros::Rate loop_rate(1);
  while(ros::ok()){
      ROS_INFO("%d",count);
      pcl_pub.publish(output);
      ros::spinOnce();
      loop_rate.sleep();
      count++;
  }
return 0;
}

进入文件夹：cd ~/catkin_ws
运行 catkin_make
运行rosrun my_pcl_tutorial pcl_pcd

4、点云可视化节点

在pcl_pcd.cpp同目录下新建visual_pcl.cpp
输入以下代码：

#include <iostream>
#include <string>
#include <ros/ros.h> 
#include <pcl/point_cloud.h> 
#include <pcl/point_types.h> 
#include <pcl_conversions/pcl_conversions.h> 
#include <sensor_msgs/PointCloud2.h> 
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pcd_data)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*pcd_data, cloud);

  pcl::visualization::CloudViewer viewer("Cloud Viewer");
  viewer.showCloud(cloud.makeShared ());
  while (!viewer.wasStopped ())
    {}
}

int main(int argc, char** argv){
    ROS_INFO("visual Start!!!!!!!");
    ros::init (argc, argv, "visual");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe ("pcd_data", 1, cloud_cb);
    ros::spin ();
}

在CMakeList.txt上增加：

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

cd ~/catkin_ws(返回catkin_ws目录下)
运行：catkin_make
运行：rosrun my_pcl_tutorial pcl_pcd
新建一个终端
运行：rosrun my_pcl_tutorial visual_pcl
再新建一个终端，运行rqt_graph,看下节点，话题关系。
运行：rosrun rqt_graph rqt_graph

但是，这个简单的可视化节点并不能做到实时显示点云。
下一步，把这个可视化节点变成一个实时显示点云的节点。

5、实时可视化节点

本来是想定义一个全局变量的点云，通过回调函数改变全局变量点云。
用PCL里面pcl_visualizer.h头文件中的方法，定义显示窗口，添加显示点云，条件循环ros::ok()，删除显示点云，ros::spinOnce调用回调函数，添加显示点云，延时函数。通过不断的添加显示点云与删除显示点云来达到实时效果。
可惜事与愿违。显示窗口没出现点云。
百度各种查询原因跟方法，发现有位大佬实现了这个功能。
大佬博客连接
学习到了。
把visual_pcl.cpp中的代码换成大佬博客中的代码，就能实现实时可视化节点。

6、点云处理节点

github