ROS三维环境建模——基于OctoMap库

在机器人操作系统（ROS）中，环境建模是实现自主导航、路径规划和环境感知的关键。除了二维地图表示方法如OccupancyGrid，ROS还支持更为精细的三维地图表示方法——OctoMap。

在ROS中，验证路径规划算法的一个重要步骤是环境建模，因而需要我们具备发布地图的能力。在二维地图中主要是基于OccupancyGrid，但是该方法不支持三维地图，本文将介绍基于OctoMap库的方法来建立三维地图。

1 安装相关库

首先需要安装octomap库。以noetic版本为例，输入以下指令即可安装octomap相关库。安装完之后可以在/opt/ros/noetic/share路径下查看。特别需要注意ros-noetic-octomap-rviz-plugins，该插件可以使地图在Rviz中显示。

sudo apt-get install ros-noetic-octomap 
sudo apt-get install ros-noetic-octomap-mapping 
sudo apt-get install ros-noetic-octomap-msgs 
sudo apt-get install ros-noetic-octomap-ros 
sudo apt-get install ros-noetic-octomap-rviz-plugins
sudo apt-get install ros-noetic-octomap-server

2 创建工作空间

#include <ros/ros.h>  

/* octomap 相关头文件 */
#include <octomap/ColorOcTree.h>            // 引入颜色Octree的相关定义
#include <octomap/Pointcloud.h>             // 引入点云的定义
#include <octomap/OcTree.h>                 // 引入Octree的定义

/* octomap_msgs 相关头文件 */
#include <octomap_msgs/Octomap.h>  
#include <octomap_msgs/conversions.h>       // 用于Octomap与Ros消息之间的消息转换

int main(int argc, char *argv[])  
{
    ros::init(argc, argv, "octomap_node");  
    ros::NodeHandle nh;  
    ros::Publisher octomap_pub = nh.advertise<octomap_msgs::Octomap>("octomap_full", 1, true);

    double resolution = 0.1;                // 设置Octree的分辨率为0.1米
    octomap::OcTree octree(resolution);     // 创建一个OcTree对象，使用指定的分辨率

    // 1、可以自行设置一些节点
    octree.updateNode(octomap::point3d(0.5, 0.5, 0.5), true); // true表示该位置被占据
    octree.updateNode(octomap::point3d(-0.5, -0.5, -0.5), true); 
    octree.updateNode(octomap::point3d(1.0, 1.0, 1.0), true); 

    // 2、也可以接收点云信息
    octomap::Pointcloud pointcloud; 
    pointcloud.push_back(0.0, 0.0, 0.0);  
    pointcloud.push_back(1.0, 0.0, 0.0);  
    pointcloud.push_back(0.0, 1.0, 0.0);  
    pointcloud.push_back(0.0, 0.0, 1.0);  
    // 遍历点云中的每一个点，并将其更新到Octree中
    for (size_t i = 0; i < pointcloud.size(); i++)
    {
        octree.updateNode(pointcloud.getPoint(i), true);  
    }
    
    octomap_msgs::Octomap octomap_msg; 
    octomap_msgs::fullMapToMsg(octree, octomap_msg);  // 将Octree转换为Octomap消息格式
    octomap_msg.header.frame_id = "map";              // 设置消息的坐标系为“map”
    octomap_msg.header.stamp = ros::Time::now();      // 设置消息的时间戳为当前时间

    while(ros::ok())  
    {
        octomap_pub.publish(octomap_msg);  
        ros::spinOnce();  
    }
    
    return 0;  
}

2.1 创建包

添加的依赖为：rospy roscpp std_msgs octomap_ros octomap_msgs

mkdir -p demo1_octomap_ws/src
cd demo1_octomap
catkin_make
cd src
catkin_create_pkg octomap_pkg rospy roscpp std_msgs octomap_ros octomap_msgs

2.2 编写.cpp文件

#include <ros/ros.h>  

/* octomap 相关头文件 */
#include <octomap/ColorOcTree.h>            // 引入颜色Octree的相关定义
#include <octomap/Pointcloud.h>             // 引入点云的定义
#include <octomap/OcTree.h>                 // 引入Octree的定义

/* octomap_msgs 相关头文件 */
#include <octomap_msgs/Octomap.h>  
#include <octomap_msgs/conversions.h>       // 用于Octomap与Ros消息之间的消息转换

int main(int argc, char *argv[])  
{
    ros::init(argc, argv, "octomap_node");  
    ros::NodeHandle nh;  
    ros::Publisher octomap_pub = nh.advertise<octomap_msgs::Octomap>("octomap_full", 1, true);

    double resolution = 0.1;                // 设置Octree的分辨率为0.1米
    octomap::OcTree octree(resolution);     // 创建一个OcTree对象，使用指定的分辨率

    // 1、可以自行设置一些节点
    octree.updateNode(octomap::point3d(0.5, 0.5, 0.5), true); // true表示该位置被占据
    octree.updateNode(octomap::point3d(-0.5, -0.5, -0.5), true); 
    octree.updateNode(octomap::point3d(1.0, 1.0, 1.0), true); 

    // 2、也可以接收点云信息
    octomap::Pointcloud pointcloud; 
    pointcloud.push_back(0.0, 0.0, 0.0);  
    pointcloud.push_back(1.0, 0.0, 0.0);  
    pointcloud.push_back(0.0, 1.0, 0.0);  
    pointcloud.push_back(0.0, 0.0, 1.0);  
    // 遍历点云中的每一个点，并将其更新到Octree中
    for (size_t i = 0; i < pointcloud.size(); i++)
    {
        octree.updateNode(pointcloud.getPoint(i), true);  
    }
    
    octomap_msgs::Octomap octomap_msg; 
    octomap_msgs::fullMapToMsg(octree, octomap_msg);  // 将Octree转换为Octomap消息格式
    octomap_msg.header.frame_id = "map";              // 设置消息的坐标系为“map”
    octomap_msg.header.stamp = ros::Time::now();      // 设置消息的时间戳为当前时间

    while(ros::ok())  
    {
        octomap_pub.publish(octomap_msg);  
        ros::spinOnce();  
    }
    
    return 0;  
}

2.3 编写CMakeLists 文件

cmake_minimum_required(VERSION 3.0.2)
project(octomap_pkg)

find_package(catkin REQUIRED COMPONENTS
  octomap_ros
  roscpp
  rospy
  std_msgs
  octomap_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES octomap_pkg
#  CATKIN_DEPENDS octomap_ros roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(octomap_node src/octomap_node.cpp)
target_link_libraries(octomap_node ${catkin_LIBRARIES})

3 实现效果

运行代码，打开rviz，按照如下步骤选择消息类型并且订阅话题。

以下是程序效果