Commit 04c2b3b3 authored by Sven Gestegård Robertz's avatar Sven Gestegård Robertz
Browse files

changed main function

parent cc689335
Loading
Loading
Loading
Loading
+43 −82
Original line number Diff line number Diff line
@@ -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()