【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