Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
executable file 77 lines (57 sloc) 1.88 KB
#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
Created on 2015/02/11
@author: spiralray
'''
import shuttle
import numpy as np
import sys
import roslib
import rospy
from geometry_msgs.msg import Point
from geometry_msgs.msg import PointStamped
from shuttle_kalman.msg import shuttle_msg
updated = 0
def callback(msg):
global updated
global lastmsg
global s
if updated == 0:
updated = 1;
else:
dt = msg.header.stamp.to_sec() - lastmsg.header.stamp.to_sec()
if dt < 0:
print "recieved data is too old!"
elif dt > 0.3:
updated = 1;
elif updated == 1:
updated = 2
s = shuttle.Shuttle( np.mat([
[msg.point.x],[msg.point.y],[msg.point.z],
[ (msg.point.x - lastmsg.point.x)/dt],[(msg.point.y - lastmsg.point.y)/dt],[(msg.point.z - lastmsg.point.z)/dt],
[0],[0],[0]
]) )
else:
for var in range(0, 20):
s.predict(dt/20)
dt = msg.header.stamp.to_sec() - lastmsg.header.stamp.to_sec()
s.update( np.mat([ [msg.point.x],[msg.point.y],[msg.point.z] ]) )
#print s.mu.T
pubmsg = shuttle_msg()
pubmsg.stamp = msg.header.stamp
pubmsg.data = []
for i in range(0, 9):
pubmsg.data.append(s.mu[i,0])
pub.publish(pubmsg)
lastmsg = msg
if __name__ == '__main__':
argv = rospy.myargv(sys.argv)
rospy.init_node('shutle_kalman')
s = shuttle.Shuttle( np.mat([[0],[0],[0],[0],[0],[0],[0],[0],[0]]) )
lastmsg = PointStamped()
lastmsg.header.stamp = rospy.Time.now()
pub = rospy.Publisher('/shuttle/status', shuttle_msg, queue_size=1)
rospy.Subscriber("/shuttle/point", PointStamped, callback)
rospy.Subscriber("/shuttle/point2", PointStamped, callback)
rospy.spin()