def publish(self):
print "publishing"
message = TwistStamped()
message.header = self.pose.header
message.twist.linear.x = self.pose.vector.x
message.twist.linear.y = self.pose.vector.y
message.twist.linear.z = 0.0
message.twist.angular.x = 0.0
message.twist.angular.y = 0.0
message.twist.angular.z = self.pose.vector.z
print self.pose.vector.z
self.pub.publish(message)
message = Vector3Stamped()
message.header = self.pose.header
message.vector = self.encoderSpeed
self.motor_test.publish(message)
