vision = Vision()
action = Action()
debugger = Debugger()
my_command1 = {"id": 0, "vx": 300, "vy": 0, "vw": 0}
my_command2 = {"id": 1, "vx": 0, "vy": 0, "vw": 1.25}
while True:
# Do something(path planning)
# action.sendCommand(id = my_command1["id"],vx=my_command1["vx"], vy=my_command1['vy'], vw=my_command1["vw"])
#debugger.draw_line(vision.robots_yellow[0].x, vision.robots_yellow[0].y, vision.my_ball.x, vision.my_ball.y)
for val in vision.robots_blue.keys():
#print(vision.robots_blue[val].y)
if val == 0:
#action.sendCommand(id = my_command1["id"],vx=my_command1["vx"], vy=my_command1['vy'], vw=my_command1["vw"])
#time.sleep(1)
#print(vision.robots_yellow.keys())
#debugger.robot_msg(vision.robots_blue[val].x, vision.robots_blue[val].y)
debugger.draw_circle(vision.robots_blue[val].x,
vision.robots_blue[val].y)
#print(val)
pass
if val == 1:
#action.sendCommand(id = my_command2["id"],vx=my_command1["vx"], vy=my_command2['vy'], vw=my_command2["vw"])
debugger.draw_line(vision.robots_blue[val].x,
vision.robots_blue[val].y, vision.my_ball.x,
vision.my_ball.y)
pass
# time.sleep(0.02)
#debugger.draw_line(120, 120, 0, 0)
class APF():
# setx,y:start point's x,y
# forx,y:end point's x,y
# Ka:attraction's contant
# da:attraction's Threshold
# Kb:rejection's contant
# db:rejection's Threshold
# This Function is used to get new information from map
def Refresh_robo(self): # 重新读取小车实时位置
self.px = self.vision.blue_robot[0].x
self.py = self.vision.blue_robot[0].y
self.vx = 0
self.vy = 0
self.orientation = self.vision.blue_robot[0].orientation
def vdistance(self, id):
if id == -1:
return [self.px - self.ox, self.py - self.oy]
return [
self.px - self.vision.yellow_robot[id].x,
self.py - self.vision.yellow_robot[id].y
]
def Euclidean_distance(self, vec):
return sqrt(vec[0]**2 + vec[1]**2)
def __init__(self, forx, fory, Ka, da, Kb, db, delta_t, distance):
self.vision = Vision()
self.debugger = Debugger()
time.sleep(0.5)
#print(self.vision.yellow_robot[0].x)
self.Refresh_robo()
self.ox = forx
self.oy = fory
self.Ka = Ka
self.da = da
self.Kb = Kb
self.db = db
self.delta_t = delta_t
self.distance = distance
self.final_list = [[self.px, self.py]]
self.ax = 1
self.ay = 1
# TYPE = 0 is attraction while TYPE = 1 is rejection
def Force(self, TYPE, id):
if TYPE == 0:
distance = self.vdistance(-1)
s = np.array(distance)
Edistance = self.Euclidean_distance(distance)
if Edistance <= self.da:
return list((-2) * self.Ka * s)
if Edistance > self.da:
return list((-2) * self.Ka * self.da / Edistance * s)
elif TYPE == 1:
distance = self.vdistance(id)
s = np.array(distance)
Edistance = self.Euclidean_distance(distance)
if Edistance <= self.db:
f = list(self.Kb * (1 / Edistance - 1 / self.db) *
((1 / Edistance)**3) * s)
return f
if Edistance > self.db:
return [0, 0]
def Total_Force(self): # 计算当前x ,y 方向上的合力
[self.ax, self.ay] = [0, 0]
#print(self.Force(0, 0))
[self.ax, self.ay
] = [self.ax + self.Force(0, 0)[0], self.ay + self.Force(0, 0)[1]]
for i in range(16):
print(self.Force(1, i))
[self.ax, self.ay] = [
self.ax + self.Force(1, i)[0], self.ay + self.Force(1, i)[1]
]
print("adddddd")
print(self.ax, self.ay)
def Refresh_Position(self):
# self.Refresh_robo() 路径规划时不执行
self.Total_Force()
# print("AA")
# print(self.ax, self.ay)
# self.vx += self.delta_t * self.ax
# self.vy += self.delta_t * self.ay
# self.px += self.vx * self.delta_t + 0.5 * self.ax * self.delta_t ** 2
# self.py += self.vy * self.delta_t + 0.5 * self.ay * self.delta_t ** 2
# print(self.px,self.py)
# self.final_list.append([self.px, self.py])
# self.debugger.draw_line(self.final_list[-1][0],self.final_list[-1][1],self.final_list[-2][0],self.final_list[-2][1],"FORWARD")
# time.sleep(1)
theta = atan(self.ay / self.ax)
print("theta")
print(theta)
self.px += self.delta_t * (self.ax / sqrt(self.ax**2 + self.ay**2))
self.py += self.delta_t * (self.ay / sqrt(self.ax**2 + self.ay**2))
self.final_list.append([self.px, self.py])
self.debugger.draw_line(self.final_list[-1][0], self.final_list[-1][1],
self.final_list[-2][0], self.final_list[-2][1],
"FORWARD")
time.sleep(0.01)
# Action() 发送具体的运动指令
def checkDestination(self):
s1 = np.array([self.ox, self.oy])
s2 = np.array(self.final_list[-1])
print("dis")
dis = self.Euclidean_distance(list(s2 - s1))
print(dis)
if dis <= self.distance:
return True
else:
return False
def Draw(self):
self.debugger.draw_final_line(self.final_list, 0, 0)
