self.next_pos(0, 0, direction*90*pi/180)
final_pose = map(operator.sub, self.odom_pose, init_pose)
print "Odom Pose: x=%.3f, y=%.3f, angle=%.3f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
self.next_pos(0, 0, direction*90*pi/180)
final_pose = map(operator.sub, self.odom_pose, init_pose)
print "Odom Pose: x=%.3f, y=%.3f, angle=%.3f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)