点云数据 bag_to_pcd sensor_msgs.point_cloud2 from rosbags.image import message_to_cvimage 图像到bag中
Linux环境
Ubuntu 16 xenial Ubuntu 18 bionic Ubuntu 20 focal
Debian 10 Buster Debian 11 bullseye
ROS中表示点云的数据结构有:
sensor_msgs::PointCloud
sensor_msgs::PointCloud2
pcl::PointCloud
Ros中表示图像的数据结构 sensor_msgs/Image sensor_msgs/CompressedImage
命令:
pcl_ros
bag_to_pcd pointcloud_to_pcd
image_view
extract_images image_saver
点云的命令行提取
rosrun pcl_ros bag_to_pcd output.bag /com /opt/pcd
方法二:pointcloud_to_pcd
一个终端通过ros发送messages,如:$ rosbag play XXX.bag
另一个终端接收, 如:$ rosrun pcl_ros pointcloud_to_pcd input:=/velodyne_points
图像的命令提取
用于从sensor_msgs/Image流中将图片保存为jpg/png格式。
rosrun image_view extract_images _sec_per_frame:=0.01 image:=/fr/image_raw
rosrun image_view image_saver _encoding:='bgr8' _filename_format:=%s image:=/fr/image_raw/compressed
从a compressed image stream 提取 /fr/image_raw/compressed
rosrun image_view image_view image:=/camera/image _image_transport:=compressed
rosrun image_view image_saver filename_format:=left%04 d.%s image:=/fr/image_raw _image_transport:=compressed
通过ros自带功能包 image_view 中的video来提取视频
rosrun image_view video_recorder _fps:=30 image:=/velodyne/image _filename:=output.avi
ROS1镜像运行
docker pull osrf/ros:noetic-desktop-full 或者 docker pull osrf/ros:melodic-desktop-full
docker run --name ros-pipe --restart always --add-host test.com:10.10.10.10 \
-v /opt/code/workflow:/opt/code/workflow \
-it osrf/ros:noetic-desktop-full /bin/bash
确定是否有 roscore,可以搜索 whereis roscore
如果没有的话,执行 source /ros_entrypoint.sh
roscore&
1.查看bag包信息
rosbag info output.bag
2.提取点云数据命令行-- bag文件转PCD文件
http://wiki.ros.org/pcl_ro
rosrun pcl_ros bag_to_pcd output.bag /com /opt/pcd
其他等价
方法二:pointcloud_to_pcd
一个终端通过ros发送messages,如:$ rosbag play XXX.bag
另一个终端接收, 如:$ rosrun pcl_ros pointcloud_to_pcd input:=/velodyne_points
3.rostopic list -v
4.删除roscore进程
killall -9 roscore
killall -9 rosmaster
roscore&
5.端口占用
lsof -i:66
lsof -i:77
fuser -v 66/udp 77/udp
source /ros_entrypoint.sh
/ros_entrypoint.sh
set -e
source "/opt/ros/$ROS_DISTRO /setup.bash"
exec "$@ "
其中
/opt/ros/noetic/setup.bash
CATKIN_SHELL=bash
_CATKIN_SETUP_DIR=$(builtin cd "`dirname " ${BASH_SOURCE[0]} "`" > /dev/null && pwd )
. "$_CATKIN_SETUP_DIR /setup.sh"
转换逻辑
01 .使用cv_bridge将ROS图像转换为OpenCV图像,反之亦然,从而实现ROS和OpenCV的交互
CvBridge是一个ROS库,提供ROS和OpenCV之间的接口
要将ROS图像消息转换为cv :: Mat,模块cv_bridge.CvBridge提供以下功能
cv_image = bridge.imgmsg_to_cv2(image_message, desired_encoding="bgr8" )
将OpenCV图像转换为ROS图像消息
image_message = cv2_to_imgmsg(cv_image, encoding="passthrough" )
02 . rosrun pcl_ros bag_to_pcd output.bag /com /opt/pcd
"/opt/ros/noetic/lib/pcl_ros/bag_to_pcd {} {} {}" .format(bag_path_nm, pc_topic, pcd_out_path) )
介绍几个ROS节点运行的几种工具。他们的作用是ROS格式点云或包与点云数据(PCD)文件格式之间的相互转换。
(1 )bag_to_pcd
用法:rosrun pcl_ros bag_to_pcd <input_file.bag> <topic> <output_directory>
读取一个包文件,保存所有ROS点云消息在指定的PCD文件中。
(2 )convert_pcd_to_image
用法:rosrun pcl_ros convert_pcd_to_image <cloud.pcd>
加载一个PCD文件,将其作为ROS图像消息每秒中发布五次。
(3 ) convert_pointcloud_to_image
用法:rosrun pcl_ros convert_pointcloud_to_image input:=/my_cloud output:=/my_imag
查看图像:rosrun image_view image_view image:=/my_image
订阅一个ROS的点云的话题并以图像的信息发布出去。
(4 )pcd_to_pointcloud
用法:rosrun pcl_ros pcd_to_pointcloud <file .pcd> [ <interval> ]
<file .pcd> is the (required) file name to read.
<interval> is the (optional) number of seconds to sleep between messages. If <interval> is zero or not specified the message is published once.
加载一个PCD文件,发布一次或多次作为ROS点云消息
(5 )pointcloud_to_pcd
例如: rosrun pcl_ros pointcloud_to_pcd input:=/velodyne/pointcloud2
订阅一个ROS的话题和保存为点云PCD文件。每个消息被保存到一个单独的文件,名称是由一个可自定义的前缀参数,ROS时间的消息,和以PCD扩展的文件
代码示例
"""" 以def 开头 和以return 结尾的中间所有行"""
def get_between_def (file_nm_path ):
with open (file_nm_path,mode='r' ,encoding='utf8' ) as fobj:
start_flag = False
for num, line in enumerate (fobj):
line = line.rstrip()
if 'def' in line.rstrip() or start_flag:
start_flag = True
print (line)
if "return" in line.strip():
start_flag = False
print ("hello world ##############" ,num)
return start_flag
if __name__ == "__main__" :
file_nm= r"D:\Imag\flow\testFile.txt"
get_between_def(file_nm)
引入第三方标识--通过改变标识来改变逻辑
通过【subprocess.getstatusoutput】获得shell返回结
o3d.t.io.write_point_cloud( save_file, pcd, write_ascii=False )
使用python提取
rosbags-image cv_bridge
CvBridge.compressed_imgmsg_to_cv2
CvBridge.imgmsg_to_cv2
python 包说明
pip install rosbags-image
from rosbags.image import message_to_cvimage
msg = ...
img = message_to_cvimage(msg)
img = message_to_cvimage(msg, 'bgr8')
代码示例
# -*- coding: utf-8 -*-
import os
import argparse
import cv2
import open3d as o3d
import numpy as np
import rosbag
import sensor_msgs.point_cloud2 as point_cloud2
from rosbags.image import message_to_cvimage
from tqdm import tqdm
def extract_bag_pcd (bag_path):
bag = rosbag.Bag (bag_path)
pcd_path = os.path.join (os.path.splittext (bag_path)[0 ],"pcd" )
if not os.path.exists (pcd_path):
os.makedirs (pcd_path)
topic = ['/points' ]
for topic, msg, t in tqdm (bag.read_messages (topics=topic)):
msg_stamp = str (msg.header.stamp)
pcd_file = os.path.join (pcd_path, '{}.{}.pcd' .format (msg_stamp[0 :10 ], msg_stamp[10 :-3 ]))
cloud = np.asarray (list (point_cloud2.read_points (msg, field_names=['x' , 'y' , 'z' , "intensity" ])))
pcd_obj = o3d.t.geometry.PointCloud ()
pcd_obj.point['positions' ] = o3d.core.Tensor (cloud[:, :3 ],dtype=o3d.core.Dtype.Float32)
pcd_obj.point['intensity' ] = o3d.core.Tensor (cloud[:, 3 ].reshape (-1 ,1 ),dtype=o3d.core.Dtype.Float32)
o3d.t.io.write_point_cloud ( pcd_file, pcd_obj, write_ascii=False)
bag.close ()
def extract_bag_image (bag_path):
bag = rosbag.Bag (bag_path)
image_path = os.path.join (os.path.splittext (bag_path)[0 ],"image" )
if not os.path.exists (image_path):
os.makedirs (image_path)
topics = ['/tt/image_raw' ,'/tt/image_raw' ]
for topic, msg, t in tqdm (bag.read_messages (topics=topics)):
msg_stamp = str (msg.header.stamp)
image_dir = os.path.join (image_path, topic.split ('/' )[1 ])
if not os.path.exists (image_dir):
os.makedirs (image_dir)
image_file = os.path.join (image_dir, '{}.{}.jpg' .format (msg_stamp[0 :10 ], msg_stamp[10 :-3 ]))
cv_image = message_to_cvimage (msg,color_space="bgr8" )
cv2.imwrite (image_file, cv_image)
bag.close ()
if __name__ == "__main__" :
parser = argparse.ArgumentParser ( description='bag' )
parser.add_argument ('--bag_path' , type=str, default='/data/1.bag' , help='bag path' )
args = parser.parse_args ()
exp_bag = args.bag_path
extract_pcd (exp_bag)
extract_image (exp_bag)
排查
1. 版本
python3 -c '''
import open3d
print(open3d.__version__)
'''
2. open3d Warning pointcloud contains intensity attribute which is not supported by PCD IO Only
open3d warning write pcd:skipping attribute
open3d.cpu.pybind.t.geometry.PointCloud
open3d.cpu.pybind.t.geometry.TensorMap
结论:reshape(-1 ,1 )是将一维数据在行上变化,而reshape(1 ,-1 )是将一维数据在列上变化
[1 ,2 ,3 ]都是 (3 ,)如果想要得到 (3 ,1 ) 或者 (1 ,3 ) 的 shape,需要使用 reshape 方法
如果 想得到 (3 ,1 ) ,调用 a.reshape(-1 , 1 )
第一个形状为(3 ,)的数组是一维数组。它的结构应该与此类似:a=[1 ,2 ,3 ]
第二个形状为(3 ,1 )。 类似于b=[[1 ],[2 ],[3 ]]
代码参考
### version 16.0
import open3d as o3d
device = o3d.core.Device("CPU:0" )
dtype = o3d.core.float32
pcd = o3d.t.geometry.PointCloud(device)
pcd.point.positions = o3d.core.Tensor([[0, 0, 0],
[1, 1, 1],
[2, 2, 2]] , dtype, device)
pcd.point.colors = o3d.core.Tensor([[0.0, 0.0, 0.0],
[0.1, 0.1, 0.1],
[0.2, 0.2, 0.2]] , dtype, device)
# User-defined attributes.
pcd.point.intensities = o3d.core.Tensor([0.3 , 0.1 , 0.4 ], dtype, device)
pcd.point.intensity = o3d.core.Tensor([[0.3], [0.1], [0.4]] , dtype, device)
pcd_file="/data/cloud/test.pcd"
o3d.t.io .write_point_cloud( pcd_file, pcd, write_ascii=False)
###version 15.0
import open3d as o3d
device = o3d.core.Device("CPU:0" )
dtype = o3d.core.float32
pcd = o3d.t.geometry.PointCloud(device)
pcd.point["positions" ] = o3d.core.Tensor([[0, 0, 0],
[1, 1, 1],
[2, 2, 2]] , dtype, device)
pcd.point["colors" ] = o3d.core.Tensor([[0.0, 0.0, 0.0],
[0.1, 0.1, 0.1],
[0.2, 0.2, 0.2]] , dtype, device)
# User-defined attributes.
pcd.point["intensities" ] = o3d.core.Tensor([0.3 , 0.1 , 0.4 ], dtype, device)
pcd.point["intensity" ] = o3d.core.Tensor([[0.3], [0.1], [0.4]] , dtype, device)
pcd_file="/data/cloud/test.pcd"
o3d.t.io .write_point_cloud( pcd_file, pcd, write_ascii=False)
# [Open3d Warning] Write PCD: Skipping intensities attribute.
pointcloud contains intensity attribute which is not supported by PCD IO
Only points,normal,colors
and attributes with shape(num_points,1 ) are supported
Expected shape:{3 ,1 }but got{3 }
cv_bridge用于ROS图像和OpenCV图像的转换
ROS以自己的sensor\msgs/Image消息格式传递图像,
希望将图像与OpenCV结合使用。在OpenCV中,图像以Mat矩阵的形式存储,
与ROS定义的图像消息的格式有一定的区别,CvBridge是一个ROS库,提供ROS和OpenCV之间的接口
msg = ...
img = message_to_cvimage(msg)
img = message_to_cvimage(msg, 'bgr8')
https://gitlab.com/ternaris/rosbags-image
CvBridge识别以下Bayer pattern encodings:
bayer_rggb8
bayer_bggr8
bayer_gbrg8
bayer_grbg8
CvBridge可以根据需要进行color或像素深度的转换。
要想使用此功能,需要将编码指定为以下格式之一:
mono8:cv8uc1,灰度图像
mono16:cv16uc1,16位灰度图像
bgr8:cv8uc3,蓝绿红顺序彩色图像
rgb8:cv8uc3,红-绿-蓝颜色顺序的彩色图像
bgra8:cv8uc4,带alpha通道的BGR彩色图像
rgba8:cv8uc4,带alpha通道的RGB彩色图像
注意:mono8和bgr8(1和3)是大多数OpenCV函数所期望的两种图像编码格式。
PIL、OpenCV和numpy
opencv读取图片的通道数默认为b,g,r。 opencv用的都是BGR格式
OpenCV读取图像文件后的存储格式就是np.array
import cv2
img = cv2.imread('1.png' )
if len (img.shape) == 2 or img.shape[-1 ] == 1 :
img= cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img= img[:, :, ::-1 ]
cv.imwirte('2.png' ,img)
img=cv2.imread('1.jpg' )
from PIL import Image
image = Image.open ("img.jpg" )
image.save('XXX.jpg' )
PIL的Image.open ()读取图片
def to_cvimage (self,img_pil ):
return cv2.cvtColor(np.asarray(img_pil), cv2.COLOR_RGB2BGR)
mask = Image.fromarray(np.uint8(mask))
将PIL image或者一个numpy.ndarray变成tensor
img_tensor = transforms.ToTensor()
img_PIL_tensor_GPU = img_tensor(img_PIL).cuda()
图像发布到ros的bag中
import os
import roslib
import rospy
import numpy as np
roslib.load_manifest('sensor_msgs' )
from ros import rosbag
from cv_bridge import CvBridge
from PIL import Image as ImagePIL
'''sort image name'''
def CompSortFileNamesNr (f ):
g = os.path.splitext(os.path.split(f)[1 ])[0 ]
numbertext = '' .join(c for c in g if c.isdigit())
return int (numbertext)
'''get image from dir'''
def ReadImages (img_dir ):
'''Generates a list of files from the directory'''
print ( "Searching directory %s" % img_dir )
all = []
left_files = []
img_timestamps=[]
if os.path.exists(img_dir):
for path, names, files in os.walk(img_dir ):
for f in sorted (files, key=CompSortFileNamesNr):
if os.path.splitext(f)[1 ] in ['.bmp' , '.png' , '.jpg' ]:
if 'left' in f or 'image_0' in path :
file_nm =os.path.join( path, f )
left_files.append( file_nm )
time_img =os.path.splitext(f)[0 ]
img_timestamps.append(time_img)
all .append( os.path.join( path, f ) )
return left_files ,img_timestamps
def create_Ros_bag (img_dir,out_bag_nm ):
out_file_dir = ReadImages(img_dir)
'''read image'''
left_imgs = out_file_dir[0 ]
'''read time stamps'''
imagetimestamps=out_file_dir[1 ]
'''Creates a bag file containing image '''
bag = rosbag.Bag(out_bag_nm, 'w' )
try :
br = CvBridge()
topic_nm_r = '/cam1/image_raw'
for i in range (len (left_imgs)):
print ("Adding {} {} {}" .format (i,imagetimestamps[i],left_imgs[i]) )
rgb_image = ImagePIL.open (left_imgs[i])
data = np.asarray(rgb_image)
rgb_image_msg = br.cv2_to_imgmsg(data)
Stamp = rospy.rostime.Time.from_sec(float (imagetimestamps[i]))
rgb_image_msg.header.stamp = Stamp
rgb_image_msg.header.frame_id = "camera/left"
rgb_image_msg.encoding = "rgb8"
bag.write(topic_nm_r, rgb_image_msg, Stamp)
finally :
bag.close()
if __name__ == "__main__" :
img_dir_exp = r"/test/"
bag_nm_exp = r"/data/test.bag"
create_Ros_bag(img_dir_exp,bag_nm_exp)
C++解析bag包
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/PointCloud2.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
int main (int argc, char *argv[])
{
if (argc<2 ){
return -1 ;
}else {
std::string path=argv[1 ];
std::string SavePath=argv[2 ];
FILE *fp;
fp=fopen (SavePath.c_str (),"w" );
rosbag::Bag bag;
std::cout<<"rosbag path:" <<path<<std::endl;
bag.open (path, rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back (std::string ("/right/rslidar_packets" ));
topics.push_back (std::string ("/usb_cam/image_raw0/compressed" ));
rosbag::View view (bag, rosbag::TopicQuery(topics)) ;
foreach(rosbag::MessageInstance const m, view)
{
std_msgs::String::ConstPtr s = m.instantiate <std_msgs::String>();
if (s != NULL )
std::cout << s->latitude << std::endl;
}
bag.close ();
return 0 ;
}
}
参考
https: //gitlab.com/ternaris /rosbags-image/ -/blob/master /src/rosbags /image/image .py
http: //www.open3d.org/docs /0.15.1/ Open3D: A Modern Library for 3D Data Processing
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· CSnakes vs Python.NET:高效嵌入与灵活互通的跨语言方案对比
· 【.NET】调用本地 Deepseek 模型
· Plotly.NET 一个为 .NET 打造的强大开源交互式图表库
· 上周热点回顾(2.17-2.23)
· 如何使用 Uni-app 实现视频聊天(源码,支持安卓、iOS)