changed main function

catkin_ws/src/gbs/scripts/lc_client.py

@@ -26,7 +26,7 @@ class ResponseMismatch(Exception):
 
 class test_client(object):
 
-    def __init__(self, host=socket.gethostname(), port=9876,version="LabComm2014.v1"):
+    def __init__(self, host, port, version="LabComm2014.v1"):
         self.host=host
         self.port=port
         self.version=version
@@ -244,87 +244,6 @@ class test_client(object):
         while run:
             run = self.run_one()
 
-if __name__ == "__main__":
-
-    port = sys.argv[1] if len(sys.argv) >= 2 else 9876
-    version = sys.argv[2] if len(sys.argv) == 3 else "LabComm2014.v1"
-    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-    server.connect((socket.gethostname(), port))
-
-    client = test_client(server)
-    t = threading.Thread(target=receiver, args=(client,))
-    t.start()
-
-    try:
-        client.CMD_UNLCK_TL()
-        client.CMD_LCK_TL()
-
-        coord = dict(x=0,y=1,z=2)
-        rot = dict(c=0,b=0,a=0)
-        client.CMD_MOVE_LIN(coord, rot, 1, 0.1)
-
-        client.CMD_MOVE_PTP(range(6), 1, 0.1)
-
-        client.CMD_READ_PTP()
-
-        coord = dict(x=0,y=1,z=2)
-        rot = dict(c=0,b=0,a=0)
-        client.CMD_MOVE_MEASURE(1, coord, rot, 1, 0.1)
-
-        start = dict(pos=dict(x=0,y=1,z=2),
-                     rot=dict(c=0,b=0,a=0))
-        finish = dict(pos=dict(x=0,y=1,z=2),
-                      rot=dict(c=0,b=0,a=0))
-        client.CMD_NAIL_LIN(1, False, start, finish, 1, 0.1)
-
-        posI = dict(pos=dict(x=0,y=1,z=2),
-                     rot=dict(c=0,b=0,a=0))
-        posF = dict(pos=dict(x=0,y=1,z=2),
-                      rot=dict(c=0,b=0,a=0))
-        client.CMD_MOVE_ARC(posI, posF, 1, 0.1)
-
-        #linear
-        move1 = dict(pos=dict(pos=dict(x=0,y=1,z=2),
-                              rot=dict(c=0,b=0,a=0)),
-                     S_Ac=dict(speed=1,
-                               acc=2))
-
-        move2 = dict(pos=dict(pos=dict(x=5,y=1,z=0),
-                              rot=dict(c=0,b=0,a=0)),
-                     S_Ac=dict(speed=1,
-                               acc=2))
-
-
-
-        #ptp
-        move3 = dict(j=[0, 0, 0, 0, 0, 0],
-                     S_Ac = dict (speed=0.1, acc=0.5))
-
-        #arc
-        move4 = dict(posI=dict(pos=dict(x=2,y=4,z=5),
-                               rot=dict(c=0,b=0,a=1)),
-                     posF=dict(pos=dict(x=0,y=1,z=0.5),
-                               rot=dict(c=0,b=1,a=0)),
-                     S_Ac = dict(speed=0.4, acc=1))
-
-        #example of how to use variable length lists to make a "union"
-
-        moves = [dict(move_lin=[move1],move_ptp=[],move_arc=[]),
-                 dict(move_lin=[move2],move_ptp=[],move_arc=[]),
-                 dict(move_lin=[],move_ptp=[move3],move_arc=[]),
-                 dict(move_lin=[],move_ptp=[],move_arc=[move4]),]
-
-        client.CMD_MOVE(moves)
-
-        client.shutdown()
-
-    except EOFError:
-        print "got EOF"
-    except Exception, e:
-        print "got Exception"
-        print e
-        traceback.print_exc()
-
 class ROS_client(test_client):
 
 #GBSPose p
@@ -472,3 +391,45 @@ class ROS_client(test_client):
             ans.err1 = GBS_ERROR
             ans.err2 = GBS_ERROR
         return ans
+
+    def add_services(self):
+        rospy.init_node('synch_gbs_server')
+        rospy.Service('gbs_circmove', GBSCircMove, self.handle_gbs_circmove)
+        rospy.Service('gbs_combmove', GBSCombMove, self.handle_gbs_combmove)
+        rospy.Service('gbs_linmove', GBSLinMove, self.handle_gbs_linmove)
+        rospy.Service('gbs_lockspindle', GBSLockSpindle, self.handle_gbs_lockspindle)
+        rospy.Service('gbs_locktool', GBSLockTool, self.handle_gbs_locktool)
+        rospy.Service('gbs_measuremove', GBSMeasureMove, self.handle_gbs_measuremove)
+        rospy.Service('gbs_naillinmove', GBSNailLinMove, self.handle_gbs_naillinmove)
+        rospy.Service('gbs_ptpmove', GBSPTPMove, self.handle_gbs_ptpmove)
+        rospy.Service('gbs_jointpose', GBSReadCurJointPose, self.handle_gbs_readcurjointpose)
+        rospy.Service('gbs_releasepanel', GBSReleasePanel, self.handle_gbs_releasepanel)
+        rospy.Service('gbs_runspindle', GBSRunSpindle, self.handle_gbs_runspindle)
+        rospy.Service('gbs_readsensor', GBSReadSensor, self.handle_gbs_readsensor)
+        rospy.Service('gbs_shakevacuum', GBSShakeVacuum, self.handle_gbs_shakevacuum)
+        rospy.Service('gbs_stopspindle', GBSStopSpindle, self.handle_gbs_stopspindle)
+        rospy.Service('gbs_switchvacuum', GBSSwitchVacuum, self.handle_gbs_switchvacuum)
+        rospy.Service('gbs_takepanel', GBSTakePanel, self.handle_gbs_takepanel)
+        rospy.Service('gbs_unlockspindle', GBSUnlockSpindle, self.handle_gbs_unlockspindle)
+        rospy.Service('gbs_unlocktool', GBSUnlockTool, self.handle_gbs_unlocktool)
+        print "Ready to receive GBS commands"
+        rospy.spin()
+
+if __name__ == "__main__":
+
+    host = sys.argv[1] if len(sys.argv) >= 2 else socket.gethostname()
+    port = sys.argv[2] if len(sys.argv) >= 3 else 9876
+    version = sys.argv[3] if len(sys.argv) == 4 else "LabComm2014.v1"
+
+    client = ROS_client(host, port, version)
+
+    try:
+        client.connect()
+        client.add_services()
+        # client.shutdown()
+    except EOFError:
+        print "got EOF"
+    except Exception, e:
+        print "got Exception"
+        print e
+        traceback.print_exc()