制作视频和惯导的rosbag

制作视频和惯导的rosbag

操作环境 ubuntu 16.04 roskinetic ；python 3.7

。

代码

下面展示一些 内联代码片。

// An highlighted block

import argparse
import os
import sys
import rospy
from rosbag import Bag
from tqdm import tqdm
from sensor_msgs.msg import Image
from sensor_msgs.msg import Imu
del sys.path[1]
import cv2
import csv
ENCODING = 'bgr8'

try:
  parser = argparse.ArgumentParser(description='Create a ROS bag using imu data.')

  parser.add_argument('--video_path', '-v' ,metavar='folder', nargs='?', help='Vedio_file')
  parser.add_argument('--imu_folder',  '-i',metavar='folder', nargs='?', help='Imu_Data folder')
  parser.add_argument('--output_bag','-b', metavar='output_bag',  default="output.bag", help='ROS bag file %(default)s')
  #parser.add_argument('--start_time', '-t' ,type = float ,metavar='folder', nargs='?', help='Vedio_Start_Time')



  if len(sys.argv)<2:
      parser.print_help()
      sys.exit(0)

  #parse the args
  parsed = parser.parse_args()

  global bag 
  bag = Bag(parsed.output_bag, 'w')

  def Vedio2Bag(video_path):
    # open the video file
      video = cv2.VideoCapture(video_path)
      # read the start time of the video from the file
      start = os.path.getmtime(video_path)-29524
      print('1.',start)
      # get the dimensions of frames in the video
      height = int(video.get(cv2.CAP_PROP_FRAME_HEIGHT))
      width = int(video.get(cv2.CAP_PROP_FRAME_WIDTH))
      dims = height, width
      # get the number of frames in the video
      frame_count = int(video.get(cv2.CAP_PROP_FRAME_COUNT))
      print(frame_count)
      # write the images to the bag
      for idx in tqdm(range(frame_count), unit='frame'):
          # get the frame's point in time relative to the origin frame
        sec = video.get(cv2.CAP_PROP_POS_MSEC) 
        sec1 = sec /1000
        #print (sec1)
          # create a timestamp for this frame
        ros_stamp = rospy.rostime.Time.from_seconds(start + sec1)
        #print (ros_stamp)
          # read an image and the flag for a next image being available
        _, image = video.read()
          # write a raw image if the argument is set
        topic = '/image_raw'
        ros_image = Image()
        ros_image.header.stamp = ros_stamp
        ros_image.header.frame_id = "camera"
        ros_image.height = dims[0]
        ros_image.width = dims[1]
        ros_image.encoding = ENCODING
        ros_image.data = image.flatten().tobytes()
        bag.write(topic, ros_image, ros_stamp)
      video.release()

  Vedio2Bag(parsed.video_path)
  print('help')
  def getImuCsvFiles(dir):
      '''Generates a list of all csv files that start with imu'''
      imu_files = list()
      if os.path.exists(dir):
          for path, folders, files in os.walk(dir):
              for file in files:
                  if file[0:3] == 'imu' and os.path.splitext(file)[1] == ".csv":
                      imu_files.append( os.path.join( path, file ) )
      
      return imu_files

  print('help2')
  def createImuMessge(timestamp_int, omega, alpha):
      timestamp_nsecs = str(timestamp_int)
      timestamp = rospy.Time( int(timestamp_nsecs[0:10]), int(timestamp_nsecs[11:]) )
      
      rosimu = Imu()
      rosimu.header.stamp = timestamp
      rosimu.angular_velocity.x = float(omega[0])
      rosimu.angular_velocity.y = float(omega[1])
      rosimu.angular_velocity.z = float(omega[2])
      rosimu.linear_acceleration.x = float(alpha[0])
      rosimu.linear_acceleration.y = float(alpha[1])
      rosimu.linear_acceleration.z = float(alpha[2])
      
      return rosimu, timestamp
      
  #create the bag
  #write imu data

  def BuildImuRosbag():
    imufiles = getImuCsvFiles(parsed.imu_folder)
    for imufile in imufiles:
        topic = '/imu0'
        with open(imufile, 'r') as csvfile:
            reader = csv.reader(csvfile, delimiter=',')
            headers = next(reader, None)
            for row in tqdm(reader,unit= 'massage'):
                imumsg, timestamp = createImuMessge(row[0], row[1:4], row[4:7])
                bag.write(topic, imumsg, timestamp)
  BuildImuRosbag()
  bag.close()
  print('help3')
  #catch the error
except Exception as e:
  print('problem',e);

操作