def run(self):
counter = 0;
# set default Q and mu values to eye(6) and zero
Q = np.eye(6)
mu = np.zeros(6)
# Initial sensor calibration
Q, mu = self.calibrateSensors()
# Initialize Kalman filter
self.P_lin = np.diag([1.,0.,1.,0.])*pow(self.initial_position_uncertainty,2)
self.Q_lin = self.G_lin.dot(Q[:2,:2]).dot(self.G_lin.T)
self.P_ang = pow(self.initial_orientation_uncertainty,2)
self.Q_ang = self.G_ang*Q[5,5]*self.G_ang
self.R_lin = np.eye(2)*self.camera_position_error
self.R_ang = self.camera_orientation_error
self.acc_bias = mu[:2]
self.gyro_bias = mu[5]
self.x_lin = np.array([self.z.translation.x, self.z.translation.y])
self.v_lin = np.array([0.,0.])
self.psi = 2*np.arccos(self.z.rotation.w)*np.sign(self.z.rotation.z) # Assume we have a quaternion with vertical axis
# Run Kalman filter
while not rospy.is_shutdown():
# Obtain IMU measurement
rospy.wait_for_service('last_imu')
try:
get_imu = rospy.ServiceProxy('last_imu', ImuSrv)
self.u = get_imu()
except rospy.ServiceException, e:
#print "Service call to IMU Server failed: %s"%e
print "No IMU update this time step"
# Assume previous measured u (Zero-Order Hold)
# Perform time and measurement updates as appropriate
self.timeUpdate(self.u)
if self.updateFlag:
self.measurementUpdate(self.z)
# Publish state estimate in topic
state = Transform()
state.translation.x = self.x_lin[0]
state.translation.y = self.x_lin[1]
state.rotation.z = 1
state.rotation.w = self.psi # Quaternion form
self.state_pub.publish(state)
state_tf = TFMessage()
state_tf.transforms = [TransformStamped()]
state_tf.transforms[0].header.seq = counter
state_tf.transforms[0].header.frame_id = self.origin_tag
state_tf.transforms[0].child_frame_id = self.mname
state_tf.transforms[0].transform = state
self.state_pub_tf.publish(state_tf)
counter = counter + 1
# Finish cycle and loop
self.rate.sleep()
def save_known_poses(self, req):
print self.known_poses.values()
with rosbag.Bag('bundle.bag','w') as bag:
tf_msg = TFMessage()
tf_msg.transforms = self.known_poses.values()
bag.write('/tf',tf_msg,rospy.Time.now())
bag.close()
return SaveKnownPosesResponse()
def callback(self):
tFMessage = TFMessage()
for lidar in self.lidars:
if self.robot.getTime() - self.lastUpdate[lidar] >= lidar.getSamplingPeriod():
self.publish(lidar, tFMessage.transforms)
if tFMessage.transforms:
self.tfPublisher.publish(tFMessage)
def wjx_save_dynamic_tf(bag, kitti, initial_time):
timestamps = map(lambda x: initial_time + x.total_seconds(),
kitti.timestamps)
for timestamp, tf_matrix in zip(timestamps, kitti.poses):
print len(kitti.poses)
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = rospy.Time.from_sec(timestamp)
tf_stamped.header.frame_id = 'world'
tf_stamped.child_frame_id = 'camera_left'
t = tf_matrix[0:3, 3]
q = tf.transformations.quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def save_static_transforms(bag, transforms, timestamps):
print("Exporting static transformations")
tfm = TFMessage()
for transform in transforms:
t = get_static_transform(from_frame_id=transform[0],
to_frame_id=transform[1],
transform=transform[2])
tfm.transforms.append(t)
for timestamp in timestamps:
time = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
for i in range(len(tfm.transforms)):
tfm.transforms[i].header.stamp = time
bag.write('/tf_static', tfm, t=time)
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
twist_msg = TwistStamped()
twist_msg.header.frame_id = gps_frame_id
twist_msg.header.stamp = rospy.Time.from_sec(
float(timestamp.strftime("%s.%f")))
twist_msg.twist.linear.x = oxts.packet.vf
twist_msg.twist.linear.y = oxts.packet.vl
twist_msg.twist.linear.z = oxts.packet.vu
twist_msg.twist.angular.x = oxts.packet.wf
twist_msg.twist.angular.y = oxts.packet.wl
twist_msg.twist.angular.z = oxts.packet.wu
bag.write(topic, twist_msg, t=twist_msg.header.stamp)
def update_simulation_world_tf(self, vehicle_simulation_tf: Transform):
"""Publish up to date simulation to world transformation to /tf.
Args:
vehicle_simulation_tf (Transform): Transformation between vehicle
and simulation frames.
"""
if rospy.Time.now().to_sec() - self._last_tf_update == 0:
return
self._last_tf_update = rospy.Time.now().to_sec()
tf_stamped = geometry_msgs.msg.TransformStamped()
tf_stamped.header = std_msgs.msg.Header()
tf_stamped.header.stamp = rospy.Time.now()
# Transform from world to simulation
tf_stamped.header.frame_id = self.param.frame.simulation
tf_stamped.child_frame_id = self.param.frame.world
# Transformation from world to
# simulation == (world to vehicle -> vehicle to simulation)
tf_stamped.transform = (
vehicle_simulation_tf *
self.vehicle_world_tf.inverse).to_geometry_msg()
self.pub_tf.publish(TFMessage([tf_stamped]))
def write_tf(self, tfs):
print(' Writing tfs')
tfm = TFMessage()
time = rospy.Time()
time.secs = 0
time.nsecs = 1
for record in tqdm(tfs):
tf_message = TransformStamped()
t = record['tran']
q = record['rot']
tf_message.header.stamp = time
tf_message.header.frame_id = record['parent']
tf_message.header.frame_id = record['label']
tf_message.transform.translation.x = float(t[0])
tf_message.transform.translation.y = float(t[1])
tf_message.transform.translation.z = float(t[2])
tf_message.transform.rotation.x = float(q[0])
tf_message.transform.rotation.y = float(q[1])
tf_message.transform.rotation.z = float(q[2])
tf_message.transform.rotation.w = float(q[3])
tfm.transforms.append(tf_message)
self.bag.write('/tf', tfm, t=time)
self.bag.write('/tf_static', tfm, t=time)
def update(self):
try:
with self.get_god_map():
robot_links = set(self.unsafe_get_robot().get_link_names())
attached_links = robot_links - self.original_links
if attached_links:
tf_msg = TFMessage()
get_fk = self.unsafe_get_robot().get_fk_pose
for link_name in attached_links:
parent_link_name = self.unsafe_get_robot().get_parent_link_of_link(link_name)
fk = get_fk(parent_link_name, link_name)
tf = TransformStamped()
tf.header = fk.header
tf.header.stamp = rospy.get_rostime()
tf.child_frame_id = link_name
tf.transform.translation.x = fk.pose.position.x
tf.transform.translation.y = fk.pose.position.y
tf.transform.translation.z = fk.pose.position.z
tf.transform.rotation = normalize_quaternion_msg(fk.pose.orientation)
tf_msg.transforms.append(tf)
self.tf_pub.publish(tf_msg)
except KeyError as e:
pass
except UnboundLocalError as e:
pass
except ValueError as e:
pass
return Status.SUCCESS
def drone_odom_callback(self, rcv):
print("ODOM: " + str(rcv))
# Publish pose for rviz
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint_drone"
quat = self.euler_to_quaternion(self.lastPose.x, self.lastPose.y,
self.lastPose.z)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
msg.transform.rotation = orientation
position = Vector3()
position.x = rcv.x
position.y = rcv.y
position.z = rcv.z
msg.transform.translation = position
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
# Publish Rviz Path
msgpath = Path()
msgpath.header.frame_id = "/map"
msgpath.header.stamp = Clock().now().to_msg()
pose = PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = Clock().now().to_msg()
pose.pose.position.x = rcv.x
pose.pose.position.y = rcv.y
pose.pose.position.z = rcv.z
self.posearray.append(pose)
if len(self.posearray) > 50:
self.posearray = self.posearray[1:]
msgpath.poses = self.posearray
self.pub_posearray.publish(msgpath)
def publish_lcs(self, lcs):
if lcs and self.publish_tf:
transforms = [
load_carrier_to_tf(lc, i) for i, lc in enumerate(lcs)
]
self.pub_tf.publish(TFMessage(transforms=transforms))
if self.publish_markers:
self.publish_lc_markers(lcs)
def send_msgs(self):
for publisher, msg in self.msgs_to_publish:
self.bag.write(publisher.name, msg, self.cur_time)
tf_msg = TFMessage(self.tf_to_publish)
self.bag.write('tf', tf_msg, self.cur_time)
super(MonoRosBridgeWithBag, self).send_msgs()
def publish_grasps(self, grasps):
if grasps and self.publish_tf:
transforms = [
grasp_to_tf(grasp, i) for i, grasp in enumerate(grasps)
]
self.pub_tf.publish(TFMessage(transforms=transforms))
if self.publish_markers:
self.publish_grasps_markers(grasps)
def send_transform_message(self, transform):
"""
:param transform: geometry_msgs.msg.TransformStamped
Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``.
"""
#tfm = TFMessage([transform])
tfm = TFMessage(transforms=[transform])
self.pub_tf.publish(tfm)
def send_msgs(self):
for publisher, msg in self.msgs_to_publish:
publisher.publish(msg)
self.msgs_to_publish = []
tf_msg = TFMessage(self.tf_to_publish)
self.publishers['tf'].publish(tf_msg)
self.tf_to_publish = []
def create_messages(params, words):
# 0 | 1 | 2 - 1+num_readings | 2+num_readings | 3+num_readings |
# xLASER | num_readings | [range_readings] | x | y |
#-------------------------------------------------------------------------------
# 4+num_readings | 5+num_readings | 6+num_readings | 7+num_readings |
# theta | odom_x | odom_y | odom_theta |
#-------------------------------------------------------------------------------
# 8+num_readings | 9+num_readings | 10+num_readings
# ipc_timestamp | ipc_hostname |logger_timestamp
laser_msg = LaserScan()
laser_msg.header.frame_id = laser_tf_child_frame
num_readings = int(words[1])
laser_msg.ranges = list(map(float, words[2:1 + num_readings + 1]))
str_laser_fov = "laser_" + param_names[words[0]] + "_fov"
if str_laser_fov in params:
a_range = float(params[str_laser_fov][0])
else:
a_range = radians(180.0)
str_laser_res = "laser_" + param_names[words[0]] + "_resolution"
if str_laser_res in params:
laser_msg.angle_increment = radians(float(params[str_laser_res][0]))
else:
laser_msg.angle_increment = a_range / float(num_readings)
laser_msg.angle_min = -a_range / 2.0
laser_msg.angle_max = a_range / 2.0
str_max_range = "robot_" + param_names[words[0]] + "_max"
if str_max_range in params:
laser_msg.range_max = float(params[str_max_range][0])
else:
laser_msg.range_max = 20.0
laser_msg.range_min = 0
laser_msg.header.stamp = rospy.Time(float(words[8 + num_readings]))
TS_odom_robot_msg = TransformStamped()
TS_odom_robot_msg.header.stamp = laser_msg.header.stamp + rospy.Duration(
0.01)
TS_odom_robot_msg.header.frame_id = laser_tf_parrent_frame
TS_odom_robot_msg.child_frame_id = laser_tf_child_frame
TS_odom_robot_msg.transform.translation = Point(
float(words[5 + num_readings]), float(words[6 + num_readings]), 0.0)
quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, float(words[7 + num_readings]))
TS_odom_robot_msg.transform.rotation.x = quaternion[0]
TS_odom_robot_msg.transform.rotation.y = quaternion[1]
TS_odom_robot_msg.transform.rotation.z = quaternion[2]
TS_odom_robot_msg.transform.rotation.w = quaternion[3]
tf_msg = TFMessage()
tf_msg.transforms.append(TS_odom_robot_msg)
return [(laser_topic_name, laser_msg, laser_msg.header.stamp),
(tf_topic_name, tf_msg, TS_odom_robot_msg.header.stamp)]
def process_transform(self, msg):
output = TFMessage()
for tf in msg.transforms:
if tf.header.frame_id == 'map' and tf.child_frame_id == 'odom':
continue
else:
output.transforms.append(tf)
if output.transforms:
self.pub.publish(output)
def listener_callback(self, msg):
up = msg.buttons[5] - msg.buttons[7]
self.transform.transform.translation.x = msg.axes[3] * 0.02
self.transform.transform.translation.y = msg.axes[2] * 0.03
self.transform.transform.translation.z = 0.1 + up * 0.03
self.transform.transform.rotation.x = -msg.axes[0] * 0.1
self.transform.transform.rotation.y = msg.axes[1] * 0.1
msg = TFMessage(transforms=[self.transform])
self.broadcaster.publish(msg)
def pose_to_tf(pose_msg, child_frame_id):
ts = TransformStamped()
ts.header = pose_msg.header
ts.child_frame_id = child_frame_id
ts.transform.translation = pose_msg.pose.position
ts.transform.rotation = pose_msg.pose.orientation
msg = TFMessage()
msg.transforms.append(ts)
return msg
def sendTransform(self, transform):
"""
Send a transform, or a list of transforms, to the Buffer associated with this TransformBroadcaster.
:param transform: A transform or list of transforms to send.
"""
if not isinstance(transform, list):
transform = [transform]
self.pub_tf.publish(TFMessage(transform))
def __init__(self):
self.bridge = CvBridge()
self.active = []
for i in range(50):
self.active.append(False)
self.tf_list = TFMessage()
for i in range(50):
self.tf_list.transforms.append(TransformStamped())
self.voronoi_list = TFMessage()
for i in range(50):
self.voronoi_list.transforms.append(TransformStamped())
self.sub = rospy.Subscriber('/PoseEstimationC', TFMessage,
self.pose_cb)
self.sub = rospy.Subscriber('/VList', TFMessage, self.voronoi_cb)
def __init__(self, test):
self.test = test
#print "recorder_core: self.test.name:", self.test.name
recorder_config = self.load_data(rospkg.RosPack().get_path("atf_recorder_plugins") +
"/config/recorder_plugins.yaml")
try:
# delete test_results directories and create new ones
if os.path.exists(self.test.generation_config["bagfile_output"]):
# shutil.rmtree(self.tests.generation_config"bagfile_output"]) #FIXME will fail if multiple test run concurrently
pass
else:
os.makedirs(self.test.generation_config["bagfile_output"])
except OSError:
pass
# lock for self variables of this class
self.lock = Lock()
# create bag file writer handle
self.lock_write = Lock()
self.bag = rosbag.Bag(self.test.generation_config["bagfile_output"] + self.test.name + ".bag", 'w')
self.bag_file_writer = atf_core.BagfileWriter(self.bag, self.lock_write)
# Init metric recorder
self.recorder_plugin_list = []
#print "recorder_config", recorder_config
if len(recorder_config) > 0:
for value in recorder_config.values():
#print "value=", value
self.recorder_plugin_list.append(getattr(atf_recorder_plugins, value)(self.lock_write,
self.bag_file_writer))
#print "self.recorder_plugin_list", self.recorder_plugin_list
#rospy.Service(self.topic + "recorder_command", RecorderCommand, self.command_callback)
rospy.on_shutdown(self.shutdown)
# wait for topics and services to become active
for topic in test.robot_config["wait_for_topics"]:
rospy.loginfo("Waiting for topic '%s'...", topic)
rospy.wait_for_message(topic, rospy.AnyMsg)
rospy.loginfo("... got message on topic '%s'.", topic)
for service in test.robot_config["wait_for_services"]:
rospy.loginfo("Waiting for service '%s'...", service)
rospy.wait_for_service(service)
rospy.loginfo("... service '%s' available.", service)
self.active_topics = {}
self.subscribers = {}
self.tf_static_message = TFMessage()
rospy.Timer(rospy.Duration(0.1), self.create_subscriber_callback)
rospy.Timer(rospy.Duration(0.1), self.tf_static_timer_callback)
rospy.loginfo("ATF recorder: started!")
def __init__(self):
self._tf_msg = TFMessage()
self._pub = rospy.Publisher(
"/tf_static_republished", TFMessage, queue_size=1, latch=True
)
self._sub = rospy.Subscriber(
"/tf_static", TFMessage, self._sub_callback
)
self._timer = rospy.Timer(rospy.Duration(1), self._timer_callback)
def publish_transform(self):
tf_msg = TFMessage()
tf = TransformStamped()
with self.lock:
tf.transform = self.transform
tf.header.frame_id = self.parent_frame
tf.header.stamp = rospy.get_rostime()
tf.child_frame_id = self.child_frame
tf_msg.transforms.append(tf)
self.tf_pub.publish(tf_msg)
def base_info_cb(self, msg):
self._odom.header.stamp = msg.stamp
self._odom.pose.pose.position.x = msg.x
self._odom.pose.pose.position.y = msg.y
self._odom.pose.pose.orientation.z = math.sin(msg.orientation / 2.0)
self._odom.pose.pose.orientation.w = math.cos(msg.orientation / 2.0)
self._odom.twist.twist.linear.x = msg.forward_velocity
self._odom.twist.twist.angular.z = msg.rotational_velocity
self.pub_odom.publish(self._odom)
self._transform.header.stamp = msg.stamp
pos = self._odom.pose.pose.position
self._transform.transform.translation = Vector3(x=pos.x,
y=pos.y,
z=pos.z)
self._transform.transform.rotation = self._odom.pose.pose.orientation
tfmsg = TFMessage()
tfmsg.transforms = [self._transform]
self.pub_tf.publish(tfmsg)
def move_it_ar_interface():
robot = MoveGroupPythonIntefaceTutorial()
rospy.Subscriber("euclidean_goal_pose", geometry_msgs.msg.Pose , move_manipulator, (robot,[t_plan_started]) )
rospy.Subscriber("ar_joint_state", Float32MultiArray, joint_goal_manipulator, (robot,[t_plan_started]) )
tf_pub = rospy.Publisher('tf_static', TFMessage , queue_size=1)
tfm = TFMessage([t])
rate = rospy.Rate(1)
while not rospy.is_shutdown():
tf_pub.publish(tfm)
rate.sleep()
def gxPose2d_to_tf(msg,t,outbag,frame_id,child_frame_id,offset):
str_gx=[x.split(":") for x in str(msg).split("\n")]
br = TransformStamped()
br.transform.translation=Point(float(str_gx[1][1]),float(str_gx[2][1]),0)
br.transform.rotation=Quaternion(*tf.transformations.quaternion_from_euler(0,0,float(str_gx[3][1])))
br.header.stamp=Time.from_sec(gx_msg_stamp(msg).to_sec() + offset)
br.header.frame_id=frame_id
br.child_frame_id=child_frame_id
tfm=TFMessage([br])
outbag.write('/tf',tfm,t)
def _prepare_msgs(self):
"""
Private Function used to prepare tf and object message to be sent out
:return:
"""
tf_msg = TFMessage(self.tf_to_publish)
self.msgs_to_publish.append((self.publishers['tf'], tf_msg))
self.tf_to_publish = []
self.publish_ros_message(
'/carla/objects', obj_sensor.get_filtered_objectarray(self, None))
def run(self):
while not self._stop_flag:
rospy.sleep(0.01)
with self._lock:
if self._transform is None:
continue
tfm = TFMessage([
self._transform,
])
self._pub_tf.publish(tfm)
def publish_object_frames(self, object_ids=None):
if object_ids is None:
object_ids = self.objects.keys()
tf_msgs = []
stamp = rospy.get_rostime()
for object_id in object_ids: # type: (str, PerceivedObject)
tf_msg = self.objects[object_id].get_tf_msg()
tf_msg.header.stamp = stamp
if tf_msg is not None:
tf_msgs.append(tf_msg)
self.tf_broadcaster.publish(TFMessage(tf_msgs))
def run(self):
marker_map = rospy.get_param('~marker_map', {})
args = {
'device': rospy.get_param('~device'),
'marker_map': marker_map,
'callback_global': self.callback_global,
'callback_local': self.callback_local,
}
parameters = self.get_options()
self.fixed_frame_id = rospy.get_param('~fixed_frame_id', 'map')
self.robot_frame_id = rospy.get_param('~robot_frame_id', 'base_link')
self.stargazer_frame_id = rospy.get_param('~stargazer_frame_id', 'stargazer')
self.map_frame_prefix = rospy.get_param('~map_frame_prefix', 'stargazer/map_')
self.marker_frame_prefix = rospy.get_param('~marker_frame_prefix', 'stargazer/marker_')
self.covariance = rospy.get_param('~covariance', None)
if self.covariance is None:
raise Exception('The "covariance" parameter is required.')
elif len(self.covariance) != 36:
raise Exception('The "covariance" parameter must be a 36 element vector.')
# Publish static TF frames for the Stargazer map.
stamp_now = rospy.Time.now()
map_tf_msg = TFMessage()
for marker_id, Tmap_marker in marker_map.iteritems():
marker_tf_msg = TransformStamped()
marker_tf_msg.header.stamp = stamp_now
marker_tf_msg.header.frame_id = self.fixed_frame_id
marker_tf_msg.child_frame_id = self.map_frame_prefix + marker_id
marker_tf_msg.transform = matrix_to_transform(Tmap_marker)
map_tf_msg.transforms.append(marker_tf_msg)
self.tf_static_pub = rospy.Publisher('tf_static', TFMessage, latch=True)
self.tf_static_pub.publish(map_tf_msg)
# Start publishing Stargazer data.
with StarGazer(**args) as stargazer:
# The StarGazer might be streaming data. Turn off streaming mode.
stargazer.stop_streaming()
# Set all parameters, possibly to their default values. This is the
# safest option because the parameters can be corrupted when the
# StarGazer is powered off.
for name, value in parameters.iteritems():
stargazer.set_parameter(name, value)
# Start streaming. ROS messages will be published in callbacks.
stargazer.start_streaming()
rospy.spin()
# Stop streaming. Try to clean up after ourselves.
stargazer.stop_streaming()
def publish(self):
t = TransformStamped()
t.header.frame_id = self.parent_frame_id
t.header.stamp = rospy.Time.now()
t.child_frame_id = self.child_frame_id
t.transform.translation = self.pose.position
t.transform.rotation = self.pose.orientation
message = TFMessage([t])
self.tf_pub.publish(message)
def save_static_transforms(bag, transforms, timestamps):
print("Exporting static transformations")
tfm = TFMessage()
for transform in transforms:
t = get_static_transform(from_frame_id=transform[0], to_frame_id=transform[1], transform=transform[2])
tfm.transforms.append(t)
for timestamp in timestamps:
time = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
for i in range(len(tfm.transforms)):
tfm.transforms[i].header.stamp = time
bag.write('/tf_static', tfm, t=time)
avg_tf = TransformStamped()
avg_tf.header.frame_id = tf_group[0].header.frame_id
avg_tf.child_frame_id = tf_group[0].child_frame_id
avg_tf.transform = numpy_to_tf_msg(avg_trans, avg_rot)
avgs.append(avg_tf)
return avgs
# Pass bagfile name as first argument
if __name__ == '__main__':
tf_groups = get_tf_groups(sys.argv[1])
avgs = avg_transforms(tf_groups)
"""
for tfg, avg in zip(tf_groups,avgs):
print '%d transforms from \'%s\' to \'%s\'' % (
len(tfg), tfg[0].header.frame_id, tfg[0].child_frame_id
)
print 'Average Transform:'
print avg.transform
"""
if len(sys.argv) > 2:
with rosbag.Bag(sys.argv[2],'w') as outbag:
tf_msg = TFMessage()
tf_msg.transforms = avgs
outbag.write("/tf", tf_msg, rospy.Time.from_sec(time.time()))
outbag.close()