无人机避障——4D毫米波雷达Octomap从点云建立三维栅格地图

Octomap安装

sudo apt-get install ros-melodic-octomap-ros
sudo apt-get install ros-melodic-octomap-msgs
sudo apt-get install ros-melodic-octomap-server
sudo apt-get install ros-melodic-octomap-rviz-plugins
# map_server安装
sudo apt-get install ros-melodic-map-server

  启动rviz

roscore
rosrun rviz rviz

点击add，可以看到多了Octomap_rviz_plugins模组：

OccupancyGrid是显示三维概率地图，也就是octomap地图。OccupancyMap是显示二维占据栅格地图

从PCD创建PointCloud2点云话题并发布出去：

参考资料：

测试的test数据采用以下第一条博客的pcd测试数据

Octomap 在ROS环境下实时显示_octomap在ros环境下实时显示-飞天熊猫-CSDN博客

学习笔记：使用Octomap将点云地图pcd转换为三维栅格地图，并在rviz中可视化_octomap功能包-CSDN博客

创建点云发布话题的工作空间：

mkdir -p ~/publish_pointcloudtest/src        #使用系统命令创建工作空间目录
cd ~/publish_pointcloudtest/src
catkin_init_workspace           # ROS的工作空间初始化命令

在工作空间下放入以下两个文件：

octomap_mapping

octomap_server

资源下载：

https://github.com/OctoMap/octomap_mapping

src下创建cpp文件pointcloud_publisher.cpp

#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>
 
#include <octomap_msgs/OctomapWithPose.h>
#include <octomap_msgs/Octomap.h>
#include <geometry_msgs/Pose.h>
 
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
 
#include <geometry_msgs/TransformStamped.h>
 
#define TESTCLOUDPOINTS 1  // 设置为 1 以测试点云发布，设置为 0 不测试
#define TESTOCTOTREE 0     // 设置为 1 以测试OctoMap发布，设置为 0 不测试
 
int main (int argc, char **argv)  
{  
    std::string topic, path, frame_id;
    int hz = 5;  // 发布频率，单位 Hz
 
    ros::init(argc, argv, "publish_pointcloud");  // 初始化ROS节点
    ros::NodeHandle nh;  // 创建节点句柄
 
    // 从参数服务器获取参数
    nh.param<std::string>("path", path, "/home/nvidia/publish_pointcloudtest/data/test.pcd");
    nh.param<std::string>("frame_id", frame_id, "map");
    nh.param<std::string>("topic", topic, "pointcloud_topic");
    nh.param<int>("hz", hz, 5);
 
    // 加载点云数据到pcl::PointCloud对象中
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud; 
    pcl::io::loadPCDFile(path, pcl_cloud);  // 从文件加载点云数据
 
#if TESTCLOUDPOINTS  // 如果 TESTCLOUDPOINTS 定义为 1，则执行这部分代码
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>(topic, 10);  // 创建Publisher对象，将点云数据发布到指定话题