def __init__(self):
self._param = NodeHandle()
if (CONTROL == 'PIDCONTROL'):
self.control = PIDControl()
self.controlY = PIDControl_Y()
self.controlYaw = PIDControl_Yaw()
elif (CONTROL == 'FUZZYCONTROL'):
self.control = FUZZYControl()
self.prev_dis = 0
self.prev_ang = 0
self.prev_vel = []
self.initPID = 0
self.state = 0
self.pre_state = 0
self.not_find = 0
# self._kp = 6
# self._ki = 0.1
# self._kd = 4.0
# self.prevIntegral = 0
# self.lastError = 0
''' rotate '''
self.rotateAng = self._param.errorRotate90
''' cross '''
self.timer = TimeCounter(time=self._param.crossTime)
''' home '''
self.homeFlag = 0
self.homeTimes = 0
def __init__(self):
self._param = NodeHandle()
if (CONTROL == 'PIDCONTROL'):
self.control = PIDControl()
self.controlY = PIDControl_Y()
self.controlYaw = PIDControl_Yaw()
# self.controlQRX = PIDControl_Qr(20.0,0.05,10.0)
# self.controlQRY = PIDControl_Qr(20.0,0.05,10.0)
# self.controlQRX = PIDControl_Qr(15,0.05,12.0)
self.controlQRX = PIDControl_Qr(15, 0.05, 14.0)
self.controlQRY = PIDControl_Qr(30.0, 0.05, 15.0)
elif (CONTROL == 'FUZZYCONTROL'):
self.control = FUZZYControl()
self.prev_dis = 0
self.prev_ang = 0
self.prev_vel = []
self.initPID = 0
self.state = 0
self.pre_state = 0
self.not_find = 0
self.stopTimes = 0
self.dualArm = 0
''' rotate '''
self.rotateAng = self._param.errorRotate0
self.pre_rotateYaw = 0
''' cross '''
self.timer = TimeCounter(time=self._param.crossTime)
self.timerQr = TimeCounter(time=1.0)
self.timerQrFlag = False
''' home '''
self.homeFlag = 0
self.homeTimes = 0
class Strategy(object):
'''
Offset track(目前停止)
prev_dis:
prev_ang:
prev_vel:
CONTROL
initPID: 初始化不要使PID不斷累加
QRCODE(目前沒用到)
state:
pre_state:
not_find:
ROTATE
rotateAng: 目標角度
CROSS
timer: 計數器
HOME
homeFlag
1: go home
0: 前進
homeTimes: 記錄走到的停止點
'''
def __init__(self):
self._param = NodeHandle()
if (CONTROL == 'PIDCONTROL'):
self.control = PIDControl()
self.controlY = PIDControl_Y()
self.controlYaw = PIDControl_Yaw()
elif (CONTROL == 'FUZZYCONTROL'):
self.control = FUZZYControl()
self.prev_dis = 0
self.prev_ang = 0
self.prev_vel = []
self.initPID = 0
self.state = 0
self.pre_state = 0
self.not_find = 0
# self._kp = 6
# self._ki = 0.1
# self._kd = 4.0
# self.prevIntegral = 0
# self.lastError = 0
''' rotate '''
self.rotateAng = self._param.errorRotate90
''' cross '''
self.timer = TimeCounter(time=self._param.crossTime)
''' home '''
self.homeFlag = 0
self.homeTimes = 0
def Process(self):
if (self._param.behavior == MOBILE_ROBOT):
if (self._param.loadParam):
self.Change_Behavior()
self.Mobile_Strategy()
elif (self._param.behavior == CORRECTION):
if (self._param.loadParam):
self.Change_Behavior()
self.Correction_Strategy()
elif (self._param.behavior == PLATFORM):
if (self._param.loadParam):
self.Change_Behavior()
self.Platform_Strategy()
elif (self._param.behavior == NEXT_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Next_Point_Strategy()
elif (self._param.behavior == HOME):
if (self._param.loadParam):
self.Change_Behavior()
self.Home_Strategy()
print('HOME')
elif (self._param.behavior == MANUAL):
if (self._param.loadParam):
self.Change_Behavior()
self.state = 0
self.initPID = 0
self.controlYaw.Init()
self.controlY.Init()
print('MANUAL')
elif (self._param.behavior == ROTATE):
if (self._param.loadParam):
self.Change_Behavior()
self.Rotate_Strategy()
elif (self._param.behavior == GO_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Go_Point_Strategy()
print('GO_POINT')
elif (self._param.behavior == RETURN_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Return_Point_Strategy()
print('RETURN_POINT')
elif (self._param.behavior == CROSS):
if (self._param.loadParam):
self.Change_Behavior()
self.Cross_Strategy()
elif (self._param.behavior == INIT):
if (self._param.loadParam):
self.Change_Behavior()
self.Init_Strategy()
print('Init')
else:
print("Don't have Behavior")
self.Robot_Stop()
def Mobile_Strategy(self):
if (self._param.scanState):
count = self._param.scanState.count(1)
if (count):
scanNum = len(self._param.scanState)
if (count <= math.ceil((scanNum) * (2. / 3))
and self._param.stopPoint == 999):
self.state = 0
''' Method 5 '''
y = self.controlY.Process(self._param.dis, self._param.ang,
self._param.minVel)
x = (self._param.minVel - abs(y)) * math.cos(
math.radians(self._param.ang)) - y * math.sin(
math.radians(self._param.ang))
if (abs(self._param.dis) > self._param.errorMoibledis):
yaw = 0
else:
if (abs(self._param.ang) > self._param.errorMoibleAng):
yaw = self.controlYaw.Process(
self._param.ang, self._param.velYaw)
else:
yaw = 0
if (self.homeFlag == 0):
self.Robot_Vel([x, y, yaw])
print(x, y, yaw)
else:
self.Robot_Vel([-x, y, yaw])
print(-x, y, yaw)
# self.prev_dis = self._param.dis
# self.prev_ang = self._param.ang
# self.prev_vel = [x,y,yaw]
elif (self._param.stopPoint != 999
and self._param.stopPoint != '91'):
print('STOP')
self.state = 1
self.Robot_Stop()
if (self.homeFlag == 1):
self._param.behavior = HOME
elif (self.homeTimes == int(self._param.stopPoint)):
self._param.behavior = CROSS
else:
self._param.behavior = ROTATE
self.homeTimes += 1
print("f**k!!!!!!!!!!!!!!! ", self.homeTimes)
self._param.stopPoint = 999
self.pre_state = self.state
else:
print('Offset track !!!!!!')
if (len(self.prev_vel)):
if (self.prev_vel[2] == 0):
x = -(self.prev_vel[0]) * 0.8
y = -self.prev_vel[1] * 1.5
yaw = 0
else:
x = (self._param.minVel *
math.cos(math.radians(self.prev_ang))) * 0.5
y = self._param.minVel * math.sin(
math.radians(self.prev_ang))
yaw = 0
else:
x = 0
y = 0
yaw = 0
print('No scan line')
# self.Robot_Vel([y,-x,yaw])
self.Robot_Stop()
else:
print('No Scan Info !!!!!!')
self.Robot_Stop()
def Correction_Strategy(self):
y = self.controlY.Process(self._param.dis, self._param.ang,
self._param.minVel)
if (self._param.dis < self._param.errorCorrectionDis):
if (self._param.qrang is not None and self._param.qrang != 999):
RPang = self.Norm_Angle(self.rotateAng - self._param.qrang)
if (abs(RPang) > self._param.errorAng):
if (RPang > 0):
x = 0
y = 0
# yaw = self._param.velYaw
yaw = self._param.rotateYaw
else:
x = 0
y = 0
# yaw = -self._param.velYaw
yaw = -self._param.rotateYaw
self.Robot_Vel([x, y, yaw])
print('CORRECTION', 'FRONT', self._param.qrang)
else:
self.Robot_Stop()
self.Robot_Stop()
self.Robot_Stop()
print('CORRECTION', self.rotateAng,
self._param.errorRotate0)
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate90
else:
self._param.behavior = PLATFORM
self.rotateAng = self._param.errorRotate0
self.initPID = 1
self.not_find = 0
else:
print('CORRECTION not find')
if (self.not_find < 100):
self.not_find += 1
self.Robot_Stop()
else:
self.not_find = 0
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate90
else:
self._param.behavior = PLATFORM
self.rotateAng = self._param.errorRotate0
self.initPID = 1
self._param.qrang = 999
else:
x = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
print('CORRECTION', 'dis', y)
def Platform_Strategy(self):
print('PLATFORM')
self.state = 0
if (self.initPID):
self.controlYaw.Init()
self.controlY.Init()
self.initPID = 0
self.Robot_Stop()
if (self.homeFlag == 0):
self.Dual_Arm_Start()
def Next_Point_Strategy(self):
print('NEXT_POINT')
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
def Rotate_Strategy(self):
if (self.rotateAng == self._param.errorRotate90):
print('rotate 90')
x = 0
y = 0
yaw = self._param.rotateYaw
self.Robot_Vel([x, y, yaw])
if (self._param.stopPoint == '90'):
self.Robot_Stop()
self._param.behavior = PLATFORM
if (self.rotateAng == self._param.errorRotate90):
self.rotateAng = self._param.errorRotate0
else:
self.rotateAng = self._param.errorRotate90
elif (self.rotateAng == self._param.errorRotate0):
print('rotate 0')
x = 0
y = 0
yaw = -self._param.rotateYaw
self.Robot_Vel([x, y, yaw])
if (self._param.stopPoint == '91' or self._param.stopPoint == '1'
or self._param.stopPoint == '2'):
self.Robot_Stop()
self._param.behavior = MOBILE_ROBOT
if (self.rotateAng == self._param.errorRotate90):
self.rotateAng = self._param.errorRotate0
else:
self.rotateAng = self._param.errorRotate90
def Go_Point_Strategy(self):
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = CORRECTION
else:
x = self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
def Return_Point_Strategy(self):
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
else:
x = -self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
def Cross_Strategy(self):
print('CROSS')
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = MOBILE_ROBOT
self.rotateAng = self._param.errorRotate0
elif (state == 0 and self.homeFlag == 0):
x = self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
elif (state == 0 and self.homeFlag == 1):
x = -self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
# if(self.pre_state == 1 and self.state == 0):
# if(self._param.scanState):
# if(self._param.qrang is not None and self._param.qrang != 999):
# x = self._param.minVel
# y = 0
# yaw = 0
# self.not_find = 0
# # self.Robot_Vel([y,-x,yaw])
# self.Robot_Vel([x,y,yaw])
# elif(self.not_find > 60):
# self.Robot_Stop()
# self._param.behavior = MOBILE_ROBOT
# self.not_find = 0
# # print('next point not find line')
# else:
# self.not_find +=1
# x = self._param.minVel
# y = 0
# yaw = 0
# # self.Robot_Vel([y,-x,yaw])
# self.Robot_Vel([x,y,yaw])
# self._param.qrang = 999
# else:
# self.Robot_Stop()
# print('f**k Cross')
def Init_Strategy(self):
self.rotateAng = self._param.errorRotate90
self.homeFlag = 0
self.homeTimes = 0
self.Robot_Stop()
self._param.behavior = MOBILE_ROBOT
# self.Reset_IMU()
def Home_Strategy(self):
print('HOME times', self.homeTimes, 'HOME stop', self._param.stopPoint)
if (self.homeFlag == 0):
print('HOME', 1)
self.homeFlag = 1
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
self.homeTimes -= 1
else:
if (self.homeTimes == 0 and self._param.stopPoint == '0'):
print('home')
self.Robot_Stop()
self._param.behavior = PLATFORM
else:
if (self.homeTimes == int(self._param.stopPoint)):
self.homeTimes -= 1
self._param.behavior = CROSS
else:
self._param.behavior = MOBILE_ROBOT
def Deg2Rad(self, deg):
return deg * math.pi / 180
def Norm_Angle(self, angle):
if (angle > 180):
angle -= 360
elif (angle < -180):
angle += 360
return angle
def Robot_Stop(self):
vel = Twist()
vel.linear.x = 0
vel.linear.y = 0
vel.angular.z = 0
self._param.pub_cmdvel.publish(vel)
def Robot_Vel(self, vec):
vel = Twist()
vel.linear.x = vec[0]
vel.linear.y = vec[1]
vel.angular.z = vec[2]
self._param.pub_cmdvel.publish(vel)
def Change_Behavior(self):
self.Robot_Stop()
self.Robot_Stop()
self._param.loadParam = False
def Dual_Arm_Start(self):
start = Bool()
start.data = True
self._param.pub_dualArm.publish(start)
def Scan_Camera_Start(self):
start = Bool()
start.data = True
self._param.pub_startCamera.publish(start)
def Scan_Camera_Stop(self):
start = Bool()
start.data = False
self._param.pub_startCamera.publish(start)
def Reset_IMU(self):
reset = Bool()
reset.data = True
self._param.pub_resetImu.publish(reset)
class Strategy(object):
def __init__(self):
self._param = NodeHandle()
if (CONTROL == 'PIDCONTROL'):
self.control = PIDControl()
self.controlY = PIDControl_Y()
self.controlYaw = PIDControl_Yaw()
elif (CONTROL == 'FUZZYCONTROL'):
self.control = FUZZYControl()
self.state = 0
self.pre_state = 0
self.prev_dis = 0
self.prev_ang = 0
self.prev_vel = []
self.initPID = 0
self.not_find = 0
# self._kp = 6
# self._ki = 0.1
# self._kd = 4.0
# self.prevIntegral = 0
# self.lastError = 0
''' rotate '''
self.rotateAng = self._param.errorRotate0
''' go_point '''
self.timer = TimeCounter(time=0.3)
''' home '''
self.homeFlag = 0
self.homeTimes = 0
def Process(self):
if (self._param.behavior == MOBILE_ROBOT):
if (self._param.loadParam):
self.Change_Behavior()
self.Mobile_Strategy()
elif (self._param.behavior == CORRECTION):
if (self._param.loadParam):
self.Change_Behavior()
self.Correction_Strategy()
elif (self._param.behavior == PLATFORM):
if (self._param.loadParam):
self.Change_Behavior()
self.Platform_Strategy()
elif (self._param.behavior == NEXT_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Next_Point_Strategy()
elif (self._param.behavior == HOME):
if (self._param.loadParam):
self.Change_Behavior()
self.Home_Strategy()
print('HOME')
elif (self._param.behavior == MANUAL):
if (self._param.loadParam):
self.Change_Behavior()
self.state = 0
self.initPID = 0
self.controlYaw.Init()
self.controlY.Init()
print('MANUAL')
elif (self._param.behavior == ROTATE):
if (self._param.loadParam):
self.Change_Behavior()
self.Rotate_Strategy()
elif (self._param.behavior == GO_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Go_Point_Strategy()
print('GO_POINT')
elif (self._param.behavior == RETURN_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Return_Point_Strategy()
print('RETURN_POINT')
elif (self._param.behavior == CROSS):
if (self._param.loadParam):
self.Change_Behavior()
self.Cross_Strategy()
elif (self._param.behavior == INIT):
if (self._param.loadParam):
self.Change_Behavior()
self.Init_Strategy()
print('Init')
else:
print("Don't have Behavior")
self.Robot_Stop()
def Mobile_Strategy(self):
if (self._param.scanState):
count = self._param.scanState.count(1)
if (count):
scanNum = len(self._param.scanState)
if (count <= math.ceil((scanNum) * (2. / 3))
and self._param.stopPoint == 999):
self.state = 0
# Method 3
#if(CONTROL == 'PIDCONTROL'):
# x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)
#elif(CONTROL == 'FUZZYCONTROL'):
# x,y,yaw = self.control.Process(self._param.dis,self._param.ang)
# yaw = 0
#self.Robot_Vel([y,-x,yaw])
#print(y,-x,yaw)
# Method 4
# x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)
# if(abs(self._param.ang) > 10.0):
# if(self._param.ang > 0):
# x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15
# y = -(self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15
# # yaw = self._param.velYaw
# yaw = (self._param.velYaw+abs(yaw))
# else:
# x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15
# y = (self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15
# # yaw = -self._param.velYaw
# yaw = -(self._param.velYaw+abs(yaw))
# else:
# x,y,_ = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)
# # x,y,_ = self.control.Process(self._param.dis,self._param.ang)
# yaw = 0
''' Method 5 '''
y = self.controlY.Process(self._param.dis, self._param.ang,
self._param.minVel)
x = (self._param.minVel - abs(y)) * math.cos(
math.radians(self._param.ang)) - y * math.sin(
math.radians(self._param.ang))
if (abs(self._param.dis) > 1000):
yaw = 0
else:
if (abs(self._param.ang) > 3.0):
yaw = self.controlYaw.Process(
self._param.ang, self._param.velYaw)
else:
yaw = 0
if (self.homeFlag == 0):
self.Robot_Vel([x, y, yaw])
print(x, y, yaw)
else:
self.Robot_Vel([-x, y, yaw])
print(-x, y, yaw)
# self.prev_dis = self._param.dis
# self.prev_ang = self._param.ang
# self.prev_vel = [x,y,yaw]
elif (self._param.stopPoint != 999):
print('STOP')
self.state = 1
self.Robot_Stop()
if (self.homeFlag == 1):
self._param.behavior = HOME
elif (self.homeTimes == int(self._param.stopPoint)):
self._param.behavior = CROSS
else:
self._param.behavior = CORRECTION
self.homeTimes += 1
self._param.stopPoint = 999
self.pre_state = self.state
else:
print('Offset track !!!!!!')
if (len(self.prev_vel)):
if (self.prev_vel[2] == 0):
x = -(self.prev_vel[0]) * 0.8
y = -self.prev_vel[1] * 1.5
yaw = 0
else:
x = (self._param.minVel *
math.cos(math.radians(self.prev_ang))) * 0.5
y = self._param.minVel * math.sin(
math.radians(self.prev_ang))
yaw = 0
else:
x = 0
y = 0
yaw = 0
print('No scan line')
# self.Robot_Vel([y,-x,yaw])
self.Robot_Stop()
else:
print('No Scan Info !!!!!!')
self.Robot_Stop()
def Correction_Strategy(self):
y = self.controlY.Process(self._param.dis, self._param.ang,
self._param.minVel)
if (self._param.dis < 600):
if (self._param.qrang is not None and self._param.qrang != 999):
RPang = self.Norm_Angle(self.rotateAng - self._param.qrang)
if (abs(RPang) > self._param.errorAng):
if (RPang > 0):
x = 0
y = 0
# yaw = self._param.velYaw
yaw = self._param.rotateYaw
else:
x = 0
y = 0
# yaw = -self._param.velYaw
yaw = -self._param.rotateYaw
self.Robot_Vel([x, y, yaw])
print('CORRECTION', 'FRONT', self._param.qrang)
else:
self.Robot_Stop()
self.Robot_Stop()
self.Robot_Stop()
print('CORRECTION', self.rotateAng,
self._param.errorRotate0)
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate90
else:
self._param.behavior = PLATFORM
self.rotateAng = self._param.errorRotate0
self.initPID = 1
self.not_find = 0
else:
print('CORRECTION not find')
if (self.not_find < 100):
self.not_find += 1
self.Robot_Stop()
else:
self.not_find = 0
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate90
else:
self._param.behavior = PLATFORM
self.rotateAng = self._param.errorRotate0
self.initPID = 1
self._param.qrang = 999
else:
x = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
print('CORRECTION', 'dis', y)
def Platform_Strategy(self):
print('PLATFORM')
self.state = 0
if (self.initPID):
self.controlYaw.Init()
self.controlY.Init()
self.initPID = 0
self.Robot_Stop()
def Next_Point_Strategy(self):
print('NEXT_POINT')
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
def Rotate_Strategy(self):
# yaw = self.controlYaw(self._param.qrang,self._param.velYaw)
if (self._param.qrang is not None and self._param.qrang != 999):
RPang = self.Norm_Angle(self.rotateAng - self._param.qrang)
if (abs(RPang) > self._param.errorAng):
if (RPang > 0):
x = 0
y = 0
# yaw = self._param.velYaw
yaw = self._param.rotateYaw
else:
x = 0
y = 0
# yaw = -self._param.velYaw
yaw = -self._param.rotateYaw
self.Robot_Vel([x, y, yaw])
print('ROTATE', 'angle', self._param.qrang)
else:
self.Robot_Stop()
self.Robot_Stop()
self.Robot_Stop()
if (self.rotateAng == self._param.errorRotate90):
self._param.behavior = CORRECTION
print('ROTATE COREECTION')
else:
self._param.behavior = CROSS
print('ROTATE CROSS')
self.not_find = 0
else:
print('ROTATE not find')
if (self.not_find < 100):
self.not_find += 1
# self.Robot_Stop()
else:
self.not_find = 0
if (self.rotateAng == self._param.errorRotate90):
self._param.behavior = CORRECTION
print('ROTATE COREECTION')
else:
self._param.behavior = CROSS
print('ROTATE CROSS')
self._param.qrang = 999
def Go_Point_Strategy(self):
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = CORRECTION
else:
x = self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
def Return_Point_Strategy(self):
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
else:
x = -self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
def Cross_Strategy(self):
print('CROSS')
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = MOBILE_ROBOT
self.rotateAng = self._param.errorRotate0
elif (state == 0 and self.homeFlag == 0):
x = self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
elif (state == 0 and self.homeFlag == 1):
x = -self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
# if(self.pre_state == 1 and self.state == 0):
# if(self._param.scanState):
# if(self._param.qrang is not None and self._param.qrang != 999):
# x = self._param.minVel
# y = 0
# yaw = 0
# self.not_find = 0
# # self.Robot_Vel([y,-x,yaw])
# self.Robot_Vel([x,y,yaw])
# elif(self.not_find > 60):
# self.Robot_Stop()
# self._param.behavior = MOBILE_ROBOT
# self.not_find = 0
# # print('next point not find line')
# else:
# self.not_find +=1
# x = self._param.minVel
# y = 0
# yaw = 0
# # self.Robot_Vel([y,-x,yaw])
# self.Robot_Vel([x,y,yaw])
# self._param.qrang = 999
# else:
# self.Robot_Stop()
# print('f**k Cross')
def Init_Strategy(self):
self.rotateAng = self._param.errorRotate0
self.homeFlag = 0
self.homeTimes = 0
self.Robot_Stop()
self._param.behavior = MOBILE_ROBOT
def Home_Strategy(self):
print('HOME times', self.homeTimes, 'HOME stop', self._param.stopPoint)
if (self.homeFlag == 0):
print('HOME', 1)
self.homeFlag = 1
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
self.homeTimes -= 1
else:
if (self.homeTimes == 0 and self._param.stopPoint == '0'):
print('home')
self.Robot_Stop()
self._param.behavior = PLATFORM
else:
if (self.homeTimes == int(self._param.stopPoint)):
self.homeTimes -= 1
self._param.behavior = CROSS
else:
self._param.behavior = MOBILE_ROBOT
def Deg2Rad(self, deg):
return deg * math.pi / 180
def Norm_Angle(self, angle):
if (angle > 180):
angle -= 360
elif (angle < -180):
angle += 360
return angle
def Robot_Stop(self):
vel = Twist()
vel.linear.x = 0
vel.linear.y = 0
vel.angular.z = 0
self._param.pub_cmdvel.publish(vel)
def Robot_Vel(self, vec):
vel = Twist()
vel.linear.x = vec[0]
vel.linear.y = vec[1]
vel.angular.z = vec[2]
self._param.pub_cmdvel.publish(vel)
def Change_Behavior(self):
self.Robot_Stop()
self.Robot_Stop()
self._param.loadParam = False
class Strategy(object):
'''
Offset track(目前停止)
prev_dis:
prev_ang:
prev_vel:
CONTROL
initPID: 初始化不要使PID不斷累加
QRCODE(目前沒用到)
state:
pre_state:
not_find:
ROTATE
rotateAng: 目標角度
CROSS
timer: 計數器
HOME
homeFlag
1: go home
0: 前進
homeTimes: 記錄走到的停止點
'''
def __init__(self):
self._param = NodeHandle()
if (CONTROL == 'PIDCONTROL'):
self.control = PIDControl()
self.controlY = PIDControl_Y()
self.controlYaw = PIDControl_Yaw()
# self.controlQRX = PIDControl_Qr(20.0,0.05,10.0)
# self.controlQRY = PIDControl_Qr(20.0,0.05,10.0)
# self.controlQRX = PIDControl_Qr(15,0.05,12.0)
self.controlQRX = PIDControl_Qr(15, 0.05, 14.0)
self.controlQRY = PIDControl_Qr(30.0, 0.05, 15.0)
elif (CONTROL == 'FUZZYCONTROL'):
self.control = FUZZYControl()
self.prev_dis = 0
self.prev_ang = 0
self.prev_vel = []
self.initPID = 0
self.state = 0
self.pre_state = 0
self.not_find = 0
self.stopTimes = 0
self.dualArm = 0
''' rotate '''
self.rotateAng = self._param.errorRotate0
self.pre_rotateYaw = 0
''' cross '''
self.timer = TimeCounter(time=self._param.crossTime)
self.timerQr = TimeCounter(time=1.0)
self.timerQrFlag = False
''' home '''
self.homeFlag = 0
self.homeTimes = 0
def Process(self):
if (self._param.behavior == MOBILE_ROBOT):
if (self._param.loadParam):
self.Change_Behavior()
self.Mobile_Strategy()
elif (self._param.behavior == CORRECTION):
if (self._param.loadParam):
self.Change_Behavior()
self.Correction_Strategy()
elif (self._param.behavior == PLATFORM):
if (self._param.loadParam):
self.Change_Behavior()
self.Platform_Strategy()
elif (self._param.behavior == NEXT_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Next_Point_Strategy()
elif (self._param.behavior == HOME):
if (self._param.loadParam):
self.Change_Behavior()
self.Home_Strategy()
print('HOME')
elif (self._param.behavior == MANUAL):
if (self._param.loadParam):
self.Change_Behavior()
self.state = 0
self.initPID = 0
self.controlYaw.Init()
self.controlY.Init()
print('MANUAL')
elif (self._param.behavior == ROTATE):
if (self._param.loadParam):
self.Change_Behavior()
self.Rotate_Strategy()
elif (self._param.behavior == GO_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Go_Point_Strategy()
print('GO_POINT')
elif (self._param.behavior == RETURN_POINT):
if (self._param.loadParam):
self.Change_Behavior()
self.Return_Point_Strategy()
print('RETURN_POINT')
elif (self._param.behavior == CROSS):
if (self._param.loadParam):
self.Change_Behavior()
self.Cross_Strategy()
elif (self._param.behavior == INIT):
if (self._param.loadParam):
self.Change_Behavior()
self.Init_Strategy()
print('Init')
elif (self._param.behavior == ARM_MOVE):
if (self._param.loadParam):
self.Change_Behavior()
self.ARM_MOVE_Strategy()
print('ARM_MOVE')
else:
print("Don't have Behavior")
self.Robot_Stop()
def Mobile_Strategy(self):
if (self._param.scanState):
count = self._param.scanState.count(1)
if (count):
scanNum = len(self._param.scanState)
if (count <= math.ceil((scanNum) * (2. / 3))
and self._param.stopPoint == 999):
self.state = 0
# Method 3
#if(CONTROL == 'PIDCONTROL'):
# x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)
#elif(CONTROL == 'FUZZYCONTROL'):
# x,y,yaw = self.control.Process(self._param.dis,self._param.ang)
# yaw = 0
#self.Robot_Vel([y,-x,yaw])
#print(y,-x,yaw)
# Method 4
# x,y,yaw = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)
# if(abs(self._param.ang) > 10.0):
# if(self._param.ang > 0):
# x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15
# y = -(self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15
# # yaw = self._param.velYaw
# yaw = (self._param.velYaw+abs(yaw))
# else:
# x = -(self._param.minVel*math.cos(math.radians(self._param.ang)))*0.15
# y = (self._param.minVel*math.sin(math.radians(self._param.ang)))*0.15
# # yaw = -self._param.velYaw
# yaw = -(self._param.velYaw+abs(yaw))
# else:
# x,y,_ = self.control.Process(self._param.dis,self._param.ang,self._param.maxVel,self._param.minVel,self._param.velYaw)
# # x,y,_ = self.control.Process(self._param.dis,self._param.ang)
# yaw = 0
''' Method 5 '''
y = self.controlY.Process(self._param.dis, self._param.ang,
self._param.minVel)
x = (self._param.minVel - abs(y)) * math.cos(
math.radians(self._param.ang)) - y * math.sin(
math.radians(self._param.ang))
if (abs(self._param.dis) > self._param.errorMoibledis):
yaw = 0
else:
if (abs(self._param.ang) > self._param.errorMoibleAng):
yaw = self.controlYaw.Process(
self._param.ang, self._param.velYaw)
else:
yaw = 0
if (self.homeFlag == 0):
self.Robot_Vel([x, y, yaw])
print(x, y, yaw)
else:
self.Robot_Vel([-x, y, yaw])
print(-x, y, yaw)
# print(self._param.ang)
# self.prev_dis = self._param.dis
# self.prev_ang = self._param.ang
# self.prev_vel = [x,y,yaw]
elif (self._param.stopPoint != 999
and self._param.stopPoint != '91'
and self._param.stopPoint != '90'):
print('STOP')
self.state = 1
self.Robot_Stop()
if (self.homeFlag == 1):
self._param.behavior = HOME
elif (self.homeTimes == int(self._param.stopPoint)):
self._param.behavior = CROSS
elif (self._param.stopPoint == '2'):
self._param.behavior = CORRECTION
print('state 2')
self.dualArm = 2
self.timerQrFlag = False
# self.Dual_Arm_Start_2()
else:
self.dualArm = 1
self._param.behavior = CORRECTION
self.homeTimes += 1
self.timerQrFlag = False
if (self.stopTimes >= 3):
self._param.stopPoint = 999
self.stopTimes = 0
else:
self.stopTimes += 1
self.pre_state = self.state
else:
print('Offset track !!!!!!')
if (len(self.prev_vel)):
if (self.prev_vel[2] == 0):
x = -(self.prev_vel[0]) * 0.8
y = -self.prev_vel[1] * 1.5
yaw = 0
else:
x = (self._param.minVel *
math.cos(math.radians(self.prev_ang))) * 0.5
y = self._param.minVel * math.sin(
math.radians(self.prev_ang))
yaw = 0
else:
x = 0
y = 0
yaw = 0
print('No scan line')
# self.Robot_Vel([y,-x,yaw])
self.Robot_Stop()
else:
print('No Scan Info !!!!!!')
self.Robot_Stop()
def Correction_Strategy(self):
# y = self.controlY.Process(self._param.dis,self._param.ang,self._param.minVel)
if (self._param.qrX == None):
print('f**k!!!!!!!!!!!!!!!!')
dis = 0
else:
# print('f**k!!!!!!!!!!!!!!!!',self._param.errorCorrectionDis)
# dis = math.sqrt(math.pow(self._param.qrX,2.0)+math.pow(self._param.qrY,2.0))
dis = self._param.qrY
if (self.timerQrFlag == False):
time, self.timerQrFlag = self.timerQr.Process()
else:
# if(self._param.dis < self._param.errorCorrectionDis):
if (abs(dis) < self._param.errorCorrectionDis):
if (self._param.qrTheta is not None
and self._param.qrTheta != 999):
RPang = self.Norm_Angle(self.rotateAng -
self._param.qrTheta)
if (abs(RPang) > self._param.errorAng):
if (RPang > 0):
x = 0
y = 0
yaw = self._param.velYaw
# yaw = self._param.rotateYaw
else:
x = 0
y = 0
yaw = -self._param.velYaw
# yaw = -self._param.rotateYaw
self.Robot_Vel([x, y, yaw])
print('CORRECTION', 'FRONT', self._param.qrTheta)
else:
self.Robot_Stop()
self.Robot_Stop()
self.Robot_Stop()
print('CORRECTION', self.rotateAng,
self._param.errorRotate0)
if (self.dualArm == 1):
# if(self.dualArm == 1 or self.dualArm == 2):
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate90
else:
self._param.behavior = PLATFORM
self.rotateAng = self._param.errorRotate0
self.initPID = 1
elif (self.dualArm == 2):
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = PLATFORM
self.not_find = 0
else:
print('CORRECTION not find')
if (self.not_find < 100):
self.not_find += 1
self.Robot_Stop()
else:
self.not_find = 0
if (self.dualArm == 1):
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate90
else:
self._param.behavior = PLATFORM
self.rotateAng = self._param.errorRotate0
self.initPID = 1
elif (self.dualArm == 2):
if (self.rotateAng == self._param.errorRotate0):
self._param.behavior = PLATFORM
self.initPID = 1
self._param.qrTheta = 999
else:
# x = 0
yaw = 0
# y = 0
x = self.controlQRX.Process(self._param.qrY,
self._param.qrTheta,
self._param.minVel)
y = self.controlQRY.Process(self._param.qrX,
self._param.qrTheta,
self._param.minVel)
self.Robot_Vel([x, y, yaw])
print('CORRECTION', 'dis', x, y)
self._param.qrX = None
def Platform_Strategy(self):
print('PLATFORM')
self.state = 0
if (self.initPID):
self.controlYaw.Init()
self.controlY.Init()
self.initPID = 0
self.Robot_Stop()
if (self.homeFlag == 0 and self.dualArm == 1):
self.Dual_Arm_Start_1()
elif (self.homeFlag == 0 and self.dualArm == 2):
self.Dual_Arm_Start_2()
def Next_Point_Strategy(self):
print('NEXT_POINT')
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
self.dualArm = 0
def Rotate_Strategy(self):
print('ROTATE f**k!!!!!!', self._param.errorRotate90)
# yaw = self.controlYaw(self._param.qrTheta,self._param.velYaw)
if (self._param.qrTheta is not None and self._param.qrTheta != 999):
RPang = self.Norm_Angle(self.rotateAng - self._param.qrTheta)
x = self.controlQRX.Process(self._param.qrY, self._param.qrTheta,
self._param.minVel)
y = self.controlQRY.Process(self._param.qrX, self._param.qrTheta,
self._param.minVel)
if (abs(RPang) > self._param.errorAng
and RPang > self._param.rotateSlowAng):
if (RPang > 0):
# x = 0
# y = 0
yaw = self._param.velYaw
self.pre_rotateYaw = yaw
# yaw = self._param.velYaw*0.8
# yaw = 0
# yaw = self._param.rotateYaw
else:
# x = 0
# y = 0
yaw = self._param.velYaw
self.pre_rotateYaw = yaw
# yaw = -self._param.velYaw*0.8
# yaw = 0
# yaw = -self._param.rotateYaw
self.Robot_Vel([x, y, yaw])
# print('ROTATE','angle',self._param.qrTheta)
print('ROTATE', 'vector', x, y)
elif ((abs(RPang) > self._param.errorAng
and RPang <= self._param.rotateSlowAng)):
if (RPang > 0):
# x = 0
# y = 0
yaw = self._param.velYaw
self.pre_rotateYaw = yaw
# yaw = self._param.velYaw*0.8
# yaw = 0
# yaw = self._param.rotateYaw*0.8
else:
# x = 0
# y = 0
yaw = -self._param.velYaw
self.pre_rotateYaw = yaw
# yaw = self._param.velYaw*0.8
# yaw = 0
# yaw = -self._param.rotateYaw*0.8
self.Robot_Vel([x, y, yaw])
# print('ROTATE','angle',self._param.qrTheta)
print('ROTATE', 'vector', x, y)
else:
self.Robot_Stop()
self.Robot_Stop()
self.Robot_Stop()
if (self.rotateAng == self._param.errorRotate90):
self._param.behavior = CORRECTION
print('ROTATE COREECTION')
else:
self._param.behavior = CROSS
print('ROTATE CROSS')
self.not_find = 0
else:
print('ROTATE not find')
if (self.not_find < 100):
self.not_find += 1
x = 0
y = 0
self.Robot_Vel([x, y, self.pre_rotateYaw])
# self.Robot_Stop()
else:
self.not_find = 0
if (self.rotateAng == self._param.errorRotate90):
self._param.behavior = CORRECTION
print('ROTATE COREECTION')
else:
self._param.behavior = CROSS
print('ROTATE CROSS')
self._param.qrTheta = 999
def Go_Point_Strategy(self):
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = CORRECTION
else:
x = self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
def Return_Point_Strategy(self):
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
else:
x = -self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
def Cross_Strategy(self):
print('CROSS')
time, state = self.timer.Process()
if (state):
self.Robot_Stop()
self._param.behavior = MOBILE_ROBOT
self.rotateAng = self._param.errorRotate0
elif (state == 0 and self.homeFlag == 0):
x = self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
elif (state == 0 and self.homeFlag == 1):
x = -self._param.minVel
y = 0
yaw = 0
self.Robot_Vel([x, y, yaw])
# if(self.pre_state == 1 and self.state == 0):
# if(self._param.scanState):
# if(self._param.qrTheta is not None and self._param.qrTheta != 999):
# x = self._param.minVel
# y = 0
# yaw = 0
# self.not_find = 0
# # self.Robot_Vel([y,-x,yaw])
# self.Robot_Vel([x,y,yaw])
# elif(self.not_find > 60):
# self.Robot_Stop()
# self._param.behavior = MOBILE_ROBOT
# self.not_find = 0
# # print('next point not find line')
# else:
# self.not_find +=1
# x = self._param.minVel
# y = 0
# yaw = 0
# # self.Robot_Vel([y,-x,yaw])
# self.Robot_Vel([x,y,yaw])
# self._param.qrTheta = 999
# else:
# self.Robot_Stop()
# print('f**k Cross')
def Init_Strategy(self):
self.rotateAng = self._param.errorRotate0
self.homeFlag = 0
self.homeTimes = 0
self.Robot_Stop()
self._param.behavior = MOBILE_ROBOT
self._param.stopPoint = 999
# self.Reset_IMU()
def Home_Strategy(self):
print('HOME times', self.homeTimes, 'HOME stop', self._param.stopPoint)
if (self.homeFlag == 0):
print('HOME', 1)
self.homeFlag = 1
self.Robot_Stop()
self._param.behavior = ROTATE
self.rotateAng = self._param.errorRotate0
self.homeTimes -= 1
else:
if (self.homeTimes == 0 and self._param.stopPoint == '0'):
print('home')
self.Robot_Stop()
self._param.behavior = PLATFORM
else:
if (self.homeTimes == int(self._param.stopPoint)):
self.homeTimes -= 1
self._param.behavior = CROSS
elif (self._param.stopPoint == 999):
self._param.behavior = CROSS
else:
self._param.behavior = MOBILE_ROBOT
def ARM_MOVE_Strategy(self):
if (self._param.armMove == 1):
self.Robot_Vel([12, 0, 0])
elif (self._param.armMove == -1):
self.Robot_Vel([-12, 0, 0])
else:
print('arm move f**k !!!!')
def Deg2Rad(self, deg):
return deg * math.pi / 180
def Norm_Angle(self, angle):
if (angle > 180):
angle -= 360
elif (angle < -180):
angle += 360
return angle
def Robot_Stop(self):
vel = Twist()
vel.linear.x = 0
vel.linear.y = 0
vel.angular.z = 0
self._param.pub_cmdvel.publish(vel)
def Robot_Vel(self, vec):
vel = Twist()
vel.linear.x = vec[0]
vel.linear.y = vec[1]
vel.angular.z = vec[2]
self._param.pub_cmdvel.publish(vel)
def Change_Behavior(self):
self.Robot_Stop()
self.Robot_Stop()
self._param.loadParam = False
def Dual_Arm_Start_1(self):
start = Int32()
start.data = 1
self._param.pub_dualArm1.publish(start)
def Dual_Arm_Start_2(self):
start = Int32()
start.data = 2
self._param.pub_dualArm1.publish(start)
def Scan_Camera_Start(self):
start = Bool()
start.data = True
self._param.pub_startCamera.publish(start)
def Scan_Camera_Stop(self):
start = Bool()
start.data = False
self._param.pub_startCamera.publish(start)
def Reset_IMU(self):
reset = Bool()
reset.data = True
self._param.pub_resetImu.publish(reset)