0. 前言
我们都知道ROS1与ROS2的通信是不一样的,而ROS2也因为DDS的通讯带来了一些问题,其中最严重的就属于在存储大数据包的时候会出现的丢帧问题。而如何解决这样的问题目前官方没有给出非常好的解决方案,所以我们只有曲线救国,通过保存图片的方式来实现bag包的完整录制。
操作系统:
Ubuntu 20.04
版本:foxy
DDS实现:Fast-RTPS
客户端库(如适用):rclcpp/rclpy
1. 问题说明
我们在使用ROS1与ROS2的bag互转时发现会存在丢帧的情况,同时我们直接在ROS2中录制bag包也会发现录制的频率是存在问题的
ROS2
Files: rosbag2_2021_05_19-11_28_34/rosbag2_2021_05_19-11_28_34_0.db3
Bag size: 1.1 GiB
Storage id: sqlite3
Duration: 42.616s
Start: May 19 2021 11:28:34.289 (1621394914.289)
End May 19 2021 11:29:16.906 (1621394956.906)
Messages: 857
Topic information: Topic: /image | Type: sensor_msgs/msg/Image | Count: 427 | Serialization Format: cdr
Topic: /scan | Type: sensor_msgs/msg/LaserScan | Count: 430 | Serialization Format: cdr
ROS1
path: 2021-05-19-19-59-37.bag
version: 2.0
duration: 42.5s
start: May 19 2021 19:59:41.91 (1621425581.91)
end: May 19 2021 20:00:24.38 (1621425624.38)
size: 1.1 GB
messages: 847
compression: none [419/419 chunks]
types: sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
topics: /image 418 msgs : sensor_msgs/Image
/scan 429 msgs : sensor_msgs/LaserScan
这里经过测试得到了下面的数据
在PC端:
在Xavier端
我们可以发现相较于ROS1稳定而言,ROS2的频率下降很严重,并不能拉满宽带。原本以为是CPU问题,但是经过测试发现CPU变化不大,个人怀疑是DDS通讯在ROS2中本身造成的问题。在ROS官网中也发现了该问题的阐述,原因是因为FastRTPS这类DDS并不能传输较大的带宽。我们可以更换DDS来缓解这样的问题,但是仍然存在丢帧的问题。为此本文提供了一个不走ros_bridge1
方式的解决方案
2.无丢帧转bag方式
首先从ROS1中订阅bag包或者摄像头的数据,并将对应的图片以时间戳的形式保存。
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
import select, termios, tty
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
import numpy as np
bridge = CvBridge()
img=[]
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
def image_callback(msg):
print("Received an image!")
# Convert your ROS Image message to OpenCV
try:
cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
except CvBridgeError as e:
print e
timestr = "%.6f" % msg.header.stamp.to_sec()
#%.6f表示小数点后带有6位,可根据精确度需要修改;
image_name = timestr+ ".jpg" #图像命名:时间戳.jpg
cv2.imwrite(image_name, cv_image) #保存;
if __name__=="__main__":
rospy.init_node('mbot_teleop')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
rospy.Subscriber('/camera/image_raw',Image,image_callback)
rospy.spin()
ROS2部分读取对应的文件夹,并将对应的文件publish出来,并通过ros2 bag record
的方式录制
import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridge, CvBridgeError
class Image_Publisher(Node):
def __init__(self):
super().__init__("image_tutorial")
self.add_msg_to_info_logger("initializing node")
self.image_publisher_ = self.create_publisher(Image, 'image', 10)
self.bridge = CvBridge()
def publish(self, cv2_image):
try:
msg = self.bridge.cv2_to_imgmsg(cv2_image, "bgr8")
self.image_publisher_.publish(msg)
except CvBridgeError as e:
self.add_msg_to_info_logger(e)
self.add_msg_to_info_logger("sending_image")
def add_msg_to_info_logger(self, msg):
self.get_logger().info(msg)
rclpy.init()
image_publisher = Image_Publisher()
image_publisher.add_msg_to_info_logger("initializing camera")
path="xxxx/xxxxx/"
for file in os.listdir(path):
file_path = os.path.join(path, file)
if os.path.isdir(file_path):
listdir(file_path, list_name)
else:
list_name.append(file_path)
len_list = len(list_name)
cv2.namedWindow("image_tutorial")
for i in len_list:
frame = cv2.imread(i)//读取图片
cv2.imshow("image_tutorial", frame)
pressed = cv2.waitKey(1)
if pressed == 27 or pressed == ord('q'):
# closing application if esc or q pressed
image_publisher.add_msg_to_info_logger("closing button pressed, closing")
break
elif pressed == 32:
# publishing image if space pressed
image_publisher.publish(frame)
# closing
image_publisher.destroy_node()
rclpy.shutdown()
cam.release()
cv2.destroyAllWindows()
评论(0)
您还未登录,请登录后发表或查看评论