def kalman_calc(event):
global gyroAngVel, odoAngVel, x, P, dt, lastInterruptTime, gyroBelieve, odoBelieve
gyroAngVel = drift_correction(gyroAngVel)
temp_mes = [[gyroAngVel, odoAngVel]]
update_Believes()
dt = event.current_real.to_sec() - lastInterruptTime
x, P = filter(x, P, temp_mes)
# Publish Message
pub_msg = kalman_output()
pub_msg.header.stamp = rospy.get_rostime()
pub_msg.yaw = x.value[0][0] % (pi * 2)
pub_msg.ang_vel = x.value[1][0]
pub_msg.ang_vel2 = x.value[2][0]
global pub, odo2
if odo2:
rotationQuaternion = tf.transformations.quaternion_from_euler(0, 0, -1 * ((x.value[0][0] - pi) % (pi * 2)))
tfbr = tf.TransformBroadcaster()
tfbr.sendTransform(
(odo2.pose.pose.position.x, odo2.pose.pose.position.y, odo2.pose.pose.position.z),
rotationQuaternion,
rospy.Time.now(),
"base_link",
"odom",
)
pub_msg = odo2
pub_msg.pose.pose.orientation.x = rotationQuaternion[0]
pub_msg.pose.pose.orientation.y = rotationQuaternion[1]
pub_msg.pose.pose.orientation.z = rotationQuaternion[2]
pub_msg.pose.pose.orientation.w = rotationQuaternion[3]
pub.publish(pub_msg)
lastInterruptTime = event.current_real.to_sec()
def kalman_calc(event):
global gyroAngVel, odoAngVel, x, P, dt, lastInterruptTime, gyroBelieve, odoBelieve
gyroAngVel = drift_correction(gyroAngVel)
temp_mes = [[gyroAngVel,odoAngVel]]
update_Believes()
dt = (event.current_real.to_sec()- lastInterruptTime)
x, P = filter(x, P, temp_mes)
#Publish Message
pub_msg = kalman_output()
pub_msg.header.stamp = rospy.get_rostime()
pub_msg.yaw = x.value[0][0] % (pi * 2)
pub_msg.ang_vel = x.value[1][0]
pub_msg.ang_vel2 = x.value[2][0]
global pub
pub.publish(pub_msg)
lastInterruptTime = event.current_real.to_sec()
