在ROS中将点云（PointCloud2）生成Octomap，rviz可视化显示

一个python文件作为publisher，发布点云数据。一个C++项目接收点云数据，引用octomap库，将点云生成octomap的tree，在将tree通过topic发布出去，rviz订阅octomap tree的topic进行可视化显示。

首先创建一个python的点云发布节点，用pycharm开发，在Project Structure中加入ros的package：在C++和Python的项目中使用ROS-CSDN博客

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import numpy as np

# 初始化ROS节点
rospy.init_node('pointcloud_publisher', anonymous=True)

# 创建ROS Publisher
pub = rospy.Publisher('your_pointcloud_topic', PointCloud2, queue_size=10)

rate = rospy.Rate(1)  # 设置发布频率为1Hz，你可以根据需要调整

while not rospy.is_shutdown():
    # 创建一个NumPy数组，包含点云数据
    point_cloud_data = np.array([
        [1.0, 2.0, 0.0],  # 三个坐标 (x, y, z) 和一个强度值
        [2.0, 3.0, 0.0],
        [3.0, 4.0, 0.0],
        # 添加更多点云数据行
    ])
    # 创建Pointcloud2消息
    header = rospy.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = 'your_frame_id'  # 设置帧ID
    cloud_msg = point_cloud2.create_cloud_xyz32(header, point_cloud_data)

    pub.publish(cloud_msg)

    print(cloud_msg.header.stamp.to_sec())

    rate.sleep()

接下来创建一个C++项目，即创建工作空间文件夹，工作空间中包括：include，src，build文件夹，以及CMakeLists.txt，src中创建main.cpp。C++项目包括三个功能，一是订阅点云topic，二是将点云转成octomap的tree，三是将tree发布，让rviz可视化。实现这三个功能，需要做如下的准备：

订阅点云topic功能的准备：

安装pcl

sudo apt install libpcl-dev

再安装pcl_msgs（不知道为什么pcl_msgs不在pcl包里），中间的melodic替换为自己的ros版本。

sudo apt-get install ros-melodic-pcl-msgs

再继续把https://github.com/ros-perception/perception_pcl/blob/melodic-devel/pcl_conversions/include/pcl_conversions/pcl_conversions.h

这个pcl_conversions.h文件放到自己项目的include文件夹中，后面回用到这个头文件里的一个函数 fromROSMsg，函数 fromROSMsg依赖pcl_msgs，其功能是将ros topic的sensor_msgs::PointCloud2::ConstPtr 数据转化为 pcl::PointCloud<pcl::PointXYZ> 数据。

点云生成octomap的准备：

安装octomap

git clone https://github.com/OctoMap/octomap.git
cd octomap
mkdir build
cd build
cmake ..
make
sudo make install

通过topic发布octomap tree的准备：

下载octomap_msgs项目：https://github.com/OctoMap/octomap_msgs/tree/melodic-devel

这里要选择自己的ros版本，noetic可以用melodic。（不知道为什么octomap_msgs不继承进octomap）

下载之后通过cmake编译

cd octomap_msgs
mkdir build
cd build
cmake ..
make

在octomap_msgs/build/devel/share/octomap_msgs/cmake中，有octomap_msgsConfig.cmake，在自己项目的要使用octomap_msgs时，CMakeLists.txt中set这个Config.cmake为 octomap_msgs_DIR，使得cmake在find_packege时，能够找到octomap_msgs。

到这里，准备工作都完成了。

C++代码以及CMakeLists.txt内容：

main.cpp这样写

#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <octomap/octomap.h>
#include <octomap/Pointcloud.h>
#include <octomap/ColorOcTree.h>
#include <octomap_msgs/Octomap.h>

#include "pcl_conversions.h"

#include "/home/username/Downloads/octomap_msgs/include/octomap_msgs/conversions.h"


// Create an Octomap message
octomap_msgs::Octomap octomap_msg;

class MapGenerator{
public:
   MapGenerator(){
      tree = new octomap::OcTree(0.1);
   };
   ~MapGenerator(){};

   void PointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);

   octomap::OcTree* tree;
};

void print_query_info(octomap::point3d query, octomap::OcTreeNode* node) {
   if (node != NULL) {
      std::cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << std::endl;
   }
   else 
      std::cout << "occupancy probability at " << query << ":\t is unknown" << std::endl;    
}


int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pointcloud_subscriber");
    // 创建ROS节点句柄
    ros::NodeHandle nh;

    MapGenerator mg;

    // 创建Subscriber来订阅PointCloud2消息
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("your_pointcloud_topic", 10, boost::bind(&MapGenerator::PointCloudCallback, &mg, _1));

    // Create a publisher for the Octomap topic
    ros::Publisher octomap_pub = nh.advertise<octomap_msgs::Octomap>("octomap_tree", 1);

    // Publish the Octomap
    ros::Rate loop_rate(1);  // Adjust the publishing rate as needed
    while (ros::ok()) {
      octomap_pub.publish(octomap_msg);
      ros::spinOnce();
      loop_rate.sleep();
    }

    return 0;
}

// 回调函数，用于处理接收到的PointCloud2消息
void MapGenerator::PointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
   pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
   pcl::fromROSMsg(*msg, pcl_cloud);

   for (const auto& point : pcl_cloud) {
      // Extract XYZ coordinates and RGB color from the point
      float x = point.x;
      float y = point.y;
      float z = point.z;

      // Convert color to octomap::ColorOcTreeNode
      octomap::point3d octomap_point(x, y, z);

      // Insert the node into the octomap
      tree->updateNode(octomap_point, true);
   }

   // octomap::point3d query = octomap::point3d(1., 2., 0.);
   // octomap::OcTreeNode* result = tree->search(query);
   // print_query_info(query, result);

   
   // Also publish as an octomap msg for visualization
   octomap_msgs::fullMapToMsg(*tree, octomap_msg);
   
   // 在这里可以对OctoMap进行其他操作，如保存地图数据
   // tree.writeBinary("your_octomap.ot");

   ROS_INFO("OctoMap created and saved.");
}

CMakeLists.txt这样写

cmake_minimum_required(VERSION 2.8)

project(my_map)

set(CMAKE_BUILD_TYPE "Debug")

find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  std_msgs
  pcl_msgs
)
find_package(octomap REQUIRED)

find_package(PCL REQUIRED)

set(octomap_msgs_DIR /home/username/Downloads/octomap_msgs/build/devel/share/octomap_msgs/cmake/octomap_msgsConfig.cmake)
find_package(octomap_msgs REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

include_directories(
./include
${catkin_INCLUDE_DIRS}
${OCTOMAP_INCLUDE_DIRS}
)

set(SRC_LIST
./src/main.cpp)

add_executable(main ${SRC_LIST})

target_link_libraries( main 
${catkin_LIBRARIES}
${OCTOMAP_LIBRARIES}
${PCL_LIBRARIES}
)

因为是使用vscode开发，c_cpp_properties.json这样写，不用vscode就不需要了。

{
    "configurations": [
        {
            "name": "Linux",
            "includePath": [
                "${workspaceFolder}/**",
                "/opt/ros/你的ros版本/include/",
                "/usr/include/eigen3",
                "/usr/include/pcl-1.10/"
            ],
            "defines": [],
            "compilerPath": "/usr/bin/gcc",
            "cStandard": "c17",
            "cppStandard": "gnu++14",
            "intelliSenseMode": "linux-gcc-x64"
        }
    ],
    "version": 4
}

代码和CMakeLists解释：

回调函数中的

pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl::fromROSMsg(*msg, pcl_cloud);

是在include文件夹中，下载的那个头文件里。

回调函数中

octomap_msgs::fullMapToMsg(*tree, octomap_msg);

这句话是将tree通过topic发布，这个函数在我们下载并编译的octomap_msgs中，为了使用它，在CMakeLists中需要包含如下代码，并且在main.cpp的开头include它的头文件。

set(octomap_msgs_DIR /home/username/Downloads/octomap_msgs/build/devel/share/octomap_msgs/cmake/octomap_msgsConfig.cmake)
find_package(octomap_msgs REQUIRED)

然后就可以编译运行了。

在ubuntu中使用vscode进行C++开发的方法，参考：Ubuntu使用cmake和vscode开发自己的项目，引用自己的头文件和openCV-CSDN博客

最后一步通过rviz可视化，只要安装一个插件就可以

sudo apt-get install ros-kinetic-octomap-rviz-plugins

kinetic换成你的ubuntu版本。

rosrun rviz rviz

Add OccupancyMap

也可以用octovis进行可视化

sudo apt-get install ros-noetic-octovis

octovis your_octomap.ot

完成！