Python Odometry.header方法代码示例

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
    def odom_transform_2d(self, data, new_frame, trans, rot):
        # NOTES: only in 2d rotation
        # also, it removes covariance, etc information
        odom_new = Odometry()
        odom_new.header = data.header
        odom_new.header.frame_id = new_frame

        odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0]
        odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1]
        odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2]
        # rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot))))
        q = data.pose.pose.orientation
        q_tuple = (q.x, q.y, q.z, q.w,)
        # Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion
        odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot))

        heading_change = quaternion_to_heading(rot)

        odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change)
        odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change)
        odom_new.twist.twist.linear.z = 0

        odom_new.twist.twist.angular.x = 0
        odom_new.twist.twist.angular.y = 0
        odom_new.twist.twist.angular.z = data.twist.twist.angular.z

        return odom_new

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
 def pub_ekf_odom(self, msg):
     odom = Odometry()
     odom.header = msg.header
     odom.child_frame_id = 'base_footprint'
     odom.pose = msg.pose
     
     self.ekf_pub.publish(odom)

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
def callback(pose):
    # cov_east = navsatfix.position_covariance[0]
    # cov_north = navsatfix.position_covariance[4]
    # if cov_east>cov_thresh or cov_north>cov_thresh:
    #     return
    odom = Odometry()
    odom.pose.pose = pose.pose
    odom.header = pose.header
    pub.publish(odom)

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
 def transition(self, state, twist, dt):
     desired = state[1]
     current = state[0]
     # do something
     n = Odometry()
     n.header = current.header
     n.twist.twist = self.update_twist(current.twist.twist, twist)
     n.pose.pose = self.update_pose(current.pose.pose, current.twist.twist, n.twist.twist, dt)
     return (n, desired, )

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
    def default(self, ci='unused'):
        odometry = Odometry()
        odometry.header = self.get_ros_header()
        odometry.child_frame_id = self.child_frame_id

        # fill pose and twist
        odometry.pose.pose = self.get_pose()
        odometry.twist.twist = self.get_twist()

        self.publish(odometry)

        # send current odometry transform
        self.sendTransform(self.get_position(),
                           self.get_orientation(),
                           odometry.header.stamp,
                           odometry.child_frame_id,
                           odometry.header.frame_id)

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
    def __init__(self, rand_size):
        self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
        self.odom = None
        self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry, self.set_odom)
        
        fprint("Shaking hands and taking names.")
        rospy.sleep(1)

        # We need to publish an inital odom message for lqrrt
        start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
        start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1)
        start_pose = navigator_tools.numpy_quat_pair_to_pose(start_pos, start_ori)
        start_odom = Odometry()
        start_odom.header = navigator_tools.make_header(frame='enu')
        start_odom.child_frame_id = 'base_link'
        start_odom.pose.pose = start_pose
        self.odom_pub.publish(start_odom)

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
    def unpackOdom(self, data):
        odom_msg = Odometry()
        # Unpack Header
        odom_msg.header = self.unpackOdomHeader(data[0:20])
        # Unpack Child_frame_id
        # Is constant and "base_link"
        cfid = unpack("xxxxccccccccc", data[20:33])
        # odom_msg.child_frame_id = (cfid[0] + cfid[1] + cfid[2] +
        # cfid[3] + cfid[4] + cfid[5] + cfid[6] + cfid[7] + cfid[8])
        odom_msg.child_frame_id = "base_footprint"
        # Unpack Pose
        pose = self.bytestr_to_array(data[33:89])
        odom_msg.pose.pose.position.x = pose[0]
        odom_msg.pose.pose.position.y = pose[1]
        odom_msg.pose.pose.position.z = pose[2]
        odom_msg.pose.pose.orientation.x = pose[3]
        odom_msg.pose.pose.orientation.y = pose[4]
        odom_msg.pose.pose.orientation.z = pose[5]
        odom_msg.pose.pose.orientation.w = pose[6]
        # Skip Unpack Pose Covariance of Pose
        # The values are all zeros since we did not know how to properly
        # calculate them. We are not properly sending the correct covarinace
        # maxtrix. Ours is 3x3 where it should be 6x6. To skip the covaraince
        # we will skip 4+ (9*8) = 76 bytes

        odom_msg.pose.covariance = covariance

        # Unpack Twist
        twist = self.bytestr_to_array(data[165:213])
        odom_msg.twist.twist.linear.x = twist[0]
        odom_msg.twist.twist.linear.y = twist[1]
        odom_msg.twist.twist.linear.z = twist[2]
        odom_msg.twist.twist.angular.x = twist[3]
        odom_msg.twist.twist.angular.y = twist[4]
        odom_msg.twist.twist.angular.z = twist[5]

        # Skip Unpack Twist Covariance of Pose
        # The values are all zeros since we did not know how to properly
        # calculate them. We are not properly sending the correct covarinace
        # maxtrix. Ours is 3x3 where it should be 6x6. To skip the covaraince
        # we will skip 4+ (9*8) = 76 bytes

        odom_msg.twist.covariance = covariance

        return odom_msg

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
def subCB(msg_in):  
  global pub
  
  msg_out = Odometry()
  msg_out.header = msg_in.header
  msg_out.header.frame_id = "Pioneer3AT/odom"
  msg_out.child_frame_id = "imu"
  
  cart = LLA2Naive(msg_in.LLA.x, msg_in.LLA.y, msg_in.LLA.z)
  
  
  msg_out.pose.pose.position.x = cart[0]
  msg_out.pose.pose.position.y = cart[1]
  msg_out.pose.pose.position.z = cart[2]
  
  q = tf.transformations.quaternion_from_euler(pi * msg_in.RPY.x /  180.0, 
                                               pi * msg_in.RPY.y /  180.0,
                                               pi * msg_in.RPY.z / -180.0 )
  msg_out.pose.pose.orientation = Quaternion(*q)
                                                                           
  pub.publish(msg_out)

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
    def publish_tf(self,pose, stamp=None):
        """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """
        if stamp == None:
            stamp = rospy.Time.now()

        # this may cause issues with the TF tree. If so, see the below code.
        self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), 
               stamp , "/laser", "/map")

        # also publish odometry to facilitate getting the localization pose
        if self.PUBLISH_ODOM:
            odom = Odometry()
            odom.header = Utils.make_header("/map", stamp)
            odom.pose.pose.position.x = pose[0]
            odom.pose.pose.position.y = pose[1]
            odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
            self.odom_pub.publish(odom)
        
        return # below this line is disabled

        """
        Our particle filter provides estimates for the "laser" frame
        since that is where our laser range estimates are measure from. Thus,
        We want to publish a "map" -> "laser" transform.

        However, the car's position is measured with respect to the "base_link"
        frame (it is the root of the TF tree). Thus, we should actually define
        a "map" -> "base_link" transform as to not break the TF tree.
        """

        # Get map -> laser transform.
        map_laser_pos = np.array( (pose[0],pose[1],0) )
        map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
        # Apply laser -> base_link transform to map -> laser transform
        # This gives a map -> base_link transform
        laser_base_link_offset = (0.265, 0, 0)
        map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T

        # Publish transform
        self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "/base_link", "/map")

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
def odom_remap(odom_msg):
    global namespace
    global previous_noiseless_pose
    global previous_noiseless_odom
    global previous_noiseless_time

    # Make our received odom reading more palatable
    current_odom = Pose2D()
    current_odom.x = odom_msg.pose.pose.position.x
    current_odom.y = odom_msg.pose.pose.position.y
    current_odom.theta = convert_quaternion_to_yaw(odom_msg.pose.pose.orientation)

    current_time = odom_msg.header.stamp

    if previous_noiseless_time is None:
        previous_noiseless_time = copy.deepcopy(current_time)

    # If we have no previous_pose, set to (0, 0, 0)
    if previous_noiseless_pose is None:
        previous_noiseless_pose = Pose2D()
        previous_noiseless_pose.x = 0
        previous_noiseless_pose.y = 0
        previous_noiseless_pose.theta = 0

    # Create the previous_odom Pose2D if it doesn't exist, also (0, 0, 0)
    if previous_noiseless_odom is None:
        previous_noiseless_odom = Pose2D()
        previous_noiseless_odom.x = 0
        previous_noiseless_odom.y = 0
        previous_noiseless_odom.theta = 0

    noiseless_odom = Odometry()
    noiseless_odom.header = odom_msg.header
    noiseless_odom.child_frame_id = namespace + 'base_footprint_filter'

    # Split movement into three distinct actions, rotate -> translate -> rotate
    delta_rotation_1 = helpers.correct_angle(math.atan2(current_odom.y - previous_noiseless_odom.y, current_odom.x - previous_noiseless_odom.x) - previous_noiseless_odom.theta)
    delta_translation = math.sqrt((previous_noiseless_odom.x - current_odom.x) ** 2 + (previous_noiseless_odom.y - current_odom.y) ** 2)
    delta_rotation_2 = helpers.correct_angle(current_odom.theta - previous_noiseless_odom.theta - delta_rotation_1)

    noiseless_odom.pose.pose.position.x = previous_noiseless_pose.x + delta_translation * math.cos(previous_noiseless_pose.theta + delta_rotation_1)
    noiseless_odom.pose.pose.position.y = previous_noiseless_pose.y + delta_translation * math.sin(previous_noiseless_pose.theta + delta_rotation_1)
    noiseless_odom_yaw = previous_noiseless_pose.theta + delta_rotation_1 + delta_rotation_2
    noiseless_odom.pose.pose.orientation = convert_yaw_to_quaternion(noiseless_odom_yaw)

    delta_time = current_time - previous_noiseless_time
    delta_time_seconds = delta_time.to_sec()
    assert delta_time_seconds >= 0

    # We will assume that our robot moves perfectly with only linear x velocity and angular z velocity
    # This is an okay assumption because we ignore other data anyways because all readings are noise and not valid
    noiseless_odom.twist.twist.linear.y = 0
    noiseless_odom.twist.twist.linear.z = 0
    noiseless_odom.twist.twist.angular.x = 0
    noiseless_odom.twist.twist.angular.y = 0

    if delta_time_seconds > 0:
        noiseless_odom.twist.twist.linear.x = delta_translation / delta_time_seconds
        noiseless_odom.twist.twist.angular.z = (noiseless_odom_yaw - previous_noiseless_pose.theta) / delta_time_seconds
    else:
        noiseless_odom.twist.twist.linear.x = 0
        noiseless_odom.twist.twist.angular.z = 0

    # From documentation on nav_msgs/Odometry:
    # Row-major representation of the 6x6 covariance matrix
    # The orientation parameters use a fixed-axis representation.
    # In order, the parameters are:
    # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
    noiseless_odom.pose.covariance = odom_msg.pose.covariance
    # Twist covariance copied from position covariance TODO this definitely wrong
    noiseless_odom.twist.covariance = odom_msg.pose.covariance

    # Publish odom message
    try:
        odom_publisher.publish(noiseless_odom)
    except rospy.ROSException as e:
        rospy.logwarn(e.message)

    # Increment previous pose for use with next message
    previous_noiseless_pose.x = noiseless_odom.pose.pose.position.x
    previous_noiseless_pose.y = noiseless_odom.pose.pose.position.y
    previous_noiseless_pose.theta = convert_quaternion_to_yaw(noiseless_odom.pose.pose.orientation)

    # Increment previous odom for use with next message
    previous_noiseless_odom.x = current_odom.x
    previous_noiseless_odom.y = current_odom.y
    previous_noiseless_odom.theta = current_odom.theta

    # Increment time
    previous_noiseless_time = current_time

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
def noisy_odom_remap(odom_msg):
    global namespace
    global previous_noisy_pose
    global previous_noisy_odom
    global previous_noisy_time

    # Odometry model taken from Probabilistic Robotics by Thrun et al.
    # Algorithm used is sample_motion_model_odometry from Table 5.6

    # Robot specific noise parameters
    # Default value = 0.02
    alpha_1 = 0.02
    alpha_2 = 0.02
    alpha_3 = 0.02
    alpha_4 = 0.02

    # Make our received odom reading more palatable
    current_odom = Pose2D()
    current_odom.x = odom_msg.pose.pose.position.x
    current_odom.y = odom_msg.pose.pose.position.y
    current_odom.theta = convert_quaternion_to_yaw(odom_msg.pose.pose.orientation)

    current_time = odom_msg.header.stamp

    if previous_noisy_time is None:
        previous_noisy_time = copy.deepcopy(current_time)

    # If we have no previous_pose, set to (0, 0, 0)
    if previous_noisy_pose is None:
        previous_noisy_pose = Pose2D()
        previous_noisy_pose.x = 0
        previous_noisy_pose.y = 0
        previous_noisy_pose.theta = 0

    # Create the previous_odom Pose2D if it doesn't exist, also (0, 0, 0)
    if previous_noisy_odom is None:
        previous_noisy_odom = Pose2D()
        previous_noisy_odom.x = 0
        previous_noisy_odom.y = 0
        previous_noisy_odom.theta = 0

    noisy_odom_x_list = []
    noisy_odom_y_list = []
    noisy_odom_yaw_list = []
    noisy_odom_x_vel_list = []
    noisy_odom_y_vel_list = []
    noisy_odom_yaw_vel_list = []

    noisy_odom = Odometry()
    noisy_odom.header = odom_msg.header
    noisy_odom.child_frame_id = namespace + 'base_footprint_filter'

    for i in range(1, 1000):
        # Split movement into three distinct actions, rotate -> translate -> rotate
        delta_rotation_1 = helpers.correct_angle(math.atan2(current_odom.y - previous_noisy_odom.y, current_odom.x - previous_noisy_odom.x) - previous_noisy_odom.theta)
        delta_translation = math.sqrt((previous_noisy_odom.x - current_odom.x) ** 2 + (previous_noisy_odom.y - current_odom.y) ** 2)
        delta_rotation_2 = helpers.correct_angle(current_odom.theta - previous_noisy_odom.theta - delta_rotation_1)

        # Create random pose from given movements by adding noise
        std_dev_1 = alpha_1 * abs(delta_rotation_1) + alpha_2 * delta_translation
        delta_rotation_1_hat = delta_rotation_1 - normal(scale=std_dev_1)

        std_dev_2 = alpha_3 * delta_translation + alpha_4 * (abs(delta_rotation_1) + abs(delta_rotation_2))
        delta_translation_hat = delta_translation - normal(scale=std_dev_2)

        std_dev_3 = alpha_1 * abs(delta_rotation_2) + alpha_2 * delta_translation
        delta_rotation_2_hat = delta_rotation_2 - normal(scale=std_dev_3)

        noisy_odom.pose.pose.position.x = previous_noisy_pose.x + delta_translation_hat * math.cos(previous_noisy_pose.theta + delta_rotation_1_hat)
        noisy_odom.pose.pose.position.y = previous_noisy_pose.y + delta_translation_hat * math.sin(previous_noisy_pose.theta + delta_rotation_1_hat)
        noisy_odom_yaw = previous_noisy_pose.theta + delta_rotation_1_hat + delta_rotation_2_hat
        noisy_odom.pose.pose.orientation = convert_yaw_to_quaternion(noisy_odom_yaw)

        delta_time = current_time - previous_noisy_time
        delta_time_seconds = delta_time.to_sec()
        assert delta_time_seconds >= 0

        # We will assume that our robot moves perfectly with only linear x velocity and angular z velocity
        # This is an okay assumption because we ignore other data anyways because all readings are noise and not valid
        noisy_odom.twist.twist.linear.y = 0
        noisy_odom.twist.twist.linear.z = 0
        noisy_odom.twist.twist.angular.x = 0
        noisy_odom.twist.twist.angular.y = 0

        if delta_time_seconds > 0:
            noisy_odom.twist.twist.linear.x = delta_translation_hat / delta_time_seconds
            noisy_odom.twist.twist.angular.z = (noisy_odom_yaw - previous_noisy_pose.theta) / delta_time_seconds
        else:
            noisy_odom.twist.twist.linear.x = 0
            noisy_odom.twist.twist.angular.z = 0

        noisy_odom_x_list.append(noisy_odom.pose.pose.position.x)
        noisy_odom_y_list.append(noisy_odom.pose.pose.position.y)
        noisy_odom_yaw_list.append(noisy_odom_yaw)
        noisy_odom_x_vel_list.append(noisy_odom.twist.twist.linear.x)
        noisy_odom_y_vel_list.append(noisy_odom.twist.twist.linear.y)
        noisy_odom_yaw_vel_list.append(noisy_odom.twist.twist.angular.z)

    # From documentation on nav_msgs/Odometry:
    # Row-major representation of the 6x6 covariance matrix
#.........这里部分代码省略.........

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
def callback(input):
    output = Odometry()
    output.header = input.header
    output.pose.pose = input.pose
    pub.publish(output)

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
while not rospy.is_shutdown():
	if opts.useoriginref:
		file_string = 'scan'+format(counter_index,'05d')+'.pcd'
	else:
		file_string = 'scanorg'+format(counter_index,'05d')+'.pcd'
	file_string = os.path.join(opts.dir_prefix, file_string)
	pcl_cloud = pcl.load(file_string)
	pcloud = PointCloud2()
	header = Header()
	header.stamp = rospy.Time.now()
	if opts.useoriginref:
		header.frame_id = 'map'
	else:
		header.frame_id = 'velodyne' #'odom' #'pose' #'frame' #'velodyne'
	pcloud = pc2.create_cloud_xyz32(header, pcl_cloud.to_array())
	#pose = graph[str(counter_index)][0]
	index_graph = graph_node_names.index(str(counter_index))
	pose =  graph.nodes[index_graph].pose
	print pose
	odom = Odometry()
	odom.header = Header()
	odom.header.frame_id = 'map'
	odom.header.stamp = rospy.Time.now()
	odom.child_frame_id = 'velodyne'
	odom.pose.pose = pose
	pub_frame.publish(pcloud)
	pub_odom.publish(odom)
	pub_tf.sendTransform((pose.position.x, pose.position.y, pose.position.z), (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), rospy.Time.now(),'velodyne','map')
	counter_index = counter_index + 1
	rate.sleep()

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
def callback(data):
    odom = Odometry()
    odom.header = data.header
    odom.child_frame_id = child_frame_id
    odom.pose = data.pose
    pub.publish(odom)

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import header [as 别名]
def main():

    parser = argparse.ArgumentParser(description="Parse the images.txt file to "
                                                 "create a rosbag with several "
                                                 "PoseStamped messages.")

    parser.add_argument('-i',  '--input',
                        help='file with poses (images.txt)',
                        metavar='poses_filename',
                        default='./images.txt',
                        type=str,
                        required=True
                        )
    parser.add_argument('-o', '--output',
                        help='output bag file (with location)',
                        metavar='bag_filename',
                        type=str,
                        required=True
                        )
    parser.add_argument('-y',
                        help='if images_adjusted.txt is found in the same folder'
                             'as the supplied filename, then use it without'
                             'asking',
                        metavar='True/False',
                        type=bool,
                        default=False,
                        choices=[True, False]
                        )

    arguments = parser.parse_args()

    images_folder, _ = os.path.split(arguments.input)
    alt_file = os.path.join(images_folder, 'images_adjusted.txt')

    if os.path.exists(alt_file):
        if arguments.y:
            images_file = open(alt_file)
        else:
            reply = None
            while reply not in ['y', 'n']:
                print("The images_adjusted.txt file is in the folder {}, do you"
                      " want to use that instead? [y/n]".format(images_folder))
                reply = raw_input()
            if reply == 'y':
                images_file = open(alt_file)
            else:
                images_file = open(arguments.input)
    else:
        images_file = open(arguments.input)

    print("Processing data from {}...".format(images_file.name))
    with rosbag.Bag(arguments.output, 'w') as outputbag:
        for lineno, line in enumerate(images_file):
            #lines must be of the form:
            #image_basename,t_android,t_movidius,r11,r12,r12,t1,r21,r22,r23,t2,r31,r32,r33,t3
            tokens = line.strip('\n').split(',')

            if (len(tokens)) != 15:
                print("Line {0} appears to be wrong: \"{1}\" ".format(lineno, tokens))
                continue

            #first elements before transformation
            image_name = tokens[0]
            ts1 = float(tokens[1])
            ts2 = float(tokens[2])

            #getting the quaterion out the rotation matrix
            rotation_matrix = np.array(map(float, tokens[3:])).reshape(3, 4)
            rotation_matrix = np.vstack((rotation_matrix, [0, 0, 0, 1]))
            quaternion = transformations.quaternion_from_matrix(rotation_matrix)

            #creating the pose
            p = PoseStamped()
            p.header.frame_id = "/tango/world"
            p.header.stamp = rospy.Time.from_seconds(ts1)
            p.header.seq = lineno
            p.pose.position.x = rotation_matrix[0, 3]
            p.pose.position.y = rotation_matrix[1, 3]
            p.pose.position.z = rotation_matrix[2, 3]
            p.pose.orientation.x = quaternion[0]
            p.pose.orientation.y = quaternion[1]
            p.pose.orientation.z = quaternion[2]
            p.pose.orientation.w = quaternion[3]
            outputbag.write("tango_imu_pose", p, rospy.Time.from_seconds(ts1))

            #creating the odometry entry
            #TODO get covariances from the covariances.txt file
            o = Odometry()
            o.header = p.header
            o.child_frame_id = o.header.frame_id
            o.pose.pose = p.pose
            outputbag.write("tango_imu_odom", o, rospy.Time.from_seconds(ts1))

            #creating the tf messages
            tfmsg = tfMessage()
            tf_stamped = TransformStamped()
            tf_stamped.header.frame_id = "/tango/world"
            tf_stamped.header.seq = lineno
            tf_stamped.header.stamp = rospy.Time.from_seconds(ts1)
            tf_stamped.child_frame_id = "/tango/imu"
#.........这里部分代码省略.........