Python rosbag.Bag方法代碼示例

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def main():
  if len(sys.argv) < 2:
    print("Usage: {} dataset_name".format(sys.argv[0]))
    exit(1)

  file_name = sys.argv[1]
  log_file = h5py.File('../dataset/log/{}.h5'.format(file_name))
  camera_file = h5py.File('../dataset/camera/{}.h5'.format(file_name))  

  zipped_log = izip(
    log_file['times'],
    log_file['fiber_accel'],
    log_file['fiber_gyro'])

  with rosbag.Bag('{}.bag'.format(file_name), 'w') as bag:
    bar = Bar('Camera', max=len(camera_file['X']))
    for i, img_data in enumerate(camera_file['X']):
      m_img = Image()
      m_img.header.stamp = rospy.Time.from_sec(0.01 * i)
      m_img.height = img_data.shape[1]
      m_img.width = img_data.shape[2]
      m_img.step = 3 * img_data.shape[2]
      m_img.encoding = 'rgb8'
      m_img.data = np.transpose(img_data, (1, 2, 0)).flatten().tolist()
      
      bag.write('/camera/image_raw', m_img, m_img.header.stamp)
      bar.next()
      
    bar.finish()

    bar = Bar('IMU', max=len(log_file['times']))
    for time, v_accel, v_gyro in zipped_log:
      m_imu = Imu()
      m_imu.header.stamp = rospy.Time.from_sec(time)
      [setattr(m_imu.linear_acceleration, c, v_accel[i]) for i, c in enumerate('xyz')]
      [setattr(m_imu.angular_velocity, c, v_gyro[i]) for i, c in enumerate('xyz')]

      bag.write('/fiber_imu', m_imu, m_imu.header.stamp)
      bar.next()

    bar.finish() 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def ToRosBag(self,filename) :
        with rosbag.Bag(filename,'w') as outbag:
            try:
                for i in range(len(self.times)):
                    print "%d / joint states = %d / times = %d / world states = %d"%(i, len(self.joint_states), len(self.times), len(self.world_states))
                    outbag.write('joint_states',self.joint_states[i],self.times[i])
                    if i >= len(self.world_states):
                        print "Done early."
                        break
                    for frame in self.world_states[0].keys():
                        outbag.write('world/%s'%frame,pm.toMsg(self.world_states[i][frame]),self.times[i])
                    outbag.write('base_tform',pm.toMsg(self.base_tform),self.times[i])
                    if len(self.gripper_cmds) > 0:
                        outbag.write('gripper_msg',self.gripper_cmds[i],self.times[i])

            finally:
                outbag.close() 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def import_snapshots(self, topic_name):
        print "Importing Snapshots from: ", topic_name

        bag = rosbag.Bag(self.bag_filter.file_name)

        self.list_snapshots = []
        self.states = np.zeros((0, 3))
        count = 0
        state_topic = '/slam/pose'
        for topic, msg, t in bag.read_messages([topic_name,state_topic]):
            if topic == topic_name:
                self.list_snapshots.append(msg)
                count = count + 1
        self.max = count
        bag.close()

        self.load_snap_from_list(0)
        print "Importing DONE" 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def load_track_from_bag(self, path,outside,inside,center):
        bag = rosbag.Bag(path)
        self.clear()
        for topic, msg, t in bag.read_messages(topics = [outside,inside,center]):
            if len(msg.polygon.points) == 0:
                continue
            if topic == outside:
                self.cones_right = self.read_polygon_to_array(msg)
            elif topic == inside:
                self.cones_left = self.read_polygon_to_array(msg)
            elif topic == center:
                self.control_points = self.read_polygon_to_array(msg)

        if self.cones_left.size and self.cones_right.size:
            self.computed_cones = True

        self.add_tk_device(4.0, x_offet = 6.0)
        self.starting_pose_front_wing = [0.0,0.0,0.0]

        bag.close() 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def __init__(self, path_to_bag, topic):
        self.path_to_bag = path_to_bag

        self.times = []
        self.images = []

        self.bridge = CvBridge()

        with rosbag.Bag(path_to_bag) as bag:
            
            topics = bag.get_type_and_topic_info().topics
            if topic not in topics:
                raise ValueError("The topic with name %s was not found in bag %s" % (topic, path_to_bag))

            for topic, msg, t in bag.read_messages(topics=[topic]):
                time = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs
                self.times.append(time)

                img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                self.images.append(img) 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def __init__(self, bag):
        """
        Create a new BagTfTransformer from an open rosbag or from a file path

        :param bag: an open rosbag or a file path to a rosbag file
        """
        if type(bag) == str:
            bag = rosbag.Bag(bag)
        self.tf_messages = sorted(
            (self._remove_slash_from_frames(tm) for m in bag if m.topic.strip("/") == 'tf' for tm in
             m.message.transforms),
            key=lambda tfm: tfm.header.stamp.to_nsec())
        self.tf_static_messages = sorted(
            (self._remove_slash_from_frames(tm) for m in bag if m.topic.strip("/") == 'tf_static' for tm in
             m.message.transforms),
            key=lambda tfm: tfm.header.stamp.to_nsec())

        self.tf_times = np.array(list((tfm.header.stamp.to_nsec() for tfm in self.tf_messages)))
        self.transformer = tf.TransformerROS()
        self.last_population_range = (rospy.Time(0), rospy.Time(0))
        self.all_frames = None
        self.all_transform_tuples = None
        self.static_transform_tuples = None 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def read_messages(self):
        for f in self.bagfiles:
            with rosbag.Bag(f, "r") as bag:
                for topic, msg, _ in bag.read_messages(topics=self.topics):
                    if self._remap_camera and topic in self._remap_camera:
                        topic = self._remap_camera[topic]
                        msg.header.frame_id = self._remap_camera[msg.header.frame_id]
                    yield topic, msg 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def Run(argv):

    bag = rosbag.Bag(argv[0])

    for topic, msg, t in bag.read_messages(topics=[argv[1]]):
        print msg.header.stamp

    bag.close() 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def __init__(self):
        # Get parameters when starting node from a launch file.
        if len(sys.argv) < 1:
            save_dir = rospy.get_param('save_dir')
            filename = rospy.get_param('filename')
            rospy.loginfo("Bag filename = %s", filename)
        # Get parameters as arguments to 'rosrun my_package bag_to_images.py <save_dir> <filename>', where save_dir and filename exist relative to this executable file.
        else:
            save_dir = os.path.join(sys.path[0], sys.argv[1])
            filename = os.path.join(sys.path[0], sys.argv[2])
            rospy.loginfo("Bag filename = %s", filename)

        # Use a CvBridge to convert ROS images to OpenCV images so they can be saved.
        self.bridge = CvBridge()

        # Open bag file.
        with rosbag.Bag(filename, 'r') as bag:
            for topic, msg, t in bag.read_messages():
                topic_parts = topic.split('/')
                # first part is empty string
                if len(topic_parts) == 3 and topic_parts[2] == self.desired_topic:
                    try:
                        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                    except CvBridgeError, e:
                        print e
                    timestr = "%.3f" % msg.header.stamp.to_sec()
                    if self.index_in_filename:
                        image_name = str(save_dir) + "/" + topic_parts[1] + "-" + format(self.image_index, self.index_format) + "-" + timestr + self.image_type
                    else:
                        image_name = str(save_dir) + "/" + topic_parts[1] + "-" + timestr + self.image_type
                    cv2.imwrite(image_name, cv_image)
                    self.image_index = self.image_index + 1

# Main function. 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def open(self, filename):
        print("Opening bag", filename)
        bag = rosbag.Bag(filename)
        self.set_bag(bag=bag, filename=filename) 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def extract(bagfile, pose_topic, msg_type, out_filename):
    n = 0
    f = open(out_filename, 'w')
    f.write('# timestamp tx ty tz qx qy qz qw\n')
    with rosbag.Bag(bagfile, 'r') as bag:
        for (topic, msg, ts) in bag.read_messages(topics=str(pose_topic)):
            if msg_type == "PoseWithCovarianceStamped":
                f.write('%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n' %
                        (msg.header.stamp.to_sec(),
                         msg.pose.pose.position.x, msg.pose.pose.position.y,
                         msg.pose.pose.position.z,
                         msg.pose.pose.orientation.x,
                         msg.pose.pose.orientation.y,
                         msg.pose.pose.orientation.z,
                         msg.pose.pose.orientation.w))
            elif msg_type == "PoseStamped":
                f.write('%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n' %
                        (msg.header.stamp.to_sec(),
                         msg.pose.position.x, msg.pose.position.y,
                         msg.pose.position.z,
                         msg.pose.orientation.x, msg.pose.orientation.y,
                         msg.pose.orientation.z, msg.pose.orientation.w))
            else:
                assert False, "Unknown message type"
            n += 1
    print('wrote ' + str(n) + ' imu messages to the file: ' + out_filename) 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def main():
    args = parse_args()

    left_cm = CameraInfoManager(namespace='left')
    left_cm.setURL('file://{}'.format(os.path.abspath(args.l)))
    left_cm.loadCameraInfo()
    left_msg = left_cm.getCameraInfo()

    right_cm = CameraInfoManager(namespace='right')
    right_cm.setURL('file://{}'.format(os.path.abspath(args.r)))
    right_cm.loadCameraInfo()
    right_msg = right_cm.getCameraInfo()

    in_bag = rosbag.Bag(args.bagfile)

    in_basename = os.path.splitext(
        os.path.basename(os.path.abspath(args.bagfile)))[0]
    out_filepath = os.path.join(
        os.path.dirname(os.path.abspath(args.bagfile)),
        in_basename + '_wcaminfo.bag')

    print 'Writing to', out_filepath

    with rosbag.Bag(out_filepath, 'w') as out_bag:
        for topic, msg, t in in_bag.read_messages():
            if topic == '/cam0/image_raw':
                left_msg.header = msg.header
                out_bag.write('/cam/left/camera_info', left_msg, t)
                out_bag.write('/cam/left/image_raw', msg, t)
            elif topic == '/cam1/image_raw':
                right_msg.header = msg.header
                out_bag.write('/cam/right/camera_info', right_msg, t)
                out_bag.write('/cam/right/image_raw', msg, t)
            else:
                out_bag.write(topic, msg, t) 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def loadFromFile(self, filename):
        filenames = filename.split(',')
        for i, f in enumerate(filenames):
            bag = rosbag.Bag(f, 'r')
            self.loadFromBag(bag, seq=i) 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def _main(filename, ignore_inputs, **kwargs):
 	obj_history = {}
 	left_hand_history = []
 	right_hand_history = []
 	demo_topic = "/vr/learning/getDemonstrationInfo"
 	alias_topic = "/vr/learning/getActivityAlias"
 	bag = rosbag.Bag(filename)

 	obj_ignore = {'surveillance_camera','Head (eye)','Controller (left)','Controller (right)','high_table'}
 	for topic, msg, _ in bag:
 		if topic == alias_topic:
 			print("Alias: ",msg.old_name,msg.new_name)
 			continue
 		if topic == demo_topic:

 			# get hands
 			hand = msg.left
 			name = 'left_hand'
 			name = getHandName(hand,name)
 			if not name in obj_history:
 				obj_history[name] = []
 			obj_history[name].append([hand.pose.position.x, hand.pose.position.y, hand.pose.position.z])

                        """
			hand = msg.right
 			name = 'right_hand'
 			name = getHandName(hand,name)
 			if not name in obj_history:
 				obj_history[name] = []
 			obj_history[name].append([hand.pose.position.x, hand.pose.position.y, hand.pose.position.z])
                        """
 	
 	# plot
 	fig = plt.figure()
 	ax = fig.gca(projection = '3d')
 	for obj, data in obj_history.items():
 		data = np.array(data)
 		ax.plot(data[:,0], data[:,1], data[:,2], label=obj)
 	plt.title('Object Positons')
 	ax.legend(loc=2, prop={'size': 6})
 	plt.show() 

# 需要導入模塊: import rosbag [as 別名]
# 或者: from rosbag import Bag [as 別名]
def open(self):
        assert (not self.is_open)

        bag_num = 0
        while os.path.exists(self._rosbag_name(bag_num)):
            bag_num += 1

        self._rosbag = rosbag.Bag(self._rosbag_name(bag_num), 'w', compression='bz2')
        self._last_write = rospy.Time.now()