点云数据 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%04d.%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镜像运行
####1.拉取docker 环境-ros的环境
docker pull osrf/ros:noetic-desktop-full 或者 docker pull osrf/ros:melodic-desktop-full
##运行docker环境
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
###docker内运行
###docker环境内部执行
确定是否有 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
其他等价
# Syntax is: /opt/ros/noetic/lib/pcl_ros/bag_to_pcd <file_in.bag> <topic> <output_directory> [<target_frame>]
#Example: /opt/ros/noetic/lib/pcl_ros/bag_to_pcd data.bag /laser_tilt_cloud ./pointclouds /base_link
方法二：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
#!/bin/bash
set -e
# setup ros environment
source "/opt/ros/$ROS_DISTRO/setup.bash"
exec "$@"
其中
/opt/ros/noetic/setup.bash
#!/usr/bin/env bash
# generated from catkin/cmake/templates/setup.bash.in
CATKIN_SHELL=bash
# source setup.sh from same directory as this file
_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扩展的文件
代码示例
#!/usr/bin/env python3
# -*- coding: UTF-8 -*-
# ---------------------------
"""" 以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 is rosbags Image or CompressedImage message instance
msg = ...
# get opencv image without any conversions
img = message_to_cvimage(msg)
# get opencv image and convert to bgr8 color space
img = message_to_cvimage(msg, 'bgr8')
###参考 https://pypi.org/project/rosbags-image/
代码示例
# -*- 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 is rosbags Image or CompressedImage message instance
msg = ...
# get opencv image without any conversions
img = message_to_cvimage(msg)
# get opencv image and convert to bgr8 color space
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') #打开图像,opencv默认读取图片的数据为: (高，宽，通道(B，G，R))。
# PLT默认读取图片数据格式：(高，宽，通道(R,G,B))
from PIL import Image
image = Image.open("img.jpg")
image.save('XXX.jpg') # 保存为XXX.jpg
PIL的Image.open()读取图片
def to_cvimage(self,img_pil):
return cv2.cvtColor(np.asarray(img_pil), cv2.COLOR_RGB2BGR)
# numpy 转 image类
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中
#!/usr/bin/python3
# -*- coding: utf-8 -*-
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)
# cv2_to_compressed_imgmsg
rgb_image_msg = br.cv2_to_imgmsg(data) # Convert the color image to a message
# Stamp = rospy.rostime.Time.from_sec(float( (float(imagetimestamps[i]))/1e6 ))
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包
// catkin_create_pkg beginner_tutorials std_msgs rospy roscpp rosbag
#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");
//打开一个bag文件
rosbag::Bag bag;
std::cout<<"rosbag path:"<<path<<std::endl;
bag.open(path, rosbag::bagmode::Read);
//设置需要遍历的topic
std::vector<std::string> topics;
topics.push_back(std::string("/right/rslidar_packets"));
topics.push_back(std::string("/usb_cam/image_raw0/compressed"));
// 读指定的topic
rosbag::View view(bag, rosbag::TopicQuery(topics));
//读取全部topic： rosbag::View view_all(view);
//用foreach遍历所有帧数据，每个messageInstance就是一帧数据
foreach(rosbag::MessageInstance const m, view)
{
std_msgs::String::ConstPtr s = m.instantiate<std_msgs::String>();
// novatel_msgs::INSPVAX::ConstPtr s = m.instantiate<novatel_msgs::INSPVAX>();
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
