def predictOrbit(mu):
point_list = []
p = Point()
k = shuttle.Shuttle(mu)
for var in range(0, 1000):
p.x = k.mu[0]
p.y = k.mu[1]
p.z = k.mu[2]
point_list.append(copy.copy(p))
if k.mu[2] <= 0:
break
k.predict(0.01)
updateMarker(point_list)
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
def predictOrbit(mu):
k = shuttle.Shuttle(mu)
T = getTransformMatrixToRacketCoordinate()
p = np.mat([[k.mu[0]], [k.mu[1]], [k.mu[2]], [1]])
t = T * p
msg = PointStamped()
msg.header.frame_id = "/map"
msg.header.stamp = rospy.Time.now()
msg.point.x = k.mu[0]
msg.point.y = k.mu[1]
msg.point.z = k.mu[2]
shuttlePub.publish(msg)
if (t[2, 0] <= -4 and k.mu[2] <= max_hight) or k.mu[4] > 0:
#rospy.logwarn('Shuttle is under ground')
roll_pub.publish(std_msgs.msg.Float32(-math.pi))
slide_pub.publish(std_msgs.msg.Float32(0))
swingPub.publish(std_msgs.msg.Float32(0))
return
elif t[2, 0] <= 0.1 and k.mu[2] <= max_hight:
#rospy.logwarn('Shuttle is under ground')
if math.sqrt(t[0, 0]**2 + t[1, 0]**2) < 2.0:
swingPub.publish(std_msgs.msg.Float32(1.0))
return
elif k.mu[2] < min_hight - 1.5:
roll_pub.publish(std_msgs.msg.Float32(-math.pi))
slide_pub.publish(std_msgs.msg.Float32(0))
swingPub.publish(std_msgs.msg.Float32(0))
return
#print t.T
swingPub.publish(std_msgs.msg.Float32(0))
for var in range(0, 200):
p = np.mat([[k.mu[0]], [k.mu[1]], [k.mu[2]], [1]])
t = T * p
if (t[2, 0] <= 0 and k.mu[2] <= max_hight
and k.mu[2] >= min_hight) or k.mu[2] < min_hight:
msg = PointStamped()
msg.header.frame_id = "/map"
msg.header.stamp = rospy.Time.now()
msg.point.x = k.mu[0]
msg.point.y = k.mu[1]
msg.point.z = k.mu[2]
pointPub.publish(msg)
y = t[1, 0] / racket_length
if y > 1:
y = 1
elif y < -1:
y = -1
racket_spin = math.fabs(math.pi / 2 - math.asin(y))
if t[0, 0] < 0:
racket_spin = -racket_spin
racket_x = math.sin(racket_spin) * racket_length
slide_x = t[0, 0] - racket_x
if slide_x > slide_limit:
slide_x = slide_limit
elif slide_x < -slide_limit:
slide_x = -slide_limit
while racket_spin - roll > math.pi:
racket_spin -= 2 * math.pi
while racket_spin - roll < -math.pi:
racket_spin += 2 * math.pi
u = slide_x + racket_x
v = y * racket_length
rp = np.mat([[u], [v], [0], [1]])
racket_p = np.linalg.solve(T, rp)
msg = PointStamped()
msg.header.frame_id = "/map"
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.01 * var)
msg.point.x = pose.position.x + p[0] - racket_p[0]
msg.point.y = pose.position.y + p[1] - racket_p[1]
msg.point.z = 0
if msg.point.x > 3:
msg.point.x = 3
elif msg.point.x < -3:
msg.point.x = -3
if msg.point.y > -1:
msg.point.y = -1
elif msg.point.y < -6:
msg.point.y = -6
targetpointPub.publish(msg)
if math.sqrt(t[0, 0]**2 + t[1, 0]**2) > 5.0:
roll_pub.publish(std_msgs.msg.Float32(-math.pi))
slide_pub.publish(std_msgs.msg.Float32(0))
return
if var < 50:
slide_pub.publish(std_msgs.msg.Float32(slide_x))
roll_pub.publish(std_msgs.msg.Float32(racket_spin))
else:
slide_pub.publish(std_msgs.msg.Float32(0.0))
#if math.fabs(racket_spin - roll)*180/math.pi /(0.01*var) > 30 :
roll_pub.publish(std_msgs.msg.Float32(racket_spin))
return
k.predict(0.01)
roll_pub.publish(std_msgs.msg.Float32(-math.pi))
slide_pub.publish(std_msgs.msg.Float32(0))
swingPub.publish(std_msgs.msg.Float32(0))
if abs(roll_target - roll) < math.pi / 180 and abs(slide_target -
slide) < 0.01:
servearm_pub.publish(std_msgs.msg.Float32(0.29))
else:
servearm_pub.publish(std_msgs.msg.Float32(0.005))
predictOrbit(copy.copy(s.mu))
if __name__ == '__main__':
argv = rospy.myargv(sys.argv)
rospy.init_node('shutle_kalman_arm')
pose = PoseStamped().pose
s = shuttle.Shuttle(np.mat([[0], [0], [0], [0], [0], [0], [0], [0], [0]]))
slide_pub = rospy.Publisher('/mb1/motor1',
std_msgs.msg.Float32,
queue_size=1)
roll_pub = rospy.Publisher('/mb1/motor2',
std_msgs.msg.Float32,
queue_size=1)
servearm_pub = rospy.Publisher('/mb2/motor',
std_msgs.msg.Float32,
queue_size=1)
rospy.Subscriber("/robot/pose", PoseStamped, poseCallback)
mode = 0
def predictOrbit(mu):
k = shuttle.Shuttle(mu)
T = getTransformMatrixToRacketCoordinate()
p = np.mat([[k.mu[0]], [k.mu[1]], [k.mu[2]], [1]])
t = T * p
msg = PointStamped()
msg.header.frame_id = "/map"
msg.header.stamp = rospy.Time.now()
msg.point.x = k.mu[0]
msg.point.y = k.mu[1]
msg.point.z = k.mu[2]
shuttlePub.publish(msg)
if t[0, 0] > 0:
rightPub.publish(std_msgs.msg.Bool(True))
else:
rightPub.publish(std_msgs.msg.Bool(False))
if k.mu[4] > 0:
#rospy.logwarn('Already recieved')
publishHome()
return
if t[2, 0] <= -4:
#rospy.logwarn('Shuttle is under ground')
publishHome()
swingPub.publish(std_msgs.msg.Float32(0))
return
elif t[2, 0] <= 0.04:
#rospy.logwarn('Shuttle is under ground')
if math.sqrt(t[0, 0]**2 + t[1, 0]**2) < 2.0:
swingPub.publish(std_msgs.msg.Float32(1.0))
return
#print t.T
swingPub.publish(std_msgs.msg.Float32(0))
for var in range(0, 500):
p = np.mat([[k.mu[0]], [k.mu[1]], [k.mu[2]], [1]])
if k.mu[2] < -1:
#rospy.logwarn( 'Shuttle has passed through the racket')
publishHome()
return
t = T * p
if t[2, 0] <= 0:
msg = PointStamped()
msg.header.frame_id = "/map"
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.01 * var)
msg.point.x = k.mu[0]
msg.point.y = k.mu[1]
msg.point.z = k.mu[2]
if msg.point.x > 3:
msg.point.x = 3
elif msg.point.x < -3:
msg.point.x = -3
if msg.point.y > -1:
msg.point.y = -1
elif msg.point.y < -6:
msg.point.y = -6
pointPub.publish(msg)
#print msg.header.stamp.to_sec()
return
k.predict(0.01)
publishHome()
ax.set_xlabel("X-axis")
ax.set_ylabel("Y-axis")
ax.set_zlabel("Z-axis")
# 表示範囲の設定
ax.set_xlim(-10, 10)
ax.set_ylim(0, 10)
ax.set_zlim(0, 10)
dt = 1.0 / 30.0
tx = []
ty = []
tz = []
truth = shuttle.Shuttle(
np.mat([[0], [0], [0.79], [0], [20.6246594234], [26.3983602458], [0],
[0], [0]]))
print "[{0:8.3f} {1:8.3f} {2:8.3f} {3:8.3f} {4:8.3f} {5:8.3f} {6:8.3f} {7:8.3f} {8:8.3f}]".format(
truth.mu[0, 0], truth.mu[1, 0], truth.mu[2, 0], truth.mu[3, 0],
truth.mu[4, 0], truth.mu[5, 0], truth.mu[6, 0], truth.mu[7, 0],
truth.mu[8, 0])
x = []
y = []
z = []
s = shuttle.Shuttle(
np.mat([[0], [0], [0.79], [0], [20.6246594234], [26.3983602458], [0],
[0], [0]]))
ox = []
oy = []