def __init__(self, **kwargs):
super(ROS_IMU, self).__init__()
Kp = rospy.get_param('Mahoney/Kp')
print Kp
self.imu = IMU_Mahoney(Ts = 0.01)
self.a = self.imu.quaternion
# self.imu = IMU_Magdwick( Ts = 0.01 )
# self.imu = IMU_Mahoney( Ts = 0.01 )
self.subscriber = dict(
raw_imu = rospy.Subscriber('/ardrone/imu', Imu, callback = self.ReceiveImu ),
)
self.publisher = dict(
state = rospy.Publisher('/ardrone/kalman_imu', Odometry)
)
class ROS_IMU(ROS_Object, object):
"""docstring for ROS_IMU"""
def __init__(self, **kwargs):
super(ROS_IMU, self).__init__()
Kp = rospy.get_param('Mahoney/Kp')
print Kp
self.imu = IMU_Mahoney(Ts = 0.01)
self.a = self.imu.quaternion
# self.imu = IMU_Magdwick( Ts = 0.01 )
# self.imu = IMU_Mahoney( Ts = 0.01 )
self.subscriber = dict(
raw_imu = rospy.Subscriber('/ardrone/imu', Imu, callback = self.ReceiveImu ),
)
self.publisher = dict(
state = rospy.Publisher('/ardrone/kalman_imu', Odometry)
)
def ReceiveImu(self, imu_raw):
self.imu.measure(imu_raw)
self.imu.predict()
self.imu.correct()
self.Talk()
#print self.a.z, self.imu.quaternion.z
def Talk( self ):
msg = Odometry()
msg.header.stamp = rospy.Time.now()
quaternion = self.imu.get_quaternion()
msg.pose.pose.orientation.x = quaternion['x']
msg.pose.pose.orientation.y = quaternion['y']
msg.pose.pose.orientation.z = quaternion['z']
msg.pose.pose.orientation.w = quaternion['w']
self.publisher['state'].publish(msg)
