Example #1
0
	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)
            )
Example #2
0
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)