def send_module():
global ids, vxs, vys
while True:
send = Send()
send.send_all(ids, vxs, vys, [0, 0, 0, 0, 0, 0, 0])
print('vx', vxs[0])
print('vy', vys[0])
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 200
self.threshold = 0.4
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
def run(color, robot_id, barriers, target_x, target_y, global_p, local_p,
receive):
global_planner = global_p
local_planner = local_p
status = False
while not status:
time_start = time.time()
receive.get_info(color, robot_id)
global_path = global_planner(receive.robot_info['x'],
receive.robot_info['y'], target_x,
target_y, barriers, receive)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
print('ori:', len(path))
path, path_lines = global_path.merge()
print('nodes:', len(path))
time_end = time.time()
print('path cost:', time_end - time_start)
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
end = time.time()
print('total cost:', end - time_start)
motion = local_planner()
motion.path_control(path, robot_id, color, receive)
control = Send()
control.send_msg(robot_id, 0, 0, 0)
print(status)
def runLine(color,
robot_id,
start_x,
start_y,
goal_x,
goal_y,
vmax=150,
threshold=0.3):
receive = Receive()
send = Send()
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
point = [now_x, now_y]
now_ori = receive.robot_info['ori']
error = distance(point, [goal_x, goal_y])
error_max = distance([start_x, start_y], [goal_x, goal_y])
while error > 10:
p = 1
orientation_need_now = math.atan2((goal_y - now_y), (goal_x - now_x))
theta = now_ori - orientation_need_now
if error < error_max * threshold:
p = error / (threshold * error_max) * math.log(2)
p = math.exp(p) - 1
vx_now = vmax * math.cos(theta) * p
vy_now = vmax * math.sin(theta) * p
send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point = [now_x, now_y]
error = distance(point, [goal_x, goal_y])
class XY_speed():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 400
self.threshold = 0.4
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
self.up = 60
def line_control(self,
path,
robot_id,
color,
receive,
target_x,
target_y,
info=None,
threshold=30,
index=1):
N = len(path)
for i in range(N - 1):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
error_max = distance(path[i], path[i + 1])
thres = 30
if i == N - 2:
thres = 7
while error > thres:
orientation_need_now = math.atan2((path[i + 1][1] - now_y),
(path[i + 1][0] - now_x))
theta = now_ori - orientation_need_now
p = 1
dis_now = distance(path[i], path[i + 1])
if dis_now < self.up:
p = 0.5
thresdist = error_max * self.threshold
if 3 * thresdist > error > thresdist:
p = 0.6 * p
if error < thresdist:
p = p * error / thresdist
vx_now = self.v * math.cos(theta) * p
vy_now = self.v * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
# print(i)
# print('cal')
self.send.send_msg(robot_id, 0, 0, 0)
return True, index
def send_module():
while True:
try:
send = Send()
send.send_msg(id, vx, vy, 0)
except:
continue
def send_module():
# vx, vy = 10, 10
while True:
try:
send = Send()
send.send_msg(id, vx, vy, 0)
sleep(0.005)
except:
continue
def run_line(color, robot_id, barriers, target_x, target_y, global_p, local_p,
receive):
global_planner = global_p
local_planner = local_p
while True:
receive.get_info(color, robot_id)
global_path = global_planner(receive.robot_info['x'],
receive.robot_info['y'],
target_x,
target_y,
barriers,
receive,
color=color,
id=robot_id)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
if distance((now_x, now_y), (target_x, target_y)) > 7:
motion = local_planner()
if not motion.line_control(path, robot_id, color, receive,
target_x, target_y, barriers):
control = Send()
control.send_msg(robot_id, 0, 0, 0)
else:
continue
else:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
return
def send_module():
# vx, vy = 10, 10
global shock, end_time, shock_time
while True:
try:
send = Send()
send.send_msg(id, vx, vy, 0)
if shock:
print('shock{}'.format(randint(0, 100)))
if time.time() - shock_time > 0.3:
shock = False
sleep(0.013)
except:
continue
def send_module():
# vx, vy = 10, 10
global shock, end_time
while True:
try:
send = Send()
send.send_msg(id, vx, vy, 0)
if shock:
sleep(0.2)
shock = False
print('shock')
else:
sleep(0.013)
except:
continue
def run_shrink(color, robot_id, barriers, target_x, target_y, global_p,
local_p, receive):
global_planner = global_p
local_planner = local_p
R = 25
index = 1
while True:
if index > 4:
index = 4
r = R / index
receive.get_info(color, robot_id)
starttime = time.time()
global_path = global_planner(
receive.robot_info['x'],
receive.robot_info['y'],
target_x,
target_y,
barriers,
# receive, color=color, id=robot_id)
receive,
color=color,
id=robot_id,
inflateRadius=r,
dis_threshold=r)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
endtime = time.time()
print('road planning time: ', endtime - starttime)
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
# import ipdb;ipdb.set_trace()
if distance((now_x, now_y), (target_x, target_y)) > 7:
motion = local_planner()
status, index = motion.line_control(path,
robot_id,
color,
receive,
target_x,
target_y,
barriers,
r,
index=index)
if not status:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
else:
index = 1
continue
else:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
return
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 300
self.threshold = 0.3
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
self.up = 60
self.x = 9
self.y = 12
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 350
self.threshold = 0.25
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
self.up = 60
self.rtt_distance = 60
self.wallthreshold = 30
self.wall_k = 2400
self.w = 300
self.h = 200
self.d_k = 360000
self.v_k = 0.7
self.time_threshold = 1
def run_while(color, robot_id, barriers, target_x, target_y, global_p, local_p,
receive):
global_planner = global_p
local_planner = local_p
while True:
time_start = time.time()
receive.get_info(color, robot_id)
global_path = global_planner(receive.robot_info['x'],
receive.robot_info['y'], target_x,
target_y, barriers, receive)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
print('ori:', len(path))
path, path_lines = global_path.merge()
print('nodes:', len(path))
time_end = time.time()
print('path cost:', time_end - time_start)
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
end = time.time()
print('total cost:', end - time_start)
if len(path) < 2:
point = path[0]
else:
point = path[1]
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
# import ipdb;ipdb.set_trace()
if distance((now_x, now_y), (target_x, target_y)) > 20:
motion = local_planner()
motion.point_control(point, robot_id, color, receive)
control = Send()
control.send_msg(robot_id, 0, 0, 0)
else:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
return
class XY_p():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 280
self.threshold = 0.5
def path_control(self, path, robot_id, color, receive):
#一开始车头朝向右边，x正方向，所以v_x对应x_path, v_y对应y_path
# T = 0.05
# ta = T / 10 # 固定加速时间
# tn = T - 2 * (0.5 * ta + 0.5 * ta) # 固定匀速运动时间，2.5秒
# x_path = np.array(path)[:, 0]
# y_path = np.array(path)[:, 1]
# v_x = np.concatenate(([0.0], (x_path[1:] - x_path[:-1]) / T, [0.0]))
# v_y = np.concatenate(([0.0], (y_path[1:] - y_path[:-1]) / T, [0.0]))
# path = interpolate_path(path)
#得到匀速运动段的速度
for i in range(len(path) - 1):
# #加速阶段
# self.send.send_msg(robot_id, 0.75 * v_x[i] + 0.25 * v_x[i + 1], 0.75 * v_y[i] + 0.25 * v_y[i + 1], 0)
# sleep(ta/4)
# self.send.send_msg(robot_id, 0.5 * v_x[i] + 0.5 * v_x[i + 1], 0.5 * v_y[i] + 0.5 * v_y[i + 1], 0)
# sleep(ta / 4)
# self.send.send_msg(robot_id, 0.25 * v_x[i] + 0.75 * v_x[i + 1], 0.25 * v_y[i] + 0.75 * v_y[i + 1], 0)
# sleep(ta / 4)
# self.send.send_msg(robot_id, v_x[i + 1], v_y[i + 1], 0)
# sleep(ta / 4)
#
# #直线运动阶段
# self.send.send_msg(robot_id, v_x[i + 1], v_y[i + 1], 0)
# sleep(tn)
# 闭环检测部分
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
point = [now_x, now_y]
now_ori = receive.robot_info['ori']
error = distance(point, path[i + 1])
error_max = distance(path[i], path[i + 1])
print('error:', error)
while error > 10:
orientation_need_now = math.atan2((path[i + 1][1] - now_y),
(path[i + 1][0] - now_x))
theta = now_ori + orientation_need_now
# p = error/error_max
# if p < self.threshold:
# p = self.threshold
p = 1
if error < error_max * self.threshold:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
vx_now = self.v * math.cos(theta) * p
vy_now = self.v * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point = [now_x, now_y]
error = distance(point, path[i + 1])
print(error)
# self.send.send_msg(robot_id, v_x[i+1], v_y[i+1], 0)
# sleep(T/100)
def point_control(self, point, robot_id, color, receive):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(
np.square(now_x - point[0]) + np.square(now_y - point[1]))
print('error:', error)
index = 0
while error > 10:
orientation_need_now = math.atan2((point[1] - now_y),
(point[0] - now_x))
theta = now_ori + orientation_need_now
if error < 20:
v = 100
vx_now = self.v * math.cos(theta)
vy_now = self.v * math.sin(theta)
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(
np.square(now_x - point[0]) + np.square(now_y - point[1]))
index += 1
def line_control(self, path, robot_id, color, receive, info=None):
for i in range(len(path) - 1):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
error_max = distance(point_now, path[i + 1])
print('error:', error)
while error > 30:
orientation_need_now = math.atan2((path[i + 1][1] - now_y),
(path[i + 1][0] - now_x))
theta = now_ori - orientation_need_now
# p = error/error_max
# if p < self.threshold:
# p = self.threshold
p = 1
if error < error_max * self.threshold:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
vx_now = self.v * math.cos(theta) * p
vy_now = self.v * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
print('error:', error)
if info is not None:
start = time.time()
status = check_path_l(receive, point_now, path[i + 1:],
info)
# import ipdb;ipdb.set_trace()
end = time.time()
print("time:", end - start)
if not status:
return False
return True
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 280
self.threshold = 0.5
class XY_speed():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 370
self.threshold = 0.4
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
self.up = 60
def line_control(self, path, robot_id, color, receive, target_x, target_y, info=None, threshold=30, index=1):
N = len(path)
for i in range(N - 1):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
error_max = distance(path[i], path[i + 1])
if distance(point_now, [target_x, target_y]) > 7:
thres = 20
if i == N - 2:
thres = 7
while error > thres:
orientation_need_now = math.atan2((path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
theta = now_ori - orientation_need_now
p = 1
dis_now = distance(path[i], path[i + 1])
if dis_now < self.up:
p = sigmoid(error / dis_now - 1)
if error < error_max * self.threshold:
if i < N - 2:
alpha = math.atan2(path[i + 2][1] - path[i + 1][1], path[i + 2][0] - path[i + 1][0])
if orientation_need_now > 0:
angle = alpha + (PI - orientation_need_now)
else:
angle = alpha - (PI + orientation_need_now)
if angle > PI:
angle = 2 * PI - angle
if abs(angle) < self.angle_threshold:
p = error / ((self.threshold + self.time_turn) * error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
#速度斥力部分
vx = 0.0
vy = 0.0
k1 = 20
k2 = 20
rr = 50
info = receive.get_infos(color=color, id=robot_id)
for index in range(len(info)):
if distance(info[index][:2], point_now) < rr:
k = (k1 * info[index][2] / 400) + k2 * rr / distance(info[index][:2], point_now)
gamma = math.atan2(info[index][1] - now_y, info[index][0] - now_x)
vx = vx + k * math.cos(gamma)
vy = vy + k * math.sin(gamma)
else:
continue
vx_now = (self.v-vx) * math.cos(theta) * p
vy_now = (self.v-vy) * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
if info is not None:
status, index = check_path_l(receive, point_now, path[i + 1:], info, color=color,
id=robot_id,
dis_threshold=threshold, index_=index)
if not status:
return False, index
else:
self.send.send_msg(robot_id, 0, 0, 0)
return True, index
return True, index
# def line_control(self, now_x, now_y, now_ori, path, i, N, target_x, target_y, infos=None, k1=10, k2=10, v_obstacle_max=400, rr=100, color="blue", robot_id=0):
# point_now = [now_x, now_y]
# error = distance(point_now, path[i+1])
# error_max = distance(path[i], path[i+1])
# if error_max < 80:
# error_max = 80
# orientation_need_now = math.atan2((path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
# theta = now_ori - orientation_need_now
# p = 1
# dis_now = distance(path[i], path[i+1])
# if distance(point_now, [target_x, target_y]) < 80:
# p = 0.2
# else:
# if error > 7:
# if dis_now < self.up:
# p = sigmoid(error/dis_now-1)
# if error < error_max * self.threshold:
# if i < N - 2:
# alpha = math.atan2(path[i + 2][1] - path[i + 1][1], path[i + 2][0] - path[i + 1][0])
# if orientation_need_now > 0:
# angle = alpha + (PI - orientation_need_now)
# else:
# angle = alpha - (PI + orientation_need_now)
# if angle > PI:
# angle = 2 * PI - angle
# if abs(angle) < self.angle_threshold:
# p = error / ((self.threshold + self.time_turn) * error_max) * math.log(2)
# p = math.exp(p) - 1
# else:
# p = error / (self.threshold * error_max) * math.log(2)
# p = math.exp(p) - 1
# else:
# p = error / (self.threshold * error_max) * math.log(2)
# p = math.exp(p) - 1
# else:
# p = 0.2
# vx_now = self.v * math.cos(theta) * p
# vy_now = self.v * math.sin(theta) * p
# return vx_now, vy_now, True
#
# #速度斥力部分
# vx = 0.0
# vy = 0.0
# for index in range(len(infos)):
# if infos[index][3] == robot_id and infos[index][2] == color:
# continue
# elif distance(infos[index][:2], point_now) < rr:
# k = (k1 * infos[index][5] / v_obstacle_max) + k2 * rr / distance(infos[index][:2], point_now)
# gamma = math.atan2(infos[index][1] - now_y, infos[index][0] - now_x)
# vx = vx + k * math.cos(gamma)
# vy = vy + k * math.sin(gamma)
# else:
# continue
#
# vx_now = (self.v * math.cos(theta)-vx) * p
# vy_now = (self.v * math.sin(theta)-vy) * p
# return vx_now, vy_now, False
class P_control():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
def path_control(self, path, robot_id, color, receive):
for i in range(len(path) - 1):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(
np.square(now_x - path[i + 1][0]) +
np.square(now_y - path[i + 1][1]))
if error > 10:
step = 0
orientation_need_now = -math.atan2((path[i + 1][1] - now_y),
(path[i + 1][0] - now_x))
radians_now = now_ori - orientation_need_now
while abs(radians_now) > 0.1:
orientation_need_now = -math.atan2(
(path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
r_v = radians_now
if abs(r_v) < 0.5:
r_v /= abs(r_v)
if abs(r_v) < 3:
r_v /= abs(r_v)
r_v *= 3
self.send.send_msg(robot_id, 0, 0, r_v)
s = time.time()
sleep(0.1)
e = time.time()
receive.get_info(color, robot_id)
now_ori = receive.robot_info['ori']
radians_now = now_ori - orientation_need_now
while error > 10 or step < 10:
v_x = error * 1
if abs(v_x) > 50:
v_x = 50
self.send.send_msg(robot_id, v_x, 0, 0)
sleep(0.1)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
error = np.sqrt(
np.square(now_x - path[i + 1][0]) +
np.square(now_y - path[i + 1][1]))
step += 1
def point_control(self, point, robot_id, color, receive):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(
np.square(now_x - point[0]) + np.square(now_y - point[1]))
if error > 10:
step = 0
orientation_need_now = -math.atan2((point[1] - now_y),
(point[0] - now_x))
radians_now = now_ori - orientation_need_now
while abs(radians_now) > 0.1:
orientation_need_now = -math.atan2((point[1] - now_y),
(point[0] - now_x))
r_v = radians_now
self.send.send_msg(robot_id, 0, 0, r_v)
s = time.time()
sleep(0.1)
e = time.time()
receive.get_info('blue', robot_id)
now_ori = receive.robot_info['ori']
radians_now = now_ori - orientation_need_now
while error > 10 or step < 10:
v_x = error
self.send.send_msg(robot_id, v_x, 0, 0)
sleep(0.1)
receive.get_info('blue', robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
error = np.sqrt(
np.square(now_x - point[0]) + np.square(now_y - point[1]))
step += 1
class XY_speed():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 300
self.threshold = 0.4
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
self.up = 60
def path_control(self, path, robot_id, color, receive):
N = len(path)
for i in range(N-1):
# 闭环检测部分
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
point = [now_x, now_y]
now_ori = receive.robot_info['ori']
error = distance(point, path[i+1])
error_max = distance(path[i], path[i+1])
# print('error:', error)
thres = 20
if i == N - 2:
thres = 7
while error > thres:
p = 1
orientation_need_now = math.atan2((path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
theta = now_ori + orientation_need_now
if error < error_max * self.threshold:
if i < N - 2:
alpha = math.atan2(path[i + 2][1] - path[i + 1][1], path[i + 2][0] - path[i + 1][0])
if orientation_need_now > 0:
angle = alpha + (PI - orientation_need_now)
else:
angle = alpha - (PI + orientation_need_now)
if angle > PI:
angle = 2 * PI - angle
if abs(angle) < self.angle_threshold:
p = error / ((self.threshold + self.time_turn) * error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
vx_now = self.v * math.cos(theta) * p
vy_now = self.v * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point = [now_x, now_y]
error = distance(point, path[i+1])
# print('error:', error)
# self.send.send_msg(robot_id, v_x[i+1], v_y[i+1], 0)
# sleep(T/100)
def point_control(self, point, robot_id, color, receive):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(np.square(now_x - point[0]) + np.square(now_y - point[1]))
print('error:', error)
index = 0
while error > 10:
orientation_need_now = math.atan2((point[1] - now_y), (point[0] - now_x))
theta = now_ori + orientation_need_now
if error < 20:
v = 100
vx_now = self.v * math.cos(theta)
vy_now = self.v * math.sin(theta)
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(np.square(now_x - point[0]) + np.square(now_y - point[1]))
index += 1
def line_control(self, path, robot_id, color, receive, target_x, target_y, info=None, threshold=30, index=1):
N = len(path)
for i in range(N - 1):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i+1])
error_max = distance(path[i], path[i+1])
if distance(point_now, [target_x, target_y]) > 7:
thres = 20
if i == N - 2:
thres = 7
while error > thres:
orientation_need_now = math.atan2((path[i+1][1] - now_y), (path[i+1][0] - now_x))
theta = now_ori - orientation_need_now
p = 1
dis_now = distance(path[i], path[i+1])
if dis_now < self.up:
p = sigmoid(error/dis_now-1)
if error < error_max * self.threshold:
if i < N - 2:
alpha = math.atan2(path[i + 2][1] - path[i + 1][1], path[i + 2][0] - path[i + 1][0])
if orientation_need_now > 0:
angle = alpha + (PI - orientation_need_now)
else:
angle = alpha - (PI + orientation_need_now)
if angle > PI:
angle = 2 * PI - angle
if abs(angle) < self.angle_threshold:
p = error / ((self.threshold + self.time_turn) * error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
vx_now = self.v * math.cos(theta) * p
vy_now = self.v * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i+1])
if info is not None:
status, index = check_path_l(receive, point_now, path[i+1:], info, color=color, id=robot_id,
dis_threshold=threshold, index_=index)
if not status:
return False, index
else:
self.send.send_msg(robot_id, 0, 0, 0)
return True, index
return True, index
def __init__(self):
self.send = Send()
self.debug = SendDebug()
class Bang_Control():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
#def path_control(self, path, robot_id, color, receive):
def point_control(self, point, robot_id, color, receive):
'''
此时机器人有本身的速度
:param point: 下一个目标点
:param robot_id:
:param color:
:param receive:
:return:
'''
receive.get_info(robot_id, color)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
now_vx = receive.robot_info['vx']
now_vy = receive.robot_info['vy']
now_w = receive.robot_info['w']
now_ax = receive.robot_info['ax']
now_ay = receive.robot_info['ay']
error = np.sqrt(np.square(now_x - point[0]) + np.square(now_y - point[1]))
exist_obstacle_dist = [0]
exist_obstacle_ax = [0]
exist_obstacle_ay = [0]
obstacle_color = 'yellow'
for i in range(8):
receive.get_info(i, obstacle_color)
dist = np.sqrt(np.square(receive.robot_info['x'] - point[0]) + np.square(receive.robot_info['y'] - point[1]))
if dist < 30:
exist_obstacle_dist.append(dist)
exist_obstacle_ax.append((100.0 * (point[0]-receive.robot_info['x'])) / (dist ** 2))
exist_obstacle_ay.append((100.0 * (point[1]-receive.robot_info['y'])) / (dist ** 2))
else:
pass
print('error:', error)
ax = sum(exist_obstacle_ax)
ay = sum(exist_obstacle_ay)
index = 0
print(ax,ay,now_vx,now_vy)
while error > 10 or index < 10:
orientation_need_now = math.atan2((point[1] - now_y), (point[0] - now_x))
theta = now_ori + orientation_need_now
vx_need = 150 * math.cos(theta)
vy_need = 150 * math.sin(theta)
self.send.send_msg(robot_id, now_vx+ax, now_vy+ay, 0)
sleep(0.01)
for i in range(5):
self.send.send_msg(robot_id, 0.2*i*vx_need+0.2*(5-i)*now_vx, 0.2*i*vx_need+0.2*(5-i)*now_vy, 0)
sleep(0.002)
self.send.send_msg(robot_id, vx_need, vy_need, 0)
sleep(0.01)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(np.square(now_x - point[0]) + np.square(now_y - point[1]))
index += 1
from message.send import Send
from message.receive import Receive
from time import sleep
if __name__ == "__main__":
receive = Receive()
send = Send()
color = 'blue'
robot_id = 0
send.send_msg(robot_id, 100, 100, 0, power=1.0, d = 300000)
sleep(3)
receive.get_info(color, robot_id)
print("vx = :", receive.robot_info['vx'])
print("vy = :", receive.robot_info['vy'])
send.send_msg(robot_id, 10, 20, 0, power=0.0, d = 1000000)
sleep(10)
receive.get_info(color, robot_id)
print("vx = :", receive.robot_info['vx'])
print("vy = :", receive.robot_info['vy'])
class XY_control():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 300
def path_control(self, path, robot_id, color, receive):
#一开始车头朝向右边，x正方向，所以v_x对应x_path, v_y对应y_path
# T = 0.05
# ta = T / 10 # 固定加速时间
# tn = T - 2 * (0.5 * ta + 0.5 * ta) # 固定匀速运动时间，2.5秒
# x_path = np.array(path)[:, 0]
# y_path = np.array(path)[:, 1]
# v_x = np.concatenate(([0.0], (x_path[1:] - x_path[:-1]) / T, [0.0]))
# v_y = np.concatenate(([0.0], (y_path[1:] - y_path[:-1]) / T, [0.0]))
# path = interpolate_path(path)
#得到匀速运动段的速度
for i in range(len(path)-1):
# #加速阶段
# self.send.send_msg(robot_id, 0.75 * v_x[i] + 0.25 * v_x[i + 1], 0.75 * v_y[i] + 0.25 * v_y[i + 1], 0)
# sleep(ta/4)
# self.send.send_msg(robot_id, 0.5 * v_x[i] + 0.5 * v_x[i + 1], 0.5 * v_y[i] + 0.5 * v_y[i + 1], 0)
# sleep(ta / 4)
# self.send.send_msg(robot_id, 0.25 * v_x[i] + 0.75 * v_x[i + 1], 0.25 * v_y[i] + 0.75 * v_y[i + 1], 0)
# sleep(ta / 4)
# self.send.send_msg(robot_id, v_x[i + 1], v_y[i + 1], 0)
# sleep(ta / 4)
#
# #直线运动阶段
# self.send.send_msg(robot_id, v_x[i + 1], v_y[i + 1], 0)
# sleep(tn)
# 闭环检测部分
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(np.square(now_x - path[i + 1][0]) + np.square(now_y - path[i + 1][1]))
print('error:', error)
while error > 10:
orientation_need_now = math.atan2((path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
theta = now_ori - orientation_need_now
vx_now = self.v * math.cos(theta)
vy_now = self.v * math.sin(theta)
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(np.square(now_x - path[i + 1][0]) + np.square(now_y - path[i + 1][1]))
# self.send.send_msg(robot_id, v_x[i+1], v_y[i+1], 0)
# sleep(T/100)
i += 1
def point_control(self, point, robot_id, color, receive, info=None):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, point)
print('error:', error)
while error > 10:
orientation_need_now = math.atan2((point[1] - now_y), (point[0] - now_x))
theta = now_ori + orientation_need_now
vx_now = self.v * math.cos(theta)
vy_now = self.v * math.sin(theta)
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, point)
print('error:', error)
def line_control(self, point, robot_id, color, receive, info=None):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, point)
print('error:', error)
while error > 10:
orientation_need_now = math.atan2((point[1] - now_y), (point[0] - now_x))
theta = now_ori + orientation_need_now
vx_now = self.v * math.cos(theta)
vy_now = self.v * math.sin(theta)
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, point)
print('error:', error)
if info is not None:
start = time.time()
status = check_two_points(receive, point_now, point, info)
# import ipdb;ipdb.set_trace()
end = time.time()
print("time:", end - start)
if not status:
return
class X_ori_speed():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 250
self.threshold = 0.4
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
self.up = 60
def line_control(self, path, robot_id, color, receive, target_x, target_y, info=None, threshold=30, index=1, Av_max=7):
N = len(path)
for i in range(N - 1):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
error_max = distance(path[i], path[i + 1])
angular_thres = 0.088
orientation_need_now = math.atan2((path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
theta = now_ori - orientation_need_now
if PI < theta < 2 * PI:
theta = theta - 2 * PI
elif -2 * PI < theta < -PI:
theta = theta + 2 * PI
flag = abs(theta)/theta
angular_error = abs(theta)
while angular_error > angular_thres:
if angular_error > 0.25:
Av = Av_max
self.send.send_msg(robot_id, 0, 0, flag * Av)
else:
Av = Av_max * angular_error / abs(theta)
self.send.send_msg(robot_id, 0, 0, flag * Av)
receive.get_info(color, robot_id)
now_ori = receive.robot_info['ori']
angular_error = now_ori - orientation_need_now
if PI < angular_error < 2 * PI:
angular_error = angular_error - 2 * PI
elif -2 * PI < angular_error < -PI:
angular_error = angular_error + 2 * PI
flag = abs(angular_error) / angular_error
angular_error = abs(angular_error)
thres = 15
if i == N - 2:
thres = 7
# print(error, theta)
while error > thres:
p = 1
dis_now = distance(path[i], path[i + 1])
if dis_now < self.up:
p = 0.5
thresdist = error_max * self.threshold
if 3*thresdist > error > thresdist:
p = 0.6 * p
if error < thresdist:
p = p * error/thresdist
now_ori = receive.robot_info['ori']
orientation_need_now = math.atan2((path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
theta = now_ori - orientation_need_now
vx_now = self.v * math.cos(theta) * p
vy_now = self.v * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
self.send.send_msg(robot_id, 0, 0, 0)
return True, index
# def line_control(self, now_x, now_y, now_ori, path, i, N, target_x, target_y, infos=None, k1=10, k2=10, v_obstacle_max=400, rr=100, color="blue", robot_id=0):
# point_now = [now_x, now_y]
# error = distance(point_now, path[i+1])
# error_max = distance(path[i], path[i+1])
# if error_max < 80:
# error_max = 80
# orientation_need_now = math.atan2((path[i + 1][1] - now_y), (path[i + 1][0] - now_x))
# theta = now_ori - orientation_need_now
# p = 1
# dis_now = distance(path[i], path[i+1])
# if distance(point_now, [target_x, target_y]) < 80:
# p = 0.2
# else:
# if error > 7:
# if dis_now < self.up:
# p = sigmoid(error/dis_now-1)
# if error < error_max * self.threshold:
# if i < N - 2:
# alpha = math.atan2(path[i + 2][1] - path[i + 1][1], path[i + 2][0] - path[i + 1][0])
# if orientation_need_now > 0:
# angle = alpha + (PI - orientation_need_now)
# else:
# angle = alpha - (PI + orientation_need_now)
# if angle > PI:
# angle = 2 * PI - angle
# if abs(angle) < self.angle_threshold:
# p = error / ((self.threshold + self.time_turn) * error_max) * math.log(2)
# p = math.exp(p) - 1
# else:
# p = error / (self.threshold * error_max) * math.log(2)
# p = math.exp(p) - 1
# else:
# p = error / (self.threshold * error_max) * math.log(2)
# p = math.exp(p) - 1
# else:
# p = 0.2
# vx_now = self.v * math.cos(theta) * p
# vy_now = self.v * math.sin(theta) * p
# return vx_now, vy_now, True
#
# #速度斥力部分
# vx = 0.0
# vy = 0.0
# for index in range(len(infos)):
# if infos[index][3] == robot_id and infos[index][2] == color:
# continue
# elif distance(infos[index][:2], point_now) < rr:
# k = (k1 * infos[index][5] / v_obstacle_max) + k2 * rr / distance(infos[index][:2], point_now)
# gamma = math.atan2(infos[index][1] - now_y, infos[index][0] - now_x)
# vx = vx + k * math.cos(gamma)
# vy = vy + k * math.sin(gamma)
# else:
# continue
#
# vx_now = (self.v * math.cos(theta)-vx) * p
# vy_now = (self.v * math.sin(theta)-vy) * p
# return vx_now, vy_now, False
class XY_PD():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 200
self.threshold = 0.4
self.time_turn = 0.3
self.angle_threshold = 5 * PI / 6
self.obstacle_distance_threshold = 50
def line_control(self,
path,
robot_id,
color,
receive,
info=None,
threshold=30,
index=1):
N = len(path)
for i in range(N - 1):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
error_max = distance(point_now, path[i + 1])
now_vx = receive.robot_info['vx']
now_vy = receive.robot_info['vy']
print('error:', error)
while error > 30:
orientation_need_now = math.atan2((path[i + 1][1] - now_y),
(path[i + 1][0] - now_x))
theta = now_ori - orientation_need_now
vx_change = 0.0
vy_change = 0.0
dist_punish = 0.0
for barrier in info:
# barrier为单个的list，有两个元素，前一个为字符串，后一个为数字
receive.get_info(barrier[0], barrier[1])
ob_x = receive.robot_info['x']
ob_y = receive.robot_info['y']
ob_distance = distance([ob_x, ob_y], [now_x, now_y])
ob_ori = receive.robot_info['ori']
ob_vx = receive.robot_info['vx']
ob_vy = receive.robot_info['vy']
if ob_vx == 0 and ob_vy == 0:
continue
if ob_distance < self.obstacle_distance_threshold:
alpha = now_ori - ob_ori
vx_change = vx_change + 1.5 * ob_vx * math.cos(
alpha) - 1.5 * ob_vy * math.cos(alpha - PI / 2)
vy_change = vy_change + 1.5 * ob_vy * math.sin(
alpha - PI / 2) - 1.5 * ob_vx * math.sin(alpha)
dist_punish = dist_punish + 10 / (1 + ob_distance)
vx_now = vx_change + dist_punish
vy_now = vy_change + dist_punish
p = 1
if error < error_max * self.threshold:
if i < N - 2:
alpha = math.atan2(path[i + 2][1] - path[i + 1][1],
path[i + 2][0] - path[i + 1][0])
if orientation_need_now > 0:
angle = alpha + (PI - orientation_need_now)
else:
angle = alpha - (PI + orientation_need_now)
if angle > PI:
angle = 2 * PI - angle
if abs(angle) < self.angle_threshold:
p = error / ((self.threshold + self.time_turn) *
error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold *
error_max) * math.log(2)
p = math.exp(p) - 1
else:
p = error / (self.threshold * error_max) * math.log(2)
p = math.exp(p) - 1
vx_now = vx_now + self.v * math.cos(theta) * p
vy_now = vy_now + self.v * math.sin(theta) * p
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point_now = [now_x, now_y]
error = distance(point_now, path[i + 1])
# print('error:', error)
if info is not None:
start = time.time()
status, index = check_path_l(receive,
point_now,
path[i + 1:],
info,
color=color,
id=robot_id,
dis_threshold=threshold,
index_=index)
# import ipdb;ipdb.set_trace()
end = time.time()
# print("time:", end - start)
if not status:
# print(status)
return False, index
return True, index
class XY_near():
def __init__(self):
self.send = Send()
self.debug = SendDebug()
self.v = 300
def path_control(self, path, robot_id, color, receive):
#一开始车头朝向右边，x正方向，所以v_x对应x_path, v_y对应y_path
# T = 0.05
# ta = T / 10 # 固定加速时间
# tn = T - 2 * (0.5 * ta + 0.5 * ta) # 固定匀速运动时间，2.5秒
# x_path = np.array(path)[:, 0]
# y_path = np.array(path)[:, 1]
# v_x = np.concatenate(([0.0], (x_path[1:] - x_path[:-1]) / T, [0.0]))
# v_y = np.concatenate(([0.0], (y_path[1:] - y_path[:-1]) / T, [0.0]))
#得到匀速运动段的速度
path = interpolate_path(path)
num = len(path)
i = 1
while True:
# #加速阶段
# self.send.send_msg(robot_id, 0.75 * v_x[i] + 0.25 * v_x[i + 1], 0.75 * v_y[i] + 0.25 * v_y[i + 1], 0)
# sleep(ta/4)
# self.send.send_msg(robot_id, 0.5 * v_x[i] + 0.5 * v_x[i + 1], 0.5 * v_y[i] + 0.5 * v_y[i + 1], 0)
# sleep(ta / 4)
# self.send.send_msg(robot_id, 0.25 * v_x[i] + 0.75 * v_x[i + 1], 0.25 * v_y[i] + 0.75 * v_y[i + 1], 0)
# sleep(ta / 4)
# self.send.send_msg(robot_id, v_x[i + 1], v_y[i + 1], 0)
# sleep(ta / 4)
#
# #直线运动阶段
# self.send.send_msg(robot_id, v_x[i + 1], v_y[i + 1], 0)
# sleep(tn)
# 闭环检测部分
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point = np.array([now_x, now_y])
i = min_dis_index(point, path[i:], i) - 1
error = distance(point, path[i + 1])
# import ipdb;ipdb.set_trace()
print('error:', error)
while error > 10:
orientation_need_now = math.atan2((path[i + 1][1] - now_y),
(path[i + 1][0] - now_x))
theta = now_ori + orientation_need_now
vx_now = self.v * math.cos(theta)
vy_now = self.v * math.sin(theta)
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
point = np.array([now_x, now_y])
error = distance(point, path[i + 1])
print('error:', error)
i = min_dis_index(point, path[i:], i)
if distance(point, path[-1]) < 10:
return
if i >= num - 1:
i = num - 2
# self.send.send_msg(robot_id, v_x[i+1], v_y[i+1], 0)
# sleep(T/100)
# i += 1
if distance(point, path[-1]) < 10:
return
def point_control(self, point, robot_id, color, receive):
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(
np.square(now_x - point[0]) + np.square(now_y - point[1]))
print('error:', error)
index = 0
while error > 10:
orientation_need_now = math.atan2((point[1] - now_y),
(point[0] - now_x))
theta = now_ori + orientation_need_now
if error < 20:
v = 100
vx_now = self.v * math.cos(theta)
vy_now = self.v * math.sin(theta)
self.send.send_msg(robot_id, vx_now, vy_now, 0)
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
now_ori = receive.robot_info['ori']
error = np.sqrt(
np.square(now_x - point[0]) + np.square(now_y - point[1]))
index += 1
