从 ros 的 bag 包里提取特定的数据

全部代码如下(python):
只要第10行、第14行、第16行根据自己的文件名进行更改就行了。


import roslib;
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

path='/home/zy/shu_ju/2021-03-03-15-57-52/image2/'
class ImageCreator():
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('2021-03-03-15-57-52.bag', 'r') as bag:
            for topic,msg,t in bag.read_messages():
                if topic == "/hik03/forwardright/image_raw":
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print(e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()

                        image_name = timestr+ ".png"
                        cv2.imwrite(path+image_name, cv_image)

if __name__ == '__main__':
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

全部代码如下(后缀为 “.launch” 的文件):
只要第2行、第4行根据自己的文件名和文件路径进行更改就行了。

<launch>
  <node pkg="rosbag" type="play" name="rosbag" args="-d 2 $(find /home/zy/shu_ju)/2021-03-03-15-57-52.bag"/>
  <node name="extract" pkg="image_view" type="extract_images" respawn="false" output="screen" cwd="ROS_HOME">
<remap from="image" to="/hik03/forwardright/image_raw"/>
  </node>
</launch>

全部代码如下(python):
只要第9行、第11行、第12行根据自己的文件名和文件路径进行更改就行了。


import sys
import argparse
from fnmatch import fnmatchcase
from rosbag import Bag
import rosbag

bag_file = '2021-03-03-15-57-52.bag'
bag = rosbag.Bag(bag_file, "r")
bag_data = bag.read_messages('/ruby128result')
save_path = "/home/zy/pcd_result/"

j = 0
for topic, msg, t in bag_data:
    timestr = "%.6f" %  msg.header.stamp.to_sec()

    cloud_name = save_path + timestr+ ".txt"
    with open(cloud_name,"w") as f:

        for i in range(len(msg.points)):
            msg.points[i].x;
            msg.points[i].y;
            msg.points[i].z;
            f.writelines([str(msg.points[i].x), ' ', str(msg.points[i].y), ' ', str(msg.points[i].z), '\n'])
    if j % 10 == 0:
        print('di ', j, ' zhen')
    j += 1

代码叫做merge_bag.py
运行的时候,命令行输入:

python merge_bag.py -v 1028msf.bag msf.bag vinReNoOutlier.bag

就是把 msf.bag 和 vinReNoOutlier.bag 完全合并在一起了,合并的新bag包叫 1028msf.bag,时间戳打的都是原来两个bag里原始的时间戳,而不是像一边rosbag play一边rosbag record一样录的是现在这个时间戳。

命令行输入:

python merge_bag.py -v –topics '/aft_mapped_to_init /gnss/data /imu/data vinReNoOutlier' msf.bag 2019-10-28-14-40-45.bag vinReNoOutlier.bag

就是把2019-10-28-14-40-45.bag里的/aft_mapped_to_init、/gnss/data、/imu/data和vinReNoOutlier.bag里的vinReNoOutlier合并在了一起,注意一定不能在vinReNoOutlier前加/,因为本身vinReNoOutlier.bag里的话题名就是vinReNoOutlier,没有/

全部代码如下(python):


import sys
import argparse
from fnmatch import fnmatchcase
from rosbag import Bag
def main():
    parser = argparse.ArgumentParser(description='Merge one or more bag files with the possibilities of filtering topics.')
    parser.add_argument('outputbag',
                        help='output bag file with topics merged')
    parser.add_argument('inputbag', nargs='+',
                        help='input bag files')
    parser.add_argument('-v', '--verbose', action="store_true", default=False,
                        help='verbose output')
    parser.add_argument('-t', '--topics', default="*",
                        help='string interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged bag file')
    args = parser.parse_args()
    topics = args.topics.split(' ')
    total_included_count = 0
    total_skipped_count = 0
    if (args.verbose):
        print("Writing bag file: " + args.outputbag)
        print("Matching topics against patters: '%s'" % ' '.join(topics))

    with Bag(args.outputbag, 'w') as o:
        for ifile in args.inputbag:
            matchedtopics = []
            included_count = 0
            skipped_count = 0
            if (args.verbose):
                print("> Reading bag file: " + ifile)
            with Bag(ifile, 'r') as ib:
                for topic, msg, t in ib:
                    if any(fnmatchcase(topic, pattern) for pattern in topics):
                        if not topic in matchedtopics:
                            matchedtopics.append(topic)
                            if (args.verbose):
                                print("Including matched topic '%s'" % topic)
                        o.write(topic, msg, t)
                        included_count += 1
                        if included_count % 10 == 0:
                            print("Included messages:", included_count)
                    else:
                        skipped_count += 1
            total_included_count += included_count
            total_skipped_count += skipped_count
            if (args.verbose):
                print("< Included %d messages and skipped %d" % (included_count, skipped_count))
    if (args.verbose):
        print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count))
if __name__ == "__main__":
    main()

Original: https://blog.csdn.net/weixin_44832383/article/details/121095955
Author: weixin_44832383
Title: 从 ros 的 bag 包里提取特定的数据

原创文章受到原创版权保护。转载请注明出处:https://www.johngo689.com/111653/

转载文章受原作者版权保护。转载请注明原作者出处!

(0)

大家都在看

免费咨询
免费咨询
扫码关注
扫码关注
联系站长

站长Johngo!

大数据和算法重度研究者!

持续产出大数据、算法、LeetCode干货,以及业界好资源!

2022012703491714

微信来撩,免费咨询:xiaozhu_tec

分享本页
返回顶部