
from jodellSdk.jodellSdkDemo import RgClawControl
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from interfaces.srv import JzCmd
import sys
import os

class JzDevNode(Node):

    def __init__(self,com_dev):
        super().__init__('jz_dev_node')
        self.srv = self.create_service(JzCmd, 'jz_cmd', self.jz_cmd_callback)
        self.com_dev = com_dev
        self.clawControl = RgClawControl()
        self.cur_loc=0
        self.cur_torque=0
        self.cur_speed=0
        self.cur_state=0
        print('jz_dev_node init')
    def runCmd(self,id):
        ret=self.clawControl.runWithParam(id,self.cur_loc,self.cur_speed,self.cur_torque)
        return ret
    def getState(self,id):
        loc = self.clawControl.getClampCurrentLocation(id)
        speed = self.clawControl.getClampCurrentSpeed(id)
        state = self.clawControl.getClampCurrentState(id)
    def jz_cmd_callback(self, request, response):
        devid=request.devid
        self.cur_loc=request.loc
        self.cur_speed=request.speed
        self.cur_torque=request.torque
        print('recv jz cmd:',devid,self.cur_loc,self.cur_speed,self.cur_torque)
        flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
        if flag == 1:
            self.runCmd()
            response.state = self.clawControl.getClampCurrentState(devid)
            self.clawControl.serialOperation(self.com_dev, 115200, False)
            print('send jz sta:',devid,response.state)
        else:
            response.state = 'uart open failed'
        return response
'''devid=6
clawControl = RgClawControl()
comList = clawControl.searchCom()
#ver = clawControl.readSoftwareVersion(devid)
print(comList)

####ret = clawControl.enableClamp(devid, False)
ret=clawControl.forceOpenBrake(devid,True)
print(ret)
if flag==1:
    loc = clawControl.getClampCurrentLocation(devid)
    speed = clawControl.getClampCurrentSpeed(devid)
    state = clawControl.getClampCurrentState(devid)
    print('loc:',loc,'speed:',speed,'state:',state)

    while True:
        ret=clawControl.runWithoutParam(devid,1)
        if ret==1:
            print('start run')
        time.sleep(3)
        ret = clawControl.enableClamp(devid, False)
        time.sleep(3)'''

def main():
    rclpy.init()
    node=JzDevNode('/dev/ttyUSB0')
    rclpy.spin(node)
    rclpy.shutdown()
    pass
if __name__ == '__main__':
    main()






