How to use _param method in localstack

Best Python code snippet using localstack_python

strategy_qr.py

Source:strategy_qr.py

1#!/usr/bin/env python32# -*- coding: utf-8 -*-+3import math4import numpy as np5# lib6from lib.nodehandle import NodeHandle7from lib.pidcontrol import PIDControl,PIDControl_Y,PIDControl_Yaw,PIDControl_Qr8from lib.fuzzycontrol import FUZZYControl9from lib.counter import TimeCounter10# rostopic msg11from geometry_msgs.msg import Twist12from std_msgs.msg import Bool,Int3213# define behavior 14MOBILE_ROBOT = 015CORRECTION = 116PLATFORM = 217NEXT_POINT = 318HOME = 419MANUAL = 520ROTATE = 621GO_POINT = 722RETURN_POINT = 823CROSS = 924INIT = 1025DELIVERY = 1126ORDER = 1227ARM_MOVE = 1328# FLAG 29CONTROL = 'PIDCONTROL'30# CONTROL = 'FUZZYCONTROL'31'''32    HOME -> FIRST33        INIT -> MOBILE -> CORRECTION_0 -> ROTATE_90 -> CORRECTION_90 -> PLATFORM34    FIRST -> SECOND 35        NEXT -> ROTATE_0 -> CROSS -> MOBILE -> CORRECTION_0 -> ROTATE_90 -> CORRECTION_90 -> PLATFORM36    POINT -> HOME37        HOME -> ROTATE -> CROSS_FIRST -> MOBILE -> PLATFORM38'''39class Strategy(object):40    '''41        Offset track(ç®ååæ¢)42            prev_dis:43            prev_ang:44            prev_vel:45        CONTROL46            initPID: åå§åä¸è¦ä½¿PIDä¸æ·ç´¯å 47        QRCODE(ç®åæ²ç¨å°)48            state:49            pre_state:50            not_find: 51        ROTATE52            rotateAng: ç®æ¨è§åº¦53        CROSS54            timer: è¨æ¸å¨55        HOME56            homeFlag57                1: go home58                0: åé²59            homeTimes: è¨éèµ°å°çåæ¢é»60    '''61    def __init__(self):62        self._param = NodeHandle()63        if(CONTROL == 'PIDCONTROL'):64            self.control = PIDControl()65            self.controlY = PIDControl_Y()66            self.controlYaw = PIDControl_Yaw()67            # self.controlQRX = PIDControl_Qr(20.0,0.05,10.0)68            # self.controlQRY = PIDControl_Qr(20.0,0.05,10.0)69            # self.controlQRX = PIDControl_Qr(15,0.05,12.0)70            self.controlQRX = PIDControl_Qr(15,0.05,14.0)71            self.controlQRY = PIDControl_Qr(30.0,0.05,15.0)72        elif(CONTROL == 'FUZZYCONTROL'):73            self.control = FUZZYControl()74        75        self.prev_dis = 076        self.prev_ang = 077        self.prev_vel = []78        self.initPID = 079        self.state = 080        self.pre_state = 081        self.not_find = 082        self.stopTimes = 083        self.dualArm = 084        85        ''' rotate  '''86        self.rotateAng = self._param.errorRotate087        self.pre_rotateYaw = 088        ''' cross '''89        self.timer = TimeCounter(time = self._param.crossTime)90        self.timerQr = TimeCounter(time = 1.0)91        self.timerQrFlag = False92        ''' home '''93        self.homeFlag = 094        self.homeTimes = 095    def Process(self):96        if(self._param.behavior == MOBILE_ROBOT):97            if(self._param.loadParam):98                self.Change_Behavior()99            self.Mobile_Strategy()100        elif(self._param.behavior == CORRECTION):101            if(self._param.loadParam):102                self.Change_Behavior()103            self.Correction_Strategy()104        elif(self._param.behavior == PLATFORM):105            if(self._param.loadParam):106                self.Change_Behavior()107            self.Platform_Strategy()108        elif(self._param.behavior == NEXT_POINT):109            if(self._param.loadParam):110                self.Change_Behavior()111            self.Next_Point_Strategy()112        elif(self._param.behavior == HOME):113            if(self._param.loadParam):114                self.Change_Behavior()115            self.Home_Strategy()116            print('HOME')117        elif(self._param.behavior == MANUAL):118            if(self._param.loadParam):119                self.Change_Behavior()120            self.state = 0121            self.initPID = 0122            self.controlYaw.Init()123            self.controlY.Init()124            print('MANUAL')125        elif(self._param.behavior == ROTATE):126            if(self._param.loadParam):127                self.Change_Behavior()128            self.Rotate_Strategy()129        elif(self._param.behavior == GO_POINT):130            if(self._param.loadParam):131                self.Change_Behavior()132            self.Go_Point_Strategy()133            print('GO_POINT')134        elif(self._param.behavior == RETURN_POINT):135            if(self._param.loadParam):136                self.Change_Behavior()137            self.Return_Point_Strategy()138            print('RETURN_POINT')139        elif(self._param.behavior == CROSS):140            if(self._param.loadParam):141                self.Change_Behavior()142            self.Cross_Strategy()143        elif(self._param.behavior == INIT):144            if(self._param.loadParam):145                self.Change_Behavior()146            self.Init_Strategy()147            print('Init')148        elif(self._param.behavior == ARM_MOVE):149            if(self._param.loadParam):150                self.Change_Behavior()151            self.ARM_MOVE_Strategy()152            print('ARM_MOVE')153        else:154            print("Don't have Behavior")155            self.Robot_Stop()156    def Mobile_Strategy(self):157        if(self._param.scanState):158            count = self._param.scanState.count(1)159            if(count):160                scanNum = len(self._param.scanState)161                if(count <= math.ceil((scanNum)*(2./3)) and self._param.stopPoint == 999):162                    self.state = 0163                    # Method 3164                    #if(CONTROL == 'PIDCONTROL'):165                    #    x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)166                    #elif(CONTROL == 'FUZZYCONTROL'):167                    #    x,y,yaw = self.control.Process(self._param.dis,self._param.ang)168                    # yaw = 0169                    #self.Robot_Vel([y,-x,yaw])170                    #print(y,-x,yaw)171                    # Method 4172                    # x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)173                    # if(abs(self._param.ang) > 10.0):174                    #     if(self._param.ang > 0):175                    #         x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15176                    #         y = -(self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15177                    #         # yaw = self._param.velYaw178                    #         yaw = (self._param.velYaw+abs(yaw))179                    #     else:180                    #         x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15181                    #         y = (self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15182                    #         # yaw = -self._param.velYaw183                    #         yaw = -(self._param.velYaw+abs(yaw))184                    # else:185                    #     x,y,_ = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)186                    #     # x,y,_ = self.control.Process(self._param.dis,self._param.ang)187                    #     yaw = 0188                        189                    ''' Method 5 '''190                    y = self.controlY.Process(self._param.dis,self._param.ang,self._param.minVel)191                    x = (self._param.minVel - abs(y))*math.cos(math.radians(self._param.ang)) - y*math.sin(math.radians(self._param.ang))192                    193                    if(abs(self._param.dis) > self._param.errorMoibledis):194                        yaw = 0195                    else:       196                        if(abs(self._param.ang) > self._param.errorMoibleAng):197                            yaw = self.controlYaw.Process(self._param.ang,self._param.velYaw)198                        else:199                            yaw = 0200                    if(self.homeFlag == 0):201                        self.Robot_Vel([x,y,yaw])202                        print(x,y,yaw)203                    else:204                        self.Robot_Vel([-x,y,yaw])205                        print(-x,y,yaw)206                    # print(self._param.ang)207                    # self.prev_dis = self._param.dis208                    # self.prev_ang = self._param.ang209                    # self.prev_vel = [x,y,yaw]210                elif(self._param.stopPoint != 999 and self._param.stopPoint != '91' and self._param.stopPoint != '90'):211                    print('STOP')212                    self.state = 1213                    self.Robot_Stop()214                    if(self.homeFlag == 1):215                        self._param.behavior = HOME216                    elif(self.homeTimes == int(self._param.stopPoint)):217                        self._param.behavior = CROSS218                    elif(self._param.stopPoint == '2'):219                        self._param.behavior = CORRECTION220                        print('state 2')221                        self.dualArm = 2222                        self.timerQrFlag = False223                        # self.Dual_Arm_Start_2()224                    else:225                        self.dualArm = 1226                        self._param.behavior = CORRECTION227                        self.homeTimes += 1228                        self.timerQrFlag = False229                if(self.stopTimes >= 3):230                    self._param.stopPoint = 999231                    self.stopTimes = 0232                else:233                    self.stopTimes += 1234                self.pre_state = self.state235            else:236                print('Offset track !!!!!!')237                if(len(self.prev_vel)):238                    if(self.prev_vel[2] == 0):239                        x = -(self.prev_vel[0])*0.8240                        y = -self.prev_vel[1]*1.5241                        yaw = 0242                    else:     243                        x = (self._param.minVel*math.cos(math.radians(self.prev_ang)))*0.5244                        y = self._param.minVel*math.sin(math.radians(self.prev_ang))245                        yaw = 0246                else:247                    x = 0248                    y = 0249                    yaw = 0250                    print('No scan line')251                # self.Robot_Vel([y,-x,yaw])252                self.Robot_Stop()253        else:254            print('No Scan Info !!!!!!')255            self.Robot_Stop()256    def Correction_Strategy(self):257        # y = self.controlY.Process(self._param.dis,self._param.ang,self._param.minVel)258        if(self._param.qrX == None):259            print('fuck!!!!!!!!!!!!!!!!')260            dis = 0261        else:262            # print('fuck!!!!!!!!!!!!!!!!',self._param.errorCorrectionDis)263            # dis = math.sqrt(math.pow(self._param.qrX,2.0)+math.pow(self._param.qrY,2.0))264            dis = self._param.qrY265        if(self.timerQrFlag == False):266            time,self.timerQrFlag = self.timerQr.Process()267        else:    268            # if(self._param.dis < self._param.errorCorrectionDis):269            if(abs(dis) < self._param.errorCorrectionDis):270                if(self._param.qrTheta is not None and self._param.qrTheta != 999):271                    RPang = self.Norm_Angle(self.rotateAng - self._param.qrTheta)272                    if(abs(RPang) > self._param.errorAng):273                        if(RPang > 0):274                            x = 0275                            y = 0276                            yaw = self._param.velYaw277                            # yaw = self._param.rotateYaw278                        else:279                            x = 0280                            y = 0281                            yaw = -self._param.velYaw282                            # yaw = -self._param.rotateYaw283                        self.Robot_Vel([x,y,yaw])284                        print('CORRECTION','FRONT',self._param.qrTheta)285                    else:286                        self.Robot_Stop()287                        self.Robot_Stop()288                        self.Robot_Stop()289                        print('CORRECTION',self.rotateAng,self._param.errorRotate0)290                        if(self.dualArm == 1):291                        # if(self.dualArm == 1 or self.dualArm == 2):292                            if(self.rotateAng == self._param.errorRotate0):293                                self._param.behavior = ROTATE294                                self.rotateAng = self._param.errorRotate90295                            else:296                                self._param.behavior = PLATFORM297                                self.rotateAng = self._param.errorRotate0298                                self.initPID = 1299                        elif(self.dualArm == 2):300                            if(self.rotateAng == self._param.errorRotate0):301                                self._param.behavior = PLATFORM302            303                        304                    self.not_find = 0305                else:306                    print('CORRECTION not find')307                    if(self.not_find < 100):308                        self.not_find += 1309                        self.Robot_Stop()310                    else:311                        self.not_find = 0312                        if(self.dualArm == 1):313                            if(self.rotateAng == self._param.errorRotate0):314                                self._param.behavior = ROTATE315                                self.rotateAng = self._param.errorRotate90316                            else:317                                self._param.behavior = PLATFORM318                                self.rotateAng = self._param.errorRotate0319                                self.initPID = 1320                        elif(self.dualArm == 2):321                            if(self.rotateAng == self._param.errorRotate0):322                                self._param.behavior = PLATFORM  323                            self.initPID = 1324                self._param.qrTheta = 999325            else:326                # x = 0327                yaw = 0328                # y = 0329                x = self.controlQRX.Process(self._param.qrY,self._param.qrTheta,self._param.minVel)330                y = self.controlQRY.Process(self._param.qrX,self._param.qrTheta,self._param.minVel)331                self.Robot_Vel([x,y,yaw])332                print('CORRECTION','dis',x,y)333                self._param.qrX = None334    335    def Platform_Strategy(self):336        print('PLATFORM')337        self.state = 0338        if(self.initPID):339            self.controlYaw.Init()340            self.controlY.Init()341            self.initPID = 0342        self.Robot_Stop()343        if(self.homeFlag == 0 and self.dualArm == 1):344            self.Dual_Arm_Start_1()345        elif(self.homeFlag == 0 and self.dualArm == 2):346            self.Dual_Arm_Start_2()347    348    def Next_Point_Strategy(self):349        print('NEXT_POINT')350        self.Robot_Stop()351        self._param.behavior = ROTATE352        self.rotateAng = self._param.errorRotate0353        self.dualArm = 0354        355            356    def Rotate_Strategy(self):357        print('ROTATE fuck!!!!!!',self._param.errorRotate90)358        # yaw = self.controlYaw(self._param.qrTheta,self._param.velYaw)359        if(self._param.qrTheta is not None and self._param.qrTheta != 999):360            RPang = self.Norm_Angle(self.rotateAng - self._param.qrTheta)361            x = self.controlQRX.Process(self._param.qrY,self._param.qrTheta,self._param.minVel)362            y = self.controlQRY.Process(self._param.qrX,self._param.qrTheta,self._param.minVel)363            if(abs(RPang) > self._param.errorAng and RPang > self._param.rotateSlowAng):364                if(RPang > 0):365                    # x = 0366                    # y = 0367                    yaw = self._param.velYaw368                    self.pre_rotateYaw = yaw369                    # yaw = self._param.velYaw*0.8370                    # yaw = 0371                    # yaw = self._param.rotateYaw372                else:373                    # x = 0374                    # y = 0375                    yaw = self._param.velYaw376                    self.pre_rotateYaw = yaw377                    # yaw = -self._param.velYaw*0.8378                    # yaw = 0379                    # yaw = -self._param.rotateYaw380                self.Robot_Vel([x,y,yaw])381                # print('ROTATE','angle',self._param.qrTheta)382                print('ROTATE','vector',x,y)383            elif((abs(RPang) > self._param.errorAng and RPang <= self._param.rotateSlowAng)):384                if(RPang > 0):385                    # x = 0386                    # y = 0387                    yaw = self._param.velYaw388                    self.pre_rotateYaw = yaw389                    # yaw = self._param.velYaw*0.8390                    # yaw = 0391                    # yaw = self._param.rotateYaw*0.8392                else:393                    # x = 0394                    # y = 0395                    yaw = -self._param.velYaw396                    self.pre_rotateYaw = yaw397                    # yaw = self._param.velYaw*0.8398                    # yaw = 0399                    # yaw = -self._param.rotateYaw*0.8400                self.Robot_Vel([x,y,yaw])401                # print('ROTATE','angle',self._param.qrTheta)402                print('ROTATE','vector',x,y)403            else:404                self.Robot_Stop()405                self.Robot_Stop()406                self.Robot_Stop()407                if(self.rotateAng == self._param.errorRotate90):408                    self._param.behavior = CORRECTION409                    print('ROTATE COREECTION')410                else:411                    self._param.behavior = CROSS412                    print('ROTATE CROSS')413            self.not_find = 0414        else:415            print('ROTATE not find')416            if(self.not_find < 100):417                self.not_find += 1418                x = 0419                y = 0420                self.Robot_Vel([x,y,self.pre_rotateYaw])421                # self.Robot_Stop()422            else:423                self.not_find = 0424                if(self.rotateAng == self._param.errorRotate90):425                    self._param.behavior = CORRECTION426                    print('ROTATE COREECTION')427                else:428                    self._param.behavior = CROSS429                    print('ROTATE CROSS')430        self._param.qrTheta = 999431    432    def Go_Point_Strategy(self):433        time,state = self.timer.Process()434        if(state):435            self.Robot_Stop()436            self._param.behavior = CORRECTION437        else:438            x = self._param.minVel439            y = 0440            yaw = 0441            self.Robot_Vel([x,y,yaw])442    def Return_Point_Strategy(self):443        time,state = self.timer.Process()444        if(state):445            self.Robot_Stop()446            self._param.behavior = ROTATE447            self.rotateAng = self._param.errorRotate0448        else:449            x = -self._param.minVel450            y = 0451            yaw = 0452            self.Robot_Vel([x,y,yaw])453    454    def Cross_Strategy(self):455        print('CROSS')456        time,state = self.timer.Process()457        if(state):458            self.Robot_Stop()459            self._param.behavior = MOBILE_ROBOT460            self.rotateAng = self._param.errorRotate0461        elif(state == 0 and self.homeFlag == 0):462            x = self._param.minVel463            y = 0464            yaw = 0465            self.Robot_Vel([x,y,yaw])466        elif(state == 0 and self.homeFlag == 1):467            x = -self._param.minVel468            y = 0469            yaw = 0470            self.Robot_Vel([x,y,yaw])471        # if(self.pre_state == 1 and self.state == 0):472        #     if(self._param.scanState):473        #         if(self._param.qrTheta is not None and self._param.qrTheta != 999):474        #             x = self._param.minVel475        #             y = 0476        #             yaw = 0477        #             self.not_find = 0478        #             # self.Robot_Vel([y,-x,yaw])479        #             self.Robot_Vel([x,y,yaw])480        #         elif(self.not_find > 60):481        #             self.Robot_Stop()482        #             self._param.behavior = MOBILE_ROBOT483        #             self.not_find = 0484        #                 # print('next point not find line')485        #         else:486        #             self.not_find +=1487        #             x = self._param.minVel488        #             y = 0489        #             yaw = 0490        #             # self.Robot_Vel([y,-x,yaw])491        #             self.Robot_Vel([x,y,yaw])492        #         self._param.qrTheta = 999493        # else:494        #     self.Robot_Stop()495        #     print('fuck Cross')496    497    def Init_Strategy(self):498        self.rotateAng = self._param.errorRotate0499        self.homeFlag = 0500        self.homeTimes = 0501        self.Robot_Stop()502        self._param.behavior = MOBILE_ROBOT503        self._param.stopPoint = 999504        # self.Reset_IMU()505    def Home_Strategy(self):506        print('HOME times',self.homeTimes,'HOME stop',self._param.stopPoint)507        if(self.homeFlag == 0):508            print('HOME',1)509            self.homeFlag = 1510            self.Robot_Stop()511            self._param.behavior = ROTATE512            self.rotateAng = self._param.errorRotate0513            self.homeTimes -= 1514        else:515            if(self.homeTimes == 0 and self._param.stopPoint == '0'):516                print('home')517                self.Robot_Stop()518                self._param.behavior = PLATFORM519            else:520                if(self.homeTimes == int(self._param.stopPoint)):521                    self.homeTimes -= 1522                    self._param.behavior = CROSS523                elif(self._param.stopPoint == 999):524                    self._param.behavior = CROSS525                else:526                    self._param.behavior = MOBILE_ROBOT527    528    def ARM_MOVE_Strategy(self):529        if(self._param.armMove == 1):530            self.Robot_Vel([12,0,0])531        elif(self._param.armMove == -1):532            self.Robot_Vel([-12,0,0])533        else:534            print('arm move fuck !!!!')535            536    def Deg2Rad(self,deg):537        return deg*math.pi/180538    539    def Norm_Angle(self,angle):540        if(angle > 180):541            angle -= 360542        elif(angle < -180):543            angle +=360544        return angle545    def Robot_Stop(self):546        vel = Twist()547        vel.linear.x = 0548        vel.linear.y = 0549        vel.angular.z = 0550        self._param.pub_cmdvel.publish(vel)551    def Robot_Vel(self,vec):552        vel = Twist()553        vel.linear.x = vec[0]554        vel.linear.y = vec[1]555        vel.angular.z = vec[2]556        self._param.pub_cmdvel.publish(vel)557    558    def Change_Behavior(self):559        self.Robot_Stop()560        self.Robot_Stop()561        self._param.loadParam = False562    563    def Dual_Arm_Start_1(self):564        start = Int32()565        start.data = 1566        self._param.pub_dualArm1.publish(start)567    568    def Dual_Arm_Start_2(self):569        start = Int32()570        start.data = 2571        self._param.pub_dualArm1.publish(start)572    def Scan_Camera_Start(self):573        start = Bool()574        start.data = True575        self._param.pub_startCamera.publish(start)576    def Scan_Camera_Stop(self):577        start = Bool()578        start.data = False579        self._param.pub_startCamera.publish(start)580    581    def Reset_IMU(self):582        reset = Bool()583        reset.data = True584        self._param.pub_resetImu.publish(reset)...

strategy.py

Source:strategy.py

1#!/usr/bin/env python32# -*- coding: utf-8 -*-+3import math4import numpy as np5# lib6from lib.nodehandle import NodeHandle7from lib.pidcontrol import PIDControl,PIDControl_Y,PIDControl_Yaw8from lib.fuzzycontrol import FUZZYControl9from lib.counter import TimeCounter10# rostopic msg11from geometry_msgs.msg import Twist12from std_msgs.msg import Bool13# define behavior 14MOBILE_ROBOT = 015CORRECTION = 116PLATFORM = 217NEXT_POINT = 318HOME = 419MANUAL = 520ROTATE = 621GO_POINT = 722RETURN_POINT = 823CROSS = 924INIT = 1025# FLAG 26CONTROL = 'PIDCONTROL'27# CONTROL = 'FUZZYCONTROL'28IMU_FLAG = True29'''30    HOME -> FIRST31        INIT -> MOBILE -> CORRECTION_0 -> ROTATE_90 -> CORRECTION_90 -> PLATFORM32    FIRST -> SECOND 33        NEXT -> ROTATE_0 -> CROSS -> MOBILE -> CORRECTION_0 -> ROTATE_90 -> CORRECTION_90 -> PLATFORM34    POINT -> HOME35        HOME -> ROTATE -> CROSS_FIRST -> MOBILE -> PLATFORM36'''37class Strategy(object):38    '''39        Offset track(ç®ååæ¢)40            prev_dis:41            prev_ang:42            prev_vel:43        CONTROL44            initPID: åå§åä¸è¦ä½¿PIDä¸æ·ç´¯å 45        QRCODE(ç®åæ²ç¨å°)46            state:47            pre_state:48            not_find: 49        ROTATE50            rotateAng: ç®æ¨è§åº¦51        CROSS52            timer: è¨æ¸å¨53        HOME54            homeFlag55                1: go home56                0: åé²57            homeTimes: è¨éèµ°å°çåæ¢é»58    '''59    def __init__(self):60        self._param = NodeHandle()61        if(CONTROL == 'PIDCONTROL'):62            self.control = PIDControl()63            self.controlY = PIDControl_Y()64            self.controlYaw = PIDControl_Yaw()65        elif(CONTROL == 'FUZZYCONTROL'):66            self.control = FUZZYControl()67        68        self.prev_dis = 069        self.prev_ang = 070        self.prev_vel = []71        self.initPID = 072        self.state = 073        self.pre_state = 074        self.not_find = 075        # self._kp = 676        # self._ki = 0.177        # self._kd = 4.078        # self.prevIntegral = 079        # self.lastError = 080        81        ''' rotate  '''82        self.rotateAng = self._param.errorRotate083        ''' cross '''84        self.timer = TimeCounter(time = self._param.crossTime)85        ''' home '''86        self.homeFlag = 087        self.homeTimes = 088    def Process(self):89        if(self._param.behavior == MOBILE_ROBOT):90            if(self._param.loadParam):91                self.Change_Behavior()92            self.Mobile_Strategy()93        elif(self._param.behavior == CORRECTION):94            if(self._param.loadParam):95                self.Change_Behavior()96            self.Correction_Strategy()97        elif(self._param.behavior == PLATFORM):98            if(self._param.loadParam):99                self.Change_Behavior()100            self.Platform_Strategy()101        elif(self._param.behavior == NEXT_POINT):102            if(self._param.loadParam):103                self.Change_Behavior()104            self.Next_Point_Strategy()105        elif(self._param.behavior == HOME):106            if(self._param.loadParam):107                self.Change_Behavior()108            self.Home_Strategy()109            print('HOME')110        elif(self._param.behavior == MANUAL):111            if(self._param.loadParam):112                self.Change_Behavior()113            self.state = 0114            self.initPID = 0115            self.controlYaw.Init()116            self.controlY.Init()117            print('MANUAL')118        elif(self._param.behavior == ROTATE):119            if(self._param.loadParam):120                self.Change_Behavior()121            self.Rotate_Strategy()122        elif(self._param.behavior == GO_POINT):123            if(self._param.loadParam):124                self.Change_Behavior()125            self.Go_Point_Strategy()126            print('GO_POINT')127        elif(self._param.behavior == RETURN_POINT):128            if(self._param.loadParam):129                self.Change_Behavior()130            self.Return_Point_Strategy()131            print('RETURN_POINT')132        elif(self._param.behavior == CROSS):133            if(self._param.loadParam):134                self.Change_Behavior()135            self.Cross_Strategy()136        elif(self._param.behavior == INIT):137            if(self._param.loadParam):138                self.Change_Behavior()139            self.Init_Strategy()140            print('Init')141        else:142            print("Don't have Behavior")143            self.Robot_Stop()144    def Mobile_Strategy(self):145        if(self._param.scanState):146            count = self._param.scanState.count(1)147            if(count):148                scanNum = len(self._param.scanState)149                if(count <= math.ceil((scanNum)*(2./3)) and self._param.stopPoint == 999):150                    self.state = 0151                    # Method 3152                    #if(CONTROL == 'PIDCONTROL'):153                    #    x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)154                    #elif(CONTROL == 'FUZZYCONTROL'):155                    #    x,y,yaw = self.control.Process(self._param.dis,self._param.ang)156                    # yaw = 0157                    #self.Robot_Vel([y,-x,yaw])158                    #print(y,-x,yaw)159                    # Method 4160                    # x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)161                    # if(abs(self._param.ang) > 10.0):162                    #     if(self._param.ang > 0):163                    #         x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15164                    #         y = -(self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15165                    #         # yaw = self._param.velYaw166                    #         yaw = (self._param.velYaw+abs(yaw))167                    #     else:168                    #         x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15169                    #         y = (self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15170                    #         # yaw = -self._param.velYaw171                    #         yaw = -(self._param.velYaw+abs(yaw))172                    # else:173                    #     x,y,_ = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)174                    #     # x,y,_ = self.control.Process(self._param.dis,self._param.ang)175                    #     yaw = 0176                        177                    ''' Method 5 '''178                    y = self.controlY.Process(self._param.dis,self._param.ang,self._param.minVel)179                    x = (self._param.minVel - abs(y))*math.cos(math.radians(self._param.ang)) - y*math.sin(math.radians(self._param.ang))180                    181                    if(abs(self._param.dis) > self._param.errorMoibledis):182                        yaw = 0183                    else:       184                        if(abs(self._param.ang) > self._param.errorMoibleAng):185                            yaw = self.controlYaw.Process(self._param.ang,self._param.velYaw)186                        else:187                            yaw = 0188                    if(self.homeFlag == 0):189                        self.Robot_Vel([x,y,yaw])190                        print(x,y,yaw)191                    else:192                        self.Robot_Vel([-x,y,yaw])193                        print(-x,y,yaw)194                    # self.prev_dis = self._param.dis195                    # self.prev_ang = self._param.ang196                    # self.prev_vel = [x,y,yaw]197                elif(self._param.stopPoint != 999 and self._param.stopPoint != '91' and self._param.stopPoint != '90'):198                    print('STOP')199                    self.state = 1200                    self.Robot_Stop()201                    if(self.homeFlag == 1):202                        self._param.behavior = HOME203                    elif(self.homeTimes == int(self._param.stopPoint)):204                        self._param.behavior = CROSS205                    # elif(self._param.stopPoint == '2'):206                    #     # self._param.behavior = PLATFORM207                    #     print('state 2')208                    #     self.Dual_Arm_Start()209                    else:210                        self._param.behavior = CORRECTION211                        self.homeTimes += 1212                self._param.stopPoint = 999213                self.pre_state = self.state214            else:215                print('Offset track !!!!!!')216                if(len(self.prev_vel)):217                    if(self.prev_vel[2] == 0):218                        x = -(self.prev_vel[0])*0.8219                        y = -self.prev_vel[1]*1.5220                        yaw = 0221                    else:     222                        x = (self._param.minVel*math.cos(math.radians(self.prev_ang)))*0.5223                        y = self._param.minVel*math.sin(math.radians(self.prev_ang))224                        yaw = 0225                else:226                    x = 0227                    y = 0228                    yaw = 0229                    print('No scan line')230                # self.Robot_Vel([y,-x,yaw])231                self.Robot_Stop()232        else:233            print('No Scan Info !!!!!!')234            self.Robot_Stop()235    def Correction_Strategy(self):236        y = self.controlY.Process(self._param.dis,self._param.ang,self._param.minVel)237        if(self._param.dis < self._param.errorCorrectionDis):238            if(self._param.qrang is not None and self._param.qrang != 999):239                RPang = self.Norm_Angle(self.rotateAng - self._param.qrang)240                if(abs(RPang) > self._param.errorAng):241                    if(RPang > 0):242                        x = 0243                        y = 0244                        # yaw = self._param.velYaw245                        yaw = self._param.rotateYaw246                    else:247                        x = 0248                        y = 0249                        # yaw = -self._param.velYaw250                        yaw = -self._param.rotateYaw251                    self.Robot_Vel([x,y,yaw])252                    print('CORRECTION','FRONT',self._param.qrang)253                else:254                    self.Robot_Stop()255                    self.Robot_Stop()256                    self.Robot_Stop()257                    print('CORRECTION',self.rotateAng,self._param.errorRotate0)258                    if(self.rotateAng == self._param.errorRotate0):259                        self._param.behavior = ROTATE260                        self.rotateAng = self._param.errorRotate90261                    else:262                        self._param.behavior = PLATFORM263                        self.rotateAng = self._param.errorRotate0264                        self.initPID = 1265                    266                self.not_find = 0267            else:268                print('CORRECTION not find')269                if(self.not_find < 100):270                    self.not_find += 1271                    self.Robot_Stop()272                else:273                    self.not_find = 0274                    if(self.rotateAng == self._param.errorRotate0):275                        self._param.behavior = ROTATE276                        self.rotateAng = self._param.errorRotate90277                    else:278                        self._param.behavior = PLATFORM279                        self.rotateAng = self._param.errorRotate0  280                        self.initPID = 1281            self._param.qrang = 999282        else:283            x = 0284            yaw = 0285            self.Robot_Vel([x,y,yaw])286            print('CORRECTION','dis',y)287    288    def Platform_Strategy(self):289        print('PLATFORM')290        self.state = 0291        if(self.initPID):292            self.controlYaw.Init()293            self.controlY.Init()294            self.initPID = 0295        self.Robot_Stop()296        if(self.homeFlag == 0):297            self.Dual_Arm_Start()298    299    def Next_Point_Strategy(self):300        print('NEXT_POINT')301        self.Robot_Stop()302        self._param.behavior = ROTATE303        self.rotateAng = self._param.errorRotate0304        305            306    def Rotate_Strategy(self):307        # yaw = self.controlYaw(self._param.qrang,self._param.velYaw)308        if(self._param.qrang is not None and self._param.qrang != 999):309            RPang = self.Norm_Angle(self.rotateAng - self._param.qrang)310            if(abs(RPang) > self._param.errorAng and RPang > self._param.rotateSlowAng):311                if(RPang > 0):312                    x = 0313                    y = 0314                    # yaw = self._param.velYaw315                    yaw = self._param.rotateYaw316                else:317                    x = 0318                    y = 0319                    # yaw = -self._param.velYaw320                    yaw = -self._param.rotateYaw321                self.Robot_Vel([x,y,yaw])322                print('ROTATE','angle',self._param.qrang)323            elif((abs(RPang) > self._param.errorAng and RPang <= self._param.rotateSlowAng)):324                if(RPang > 0):325                    x = 0326                    y = 0327                    # yaw = self._param.velYaw328                    yaw = self._param.rotateYaw*0.8329                else:330                    x = 0331                    y = 0332                    # yaw = -self._param.velYaw333                    yaw = -self._param.rotateYaw*0.8334                self.Robot_Vel([x,y,yaw])335                print('ROTATE','angle',self._param.qrang)336            else:337                self.Robot_Stop()338                self.Robot_Stop()339                self.Robot_Stop()340                if(self.rotateAng == self._param.errorRotate90):341                    self._param.behavior = CORRECTION342                    print('ROTATE COREECTION')343                else:344                    self._param.behavior = CROSS345                    print('ROTATE CROSS')346            self.not_find = 0347        else:348            print('ROTATE not find')349            if(self.not_find < 100):350                self.not_find += 1351                # self.Robot_Stop()352            else:353                self.not_find = 0354                if(self.rotateAng == self._param.errorRotate90):355                    self._param.behavior = CORRECTION356                    print('ROTATE COREECTION')357                else:358                    self._param.behavior = CROSS359                    print('ROTATE CROSS')360        self._param.qrang = 999361    362    def Go_Point_Strategy(self):363        time,state = self.timer.Process()364        if(state):365            self.Robot_Stop()366            self._param.behavior = CORRECTION367        else:368            x = self._param.minVel369            y = 0370            yaw = 0371            self.Robot_Vel([x,y,yaw])372    def Return_Point_Strategy(self):373        time,state = self.timer.Process()374        if(state):375            self.Robot_Stop()376            self._param.behavior = ROTATE377            self.rotateAng = self._param.errorRotate0378        else:379            x = -self._param.minVel380            y = 0381            yaw = 0382            self.Robot_Vel([x,y,yaw])383    384    def Cross_Strategy(self):385        print('CROSS')386        time,state = self.timer.Process()387        if(state):388            self.Robot_Stop()389            self._param.behavior = MOBILE_ROBOT390            self.rotateAng = self._param.errorRotate0391        elif(state == 0 and self.homeFlag == 0):392            x = self._param.minVel393            y = 0394            yaw = 0395            self.Robot_Vel([x,y,yaw])396        elif(state == 0 and self.homeFlag == 1):397            x = -self._param.minVel398            y = 0399            yaw = 0400            self.Robot_Vel([x,y,yaw])401        # if(self.pre_state == 1 and self.state == 0):402        #     if(self._param.scanState):403        #         if(self._param.qrang is not None and self._param.qrang != 999):404        #             x = self._param.minVel405        #             y = 0406        #             yaw = 0407        #             self.not_find = 0408        #             # self.Robot_Vel([y,-x,yaw])409        #             self.Robot_Vel([x,y,yaw])410        #         elif(self.not_find > 60):411        #             self.Robot_Stop()412        #             self._param.behavior = MOBILE_ROBOT413        #             self.not_find = 0414        #                 # print('next point not find line')415        #         else:416        #             self.not_find +=1417        #             x = self._param.minVel418        #             y = 0419        #             yaw = 0420        #             # self.Robot_Vel([y,-x,yaw])421        #             self.Robot_Vel([x,y,yaw])422        #         self._param.qrang = 999423        # else:424        #     self.Robot_Stop()425        #     print('fuck Cross')426    427    def Init_Strategy(self):428        self.rotateAng = self._param.errorRotate0429        self.homeFlag = 0430        self.homeTimes = 0431        self.Robot_Stop()432        self._param.behavior = MOBILE_ROBOT433        self._param.stopPoint = 999434        # self.Reset_IMU()435    def Home_Strategy(self):436        print('HOME times',self.homeTimes,'HOME stop',self._param.stopPoint)437        if(self.homeFlag == 0):438            print('HOME',1)439            self.homeFlag = 1440            self.Robot_Stop()441            self._param.behavior = ROTATE442            self.rotateAng = self._param.errorRotate0443            self.homeTimes -= 1444        else:445            if(self.homeTimes == 0 and self._param.stopPoint == '0'):446                print('home')447                self.Robot_Stop()448                self._param.behavior = PLATFORM449            else:450                if(self.homeTimes == int(self._param.stopPoint)):451                    self.homeTimes -= 1452                    self._param.behavior = CROSS453                elif(self._param.stopPoint == 999):454                    self._param.behavior = CROSS455                else:456                    self._param.behavior = MOBILE_ROBOT457            458    def Deg2Rad(self,deg):459        return deg*math.pi/180460    461    def Norm_Angle(self,angle):462        if(angle > 180):463            angle -= 360464        elif(angle < -180):465            angle +=360466        return angle467    def Robot_Stop(self):468        vel = Twist()469        vel.linear.x = 0470        vel.linear.y = 0471        vel.angular.z = 0472        self._param.pub_cmdvel.publish(vel)473    def Robot_Vel(self,vec):474        vel = Twist()475        vel.linear.x = vec[0]476        vel.linear.y = vec[1]477        vel.angular.z = vec[2]478        self._param.pub_cmdvel.publish(vel)479    480    def Change_Behavior(self):481        self.Robot_Stop()482        self.Robot_Stop()483        self._param.loadParam = False484    485    def Dual_Arm_Start(self):486        start = Bool()487        start.data = True488        self._param.pub_dualArm.publish(start)489    def Scan_Camera_Start(self):490        start = Bool()491        start.data = True492        self._param.pub_startCamera.publish(start)493    def Scan_Camera_Stop(self):494        start = Bool()495        start.data = False496        self._param.pub_startCamera.publish(start)497    498    def Reset_IMU(self):499        reset = Bool()500        reset.data = True501        self._param.pub_resetImu.publish(reset)...

Automation Testing Tutorials

Learn to execute automation testing from scratch with LambdaTest Learning Hub. Right from setting up the prerequisites to run your first automation test, to following best practices and diving deeper into advanced test scenarios. LambdaTest Learning Hubs compile a list of step-by-step guides to help you be proficient with different test automation frameworks i.e. Selenium, Cypress, TestNG etc.