Support more gbs cmnds (884ce343) · Commits · Mathias Haage / GBS

catkin_ws/src/gbs/scripts/synch_gbs_server_rs_v3.py

+81 −72

Original line number	Original line	Diff line number	Diff line
	@@ -82,7 +82,10 @@ def prettyprint(array):
	ac = array.array[10+i]		ac = array.array[10+i]
	i = i + 11		i = i + 11
	print "ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(j1,j2,j3,j4,j5,j6,s,ac)		print "ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(j1,j2,j3,j4,j5,j6,s,ac)
	elif near(c0,2) and near(c1,1) and near(c2,4):		elif near(c0,2) and near(c1,3) and near(c2,1):
			i = i + 3
			print "noop"
			elif near(c0,2) and near(c1,4) and near(c2,1):
	i = i + 3		i = i + 3
	print "readlibrary"		print "readlibrary"
	elif near(c0,2) and near(c1,5) and near(c2,1):		elif near(c0,2) and near(c1,5) and near(c2,1):
	@@ -94,38 +97,38 @@ def prettyprint(array):
	elif near(c0,2) and near(c1,7) and near(c2,1):		elif near(c0,2) and near(c1,7) and near(c2,1):
	i = i + 3		i = i + 3
	print "unlocktool"		print "unlocktool"
	elif near(c0,18) and near(c1,1) and near(c2,8):		elif near(c0,17) and near(c1,8):
	id = array.array[3+i]		id = array.array[2+i]
	bstop = array.array[4+i]		bstop = array.array[3+i]
	s = array.array[5+i]		s = array.array[4+i]
	ac = array.array[6+i]		ac = array.array[5+i]
	x1 = array.array[7+i]		x1 = array.array[6+i]
	y1 = array.array[8+i]		y1 = array.array[7+i]
	z1 = array.array[9+i]		z1 = array.array[8+i]
	c1 = array.array[10+i]		c1 = array.array[9+i]
	b1 = array.array[11+i]		b1 = array.array[10+i]
	a1 = array.array[12+i]		a1 = array.array[11+i]
	x2 = array.array[13+i]		x2 = array.array[12+i]
	y2 = array.array[14+i]		y2 = array.array[13+i]
	z2 = array.array[15+i]		z2 = array.array[14+i]
	c2 = array.array[16+i]		c2 = array.array[15+i]
	b2 = array.array[17+i]		b2 = array.array[16+i]
	a2 = array.array[18+i]		a2 = array.array[17+i]
	i = i + 19		i = i + 18
	print "naillinmove id=%f bstop=%f s=%f ac=%f x1=%f y1=%f z1=%f c1=%f b1=%f a1=%f x2=%f y2=%f z2=%f c2=%f b2=%f a2=%f"%(id,bstop,s,ac,x1,y1,z1,c1,b1,a1,x2,y2,z2,c2,b2,a2)		print "naillinmove id=%f bstop=%f s=%f ac=%f x1=%f y1=%f z1=%f c1=%f b1=%f a1=%f x2=%f y2=%f z2=%f c2=%f b2=%f a2=%f"%(id,bstop,s,ac,x1,y1,z1,c1,b1,a1,x2,y2,z2,c2,b2,a2)
	elif near(c0,11) and near(c1,1) and near(c2,8):		elif near(c0,10) and near(c1,9):
	id = array.array[3+i]		id = array.array[2+i]
	x = array.array[4+i]		j1 = array.array[3+i]
	y = array.array[5+i]		j2 = array.array[4+i]
	z = array.array[6+i]		j3 = array.array[5+i]
	c = array.array[7+i]		j4 = array.array[6+i]
	b = array.array[8+i]		j5 = array.array[7+i]
	a = array.array[9+i]		j6 = array.array[8+i]
	s = array.array[10+i]		s = array.array[9+i]
	ac = array.array[11+i]		ac = array.array[10+i]
	i = i + 12		i = i + 11
	print "measuremove id=%f x=%f y=%f z=%f c=%f b=%f a=%f s=%f ac=%f"%(id,x,y,z,c,b,a,s,ac)		print "measuremove id=%f j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(id,j1,j2,j3,j4,j5,j6,s,ac)
	elif near(c0,16) and near(c1,1) and near(c2,10):		elif near(c0,16) and near(c1,10) and near(c2,1):
	xi = array.array[3+i]		xi = array.array[3+i]
	yi = array.array[4+i]		yi = array.array[4+i]
	zi = array.array[5+i]		zi = array.array[5+i]
	@@ -142,7 +145,7 @@ def prettyprint(array):
	ac = array.array[16+i]		ac = array.array[16+i]
	i = i + 17		i = i + 17
	print "circmove xi=%f yi=%f zi=%f ci=%f bi=%f ai=%f xf=%f yf=%f zf=%f cf=%f bf=%f af=%f s=%f ac=%f"%(xi,yi,zi,ci,bi,ai,xf,yf,zf,cf,bf,af,s,ac)		print "circmove xi=%f yi=%f zi=%f ci=%f bi=%f ai=%f xf=%f yf=%f zf=%f cf=%f bf=%f af=%f s=%f ac=%f"%(xi,yi,zi,ci,bi,ai,xf,yf,zf,cf,bf,af,s,ac)
	elif near(c0,35) and near(c1,1) and near(c2,11):		elif near(c1,11) and near(c2,1):
	i = i + 3		i = i + 3
	print "combmove"		print "combmove"
	while True:		while True:
	@@ -188,32 +191,32 @@ def prettyprint(array):
	print " ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(j1,j2,j3,j4,j5,j6,s,ac)		print " ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(j1,j2,j3,j4,j5,j6,s,ac)
	else:		else:
	break		break
	elif near(c0,3) and near(c1,1) and near(c2,12):		elif near(c0,3) and near(c1,12) and near(c2,1):
	rpm = array.array[3+i]		rpm = array.array[3+i]
	i = i + 4		i = i + 4
	print "runspindle rpm=%f"%(rpm)		print "runspindle rpm=%f"%(rpm)
	elif near(c0,2) and near(c1,1) and near(c2,13):		elif near(c0,2) and near(c1,13) and near(c2,1):
	i = i + 3		i = i + 3
	print "stopspindle"		print "stopspindle"
	elif near(c0,2) and near(c1,1) and near(c2,14):		elif near(c0,2) and near(c1,14) and near(c2,1):
	i = i + 3		i = i + 3
	print "lockspindle"		print "lockendeffector"
	elif near(c0,2) and near(c1,1) and near(c2,15):		elif near(c0,2) and near(c1,15) and near(c2,1):
	i = i + 3		i = i + 3
	print "unlockspindle"		print "unlockendeffector"
	elif near(c0,2) and near(c1,1) and near(c2,16):		elif near(c0,2) and near(c1,16) and near(c2,1):
	i = i + 3		i = i + 3
	print "getsensorvalues"		print "getmeasure"
	elif near(c0,2) and near(c1,1) and near(c2,17):		elif near(c0,2) and near(c1,17) and near(c2,1):
	i = i + 3		i = i + 3
	print "takepanel"		print "lockpanel"
	elif near(c0,2) and near(c1,1) and near(c2,18):		elif near(c0,2) and near(c1,18) and near(c2,1):
	i = i + 3		i = i + 3
	print "releasepanel"		print "unlockpanel"
	elif near(c0,2) and near(c1,1) and near(c2,19):		elif near(c0,2) and near(c1,19) and near(c2,1):
	i = i + 3		i = i + 3
	print "switchvacuum"		print "switchvacuum"
	elif near(c0,2) and near(c1,1) and near(c2,20):		elif near(c0,2) and near(c1,20) and near(c2,1):
	i = i + 3		i = i + 3
	print "shakevacuum"		print "shakevacuum"
	else:		else:
	@@ -223,12 +226,13 @@ def prettyprint(array):
	def handle_gbs_circmove(req):		def handle_gbs_circmove(req):
	array = Array()		array = Array()
	array.n = 17		array.n = 17
	array.array = [16,1,10, req.pi.pos.x,req.pi.pos.y,req.pi.pos.z, req.pi.ori.c,req.pi.ori.b,req.pi.ori.a, req.pf.pos.x,req.pf.pos.y,req.pf.pos.z, req.pf.ori.c,req.pf.ori.b,req.pf.ori.a, req.s, req.ac]		array.array = [16,10,1, req.pi.pos.x,req.pi.pos.y,req.pi.pos.z, req.pi.ori.c,req.pi.ori.b,req.pi.ori.a, req.pf.pos.x,req.pf.pos.y,req.pf.pos.z, req.pf.ori.c,req.pf.ori.b,req.pf.ori.a, req.s, req.ac]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSCircMoveResponse()		ans = GBSCircMoveResponse()
	ans.err1 = comm.receiveFloat()		#ans.err1 = comm.receiveFloat()
	comm.receiveFloat()		#comm.receiveFloat()
			ans.err1 = 0
	ans.err2 = comm.receiveFloat()		ans.err2 = comm.receiveFloat()
	comm.receiveFloat()		comm.receiveFloat()
	return ans		return ans
	@@ -236,14 +240,15 @@ def handle_gbs_circmove(req):
	def handle_gbs_combmove(req):		def handle_gbs_combmove(req):
	array = Array()		array = Array()
	array.n = 3 + req.n		array.n = 3 + req.n
	array.array = [35,1,11] + [-1 for i in range(req.n + 1)]		array.array = [array.n-1,11,1] + [-1 for i in range(req.n + 1)]
	for i in range(req.n):		for i in range(req.n):
	array.array[3 + i] = req.array[i]		array.array[3 + i] = req.array[i]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSCombMoveResponse()		ans = GBSCombMoveResponse()
	ans.err1 = comm.receiveFloat()		#ans.err1 = comm.receiveFloat()
	comm.receiveFloat()		#comm.receiveFloat()
			ans.err1 = 0
	ans.err2 = comm.receiveFloat()		ans.err2 = comm.receiveFloat()
	comm.receiveFloat()		comm.receiveFloat()
	return ans		return ans
	@@ -265,11 +270,11 @@ def handle_gbs_linmove(req):
	def handle_gbs_lockspindle(req):		def handle_gbs_lockspindle(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,14]		array.array = [2,14,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSLockSpindleResponse()		ans = GBSLockSpindleResponse()
	ans.err = com.receiveFloat()		ans.err = comm.receiveFloat()
	comm.receiveFloat()		comm.receiveFloat()
	return ans		return ans

	@@ -286,26 +291,28 @@ def handle_gbs_locktool(req):

	def handle_gbs_measuremove(req):		def handle_gbs_measuremove(req):
	array = Array()		array = Array()
	array.n = 12		array.n = 11
	array.array = [11,1,8, req.id, req.p.pos.x,req.p.pos.y,req.p.pos.z, req.p.ori.c,req.p.ori.b,req.p.ori.a, req.s, req.ac]		array.array = [10,9, req.id, req.p.pos.x,req.p.pos.y,req.p.pos.z, req.p.ori.c,req.p.ori.b,req.p.ori.a, req.s, req.ac]
	prettyprint(array)		prettyprint(array)
	comm.sendArray()		comm.sendArray(array)
	ans = GBSMeasureMoveResponse()		ans = GBSMeasureMoveResponse()
	ans.err1 = comm.receiveFloat()		#ans.err1 = comm.receiveFloat()
	comm.receiveFloat()		#comm.receiveFloat()
			ans.err1 = 0
	ans.err2 = comm.receiveFloat()		ans.err2 = comm.receiveFloat()
	comm.receiveFloat()		comm.receiveFloat()
	return ans		return ans

	def handle_gbs_naillinmove(req):		def handle_gbs_naillinmove(req):
	array = Array()		array = Array()
	array.n = 19		array.n = 18
	array.array = [18,1,8, req.id, req.bstop, req.s, req.ac, req.p1.pos.x,req.p1.pos.y,req.p1.pos.z, req.p1.ori.c,req.p1.ori.b,req.p1.ori.a, req.p2.pos.x,req.p2.pos.y,req.p2.pos.z, req.p2.ori.c,req.p2.ori.b,req.p2.ori.a]		array.array = [17,8, req.id, req.bstop, req.s, req.ac, req.p1.pos.x,req.p1.pos.y,req.p1.pos.z, req.p1.ori.c,req.p1.ori.b,req.p1.ori.a, req.p2.pos.x,req.p2.pos.y,req.p2.pos.z, req.p2.ori.c,req.p2.ori.b,req.p2.ori.a]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSNailLinMoveResponse()		ans = GBSNailLinMoveResponse()
	ans.err1 = comm.receiveFloat()		#ans.err1 = comm.receiveFloat()
	comm.receiveFloat()		#comm.receiveFloat()
			ans.err1 = 0
	ans.err2 = comm.receiveFloat()		ans.err2 = comm.receiveFloat()
	comm.receiveFloat()		comm.receiveFloat()
	return ans		return ans
	@@ -344,7 +351,7 @@ def handle_gbs_readcurjointpose(req):
	def handle_gbs_releasepanel(req):		def handle_gbs_releasepanel(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,18]		array.array = [2,18,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSReleasePanelResponse()		ans = GBSReleasePanelResponse()
	@@ -355,16 +362,18 @@ def handle_gbs_releasepanel(req):
	def handle_gbs_runspindle(req):		def handle_gbs_runspindle(req):
	array = Array()		array = Array()
	array.n = 4		array.n = 4
	array.array = [3,1,12, req.rpm]		array.array = [3,12,1, req.rpm]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSRunSpindleResponse()		ans = GBSRunSpindleResponse()
			err = comm.receiveFloat()
			comm.receiveFloat()
	return ans		return ans

	def handle_gbs_readsensor(req):		def handle_gbs_readsensor(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,16]		array.array = [2,16,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSReadSensorResponse()		ans = GBSReadSensorResponse()
	@@ -378,7 +387,7 @@ def handle_gbs_readsensor(req):
	def handle_gbs_shakevacuum(req):		def handle_gbs_shakevacuum(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,20]		array.array = [2,20,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSShakeVacuumResponse()		ans = GBSShakeVacuumResponse()
	@@ -389,7 +398,7 @@ def handle_gbs_shakevacuum(req):
	def handle_gbs_stopspindle(req):		def handle_gbs_stopspindle(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,13]		array.array = [2,13,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSStopSpindleResponse()		ans = GBSStopSpindleResponse()
	@@ -400,7 +409,7 @@ def handle_gbs_stopspindle(req):
	def handle_gbs_switchvacuum(req):		def handle_gbs_switchvacuum(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,19]		array.array = [2,19,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSSwitchVacuumResponse()		ans = GBSSwitchVacuumResponse()
	@@ -411,7 +420,7 @@ def handle_gbs_switchvacuum(req):
	def handle_gbs_takepanel(req):		def handle_gbs_takepanel(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,17]		array.array = [2,17,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSTakePanelResponse()		ans = GBSTakePanelResponse()
	@@ -422,7 +431,7 @@ def handle_gbs_takepanel(req):
	def handle_gbs_unlockspindle(req):		def handle_gbs_unlockspindle(req):
	array = Array()		array = Array()
	array.n = 3		array.n = 3
	array.array = [2,1,15]		array.array = [2,15,1]
	prettyprint(array)		prettyprint(array)
	comm.sendArray(array)		comm.sendArray(array)
	ans = GBSUnlockSpindleResponse()		ans = GBSUnlockSpindleResponse()

catkin_ws/src/gbs/scripts/test_client_synch_rs_v3.py

+10 −10

Original line number	Original line	Diff line number	Diff line
	@@ -231,19 +231,19 @@ def test():
	print 'Current robot pose ['+str(ans.p.j1)+','+str(ans.p.j2)+','+str(ans.p.j3)+','+str(ans.p.j4)+','+str(ans.p.j5)+','+str(ans.p.j6)+']'		print 'Current robot pose ['+str(ans.p.j1)+','+str(ans.p.j2)+','+str(ans.p.j3)+','+str(ans.p.j4)+','+str(ans.p.j5)+','+str(ans.p.j6)+']'
	ans = locktool(gbs_locktool)		ans = locktool(gbs_locktool)
	ans = unlocktool(gbs_unlocktool)		ans = unlocktool(gbs_unlocktool)
	#ans = measuremove(gbs_measuremove, 111, 10, 11, 12, 35, 0, 0, 10, 6)		ans = measuremove(gbs_measuremove, 1, 0, 0, 0, 0, 5, 0, 1, 1)
	#ans = naillinmove(gbs_naillinmove, 1, 1, 3, 3, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12)		ans = naillinmove(gbs_naillinmove, 1, 1, 1, 1, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 2, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175))
	#ans = circmove(gbs_circmove, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5)		#ans = circmove(gbs_circmove, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5)
	#ans = combmove(gbs_combmove, (1, 0,0,0,0,0,0,10,5, 10, 100,0,0,35,0,0,15,25,35,1,2,3,10,5, 2, 1,2,3,4,5,6,10,5))		#ans = combmove(gbs_combmove, (1, 0,0,0,0,0,0,10,5, 10, 100,0,0,35,0,0,15,25,35,1,2,3,10,5, 2, 1,2,3,4,5,6,10,5))
	#ans = runspindle(gbs_runspindle, 5000)		ans = runspindle(gbs_runspindle, 5000)
	#ans = stopspindle(gbs_stopspindle)		ans = stopspindle(gbs_stopspindle)
	#ans = lockspindle(gbs_lockspindle)		ans = lockspindle(gbs_lockspindle)
	#ans = unlockspindle(gbs_unlockspindle)		ans = unlockspindle(gbs_unlockspindle)
	#ans = getsensorvalues(gbs_readsensor)		#ans = getsensorvalues(gbs_readsensor)
	#ans = takepanel(gbs_takepanel)		ans = takepanel(gbs_takepanel)
	#ans = releasepanel(gbs_releasepanel)		ans = releasepanel(gbs_releasepanel)
	#ans = switchvacuum(gbs_switchvacuum)		ans = switchvacuum(gbs_switchvacuum)
	#ans = shakevacuum(gbs_shakevacuum)		ans = shakevacuum(gbs_shakevacuum)
	except rospy.ServiceException, e:		except rospy.ServiceException, e:
	print "Service call failed: %s"%e		print "Service call failed: %s"%e