【Point Cloud & ROS】创建基于点云的随机地图并发布

【Point Cloud & ROS】创建基于点云的随机地图并发布

什么是PCL？

PCL库是一个最初发布于2013年的开源C++库。它实现了大量点云相关的通用算法和高效的数据管理。支持多种操作系统平台，可在Windows、Linux、Android、Mac OS X、部分嵌入式实时系统上运行。

什么是点云数据？

点云数据是用来采集多维度的物体信息，其中主要包含物体的XYZ三维坐标，以及物体的表面颜色信息。 XYZ+表面颜色信息组成了4D的点云数据。

点云的数据类型

ROS与PCL的接口

1.创建名为map_generator的包

在终端中输入

catkin_create_pkg map_generator std_msgs rospy roscpp sensor_msgs pcl_ros pcl_conversions std_srvs message_generation

2.创建名为random_map_pub_node.cpp文件

#include <cstdlib>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
using namespace std;

int main(int argc, char  *argv[])
{
    ros::init(argc, argv, "random_map_pub_node");//设置节点名称

    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("random_map_publisher_topic", 100);//设置发布的话题名称及消息缓存区大小

    ros::Rate rate(0.5);//设置地图刷新频率为0.5hz

    pcl::PointCloud<pcl::PointXYZ> cloud;//建立PCL点云
    pcl::PointXYZ random_point;

    sensor_msgs::PointCloud2 output_msg;//ROS的点云

    //参数
    int obstacle_num ;
    int range_min;
    int range_max;
    int size_min;
    int size_max ;
    double resolution;

    //将参数链接到launch文件中，可在launch文件中直接修改
    ros::param::get("~obstacle_num",obstacle_num);
    ros::param::get("~range_min",range_min);
    ros::param::get("~range_max",range_max);
    ros::param::get("~size_min",size_min);
    ros::param::get("~size_max",size_max);
    ros::param::get("~resolution",resolution);

    float x;
    float y;
    float z;

    float p;
    float size;

    while (ros::ok()) {

        output_msg.header.stamp = ros::Time::now();

        for (int i = 0;i < obstacle_num;i++) {
            p = (rand()%(range_max-range_min+1))+range_min;
            cout<<"p = "<<p<<endl;
            size = (rand()%(size_max-size_min+1))+size_min;
            cout<<"size = "<<size<<endl;
            for (x=p-size/2; x<=p+size/2; x+= resolution ) {
                for (y=p-size/2; y<=p+size/2; y+= resolution ) {
                    for (z=p-size/2; z<=p+size/2; z+= resolution ) {
                        random_point.x = x;
                        random_point.y = y;
                        random_point.z = z;
                        cloud.push_back(random_point);
                        }
                }
            }  
        }

        pcl::toROSMsg(cloud,output_msg);//将点云转化为ROS消息
        output_msg.header.frame_id = "random_map_frame_id";//点云坐标系
        pub.publish(output_msg);//发布


        rate.sleep();

        cloud.erase(cloud.begin(),cloud.end());//清除当前点云，进行刷新
    }
    return 0;
}

3.在CMkeLists.txt文件中加入编译规则

find_package(PCL REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})

catkin_package( )

include_directories(
  ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}
)

link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(random_map_pub_node src/random_map_pub_node.cpp)
target_link_libraries(random_map_pub_node ${PCL_LIBRARIES}  ${catkin_LIBRARIES} )

4.编写Launch启动文件

运行启动文件，可直接加载事先保存好的rviz设置，并且可以在文件中直接修改参数

<launch>

  <node pkg = "rviz" type = "rviz" name = "rviz" args = "-d $(find map_generator)/src/rviz/random_map_pub_node.rviz"></node>

  
  <node pkg="map_generator" type="random_map_pub_node" name="random_map_pub_node" output="screen">

  <param name="obstacle_num" type="int" value="5" />
  <param name="range_min" type="int" value="-20" />
  <param name="range_max" type="int" value="20" />
  <param name="size_min" type="int" value="0" />
  <param name="size_max" type="int" value="10" />
  <param name="resolution" type="double" value="1.0" />

  </node>

</launch>

5.运行结果

roslaunch map_generator random_map_pub_node.launch