ROS高效入门第九章 -- 掌握rosbag工具，编写python包录制、读取、截断和回放样例

1 资料

本章是本人的ROS高效入门系列的最后一章，也是上一章节 ROS高效入门第八章 – 掌握rosbag工具，编写c++包录制、读取、截断和回放样例 的姊妹篇，使用 python 重新实现 bag录制，读取，截断和回放四个样例。参考资料是一样的：
（1）《机器人操作系统（ROS）浅析》[美] Jason M. O’Kane 著 肖军浩 译，第9章
（2）ros Tutorials 初级教程的10，17和18节： ros Tutorials
（3）chatgpt 4.0 （chatGPT少不了）
本系列博客汇总：ROS高效入门系列。

2 正文

2.1 python 录包样例

（1）创建 rosbag_test 软件包和文件，这个软件包将实现bag录制，读取，截断和回放四个功能，这里先讲解bag录制，实现功能与上一章节c++版是完全一样的。

cd ~/catkin_ws/src/cpp
// topic_tools 是用来写rosbag play程序用的
catkin_create_pkg rosbag_test rosbag turtlesim roslib roscpp rospy
cd rosbag_test 
mkdir scripts launch
touch launch/start_record.launch scripts/bag_record.py

（2）编写bag_record.py

#! /usr/bin/env python3

import rosbag
import rospy
import argparse
import logging
from datetime import datetime
import roslib.message
from threading import Lock
import signal, sys

logging.basicConfig(level=logging.DEBUG)

class BagRecord(object):
  def __init__(self, output_bag, topic_infos):
    self.__output_bag = output_bag
    self.__topic_infos = topic_infos
    self.__subscribers = []
    # __Lock is to protect bag write and bag close in two thread
    # in python, this is necessary !
    self.__Lock = Lock()
    self.__bag = rosbag.Bag(self.__output_bag, "w")
    signal.signal(signal.SIGINT, self.signal_handler)

  // 不同于cpp，python中多个回调函数是并发执行的，因此需要加上互斥锁保护__bag句柄
  # msg must be placed before topic !!
  def topic_callback(self, msg, topic):
    self.__Lock.acquire()
    try:
      self.__bag.write(topic, msg, rospy.Time.now())
    except Exception as e:
      // 当CTRL+C关闭__bag后，这里会发生写错误，所以需要捕捉下，避免影响程序正常结束
      rospy.logerr("%s" %e)
    self.__Lock.release()
  
  // 使用 rospy.get_published_topics() 和 roslib.message.get_message_class() 可以在不知道消息类型的情况下，创建subscriber，关键！！
  def start_recorder(self):
    for topic, topic_type in self.__topic_infos:
      msg_class = roslib.message.get_message_class(topic_type)
      self.__subscribers.append(rospy.Subscriber(topic, msg_class, self.topic_callback, topic))
 
  // 由于python中写bag是并发执行的，因此当用户ctrl+c结束录包时，也是异步关闭__bag，因此也需要加锁保护，否则bag就是残损的，无法正确读出来
  def signal_handler(self, sig, frame):
    rospy.loginfo("ctrl+c, close bag")
    self.__Lock.acquire()
    self.__bag.close()
    self.__Lock.release()
    sys.exit(0)

def main():
  // 支持录制特定topic，或者录制当前活跃的所有topic
  parser = argparse.ArgumentParser(description="record topic to rosbag")
  parser.add_argument("-O", "--output_bag", type=str, help="specify rosbag")
  parser.add_argument("-t", "--topics", nargs="+", type=str, help="specify topic")
  parser.add_argument("-a", "--all", action="store_true", help="record all topic")
  parser.add_argument('extra_args', nargs='*', help=argparse.SUPPRESS)
  args=parser.parse_args()  
  // -t 和 -a 必须给出一个
  if not args.all and not args.topics:
    logging.error("please specify topic using -t or -a")
    return

  rospy.init_node("bag_recorder")
  // 默认以时间戳作为bag名
  output_bag = datetime.now().strftime("%Y-%m-%d-%H-%M-%S") + ".bag"
  if args.output_bag:
    output_bag = args.output_bag
  // 如果 -a 和 -t 同时出现，以 -a 为准
  topic_infos = []
  if args.all:
    topic_infos = rospy.get_published_topics()
  elif args.topics:
    topic_infos = [(topic, msg_class) for topic, msg_class in rospy.get_published_topics() if topic in args.topics]

  recorder = BagRecord(output_bag, topic_infos)
  rospy.loginfo("start recorder...")
  recorder.start_recorder()
  rospy.spin()

if __name__ == "__main__":
  main()

（3）编写start_record.launch

<launch>
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    required="true"
/>
<node
    pkg="turtlesim"
    type="draw_square"
    name="draw_square"
    required="true"
/>
<node
    pkg="rosbag_test"
    type="bag_record.py"
    name="bag_recorder"
    output="screen"
    launch-prefix="bash -c 'sleep 5; $0 $@'"
    args="-O /home/ycao/Study/ros_noetic/bag_dir/square_1.bag -a"
/>
</launch>

（4）编译并运行

cd ~/catkin_ws/
catkin_make --source src/python/rosbag_test/
source devel/setup.bash
rosbag_test start_record.launch

2.2 python 查看包样例

（1）创建并编写bag_echo.py

cd ~/catkin_ws/src/python/rosbag_test
touch scripts/bag_echo.py

bag_echo.py

#! /usr/bin/env python3

import os
import argparse
import logging
import rosbag

logging.basicConfig(level=logging.DEBUG)

class BagEcho(object):
  def __init__(self, bag, topics):
      self.__bag = bag
      self.__topics = topics
  
  def echo_bag(self):
      msg_cnts = {}
      bag = rosbag.Bag(self.__bag, "r")
      for topic, msg, t in bag.read_messages(topics=self.__topics, raw=False):
          if topic not in msg_cnts:  
            msg_cnts[topic] = 1
          else:
            msg_cnts[topic] += 1

          print("----------[%s]---------" %topic)
          // python查看消息内容远比c++方便的多，使用bag.read_messages()，直接就能打印出来，不需要引入任何消息类型
          // 而且速度远快于命令行工具：rostopic echo -b ***.bag，但略慢于c++版。
          print(msg)

      bag.close()

      logging.info("----------[rosbag summary]---------")
      for topic in msg_cnts:  
        logging.info("topic %s msg cnt is %d" %(topic, msg_cnts[topic]))

def main():
  // 支持查看指定topic，如果不指定，默认查看所有topic内容。c++版要求必须给出topic，没有默认选项。
  parser = argparse.ArgumentParser(description="echo rosbag depends topic")
  parser.add_argument("-b", "--bag", type=str, required=True, help="specify rosbag")
  parser.add_argument("-t", "--topics", nargs="+", type=str, help="specify topic")
  parser.add_argument('extra_args', nargs='*', help=argparse.SUPPRESS)
  args=parser.parse_args()

  if not os.path.isfile(args.bag):
      logging.error("%s is not found!" %args.bag)
      return

  echo = BagEcho(args.bag, args.topics)
  echo.echo_bag()

if __name__ == "__main__":
    main()

（2）编译并运行

cd ~/catkin_ws/
catkin_make --source src/python/rosbag_test/
source devel/setup.bash
./devel/lib/rosbag_test/bag_echo.py -b ../bag_dir/square_1.bag
./devel/lib/rosbag_test/bag_echo.py -b ../bag_dir/square_1.bag -t /rosout

2.3 python 截取包样例

（1）创建并编写bag_filter.py

cd ~/catkin_ws/src/python/rosbag_test
touch scripts/bag_filter.py

bag_filter.py

#! /usr/bin/env python3

import os
import argparse
import rosbag
import logging

logging.basicConfig(level=logging.DEBUG)

class BagFilter(object):
  def __init__(self, input_bag, output_bag, start_offset, end_offset, topics):
    self.__input_bag = input_bag
    self.__output_bag = output_bag
    self.__start_offset = start_offset
    self.__end_offset = end_offset
    self.__topics = topics

  def filter_bag(self):
    inbag = rosbag.Bag(self.__input_bag, "r")
    start_time = inbag.get_start_time() + self.__start_offset
    end_time = inbag.get_end_time() - self.__end_offset

    with rosbag.Bag(self.__output_bag, "w") as outbag:
      // 如果不指定 -t ，这里的self.__topics就是空，read_messages()就会把全量topic读出来，很方便
      for topic, msg, t in inbag.read_messages(topics=self.__topics, raw=False):
        if start_time <= t.to_sec() <= end_time:
          outbag.write(topic, msg, t)

      logging.info("------------[%s summary]------------" %self.__output_bag)
      logging.info("time span %f [origin %f]" %(end_time-start_time, inbag.get_end_time()-inbag.get_start_time()))
      topic_list = [topic for topic in outbag.get_type_and_topic_info()[1].keys()]
      logging.info("topic list %s" %topic_list)

    inbag.close()

def main():
  // 支持前后时间偏移截取，支持提取指定topic
  parser = argparse.ArgumentParser(description="filter rosbag depends time offset and topics")
  parser.add_argument("-i", "--input_bag", type=str, required=True, help="specify input rosbag")
  parser.add_argument("-o", "--output_bag", type=str, required=True, help="specify output rosbag")
  parser.add_argument("-s", "--start_offset", type=int, default=0, help="specify start offset time")
  parser.add_argument("-e", "--end_offset", type=int, default=0, help="specify end offset time")
  parser.add_argument("-t", "--topics", nargs="+", type=str, help="specify topic")
  parser.add_argument('extra_args', nargs='*', help=argparse.SUPPRESS)
  args=parser.parse_args()

  if not os.path.isfile(args.input_bag):
    logging.error("%s is no found!" %args.input_bag)
    return

  filter = BagFilter(args.input_bag, args.output_bag, args.start_offset, args.end_offset, args.topics)
  filter.filter_bag()

if __name__ == "__main__":
    main()

（2）编译并运行

cd ~/catkin_ws/
catkin_make --source src/python/rosbag_test/
source devel/setup.bash
./devel/lib/rosbag_test/bag_filter.py -i ../bag_dir/square_1.bag -o filter.bag -s 1 -e 1

2.4 python 包播放样例

（1）创建并编写bag_play.py

cd ~/catkin_ws/src/python/rosbag_test
touch scripts/bag_play.py

bag_play.py

#! /usr/bin/env python3

import os, sys
import rospy
import rosbag
import argparse

class BagPlay(object):
  def __init__(self, bag, start_offset, rate, topic_list=[]):
    self.__bag = bag
    self.__start_ofset = start_offset
    self.__rate = rate
    self.__topic_list = topic_list
    self.__publisher = {}
    
  // 必须持久化publisher
  def create_publisher(self):
    bag = rosbag.Bag(self.__bag, 'r')

    for topic, msg, t in bag.read_messages(topics=self.__topic_list, raw=False):
      if topic not in self.__publisher:
        self.__publisher[topic] = rospy.Publisher(topic, type(msg), queue_size=10)

    bag.close()

  def start_play(self):
    bag = rosbag.Bag(self.__bag, "r")

    bag_start_time = bag.get_start_time() + self.__start_ofset
    rospy_start_time =rospy.get_time()
    for topic, msg, t in bag.read_messages(topics=self.__topic_list, start_time=rospy.Time.from_sec(bag_start_time)):
      // 这里实现了倍速播放的机制，比c++版逻辑更清晰简介 !!
      relative_time = (t.to_sec() - bag_start_time) / self.__rate
      while rospy.get_time() < (rospy_start_time + relative_time):
        rospy.sleep(0.01)

      self.__publisher[topic].publish(msg)

      // 这里是进度显示功能
      cur = t.to_sec()
      du = rospy.Duration(cur - bag_start_time).to_sec()
      sys.stdout.write("[RUNNING] Bag Time: %f Duration: %f\r" %(cur, du))
      sys.stdout.flush()

      if rospy.is_shutdown():
        break

    print("\n")
    bag.close

def main():
  // 支持偏移起点播放，支持倍速播放，支持播放指定topic
  parser = argparse.ArgumentParser(description="play topic from rosbag")
  parser.add_argument("-b", "--bag", type=str, required=True, help="specify rosbag")
  parser.add_argument("-s", "--start_offset", type=int, default=0, help="offset time from bag start")
  parser.add_argument("-r", "--rate", type=float, default=1.0, help="specify play rate")
  parser.add_argument("-t", "--topic_list", nargs="+", type=str, help="specify play topics")
  parser.add_argument("extra_args", nargs='*', help=argparse.SUPPRESS)
  args = parser.parse_args()

  if not os.path.isfile(args.bag):
    print("%s is not found!" %args.bag)
    return

  rospy.init_node("bag_player")

  player = BagPlay(args.bag, args.start_offset, args.rate, args.topic_list)
  player.create_publisher()

  print("start play bag after 2 sec...")
  rospy.sleep(2.0)

  player.start_play()

if __name__ == "__main__":
  main()

（2）创建并编写start_play.launch

cd ~/catkin_ws/src/python/rosbag_test
touch launch/start_play.launch

start_play.launch

<launch>
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    required="true"
/>
<node
    pkg="rosbag_test"
    type="bag_play.py"
    name="bag_player"
    args="-b /home/ycao/Study/ros_noetic/bag_dir/square_1.bag -t /turtle1/cmd_vel"
    output="screen"
/>
</launch>

（3）这里给出覆盖四个样例的CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(rosbag_test)
find_package(catkin REQUIRED COMPONENTS
  rosbag
  roscpp
  rospy
  turtlesim
  roslib
)
catkin_package()
include_directories(
  ${catkin_INCLUDE_DIRS}
)
catkin_install_python(PROGRAMS
  scripts/bag_echo.py
  scripts/bag_filter.py
  scripts/bag_record.py
  scripts/bag_play.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

（4）编译并运行

cd ~/catkin_ws/
catkin_make --source src/python/rosbag_test/
source devel/setup.bash
roslaunch rosbag_test start_play.launch

3 总结

本章用python重新实现了bag录制，读取，截断和回放四个样例，相比于c++版，代码明显更短，实现起来更容易。本文中的例子放在了本人的github上： ros_src

3.1 chatGPT4.0使用经验

写代码的过程中，大量使用了chatGPT4.0，从一开始无脑信任它，到最后不知道该不该信任它。多次受骗以后，我逐步摸索出了一套沟通方法，记录在此：
（1）对于较为复杂的代码，chatGPT给出来的往往是有bug的，而且很隐蔽，需要你调试半天，所以千万不要无脑相信它。
（2）对于编译运行出现的问题，chatGPT的回答也是模棱两可的，所以调试代码，千万不要无脑依赖chatGPT。
（3）因此，使用chatGPT，需要开发者自己要有清晰的思路，当遇到问题的时候，也要有自己的思考，用自己的思路引导它帮你。
（4）对于chatGPT给出的不准确的地方，一定要使用google及时进行校验，不要一直无脑的问它，否则肯定会浪费大量时间。