def debugCallback(self):
msg = PoseStamped()
msg.header = self.header
msg.pose.orientation = self.thetaToOrientation(self.attitudeComputation.getTheta())
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 0.0
self.debugPublisher[0].publish(msg)
msg = PoseStamped()
msg.header = self.header
msg.pose.orientation = self.thetaToOrientation(self.measurementHandler.referenceOrientation)
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 0.0
self.debugPublisher[1].publish(msg)
msg = PoseStamped()
msg.header = self.header
msg.pose.orientation = self.thetaToOrientation(self.attitudeComputation.computedTheta)
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 0.0
self.debugPublisher[2].publish(msg)
msg = Vector3()
degree = np.degrees(self.attitudeComputation.getTheta())
msg.x = degree[0]
msg.y = degree[1]
msg.z = degree[2]
self.debugPublisher[3].publish(msg)
msg = Vector3()
degree = np.degrees(self.measurementHandler.referenceOrientation)
msg.x = degree[0]
msg.y = degree[1]
msg.z = degree[2]
self.debugPublisher[3].publish(msg)
