带颜色的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下的对应发布的点云名字
即可看到发布的带有颜色的点云图
在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/diannao/31702.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

TikTok 推出专属AI 内容工具

TikTok最近推出了一款极具实用性的新工具包——TikTok Symphony。它融合了生成式人工智能技术&#xff0c;让内容创作变得更加迅速和便捷。 无论是营销人员还是创作者&#xff0c;都能在TikTok上轻松制作出高质量的内容。Symphony将人类的创造力与AI的高效性完美融合&#xff0…

市值3万亿英伟达的崛起:技术、坚持与市场的力量,厚积薄发的经典案例

在科技领域&#xff0c;英伟达&#xff08;NVIDIA&#xff09;的故事无疑是一个厚积薄发的经典案例。作为一家专注于图形处理单元&#xff08;GPU&#xff09;的公司&#xff0c;英伟达用31年的时间证明了技术的价值、计算的价值和坚持的价值。本文将详细探讨英伟达如何从一家市…

算法题day48(补6.3日卡:dp07)

一、多重背包理论基础&#xff1a; 有N种物品和一个容量为V 的背包。第i种物品最多有Mi件可用&#xff0c;每件耗费的空间是Ci &#xff0c;价值是Wi 。求解将哪些物品装入背包可使这些物品的耗费的空间 总和不超过背包容量&#xff0c;且价值总和最大。 其实就是变相的01背包…

python-Langchain-Chatchat+ChatGLM2-6B在16G内存电脑上运行

python-Langchain-Chatchat+ChatGLM2-6B在16G内存电脑上运行 下载代码ChatGLMLangchain-Chatchat环境准备下载模型Langchain-Chatchat配置configs中example文件修改configs/model_config.py修改修改 server配置知识库初始化启动项目遇到的报错参考链接下载代码 ChatGLM https…

如何系列 如何学习外企英语(持续学习中...)

文章目录 背景计划相关软件和资料单词 短语 背景 软件开发行业&#xff0c;想进外企或者已在外企进行英语提升。 计划 1.每天最少30分钟多邻国。 2.记录平常工作中遇到的句子和单词&#xff0c;并给出分析和示例。 3.把手机和电脑等都调成英文。 4.听 podcast 磨耳朵(pod…

【Go】十四、图形验证码、短信验证码、注册接口与redis的简单使用

图形验证码 如何嵌入图形验证码工作&#xff1a; 这里选择使用captcha 开源库进行验证码设计&#xff1a; 选用下面的地址进行验证码开发工作 https://zh.mojotv.cn/go/refactor-base64-captcha 基础功能构建 在 api 目录下创建 captcha.go 用来编写验证码操作 package …

车辆轨迹预测系列 (二):常见数据集介绍

车辆轨迹预测系列 (二)&#xff1a;常见数据集介绍 文章目录 车辆轨迹预测系列 (二)&#xff1a;常见数据集介绍1、NuScenes (2020)&#xff1a;1、下载2、说明 2、Waymo Open Dataset (2020)&#xff1a;1、介绍2、概述3、下载4、教程5、参考 3、Lyft Level 5 (2020)&#xff…

VOC数据集

VOC&#xff08;Visual Object Classes&#xff09;格式的数据集是一种用于计算机视觉任务的标准数据集格式&#xff0c;它最初是由Pascal VOC&#xff08;PASCAL Visual Object Classes&#xff09;数据集引入的。VOC数据集格式定义了一套标准化的数据集结构&#xff0c;包括X…

SD-WAN为什么适合小企业

SD-WAN&#xff08;软件定义广域网&#xff09;是一种革新性的网络技术&#xff0c;通过软件智能管理&#xff0c;实现灵活和高效的网络连接。在数字化转型浪潮中&#xff0c;企业对网络稳定性和性能的要求不断提升&#xff0c;SD-WAN因此受到了广泛关注。对于资源有限的小型企…

JAVA NIO(二) Buffer和Channel

一&#xff0c;基本使用 1&#xff0c; 一个Socket连接使用一个Channel来表示&#xff0c;以前直接操作Socket文件描述符来对读写缓冲区操作&#xff0c;比如读数据到用户空间的一个byte数组&#xff0c;NIO中Channel对这个过程作了封装&#xff0c;其中用户空间的byte数组就类…

macbook rust项目编译跨平台windows,linux

创建项目 cargo new test11, 编译windows exe包 rustup target add x86_64-pc-windows-gnu brew install mingw-w64 cargo build --target i686-pc-windows-gnu2. 编译ubuntu linux包 brew install FiloSottile/musl-cross/musl-cross rustup target add x86_64-unknown-lin…

Elk安装及使用

es安装及使用 单机版安装 集群安装 132 node-01 133 node-02 135 node-03 日志用户权限有问题 看日志 解决方案&#xff1a; 出现错误后&#xff0c;再次重启前&#xff0c;需要删除三个节点/data/下的内容 9300-http 9300-tcp logstasha安装及使用 Ssh错误 Yum安装默认路…

2024-06-21力扣每日一题

链接&#xff1a; LCP 61. 气温变化趋势 题意 A、B两个数组&#xff0c;数组内相邻两个数字有大于、等于、小于三种变化情况&#xff0c;求最长的一段&#xff0c;使两个数组的这一段变化情况相同&#xff0c;并且不要求这一段只能有一种变化 解&#xff1a; 因为数组内只…

UBUNTU安装KVM并加速安卓模拟器

在 Linux 上安装安卓模拟器&#xff08;如 Genymotion 或 Android Studio 的内置模拟器&#xff09;通常需要硬件 虚拟化支持&#xff0c;这可以通过 KVM 完成。本人亲测在ubuntu24.04上可用&#xff0c;但是是不是真的有加速效果不知&#xff0c;毕竟该卡的时候还是卡。 1.安…

逻辑回归(Logistic Regression)及其在机器学习中的应用

&#x1f680;时空传送门 &#x1f50d;逻辑回归原理&#x1f4d5;Sigmoid函数&#x1f388;逻辑回归模型 &#x1f4d5;损失函数与优化&#x1f388;损失函数&#x1f680;优化算法 &#x1f50d;逻辑回归的应用场景&#x1f340;使用逻辑回归预测客户流失使用scikit-learn库实…

Golang学习笔记02

封装 将结构体&#xff0c;字段的属性都小写&#xff0c;类似于private 给结构体提供一个工厂模式的函数&#xff0c;首字母大写 提供一个首字母大写的set方法&#xff0c;对属性赋值 提供一个首字母大写的get方法&#xff0c;对属性访问 package mainimport "fmt"ty…

全外显子测序分析流程1 - Fastq质控与去接头、低质量和引物序列

全外显子测序分析流程1 - Fastq质控与去接头、低质量和引物序列 1. 运行实例 # -d 样本根目录 # -s 样本名称 python trim_fastq.py -d /result/WES/sample -s sample2. fastqc质控报告与去接头、低质量序列主程序 对raw fastq和clean fastq生成质控QC报告trim_galore去接头、…

Avalonia:一个.NET跨平台UI框架

概述 Avalonia是一个强大的框架&#xff0c;使开发人员能够使用. NET创建跨平台应用程序。它使用自己的渲染引擎来绘制UI控件&#xff0c;确保在各种平台上保持一致的外观和行为&#xff0c;包括Windows&#xff0c;macOS&#xff0c;Linux&#xff0c;Android&#xff0c;iOS…

Linux命令详解

Linux关机重启 命令解释shutdown -h now或poweroff立刻关机shutdown -h 11分钟后关机&#xff0c;不带参数默认1分钟shutdown -r now立刻重启halt立刻关机reboot立刻重启sync把内存的数据同步到磁盘&#xff0c;关机和重启前应该先做这一步&#xff0c;目前shutdown、reboot、…

深入理解RunLoop

RunLoop 是 iOS 和 OSX 开发中非常基础的一个概念&#xff0c;这篇文章将从 CFRunLoop 的源码入手&#xff0c;介绍 RunLoop 的概念以及底层实现原理。之后会介绍一下在 iOS 中&#xff0c;苹果是如何利用 RunLoop 实现自动释放池、延迟回调、触摸事件、屏幕刷新等功能的。 一…