读取PCD文件的点云并在 RVIZ显示

1. 创建功能包 catkin_init 之类的代码

2. src 源文件如下 :

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

int main(int argc, char **argv)
{
    ros::init(argc,argv,"UanBdet");
    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output",1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;

    pcl::io::loadPCDFile("cloudWithoutGround.pcd",cloud);
    pcl::toROSMsg(cloud,output);
    output.header.frame_id = "odom1";   
    ros::Rate loop_rate(1);
    while(ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

CmakeList.txt 文件如下, 注意添加catkin 的头文件和库文件

cmake_minimum_required(VERSION 3.0.2)
project(pcd2ros)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

find_package(PCL 1.7 REQUIRED)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}   # 添加Ros 头文件
  ${PCL_INCLUDE_DIRS}
)
add_definitions(${PCL_DEFINITIONS})

add_executable(pcd2ros src/redpcd.cpp)
target_link_libraries(pcd2ros ${PCL_LIBRARIES} ${catkin_LIBRARIES}  ) # 添加Ros 库文件

3. 打开Rviz 接受点云:

rostopic list    ## 找到你的话题 pcl_output

读取PCD文件的点云并在 RVIZ显示
打印你的消息,会看到有数据

rostopic echo /pcl_output
roscore
rviz # 打开RViz

添加 PointCLoud2 点云类型,此时不是八叉树, 选择 Fixed Frame 为 odom1 (程序里命名的,容易忘记) 和 topic 便可以显示
读取PCD文件的点云并在 RVIZ显示

上一篇:ROS 第五讲 在模拟器中构建第一个机器人


下一篇:【ROS学习记录】2021/7/2 Gazebo+rviz仿真(一)