def publish_pose(x1, y1, x2, y2, z2, t):
x1 = (x1 + minX) * resolution_2d
y1 = (y1 + minY) * resolution_2d
x2 = (x2 + minX) * resolution_2d
y2 = (y2 + minY) * resolution_2d
msg = PoseStamped()
msg.header = std_msgs.msg.Header()
msg.header.stamp = t
msg.header.frame_id = '/map'
msg.pose.position.x = x2
msg.pose.position.y = y2
msg.pose.position.z = z2
v = numpy.array([x2 - x1, -(y2 - y1)]).astype(float)
v /= numpy.linalg.norm(v)
theta = numpy.arctan2(v[1], v[0])
msg.pose.orientation.x = 0
msg.pose.orientation.y = 0
msg.pose.orientation.z = 0
msg.pose.orientation.w = 1
if not hasattr(publish_pose, 'path'):
publish_pose.path = []
publish_pose.path.append(msg)
bag.write('slam_out_pose', msg, t=t)
msg = Path()
msg.header = std_msgs.msg.Header()
msg.header.stamp = t
msg.header.frame_id = '/map'
msg.poses = publish_pose.path
bag.write('trajectory', msg, t=t)
