Octomap

Octomap

可以将点云数据转换为栅格数据(OccupancyGrid)

源文件

加载点云数据并发布话题/pointcloud/output

#include <iostream>
#include <string>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <vector>

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

using namespace std;

int main(int argc, char **argv)
{
    std::string topic, path, frame_id;
    int hz = 5;

    ros::init(argc, argv, "publish_pointcloud");
    ros::NodeHandle nh;

    nh.param<std::string>("path", path, "/home/kavin/ROSProjects/ros_learning/catkin_ws_cv/src/publish_pointcloud/data/test2.pcd");
    nh.param<std::string>("frame_id", frame_id, "camera");
    nh.param<std::string>("topic", topic, "/pointcloud/output");
    nh.param<int>("hz", hz, 5);

    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>(topic, 10);

    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;
    pcl::io::loadPCDFile(path, cloud);
    pcl::toROSMsg(cloud, output); // 转换成ROS下的数据类型, 最终通过topic发布

    output.header.stamp = ros::Time::now();
    output.header.frame_id = frame_id;

    cout << "path = " << path << endl;
    cout << "frame_id = " << frame_id << endl;
    cout << "topic = " << topic << endl;
    cout << "hz = " << hz << endl;

    ros::Rate loop_rate(hz);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

launch文件

启动octomap_server节点

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.05" />

    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <param name="frame_id" type="string" value="/camera" />

    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="100.0" />
    <param name="latch" value="true" />

    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="1000" />
    <param name="pointcloud_min_z" value="0" />

    <!-- topic from where pointcloud2 messages are subscribed -->
    <remap from="/cloud_in" to="/pointcloud/output" />
 
  </node>
</launch>

启动点云发布节点

<?xml version="1.0" encoding="UTF-8"?>
<launch>
 
  <node name="publish_pointcloud" pkg="publish_pointcloud" type="publish_pointcloud">
    <param name="path" value="$(find publish_pointcloud)/data/test.pcd" type="str" />
    <param name="frame_id" value="camera" type="str" />
    <param name="topic" value="/pointcloud/output" type="str" />
    <param name="hz" value="2" type="int" />
  </node>

  <!-- Load ocotmap launch -->
  <include file="$(find publish_pointcloud)/launch/octomaptransform.launch" />

  <!-- RViz -->
  <node pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find publish_pointcloud)/rviz/OctomapShow.rviz"/>

</launch>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(publish_pointcloud)

find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    std_msgs
    rostime 
    sensor_msgs 
    message_filters 
)

set(OpenCV_DIR "~/Library/opencv-3.3.1/build")
find_package(OpenCV REQUIRED)
find_package(OpenMP)
find_package(PCL REQUIRED)

catkin_package()

include_directories(
    ${catkin_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
)

// 生成可执行文件
add_executable(publish_pointcloud src/publish_pointcloud.cpp)

// 链接库文件
target_link_libraries(publish_pointcloud
    ${catkin_LIBRARIES}
    ${OpenCV_LIBRARIES}
    ${PCL_LIBRARIES}
)

install(TARGETS publish_pointcloud
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)