带颜色的3D点云数据发布到ros1中(通过rviz显示)python、C++

2024-06-21 23:04

本文主要是介绍带颜色的3D点云数据发布到ros1中(通过rviz显示)python、C++,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

ros中发布点云数据xyz以及带颜色的点云数据xyzrgb

  • ros中发布点云数据xyz
    • 可以直接用python来做或者C++(看个人偏好)
  • ros中发布带颜色的点云数据xyzrgb
    • 环境
    • 1.新建ROS工作空间
    • 2.创建功能包

ros中发布点云数据xyz

可以直接用python来做或者C++(看个人偏好)

在这里我们带有颜色的点云数据格式为x y z c
其中c值为float型,有四种值1.0,2.0,3.0,4.0
在这里插入图片描述

代码文件b.py,具体内容如下:

import rospy
from sensor_msgs.msg import Image,PointCloud2
from cv_bridge import CvBridge
import numpy as np
import os
import cv2
from a import *
import open3d as o3dDATA_PATH='/home/cxh/project/0618/point_cloud_selected.txt'
if __name__=='__main__':rospy.init_node('jizhui_node',anonymous=True)# cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)pcl_pub=rospy.Publisher('/jizhui_pcl',PointCloud2,queue_size=1)#创建点云发布者,queue_size=10表示消息队列的大小bridge=CvBridge()#创建一个CvBridge实例,用于在OpenCV图像与ROS图像消息之间进行转换# rate=rospy.Rate(1)#设置发布频率为10Hzrate=rospy.Rate(5,reset=True)frame=0#初始化帧计数器def load_point_cloud(file_path):"""从文本文件中加载点云数据"""point_cloud = []color=[]with open(file_path, 'r') as f:for line in f:if line.strip():  # 忽略空行try:# 假设每行的格式为: x y zx, y, z, c= map(lambda x: round(float(x) / 100, 5) if x != line.strip().split()[-1] else float(x), line.strip().split())point_cloud.append([x, y, z])#读取c的值,并把c的值映射成对应RGB值# 1.0:灰色[220,220,220]# 2.0:蓝色[173,216,230]# 3.0:黄色[255,215,0]# 4.0:红色[255,182,193]# r, g, b = color_mapping(c)# color.append([r, g, b])# print("x y z",point_cloud)except ValueError:rospy.logerr("Error parsing line: {}".format(line))return np.array(point_cloud)# 循环播放,通过frame控制帧数while not rospy.is_shutdown():#主循环在ROS节点关闭前一直运行# 读取点云数据    point_cloud= load_point_cloud(DATA_PATH)#,color# print("shape:",point_cloud.shape)#49行3列publish_pcl(point_cloud,pcl_pub,'map')#调用publish_pcl函数将点云数据发布到ROS主题jizhui_pcl。color,# pcl_pub.publish(pcl2_msg)rospy.loginfo('published')#在日志中记录发布信息rate.sleep()#按照设定的频率进行休眠

文件a.py具体内容如下:

from sensor_msgs.msg import PointCloud2,PointField
import sensor_msgs.point_cloud2 as pcl2from std_msgs.msg import Header
import rospy
import numpy as np
def publish_pcl(point_cloud,pcl_pub,frame_id):#定义点云数据的ROS发布者。if point_cloud.size == 0:rospy.logwarn("Empty point cloud data, skipping publish.")returnheader=Header()header.stamp=rospy.Time.now()header.frame_id=frame_idpoint_cloud_message=pcl2.create_cloud_xyz32(header,point_cloud)pcl_pub.publish(point_cloud_message)

发布到ros的步骤如下

#启动 ROS master
#启动一个终端键入
roscore
#在python文件b.py下运行代码
python b.py
#再另起一个终端键入
rviz

启动了 RViz 后点击界面左下角的Add按钮并添加一个 PointCloud2 显示
即可在 RViz 中看到点云了
**注意:**对于发布带颜色的点云数据,由于python版没有creat_xyzrgb32 ,这个功能函数只有C++有,python的需要自己写一个这样的功能函数。我本人也参考b站博主学习视频链接: 用python将着色点云在ros中发布—解析PointCloud2与Rviz可视化源码弄了一下午没成功,就改用C++来做了!!
(ps这里还有一个特别需要注意的点,就是有的点云图很大且高,小窗口下不容易发现,我们一开始就因为这个问题郁闷了很久,一遍遍检查代码,后来发现早就生成了,只是没有吧窗口放大,多拖拉几下就能找到躲在角落处的点云图!!!)

ros中发布带颜色的点云数据xyzrgb

环境

环境:
Ubuntu20.04
ROS noetic
C++

1.新建ROS工作空间

mkdir -p PointCloundShow_ws/src
cd PointCloundShow_ws/src
catkin_init_workspace
cd ..
catkin_make 

2.创建功能包

cd src
catkin_create_pkg pointcloundshow pcl_ros roscpp rospy sensor_msgs std_msgs 

在功能包的src文件夹下新建cpp文件pcl_colored.cpp
代码文件pcl_colored.cpp内容如下:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <fstream>
#include <sstream>
#include <vector>
#include <iostream>
#include <string>using namespace std;
uint32_t color_mapping(float c) {if (c == 1.0) {return (220 << 16) | (220 << 8) | 220;  // 灰色} else if (c == 2.0) {return (173 << 16) | (216 << 8) | 230;  // } else if (c == 3.0) {return (255 << 16) | (215 << 8) | 0;  // } else {return (255 << 16) | (182 << 8) | 193;  // }
}void readPointCloudFromFile(const string& filename, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud) {ifstream infile(filename);if (!infile.is_open()) {cerr << "Could not open file: " << filename << endl;return;}string line;while (getline(infile, line)) {istringstream iss(line);float x, y, z, c;if (!(iss >> x >> y >> z >> c)) {cerr << "Error reading line: " << line << endl;continue;}// 缩小 x, y, z 的值100倍x /= 100.0f;y /= 100.0f;z /= 100.0f;pcl::PointXYZRGB point;point.x = x;point.y = y;point.z = z;uint32_t rgb = color_mapping(c);point.rgb = *reinterpret_cast<float*>(&rgb);cloud->points.push_back(point);}cloud->width = cloud->points.size();cloud->height = 1;cloud->is_dense = true;infile.close();
}
int main(int argc, char** argv) {// 初始化ROS节点ros::init(argc, argv, "pcl_create_xyzrgb");ros::NodeHandle nh;// 创建一个发布者ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output_colored", 1);// 创建一个点云对象pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());// 从文件读取点云数据string filename = "/home/cxh/project/0618/point_cloud_selected.txt";readPointCloudFromFile(filename, cloud);// 将点云数据转换为ROS消息sensor_msgs::PointCloud2 output;pcl::toROSMsg(*cloud, output);output.header.frame_id = "map";// 发布点云消息ros::Rate loop_rate(1);while (ros::ok()) {output.header.stamp = ros::Time::now();pcl_pub.publish(output);ros::spinOnce();loop_rate.sleep();}return 0;
}

然后将下面的编译规则写到功能包的CMakeLists.txt文件中

find_package(PCL REQUIRED) 
include_directories(include${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS}) 
add_executable(pcl_colored.cpp src/pcl_colored.cpp)
target_link_libraries(pcl_colored.cpp ${catkin_LIBRARIES} ${PCL_LIBRARIES})

翻倒CMakeLists.txt文件的最后一行添加,我们的如下:
在这里插入图片描述
如图所示,因为我们用C++写了两个文件,一个是pcl_create.cpp另一个是目前的pcl_colored.cpp,所以我们这个是追加到后面的,供参考!

回到工作空间路径下也就是PointCloundShow_ws,输入catkin_make进行编译
然后再更新一下:source devel/setup.bash(**注意:**只要你修改过你工作空间任何一处代码,每次都需要重新编译和更新!!!另外每新建了一个cpp文件都需要在CMakeLists.txt文件做添加!!!)
然后新起一个终端终端运行roscore指令,
再在刚的source命令那个终端运行rosrun pointcloundshow pcl_create
最后再另起一个终端允许rviz指令

打开rviz
在rviz中左下角点Add增加PointCloud2d
topic 选 /pcl_output
fixed Frame 输入odom
或者直接点左下角的Add然后会弹出一个名字为rviz的框,选择By topic下的对应发布的点云名字
即可看到发布的带有颜色的点云图
在这里插入图片描述

这篇关于带颜色的3D点云数据发布到ros1中(通过rviz显示)python、C++的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1082588

相关文章

使用Python从PPT文档中提取图片和图片信息(如坐标、宽度和高度等)

《使用Python从PPT文档中提取图片和图片信息(如坐标、宽度和高度等)》PPT是一种高效的信息展示工具,广泛应用于教育、商务和设计等多个领域,PPT文档中常常包含丰富的图片内容,这些图片不仅提升了... 目录一、引言二、环境与工具三、python 提取PPT背景图片3.1 提取幻灯片背景图片3.2 提取

C++如何通过Qt反射机制实现数据类序列化

《C++如何通过Qt反射机制实现数据类序列化》在C++工程中经常需要使用数据类,并对数据类进行存储、打印、调试等操作,所以本文就来聊聊C++如何通过Qt反射机制实现数据类序列化吧... 目录设计预期设计思路代码实现使用方法在 C++ 工程中经常需要使用数据类,并对数据类进行存储、打印、调试等操作。由于数据类

Python实现图片分割的多种方法总结

《Python实现图片分割的多种方法总结》图片分割是图像处理中的一个重要任务,它的目标是将图像划分为多个区域或者对象,本文为大家整理了一些常用的分割方法,大家可以根据需求自行选择... 目录1. 基于传统图像处理的分割方法(1) 使用固定阈值分割图片(2) 自适应阈值分割(3) 使用图像边缘检测分割(4)

一文带你搞懂Python中__init__.py到底是什么

《一文带你搞懂Python中__init__.py到底是什么》朋友们,今天我们来聊聊Python里一个低调却至关重要的文件——__init__.py,有些人可能听说过它是“包的标志”,也有人觉得它“没... 目录先搞懂 python 模块(module)Python 包(package)是啥?那么 __in

使用Python实现图像LBP特征提取的操作方法

《使用Python实现图像LBP特征提取的操作方法》LBP特征叫做局部二值模式,常用于纹理特征提取,并在纹理分类中具有较强的区分能力,本文给大家介绍了如何使用Python实现图像LBP特征提取的操作方... 目录一、LBP特征介绍二、LBP特征描述三、一些改进版本的LBP1.圆形LBP算子2.旋转不变的LB

Python中__init__方法使用的深度解析

《Python中__init__方法使用的深度解析》在Python的面向对象编程(OOP)体系中,__init__方法如同建造房屋时的奠基仪式——它定义了对象诞生时的初始状态,下面我们就来深入了解下_... 目录一、__init__的基因图谱二、初始化过程的魔法时刻继承链中的初始化顺序self参数的奥秘默认

SpringBoot使用GZIP压缩反回数据问题

《SpringBoot使用GZIP压缩反回数据问题》:本文主要介绍SpringBoot使用GZIP压缩反回数据问题,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录SpringBoot使用GZIP压缩反回数据1、初识gzip2、gzip是什么,可以干什么?3、Spr

Python实现特殊字符判断并去掉非字母和数字的特殊字符

《Python实现特殊字符判断并去掉非字母和数字的特殊字符》在Python中,可以通过多种方法来判断字符串中是否包含非字母、数字的特殊字符,并将这些特殊字符去掉,本文为大家整理了一些常用的,希望对大家... 目录1. 使用正则表达式判断字符串中是否包含特殊字符去掉字符串中的特殊字符2. 使用 str.isa

Linux下如何使用C++获取硬件信息

《Linux下如何使用C++获取硬件信息》这篇文章主要为大家详细介绍了如何使用C++实现获取CPU,主板,磁盘,BIOS信息等硬件信息,文中的示例代码讲解详细,感兴趣的小伙伴可以了解下... 目录方法获取CPU信息:读取"/proc/cpuinfo"文件获取磁盘信息:读取"/proc/diskstats"文

python中各种常见文件的读写操作与类型转换详细指南

《python中各种常见文件的读写操作与类型转换详细指南》这篇文章主要为大家详细介绍了python中各种常见文件(txt,xls,csv,sql,二进制文件)的读写操作与类型转换,感兴趣的小伙伴可以跟... 目录1.文件txt读写标准用法1.1写入文件1.2读取文件2. 二进制文件读取3. 大文件读取3.1