def main(): import memcache shared = memcache.Client(['127.0.0.1:11211'], debug=False) paddingX = 300 paddingY = 200 points = [ Vector2D((HALF_FIELD_MAXX - paddingX), (HALF_FIELD_MAXY - paddingY)), Vector2D((HALF_FIELD_MAXX - paddingX), -(HALF_FIELD_MAXY - paddingY)), Vector2D(-(HALF_FIELD_MAXX - paddingX), -(HALF_FIELD_MAXY - paddingY)), Vector2D(-(HALF_FIELD_MAXX - paddingX), (HALF_FIELD_MAXY - paddingY)), ] BOT_ID = 0 pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000) state = shared.get('state') kub = kubs.kubs(BOT_ID, state, pub) while True: for p in points: state = shared.get('state') kub.update_state(state) g_fsm = GoToPoint.GoToPoint() g_fsm.add_kub(kub) g_fsm.add_point(point=p, orient=state.homePos[kub.kubs_id].theta) g_fsm.spin()
def planner_callback(state): print("incoming planner_callback") global pub bot_id = 0 ballPos = Vector2D(state.ballPos.x, state.ballPos.y) botpos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y) print("dist is ", ballPos.dist(botpos)) new = TestTac.TestTac(bot_id, state) new.execute(state, pub) print(" outgoing planner_callback")
def planner_callback(state): global ref_command, pub if not ref_command: # print("incoming planner_callback") bot_id = 0 ballPos = Vector2D(state.ballPos.x, state.ballPos.y) botpos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y) # print("dist is ",ballPos.dist(botpos)) new = TestTac.TestTac(bot_id, state) new.execute(state, pub)
def skills_GoToPoint(state, bot_id, point): sParams = skills_union.SParam() ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballVel = Vector2D(int(state.ballVel.x), int(state.ballVel.y)) distance = botPos.dist(ballPos) sParams.GoToPointP.x = point.x sParams.GoToPointP.y = point.y sGoToPoint.execute(sParams, state, bot_id, pub)
def LDefender_callback(state): global pub, LDefender_tac # LDefender_id = 0 LDefender_id = 1 ballPos = Vector2D(state.ballPos.x, state.ballPos.y) botpos = Vector2D(state.homePos[0].x, state.homePos[0].y) print("dist ", ballPos.dist(botpos)) # return if LDefender_tac == None: LDefender_tac = TLDefender.TLDefender(LDefender_id, state) LDefender_tac.execute(state, pub)
def get_all(state, bot_id): global fuzz_passing, HALF_FIELD_MAXX_REAL team = [x for x in xrange(6)] opp_player = pass_analysis(bot_id, state) param = () max_score = -1 index = -1 botPos = Vector2D(state.awayPos[bot_id].x, state.awayPos[bot_id].y) ourgoalPos = Vector2D(-HALF_FIELD_MAXX_REAL, 0) for player in team: if player == bot_id: continue else: recPos = Vector2D(state.awayPos[player].x, state.awayPos[player].y) #param = our_player.ratios( state, player) sep = botPos.dist(recPos) sep_from_goal = recPos.dist(ourgoalPos) if sep_from_goal < HALF_FIELD_MAXX_REAL * 2.0: fuzz_passing.input['dis_from_goal'] = sep_from_goal else: fuzz_passing.input['dis_from_goal'] = HALF_FIELD_MAXX_REAL * 2 if sep < HALF_FIELD_MAXX_REAL * 2.0: fuzz_passing.input['dis_bn_p_r'] = sep else: fuzz_passing.input['dis_bn_p_r'] = HALF_FIELD_MAXX_REAL * 2.0 t = time.time() param = opp_player.evr_params(state, player, botPos, recPos, sep) print "$" * 20, " ", time.time() - t, " ", "$" * 20 fuzz_passing.input['danger_no'] = param[1] fuzz_passing.input['net_ratio'] = param[0] fuzz_passing.compute() print "$" * 20, " ", time.time() - t, " ", "$" * 20 if len(T) < 5: T.append([time.time() - t, player]) score = fuzz_passing.output['pass_score'] #print player, state.homePos[player].x, state.homePos[player].y print " --------- " print "dis_from_goal: ", recPos.dist( oppgoalPos), "\ndis_bn_p_r: ", botPos.dist( recPos), "\nintercept_ratio: ", param[ 0], "\nnearness_ratio: ", param[1] print "SCORE--> ", score print recPos.x, " ", recPos.y if score > max_score: max_score = score index = player print "#" * 50 print T print "BOT_TO_PASS---->" return index, max_score
def init(_kub, ballpos, pointB, theta_whiledribble): global kub, APPROACH_POINT, BALLPOS, GOAL_POINT, rotate, FLAG_turn, FLAG_dribble, FLAG_move, FIRST_CALL kub = _kub GOAL_POINT = Vector2D() APPROACH_POINT = Vector2D() BALLPOS = Vector2D() BALLPOS.x = ballpos.x BALLPOS.y = ballpos.y rotate = theta_whiledribble GOAL_POINT.x = pointB.x GOAL_POINT.y = pointB.y FLAG_move = False FLAG_dribble = False FLAG_turn = False FIRST_CALL = True
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 skills_GoToBall(state, bot_id, slope=None): sParams = skills_union.SParam() ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballVel = Vector2D(int(state.ballVel.x), int(state.ballVel.y)) distance = botPos.dist(ballPos) sParams.GoToPointP.x = ballPos.x sParams.GoToPointP.y = ballPos.y if slope: sParams.GoToBallP.align = True sParams.GoToBallP.finalslope = slope else: sParams.GoToBallP.align = False sGoToBall.execute(sParams, state, bot_id, pub)
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 intercept_ratio(self, state, reciever_id): rivals = [x for x in range(6)] min_ratio = 9999999 index = -1 botPos = Vector2D(state.homePos[self.bot_id].x, state.homePos[self.bot_id].y) for player in rivals: distance = botPos.dist( Vector2D(state.awayPos[player].x, state.awayPos[player].y)) dist = self.line(player, self.bot_id, reciever_id, state) in_line_dist = (distance**2 - dist**2)**(0.5) if in_line_dist * 1.0 / dist < min_ratio: min_ratio = in_line_dist * 1.0 / dist index = player else: continue return min_ratio
def debug(param, state, bot_id): botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y) destination = Vector2D(param.GoToPointP.x, param.GoToPointP.y) align = param.GoToPointP.align finalSlope = param.GoToPointP.finalSlope dist = botPos.dist(destination) print '#' * 50 print 'In sGoToPoint' print 'Current bot pos: {}, {}'.format(state.homePos[bot_id].x, state.homePos[bot_id].y) print 'Target pos: {}, {}'.format(destination.x, destination.y) print 'Align: {}'.format(align) if align: print 'Finalslope: {}'.format(finalSlope) print 'Bot Orientation {}'.format(state.homePos[bot_id].theta) print 'Distance: {}'.format(dist) print '#' * 50
def assign(self, state, botL, taskD): Mat = {} for x in botL: Mat[x] = {} botPos = Vector2D(state.homePos[x].x, state.homePos[x].y) for y in taskD.keys(): Mat[x][y] = botPos.dist(taskD[y]) return Mat
def RDefender_callback(state): global pub, RDefender_tac # RDefender_id = 1 RDefender_id = 2 ballPos = Vector2D(state.ballPos.x, state.ballPos.y) if RDefender_tac == None: RDefender_tac = TRDefender.TRDefender(RDefender_id, state) RDefender_tac.execute(state, pub)
def nearness_ratio(self, state, reciever_id): rivals = [x for x in range(6)] min_ratio = 9999999 index = -1 bot_Pos = Vector2D(state.homePos[reciever_id].x, state.homePos[reciever_id].y) passerPos = Vector2D(state.homePos[self.bot_id].x, state.homePos[self.bot_id].y) distance = bot_Pos.dist(passerPos) for player in rivals: dist = bot_Pos.dist( Vector2D(state.awayPos[player].x, state.awayPos[player].y)) if distance * 1.0 / dist < min_ratio: min_ratio = distance * 1.0 / dist index = player else: continue return min_ratio
def g(id_): kub = kubs.kubs(id_, pub) print(kub.kubs_id) m = -1 if id_ % 2 == 0 else 1 g_fsm = GoToPoint.GoToPoint(kub, Vector2D(400 * m * id_ * 0, 800 * id_ * m), 1.57) # g_fsm.as_graphviz() # g_fsm.write_diagram_png() g_fsm.spin()
def ratios(self, state, reciever_id): rivals = [x for x in range(6)] intercept_ratio = -1 nearness_ratio = -1 #index = -1 botPos = Vector2D(state.homePos[self.bot_id].x, state.homePos[self.bot_id].y) recieverPos = Vector2D(state.homePos[reciever_id].x, state.homePos[reciever_id].y) sep = botPos.dist(recieverPos) print "#" * 50 print "PLAYER: ", reciever_id, " in_line_dist sep sep_from_rival perp_dist in_line_dist2" for player in rivals: sep_from_rival = recieverPos.dist( Vector2D(state.awayPos[player].x, state.awayPos[player].y)) distance = botPos.dist( Vector2D(state.awayPos[player].x, state.awayPos[player].y)) distance2 = recieverPos.dist( Vector2D(state.awayPos[player].x, state.awayPos[player].y)) dist = self.line(player, self.bot_id, reciever_id, state) in_line_dist = (distance**2 - dist**2)**(0.5) in_line_dist2 = (distance2**2 - dist**2)**(0.5) print "DETAILS--> ", player, in_line_dist, sep, sep_from_rival, dist, in_line_dist2 if in_line_dist * 1.0 / dist > intercept_ratio and in_line_dist < sep and in_line_dist2 < sep: intercept_ratio = in_line_dist * 1.0 / dist #index = player if sep * 1.0 / sep_from_rival > nearness_ratio: nearness_ratio = sep * 1.0 / sep_from_rival if nearness_ratio > 400.0 / 27: nearness_ratio = 400.0 / 27 # if self.if_intercept_poss( self, state, rival, reciever_id, in_line_dist, perp_dist, sep, in_line_dist2) == 0: # intercept_ratio = 0 # return intercept_ratio, nearness_ratio if intercept_ratio * intercept_ratio < RATIO * RATIO - 1: intercept_ratio = 0 return intercept_ratio, nearness_ratio if intercept_ratio == -1: print "GOTCHA...." intercept_ratio = 0 return intercept_ratio, nearness_ratio if intercept_ratio > 400.0 / 27: intercept_ratio = 400.0 / 27 return intercept_ratio, nearness_ratio
def evr_params(self, state, reciever_id, bp, rp, sp): global RATIO ourteam = [x for x in xrange(6)] botPos = bp recieverPos = rp sep = sp nearness_ratio = -1 intercept_ratio = -1 count1 = 0 count2 = 0 print "#" * 50 print "PLAYER: ", reciever_id, " in_line_dist sep sep_from_rival perp_dist in_line_dist2" for player in ourteam: distance = botPos.dist( Vector2D(state.homePos[player].x, state.homePos[player].y)) distance2 = recieverPos.dist( Vector2D(state.homePos[player].x, state.homePos[player].y)) if distance2 > HALF_FIELD_MAXX * 0.5 and distance > HALF_FIELD_MAXX * 0.75: continue dist = self.line(player, self.bot_id, reciever_id, state) in_line_dist = (distance**2 - dist**2)**(0.5) in_line_dist2 = (distance2**2 - dist**2)**(0.5) if in_line_dist > sep or in_line_dist2 > sep: if nearness_ratio < sep * 1.0 / distance2: nearness_ratio = sep * 1.0 / distance2 if sep * 1.0 / distance2 > RATIO * 0.75: count1 = count1 + 1 else: if intercept_ratio < in_line_dist * 1.0 / dist: intercept_ratio = in_line_dist * 1.0 / dist # if self.prod(in_line_dist, dist, sep) < 0: # count2 = count2 + 1 print "DETAILS--> ", player, in_line_dist, sep, distance2, dist, in_line_dist2 if intercept_ratio == -1 and nearness_ratio == -1: return 0.1, 0 if intercept_ratio > 400.0 / 27: intercept_ratio = 400.0 / 27 if nearness_ratio > 400.0 / 27: nearness_ratio = 400.0 / 27 if intercept_ratio > nearness_ratio: return intercept_ratio, count1 + count2 else: return nearness_ratio, count2 + count1
def shouldReplan(self): # print("velocity = ",self.v.velocity) if self.v.velocity < 10: return False myPos = Vector2D(int(self.homePos[self.kubid].x),int(self.homePos[self.kubid].y)) obsPos = Vector2D() index = self.v.GetExpectedPositionIndex() for i in xrange(len(self.homePos)): if i == self.kubid: pass else: obsPos.x = int(self.homePos[i].x) obsPos.y = int(self.homePos[i].y) if self.v.ellipse(myPos,obsPos,self.v.motionAngle[index]): return True for i in xrange(len(self.awayPos)): obsPos.x = int(self.awayPos[i].x) obsPos.y = int(self.awayPos[i].y) if self.v.ellipse(myPos,obsPos,self.v.motionAngle[index]): return True return False
def shouldReplan(): global homePos, awayPos, kubid if v.velocity < 10: return False myPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y)) obsPos = Vector2D() index = v.GetExpectedPositionIndex() for i in xrange(len(homePos)): if i == kubid: pass else: obsPos.x = int(homePos[i].x) obsPos.y = int(homePos[i].y) if v.ellipse(myPos, obsPos, v.motionAngle[index]): return True for i in xrange(len(awayPos)): obsPos.x = int(awayPos[i].x) obsPos.y = int(awayPos[i].y) if v.ellipse(myPos, obsPos, v.motionAngle[index]): return True return False
from krssg_ssl_msgs.msg import BeliefState from role import GoToBall, GoToPoint, KickToPointP from multiprocessing import Process from kubs import kubs from krssg_ssl_msgs.srv import bsServer from math import atan2, pi from utils.functions import * pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000) BOT_ID = int(sys.argv[1]) x = float(sys.argv[2]) y = float(sys.argv[3]) xx = int(x) yy = int(y) target = Vector2D(x, y) def function(id_, state): kub = kubs.kubs(id_, state, pub) kub.update_state(state) print(kub.kubs_id) g_fsm = KickToPointP.KickToPoint(target) # g_fsm = GoToPoint.GoToPoint() g_fsm.add_kub(kub) # g_fsm.add_point(point=kub.state.ballPos,orient=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x-3000))) g_fsm.add_theta(theta=normalize_angle( atan2(target.y - state.ballPos.y, target.x - state.ballPos.x))) g_fsm.get_pos_as_vec2d(target) #g_fsm.as_graphviz() #g_fsm.write_diagram_png()
def execute(param, state, bot_id, gv, pub, dribller=False): # debug(param,state, bot_id) pointPos = Vector2D() pointPos.x = int(param.GoToPointP.x) pointPos.y = int(param.GoToPointP.y) t = rospy.Time.now() t = t.secs + 1.0 * t.nsecs / pow(10, 9) [vx, vy, vw, REPLANNED, maxDisToTurn] = gv[bot_id].execute(t, bot_id, pointPos, state.homePos, state.awayPos) #vx, vy, vw, replanned if (REPLANNED): print("REPLANNED {}".format(bot_id)) reset(bot_id) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) v = Vector2D() distan = botPos.dist(pointPos) ballPos = Vector2D(state.ballPos) # maxDisToTurn = distan if param.GoToPointP.align == True: angleToTurn = pointPos.normalizeAngle(param.GoToPointP.finalSlope - state.homePos[bot_id].theta) else: angleToTurn = pointPos.normalizeAngle( botPos.angle(ballPos) - state.homePos[bot_id].theta) # angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_OMEGA maxReachTime = maxDisToTurn / MIN_BOT_OMEGA minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if angleToTurn == 0: omega = 0 print("bot id{} vx: {}, vy:{}".format(bot_id, vx, vy)) if param.GoToPointP.align == False: if distan < DRIBBLER_BALL_THRESH: if distan < BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: if distan > BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller)
""" This is a test file for optimalAssignment """ from utils.geometry import Vector2D from optimalAssignment import * botL = [1,2,3,4,5] taskD = { 't1': Vector2D(500,500), 't2': Vector2D(1000,1000), 't3': Vector2D(2000,2000) } obj = optimalAssignment(state, botL, taskD) print obj.decider(botL, taskD)
def execute(param, state, bot_id, pub, dribller=False): pointPos = Vector2D() pointPos.x = int(state.ballPos.x) pointPos.y = int(state.ballPos.y) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballPos = Vector2D(state.ballPos) oppGoal = Vector2D(HALF_FIELD_MAXX, 0) v = Vector2D() targetPoint = Vector2D() # maxDisToTurn = distan if param.GoToBallP.align: angle = param.GoToBallP.finalslope angleToTurn = v.normalizeAngle(param.GoToBallP.finalslope - state.homePos[bot_id].theta) else: angle = ballPos.angle(oppGoal) angleToTurn = v.normalizeAngle( ballPos.angle(oppGoal) - state.homePos[bot_id].theta) # angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta)) targetPoint.x = ballPos.x - 2 * DRIBBLER_BALL_THRESH * cos(angle) targetPoint.y = ballPos.y - 2 * DRIBBLER_BALL_THRESH * sin(angle) distan = botPos.dist(targetPoint) t = rospy.Time.now() t = t.secs + 1.0 * t.nsecs / pow(10, 9) start_time = float(os.environ.get('bot' + str(bot_id))) if distan <= DRIBBLER_BALL_THRESH: # print("in DRIBBLER_BALL_THRESH") [vx, vy, vw, REPLANNED, maxDisToTurn] = Get_Vel(start_time, t, bot_id, state.ballPos, state.homePos, state.awayPos, avoid_ball=False) #vx, vy, vw, replanned omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP: omega = 0 if distan < BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: [vx, vy, vw, REPLANNED, maxDisToTurn] = Get_Vel(start_time, t, bot_id, targetPoint, state.homePos, state.awayPos, avoid_ball=True) #vx, vy, vw, replanned omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP: omega = 0 skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) if (REPLANNED): reset(bot_id)
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]
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]
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 execute(startTime, DIST_THRESH, avoid_ball=False, factor=0.7): global kub, start_time, getState, BALLPOS, APPROACH_POINT, GOAL_POINT, rotate, FLAG_dribble, FLAG_turn, FLAG_move, FIRST_CALL, prev_state, vx_end, vy_end if FIRST_CALL: print("First Call of _Dribble_ ---") start_time = start_time FIRST_CALL = False vx_end, vy_end = 0, 0 #finding APPROACH_POINT: phi = kub.state.homePos[kub.kubs_id].theta APPROACH_POINT = getPointBehindTheBall(BALLPOS, phi, -4) if not FLAG_dribble: while not (FLAG_move): try: kub.state = getState(prev_state).stateB except: print("Error", e) if not (prev_state == kub.state): prev_state = kub.state t = rospy.Time.now() t = t.secs + 1.0 * t.nsecs / pow(10, 9) [vx, vy, vw, REPLANNED ] = velocity.run.Get_Vel(start_time, t, kub.kubs_id, APPROACH_POINT, kub.state.homePos, kub.state.awayPos, avoid_ball) velocity_magnitude = Vector2D(vx, vy).abs(Vector2D(vx, vy)) if velocity_magnitude > MAX_BOT_SPEED: angle_movement = math.atan2(vy, vx) # print("_____________Velocity Changed____________") vy = MAX_BOT_SPEED * math.sin(angle_movement) vx = MAX_BOT_SPEED * math.cos(angle_movement) if (REPLANNED): reset() if not vx and not vy: vx, vy = vx_end, vy_end else: vx_end, vy_end = vx, vy if dist(kub.state.homePos[kub.kubs_id], APPROACH_POINT) < BOT_RADIUS * factor: kub.move(0, 0) # print("Distance completed"*200) FLAG_move = True FLAG_dribble = True else: # print("Sending velocity",vx,vy) kub.move(vx, vy) kub.execute() yield kub, APPROACH_POINT else: kub.dribbler(FLAG_dribble) kub.execute() FLAG_move = False FLAG_turn = False while not (FLAG_move and FLAG_turn): try: kub.state = getState(prev_state).stateB except: print("Error", e) if not (prev_state == kub.state): prev_state = kub.state t = rospy.Time.now() t = t.secs + 1.0 * t.nsecs / pow(10, 9) [vx, vy, vw, REPLANNED] = velocity.run.Get_Vel(start_time, t, kub.kubs_id, GOAL_POINT, kub.state.homePos, kub.state.awayPos, False) velocity_magnitude = Vector2D(vx, vy).abs(Vector2D(vx, vy)) if velocity_magnitude > MAX_BOT_SPEED: angle_movement = math.atan2(vy, vx) # print("_____________Velocity Changed____________") vy = MAX_BOT_SPEED * math.sin(angle_movement) vx = MAX_BOT_SPEED * math.cos(angle_movement) vw = Get_Omega(kub.kubs_id, rotate, kub.state.homePos) if not vw: vw = 0 if (REPLANNED): reset() if not vx and not vy: vx, vy = vx_end, vy_end else: vx_end, vy_end = vx, vy if abs( normalize_angle(kub.state.homePos[kub.kubs_id].theta - rotate)) < ROTATION_FACTOR: kub.turn(0) # print("Angle completed") FLAG_turn = True else: kub.turn(vw) if dist(kub.state.homePos[kub.kubs_id], GOAL_POINT) < BOT_RADIUS * factor: kub.move(0, 0) # print("Distance completed"*200) FLAG_move = True else: # print("Sending velocity",vx,vy) kub.move(vx, vy) kub.execute() yield kub, GOAL_POINT kub.dribble(FLAG_dribble) kub.execute() yield kub, GOAL_POINT
from kubs import kubs, cmd_node from pid.run import * from pid.run_w import * import rospy,sys from krssg_ssl_msgs.msg import point_2d from krssg_ssl_msgs.msg import BeliefState from krssg_ssl_msgs.msg import gr_Commands from krssg_ssl_msgs.msg import gr_Robot_Command from utils.geometry import Vector2D from utils.config import * from utils.math_functions import * kub = None start_time = None start_time_w = None GOAL_POINT = Vector2D(-3000,-2000) point_to = Vector2D(0,0) BScall = None theta = None FLAG_move = True FLAG_turn = True totalAngle = 0 init_angle = 0 FIRST_CALL = True def init(_kub,_point_to): global kub,point_to kub = _kub start_time = None point_to = _point_to
# g_fsm.as_graphviz() # g_fsm.write_diagram_png() g_fsm.spin() # print # print kub.state.homePos[kub.kubs_id].theta,t # g(0) # for i in xrange(0): # p = Process(target=g,args=(i,)) # p.start() #g_fsm1 = GoToBall.GoToBall(kub,deg_2_radian(45)) kub1 = kubs.kubs(0, pub) # print kub1.state.ballPos.y,kub1.state.ballPos.x # while True: # pass g1_fsm = GoToBall.GoToBall(kub1, Vector2D(0, 3000)) g1_fsm.as_graphviz() g1_fsm.write_diagram_png() # g_fsm1.spin() g1_fsm.spin() #t = threading.Thread(target=g_fsm.spin()) #t1 = threading.Thread(target=g1_fsm.spin()) #t.start() #t1.start()*