def findPath(self,startPoint,end,avoid_ball=False): print("Bot id: {}, calculating path".format(self.kubid)) # global FLAG_PATH_RECEIVED, self.REPLAN # self.FLAG_PATH_RECEIVED = 1 self.REPLAN = 1 startPt = point_2d() self.target = point_2d() startPt.x = startPoint.x startPt.y = startPoint.y self.target.x = end.x self.target.y = end.y # print("Start Point ",startPt.x,startPt.y) # print("self.Target Point",self.target.x,self.target.y) # print("Waiting for service") rospy.wait_for_service('planner') planner = rospy.ServiceProxy('planner', path_plan) message = planner(self.kubid,startPt,self.target,avoid_ball) path = [] for i in xrange(len(message.path)): path = path + [Vector2D(int(message.path[i].x),int(message.path[i].y))] # start = rospy.Time.now() # start = 1.0*start.secs + 1.0*start.nsecs/pow(10,9) self.start_time = rospy.Time.now() self.start_time = 1.0*self.start_time.secs + 1.0*self.start_time.nsecs/pow(10,9) os.environ['bot'+str(self.kubid)]=str(self.start_time) self.v = Velocity(path,self.start_time,startPt) self.v.updateAngle() self.expectedTraverseTime = self.v.getTime(self.v.GetPathLength()) time_cal = self.expectedTraverseTime # self.pso = self.PSO(5,20,1000,1,1,0.5) self.errorInfo = Error()
def findPath(startPoint, end, avoid_ball=False): global FLAG_PATH_RECEIVED, REPLAN FLAG_PATH_RECEIVED = 1 REPLAN = 1 global v, expectedTraverseTime, kubid global start, target global pso, errorInfo startPt = point_2d() target = point_2d() startPt.x = startPoint.x startPt.y = startPoint.y target.x = end.x target.y = end.y # print("Start Point ",startPt.x,startPt.y) # print("Target Point",target.x,target.y) # print("Waiting for service") rospy.wait_for_service('planner') planner = rospy.ServiceProxy('planner', path_plan) message = planner(kubid, startPt, target, avoid_ball) path = [] for i in xrange(len(message.path)): path = path + [ Vector2D(int(message.path[i].x), int(message.path[i].y)) ] start = rospy.Time.now() start = 1.0 * start.secs + 1.0 * start.nsecs / pow(10, 9) v = Velocity(path, start, startPt) v.updateAngle() expectedTraverseTime = v.getTime(v.GetPathLength()) global time_cal time_cal = expectedTraverseTime # pso = PSO(5,20,1000,1,1,0.5) errorInfo = Error()
def init(_kub, target, theta): global kub, GOAL_POINT, rotate, FLAG_turn, FLAG_move, FIRST_CALL kub = _kub GOAL_POINT = point_2d() rotate = theta GOAL_POINT.x = target.x GOAL_POINT.y = target.y FLAG_move = False FLAG_turn = False FIRST_CALL = True
def init(_kub, target, theta): global kub, GOAL_POINT, totalAngle kub = _kub start_time = None GOAL_POINT = point_2d() totalAngle = theta * 1.1 # while True: # print kub.state.homePos # pass # init_angle = kub.state.homePos[kub.kubs_id].theta # print theta,totalAngle #print target.x,target.y # while(1): # pass GOAL_POINT.x = target.x GOAL_POINT.y = target.y
def execute(self, t, kub_id, target, homePos_, awayPos_,avoid_ball=False): # Return vx,vy,vw,self.replan,remainingDistance self.target = target self.REPLAN = 0 self.homePos = homePos_ self.awayPos = awayPos_ self.kubid = kub_id #self.FIRST_CALL = int(os.environ.get('fc'+str(self.kubid))) print(self.FIRST_CALL) # if not self.prev_target==None: if isinstance(self.prev_target, Vector2D): dist = distance_(self.target, self.prev_target) if(dist>DESTINATION_THRESH): self.REPLAN = 1 self.prev_target = self.target # print("in getVelocity, self.FIRST_CALL = ",self.FIRST_CALL) curPos = Vector2D(int(self.homePos[self.kubid].x),int(self.homePos[self.kubid].y)) distance = sqrt(pow(self.target.x - self.homePos[self.kubid].x,2) + pow(self.target.y - self.homePos[self.kubid].y,2)) if(self.FIRST_CALL): print("BOT id:{}, in first call, timeIntoLap: {}".format(self.kubid, t-self.start_time)) startPt = point_2d() startPt.x = self.homePos[self.kubid].x startPt.y = self.homePos[self.kubid].y self.findPath(startPt, self.target, avoid_ball) #os.environ['fc'+str(self.kubid)]='0' self.FIRST_CALL = 0 else: print("Bot id:{}, not first call, timeIntoLap: {}".format(self.kubid,t-self.start_time)) if distance < 1.5*BOT_BALL_THRESH: return [0,0,0,0,0] # print("ex = ",self.expectedTraverseTime) # print("t = ",t," start = ",start) remainingDistance = 0 # print("ex = ",self.expectedTraverseTime) # print("t = ",t," start = ",start) if (t-self.start_time< self.expectedTraverseTime): if self.v.trapezoid(t-self.start_time,curPos): index = self.v.GetExpectedPositionIndex() if index == -1: vX,vY,eX,eY = self.v.sendVelocity(self.v.getVelocity(),self.v.motionAngle[index],index) vX,vY = 0,0 else: remainingDistance = self.v.GetPathLength(startIndex=index) vX,vY,eX,eY = self.v.sendVelocity(self.v.getVelocity(),self.v.motionAngle[index],index) else: # print(t-self.start_time, self.expectedTraverseTime) if self.expectedTraverseTime == 'self.REPLAN': self.REPLAN = 1 # print("Motion Not Possible") vX,vY,eX,eY = 0,0,0,0 flag = 1 else: # print("TimeOUT, self.REPLANNING") vX,vY,eX,eY = 0,0,0,0 self.errorInfo.errorIX = 0.0 self.errorInfo.errorIY = 0.0 self.errorInfo.lastErrorX = 0.0 self.errorInfo.lastErrorY = 0.0 startPt = point_2d() startPt.x = self.homePos[self.kubid].x startPt.y = self.homePos[self.kubid].y self.findPath(startPt,self.target, avoid_ball) errorMag = sqrt(pow(eX,2) + pow(eY,2)) should_replan = self.shouldReplan() if(should_replan == True): self.v.velocity = 0 # print("self.v.velocity now = ",self.v.velocity) # print("entering if, should_replan = ", should_replan) if should_replan or \ (errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) or \ self.REPLAN == 1: # print("______________here____________________") # print("condition 1",should_replan) # print("error magnitude", errorMag) # print("distance threshold",distance > 1.5* BOT_BALL_THRESH) # print("condition 2",errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) # print("condition 3",self.REPLAN) # print("Should self.Replan",should_replan) # print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH) self.REPLAN = 1 startPt = point_2d() startPt.x = self.homePos[self.kubid].x startPt.y = self.homePos[self.kubid].y self.findPath(startPt,self.target, avoid_ball) return [0,0,0, self.REPLAN,0] else: self.errorInfo.errorX = eX self.errorInfo.errorY = eY vX,vY = pid(vX,vY,self.errorInfo,self.pso) botAngle = self.homePos[self.kubid].theta vXBot = vX*cos(botAngle) + vY*sin(botAngle) vYBot = -vX*sin(botAngle) + vY*cos(botAngle) return [vXBot, vYBot, 0, self.REPLAN,remainingDistance] return [vXBot, vYBot, 0, self.REPLAN]
from krssg_ssl_msgs.msg import point_SF from utils.math_functions import * from utils.config import * from krssg_ssl_msgs.srv import bsServer import subprocess BOT_ID = int(sys.argv[1]) x=int(sys.argv[2]) y=int(sys.argv[3]) target=Vector2D(x,y) print "BOT_ID Received",BOT_ID pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000) GOAL_POINT = point_2d() GOAL_POINT.x = 1000 GOAL_POINT.y = 1200 READY_TO_KICK = False REPLANNED = 0 homePos = None awayPos = None state = None rospy.wait_for_service('bsServer',) getState = rospy.ServiceProxy('bsServer',bsServer) try: state = getState(state) except rospy.ServiceException, e: print e
def Get_Vel(start, t, kub_id, target, homePos_, awayPos_, avoid_ball=False): # Return vx,vy,vw,replan,remainingDistance global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid, prev_target REPLAN = 0 homePos = homePos_ awayPos = awayPos_ kubid = kub_id # if not prev_target==None: if isinstance(prev_target, Vector2D): dist = distance_(target, prev_target) if (dist > DESTINATION_THRESH): REPLAN = 1 prev_target = target # print("in getVelocity, FIRST_CALL = ",FIRST_CALL) curPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y)) distance = sqrt( pow(target.x - homePos[kubid].x, 2) + pow(target.y - homePos[kubid].y, 2)) if (FIRST_CALL): startPt = point_2d() startPt.x = homePos[kubid].x startPt.y = homePos[kubid].y findPath(startPt, target, avoid_ball) FIRST_CALL = 0 if distance < 1.5 * BOT_BALL_THRESH: return [0, 0, 0, 0, 0] # print("ex = ",expectedTraverseTime) # print("t = ",t," start = ",start) remainingDistance = 0 # print("ex = ",expectedTraverseTime) # print("t = ",t," start = ",start) if (t - start < expectedTraverseTime): if v.trapezoid(t - start, curPos): index = v.GetExpectedPositionIndex() if index == -1: vX, vY, eX, eY = v.sendVelocity(v.getVelocity(), v.motionAngle[index], index) vX, vY = 0, 0 else: remainingDistance = v.GetPathLength(startIndex=index) vX, vY, eX, eY = v.sendVelocity(v.getVelocity(), v.motionAngle[index], index) else: # print(t-start, expectedTraverseTime) if expectedTraverseTime == 'REPLAN': REPLAN = 1 # print("Motion Not Possible") vX, vY, eX, eY = 0, 0, 0, 0 flag = 1 else: # print("TimeOUT, REPLANNING") vX, vY, eX, eY = 0, 0, 0, 0 errorInfo.errorIX = 0.0 errorInfo.errorIY = 0.0 errorInfo.lastErrorX = 0.0 errorInfo.lastErrorY = 0.0 startPt = point_2d() startPt.x = homePos[kubid].x startPt.y = homePos[kubid].y findPath(startPt, target, avoid_ball) errorMag = sqrt(pow(eX, 2) + pow(eY, 2)) should_replan = shouldReplan() if (should_replan == True): v.velocity = 0 # print("v.velocity now = ",v.velocity) # print("entering if, should_replan = ", should_replan) if should_replan or \ (errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) or \ REPLAN == 1: # print("______________here____________________") # print("condition 1",should_replan) # print("error magnitude", errorMag) # print("distance threshold",distance > 1.5* BOT_BALL_THRESH) # print("condition 2",errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) # print("condition 3",REPLAN) # print("Should Replan",should_replan) # print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH) REPLAN = 1 startPt = point_2d() startPt.x = homePos[kubid].x startPt.y = homePos[kubid].y findPath(startPt, target, avoid_ball) return [0, 0, 0, REPLAN, 0] else: errorInfo.errorX = eX errorInfo.errorY = eY vX, vY = pid(vX, vY, errorInfo, pso) botAngle = homePos[kubid].theta vXBot = vX * cos(botAngle) + vY * sin(botAngle) vYBot = -vX * sin(botAngle) + vY * cos(botAngle) return [vXBot, vYBot, 0, REPLAN, remainingDistance] return [vXBot, vYBot, 0, REPLAN]
def Get_Vel(start, t, kub_id, target, homePos_, awayPos_, avoid_ball=False): global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid REPLAN = 0 homePos = homePos_ awayPos = awayPos_ kubid = kub_id curPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y)) distance = sqrt( pow(target.x - homePos[kubid].x, 2) + pow(target.y - homePos[kubid].y, 2)) if (FIRST_CALL): startPt = point_2d() startPt.x = homePos[kubid].x startPt.y = homePos[kubid].y findPath(startPt, target, avoid_ball) FIRST_CALL = 0 if distance < 1.5 * BOT_BALL_THRESH: return [0, 0, 0, 0] # print("ex = ",expectedTraverseTime) # print("t = ",t," start = ",start) # print("t - start = ",t-start) if (t - start < expectedTraverseTime): if v.trapezoid(t - start, curPos): index = v.GetExpectedPositionIndex() if index == -1: vX, vY, eX, eY = v.sendVelocity(v.getVelocity(), v.motionAngle[index], index) vX, vY = 0, 0 else: vX, vY, eX, eY = v.sendVelocity(v.getVelocity(), v.motionAngle[index], index) else: # print(t-start, expectedTraverseTime) if expectedTraverseTime == 'REPLAN': REPLAN = 1 # print("Motion Not Possible") vX, vY, eX, eY = 0, 0, 0, 0 flag = 1 else: # print("TimeOUT, REPLANNING") vX, vY, eX, eY = 0, 0, 0, 0 errorInfo.errorIX = 0.0 errorInfo.errorIY = 0.0 errorInfo.lastErrorX = 0.0 errorInfo.lastErrorY = 0.0 startPt = point_2d() startPt.x = homePos[kubid].x startPt.y = homePos[kubid].y findPath(startPt, target, avoid_ball) errorMag = sqrt(pow(eX, 2) + pow(eY, 2)) if shouldReplan() or \ (errorMag > 350 and distance > 2* BOT_BALL_THRESH) or \ REPLAN == 1: # print("Should Replan",shouldReplan()) # print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH) REPLAN = 1 startPt = point_2d() startPt.x = homePos[kubid].x startPt.y = homePos[kubid].y findPath(startPt, target, avoid_ball) return [0, 0, 0, REPLAN] else: errorInfo.errorX = eX errorInfo.errorY = eY vX, vY = pid(vX, vY, errorInfo, pso) botAngle = homePos[kubid].theta vXBot = vX * cos(botAngle) + vY * sin(botAngle) vYBot = -vX * sin(botAngle) + vY * cos(botAngle) return [vXBot, vYBot, 0, REPLAN]