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]
