赞
踩
一个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可视化。实现这三个功能,需要做如下的准备:
安装pcl
sudo apt install libpcl-dev
再安装pcl_msgs(不知道为什么pcl_msgs不在pcl包里),中间的melodic替换为自己的ros版本。
sudo apt-get install ros-melodic-pcl-msgs
这个pcl_conversions.h文件放到自己项目的include文件夹中, 后面回用到这个头文件里的一个函数 fromROSMsg,函数 fromROSMsg依赖pcl_msgs,其功能是将ros topic的sensor_msgs::PointCloud2::ConstPtr 数据转化为 pcl::PointCloud<pcl::PointXYZ> 数据。
安装octomap
- git clone https://github.com/OctoMap/octomap.git
- cd octomap
- mkdir build
- cd build
- cmake ..
- make
- sudo make install
下载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。
到这里,准备工作都完成了。
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
完成!
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。