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

class JzDevNode(Node):
    def __init__(self):
        super().__init__('jz_dev_node')
        self.declare_parameter('port','/dev/ttyUSB0')
        param = self.get_parameter('port')
        self.com_dev = param.value
        self.declare_parameter('devid',9)
        param = self.get_parameter('devid')
        self.devid = param.value

        self.declare_parameter('name','/jz_cmd0')
        param = self.get_parameter('name')
        self.srv_name = param.value
        print('param:',self.com_dev,self.devid,self.srv_name)
        self.get_logger().info(f'ros param:{self.com_dev},{self.devid},{self.srv_name}')
        self.action=ActionServer(self,JzCmd,self.srv_name,self.jz_cmd_callback_action)
        
        self.clawControl = RgClawControl()
        self.target_loc=0
        self.target_torque=0
        self.target_speed=0
        self.cur_loc=-1
        self.cur_torque=-1
        self.cur_goal=None
        self.timer_on=False
        self.target_mode=0
        print('jz_dev_node init:',self.com_dev)
        #self.feedback_timer = self.create_timer(0.1, self.update_feedback) 
    def jz_cmd_callback_action(self,goal):
        self.cur_goal=goal
        req=goal.request
        self.target_loc=req.loc
        self.target_speed=req.speed
        self.target_torque=req.torque
        self.target_mode=req.mode
        print('recv goal',self.devid,self.target_loc,self.target_speed,self.target_torque,self.target_mode)
        flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
        if flag==1:
            ret = self.clawControl.enableClamp(self.devid, True)
            if ret!=1:
                print('enble fail!')
                result=JzCmd.Result()
                result.state="jz enable fail!"
                result.result=0
                goal.abort()
                return result
            print('start run...')
            self.clawControl.runWithParam(self.devid,self.target_loc,self.target_speed,self.target_torque)
            ##feedback
            loop_flag=True
            while loop_flag:
                loc = self.clawControl.getClampCurrentLocation(self.devid)
                speed = self.clawControl.getClampCurrentSpeed(self.devid)
                torque = self.clawControl.getClampCurrentTorque(self.devid)
                self.cur_loc=loc[0]
                self.cur_speed=speed[0]
                if self.target_mode==0:
                    loop_flag=abs(self.cur_loc-self.target_loc)>2
                elif self.target_mode==1:
                    loop_flag=abs(self.cur_torque-self.target_torque)>2
                else:
                    loop_flag=abs(self.cur_loc-self.target_loc)>2 and abs(self.cur_loc-self.target_loc)>2
                #print('feedback:',self.cur_loc,speed,torque)
                feedback_msg = JzCmd.Feedback()
                feedback_msg.loc=loc[0]
                feedback_msg.speed=speed[0]
                feedback_msg.torque=torque[0]
                print('update feedback:',feedback_msg)
                self.cur_goal.publish_feedback(feedback_msg)
                time.sleep(1)
        else:
            result=JzCmd.Result()
            result.state="uart open fail!"
            result.result=0
            goal.abort()
            return result
        #print('start timer')
        self.timer_on=True
        print('location diff:',self.cur_loc,self.target_loc)
        state = self.clawControl.getClampCurrentState(self.devid)
        result=JzCmd.Result()
        result.state=state[0]
        result.result=1
        goal.succeed()
        print('action finish',state)
        return result
        
    def jz_cmd_callback(self, request, response):
        self.cur_loc=request.loc
        self.cur_speed=request.speed
        self.cur_torque=request.torque
        print('recv jz cmd:',self.cur_loc,self.cur_speed,self.cur_torque)
        flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
        if flag == 1:
            #self.runCmd()
            #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
def main():
    rclpy.init()
    node=JzDevNode()
    rclpy.spin(node)
    rclpy.shutdown()
    pass
if __name__ == '__main__':
    main()






