""" Path tracking simulation with pure pursuit steering control and PID speed control. author: Atsushi Sakai (@Atsushi_twi) """ import datetime import time from loguru import logger import math import sqlite3 import threading import time import numpy as np import geartest2 import pure_change_banck import sped import tcp4 import test5 k = 0 # look forward gain Lfc = 5 # look-ahead distance Kp = 1.0 # speed propotional gain dt = 0.1 # [s] L = 2.9 # [m] wheel base of vehicle tx = 0 ty = 0 fangxp = 0 pure = 0 a_trace = 0 brk_open = 0 brk_change=0 e_trace=0 l_out_strat_trace=0 l_in_start_trace=0 right=0 left=0 # show_animation = True # class State: def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed): self.x = x self.y = y self.yaw = yaw self.v = v def update(state, a): state.x = tcp4.x_car # print('stax', state.x) state.y = tcp4.y_car # print('stay', state.y) state.yaw = tcp4.car_yaw # print('stayaw', state.car_yaw) state.v = sped.speed + a * dt # state.x = tcp4.x_car + sped.speed * math.cos(tcp4.car_yaw) * dt # # print("state",state.x) # state.y = tcp4.y_car + sped.speed * math.sin(tcp4.car_yaw) * dt # state.car_yaw = tcp4.car_yaw + sped.speed / L * math.tan(delta) * dt # state.v = sped.speed + a * dt return state # return state def PIDControl(target, current): a = Kp * (target - current) return a def pure_pursuit_control(state, cx, cy, pind): global tx, ty, fangxp ind = calc_target_index(state, cx, cy) # if pind >= ind: # ind = pind if ind < len(cx): tx = cx[ind] ty = cy[ind] else: tx = cx[-1] ty = cy[-1] ind = len(cx) - 1 alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw if state.v < 0: # back alpha = math.pi - alpha Lf = k * state.v + Lfc delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) if delta > np.pi / 6.0: delta = np.pi / 6.0 # print('delta1', delta) elif delta < - np.pi / 6.0: delta = - np.pi / 6.0 # print('delta2', delta) angle = delta * 57.3 fangxp = int(angle * 18) print('fxp', fangxp) db_file = "turndb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO turndb(id) VALUES(1);' sql = "update turndb set angle='%s' WHERE id = 1" % fangxp cur.execute(sql) conn.commit() cur.close() conn.close() time.sleep(0.5) return delta, ind def calc_target_index(state, cx, cy): # search nearest point index dx = [state.x - icx for icx in cx] dy = [state.y - icy for icy in cy] d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] ind = d.index(min(d)) L = 0.0 Lf = k * state.v + Lfc # search look ahead target point index while Lf > L and (ind + 1) < len(cx): dx = cx[ind + 1] - cx[ind] dy = cx[ind + 1] - cx[ind] L += math.sqrt(dx ** 2 + dy ** 2) ind += 1 return ind def main(): global pure,e_trace # target course cx = [70.733, 70.703, 70.681, 70.661, 70.631, 70.608, 70.579, 70.554, 70.533, 70.509, 70.482, 70.457, 70.426, 70.402, 70.38, 70.352, 70.322, 70.298, 70.277, 70.253, 70.23, 70.205, 70.18, 70.151, 70.121, 70.093, 70.072, 70.046, 70.025, 70.0, 69.975, 69.95, 69.919, 69.895, 69.87, 69.84, 69.808, 69.778, 69.753, 69.722, 69.693, 69.668, 69.642, 69.605, 69.573, 69.543, 69.524, 69.5, 69.47, 69.442, 69.414, 69.389, 69.367, 69.344, 69.323, 69.297, 69.28, 69.256, 69.231, 69.21, 69.192, 69.166, 69.147, 69.127, 69.105, 69.087, 69.064, 69.04, 69.014, 68.987, 68.957, 68.926, 68.895, 68.866, 68.837, 68.804, 68.776, 68.746, 68.72, 68.696, 68.663, 68.636, 68.606, 68.573, 68.545, 68.519, 68.488, 68.46, 68.436, 68.406, 68.374, 68.343, 68.317, 68.29, 68.26, 68.228, 68.198, 68.17, 68.147, 68.121, 68.099, 68.075, 68.052, 68.024, 67.995, 67.969, 67.946, 67.912, 67.885, 67.864, 67.832, 67.799, 67.777, 67.748, 67.725, 67.689, 67.668, 67.64, 67.613, 67.584, 67.557, 67.532, 67.506, 67.475, 67.448, 67.415, 67.382, 67.347, 67.314, 67.287, 67.25, 67.221, 67.189, 67.165, 67.14, 67.12, 67.091, 67.064, 67.022, 66.999] cy = [33.88, 33.985, 34.086, 34.188, 34.298, 34.409, 34.516, 34.633, 34.736, 34.847, 34.962, 35.077, 35.195, 35.318, 35.441, 35.563, 35.688, 35.812, 35.91, 36.011, 36.117, 36.227, 36.343, 36.506, 36.632, 36.758, 36.885, 37.013, 37.147, 37.275, 37.408, 37.542, 37.681, 37.818, 37.956, 38.1, 38.241, 38.382, 38.527, 38.666, 38.804, 38.95, 39.096, 39.24, 39.383, 39.526, 39.626, 39.765, 39.902, 40.033, 40.176, 40.31, 40.432, 40.561, 40.679, 40.793, 40.904, 41.011, 41.116, 41.215, 41.314, 41.442, 41.544, 41.649, 41.758, 41.867, 41.98, 42.1, 42.22, 42.345, 42.467, 42.592, 42.719, 42.839, 42.964, 43.079, 43.189, 43.305, 43.414, 43.517, 43.644, 43.744, 43.871, 43.996, 44.111, 44.229, 44.355, 44.462, 44.564, 44.672, 44.777, 44.88, 44.982, 45.083, 45.18, 45.302, 45.409, 45.505, 45.606, 45.713, 45.815, 45.919, 46.026, 46.127, 46.24, 46.348, 46.448, 46.565, 46.686, 46.786, 46.884, 47.015, 47.122, 47.221, 47.321, 47.448, 47.554, 47.66, 47.761, 47.862, 47.971, 48.082, 48.188, 48.295, 48.411, 48.525, 48.641, 48.76, 48.875, 48.991, 49.111, 49.218, 49.327, 49.433, 49.545, 49.645, 49.748, 49.845, 49.997, 50.097] target_speed = 10.0 / 3.6 # [m/s] # initial state state = State(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed) lastIndex = len(cx) - 1 time = 0.0 x = [state.x] y = [state.y] yaw = [state.yaw] v = [state.v] t = [0.0] target_ind = calc_target_index(state, cx, cy) while pure == 1: ai = PIDControl(target_speed, state.v) di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) state = update(state, ai) time = time + dt x.append(state.x) x1 = round(x[-1], 3) # print('x1', x[-1]) y.append(state.y) y1 = round(y[-1], 3) yaw.append(state.yaw) v.append(state.v) t.append(time) print('纯跟踪运行') e_trace = abs(math.sqrt(pow((float(tcp4.east) - 67.872), 2) + pow((float(tcp4.north) - 45.89), 2))) print('e_trace', e_trace) gear_d() if e_trace < 1.2 and 160 < (float(tcp4.fang_xiang)) < 180: pure = 2 print('退出纯跟踪') shache() print('刹车') # logger.add( # '预瞄点记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute)) # logger.debug('y {},x {},tx {}，ty {},fxp{},car_yaw{},北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(y1), # str(x1), str(tx), str(ty), str(fangxp), # str(state.yaw), str(tcp4.east), str(tcp4.north), str(tcp4.x_car), # str(tcp4.y_car), str(tcp4.beidou[5]), str(tcp4.heading)) # if show_animation: # plt.cla() # plt.plot(cx, cy, ".r", label="course") # plt.plot(x, y, "-b", label="trajectory") # plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") # plt.axis("equal") # plt.grid(True) # plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4]) # plt.pause(0.001) # # # Test # assert lastIndex >= target_ind, "Cannot goal" # # if show_animation: # plt.plot(cx, cy, ".r", label="course") # plt.plot(x, y, "-b", label="trajectory") # plt.legend() # plt.xlabel("x[m]") # plt.ylabel("y[m]") # plt.axis("equal") # plt.grid(True) # # flg, ax = plt.subplots(1) # plt.plot(t, [iv * 3.6 for iv in v], "-r") # plt.xlabel("Time[s]") # plt.ylabel("Speed[km/h]") # plt.grid(True) # plt.show() # def date_write(): # shijian = datetime.datetime.now() # # logger.add( # '预瞄点记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute)) # logger.debug('y {},x {},tx {}，ty {},fxp{},car_yaw{},北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(y1), # str(x1), str(tx), str(ty), str(fangxp), # str(state.yaw), str(tcp4.east), str(tcp4.north), str(tcp4.x_car), # str(tcp4.y_car), str(tcp4.beidou[5]), str(tcp4.heading)) def shache(): global brk_open,brk_change brk_open = 10 brk_change = 1 db_file = "geardb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO geardb(id) VALUES(1);' sql = "update geardb set brk_open='%s',brk_change='%s' WHERE id = 1" % (brk_open, brk_change) cur.execute(sql) conn.commit() cur.close() conn.close() def gear_p(): print('P') gear_cmd = 1 auto_acc = 0 db_file = "geardb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO geardb(id) VALUES(1);' sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc) cur.execute(sql) conn.commit() cur.close() conn.close() def shache2(): brk_change=0 db_file = "geardb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO geardb(id) VALUES(1);' sql = "update geardb set brk_change='%s' WHERE id = 1"%brk_change cur.execute(sql) conn.commit() cur.close() conn.close() def gear_r(): gear_cmd = 2 # r print('r') auto_acc = 70 db_file = "geardb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO geardb(id) VALUES(1);' sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc) cur.execute(sql) conn.commit() cur.close() conn.close() def gear_d(): gear_cmd = 4 # r # print('d') auto_acc = 50 db_file = "geardb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO geardb(id) VALUES(1);' sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc) cur.execute(sql) conn.commit() cur.close() conn.close() def car_back(): global fangxp, a_trace, l_out_strat_trace,right print('进入右倒库') while right==1: db_file = "turndb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO turndb(id) VALUES(1);' sql = "update turndb set angle='%s' WHERE id = 1" % fangxp cur.execute(sql) conn.commit() cur.close() conn.close() # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2)) l_out_strat_trace=abs(math.sqrt(pow((float(tcp4.east) - 76.06), 2) + pow((float(tcp4.north) - 38.42), 2))) print('l_out',l_out_strat_trace) a_trace = abs(math.sqrt(pow((float(tcp4.east) - 68.35), 2) + pow((float(tcp4.north) - 43.85), 2))) print('a_trace', a_trace) b_trace = abs(math.sqrt((float(tcp4.east) - 70.15) ** 2 + (float(tcp4.north) - 38.6) ** 2)) c_trace = abs(math.sqrt((float(tcp4.east) - 70.7) ** 2 + (float(tcp4.north) - 38.4) ** 2)) d_trace = abs(math.sqrt((float(tcp4.east) - 72) ** 2 + (float(tcp4.north) - 37.8) ** 2)) # print("tcp4.east",tcp4.east) print('tcp_fangxiang', tcp4.fang_xiang) if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and a_trace < 0.7: fangxp = -540 print('右打死', fangxp) elif 70 < float(tcp4.fang_xiang) < 110 and b_trace < 1: fangxp = 0 print('回正', fangxp) elif 70 < float(tcp4.fang_xiang) < 110 and c_trace < 0.8: fangxp = -540 print('右打死2', fangxp) elif 60 < float(tcp4.fang_xiang) < 90 and d_trace < 1: fangxp = 0 print('回正2', fangxp) elif l_out_strat_trace<1.5 and 60< float(tcp4.fang_xiang)< 80: right=2 print('右倒库结束') # shache() print('刹车') else: print('lose') time.sleep(0.5) def car_back_lelf_fly(): global fangxp,l_in_start_trace,left print('进入左出库') fangxp=0 while left==1: db_file = "turndb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO turndb(id) VALUES(1);' sql = "update turndb set angle='%s' WHERE id = 1" % fangxp cur.execute(sql) conn.commit() cur.close() conn.close() # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2)) l_in_start_trace=abs(math.sqrt(pow((float(tcp4.east)-72),2) + pow((float(tcp4.north)-28.54),2))) print('l_in',l_in_start_trace) lfa_trace = abs(math.sqrt(pow((float(tcp4.east)-72.02),2) + pow((float(tcp4.north)-37.51),2))) print('a_trace',lfa_trace) lfb_trace = abs(math.sqrt((float(tcp4.east)-71.66) ** 2 + (float(tcp4.north)-31.27)** 2)) # print('b_trace',b_trace) # print("tcp4.east",tcp4.east) print('tcp_fangxiang',tcp4.fang_xiang) if 60.0 < (float(tcp4.fang_xiang)) < 100.0 and lfa_trace<1.2: fangxp = 540 print('左打死',fangxp) elif 330.0< float(tcp4.fang_xiang)< 370.0 and lfb_trace<1.2: fangxp = 0 print('回正',fangxp) elif l_in_start_trace<1 and 330< float(tcp4.fang_xiang)< 355: left=2 print('开始左倒库') # shache() print('刹车') else: print('lose') print('\n') time.sleep(0.5) def car_back_lelf(): global fangxp, l_in_start_trace print('进入左倒库') while True: db_file = "turndb.db" conn = sqlite3.connect(db_file) cur = conn.cursor() # sql='INSERT INTO turndb(id) VALUES(1);' sql = "update turndb set angle='%s' WHERE id = 1" % fangxp cur.execute(sql) conn.commit() cur.close() conn.close() # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2)) la_trace = abs(math.sqrt(pow((float(tcp4.east)-71.75),2) + pow((float(tcp4.north)-30.73),2))) print('a_trace',la_trace) lb_trace = abs(math.sqrt((float(tcp4.east)-71.06) ** 2 + (float(tcp4.north)-36)** 2)) print('b_trace',lb_trace) lc_trace = abs(math.sqrt((float(tcp4.east)-71.88) ** 2 + (float(tcp4.north)-36.50) ** 2)) print('c_trace',lc_trace) ld_trace = abs(math.sqrt((float(tcp4.east)-73.68) ** 2 + (float(tcp4.north)-38.10) ** 2)) print('d_trace',ld_trace) # print("tcp4.east",tcp4.east) print('tcp_fangxiang',tcp4.fang_xiang) if 330.0 < (float(tcp4.fang_xiang)) < 370.0 and la_trace<1.2: fangxp = 540 print('左打死',fangxp) elif 40.0< float(tcp4.fang_xiang)< 80.0 and lb_trace<1.2: fangxp = 0 print('回正',fangxp) elif 60.0 < float(tcp4.fang_xiang) < 100.0 and lc_trace<1.5: fangxp = 540 print('左打死',fangxp) elif 70 < float(tcp4.fang_xiang) < 90 and ld_trace < 1.2: fangxp = 0 print('回正',fangxp) else: print('lose') print('\n') time.sleep(0.5) def car_back_comply(): global pure, brk_open,brk_change,e_trace,l_out_strat_trace,right,fangxp,left # fangxp = 0 pure= 1 print('开始跟踪') shache2() main() while True: if e_trace < 1.2 and sped.speed == 0: # gear_p() brk_open = 0 # shache2() # print('松刹车') gear_r() print('挂倒挡') pure_change_banck.back = 1 pure_change_banck.main() right=1 car_back() if l_out_strat_trace<1.5 and 60< float(tcp4.fang_xiang)< 80: print('右倒库结束，开始左飞') left=1 # fangxp = 0 # gear_p() brk_open = 0 shache2() print('松刹车') gear_d() car_back_lelf_fly() if l_in_start_trace<1 and 330< float(tcp4.fang_xiang)< 355: print('开始左倒库') # gear_p() brk_open = 0 shache2() print('松刹车') gear_r() car_back_lelf() time.sleep(0.05) if __name__ == '__main__': t1 = threading.Thread(target=test5.can165) # 读取eps状态 t2 = threading.Thread(target=test5.can0a5) # 读取目前方向盘角度 t3 = threading.Thread(target=test5.main) t4 = threading.Thread(target=test5.update) t5 = threading.Thread(target=car_back_comply) t6 = threading.Thread(target=tcp4.zuo_biao) t7 = threading.Thread(target=sped.sped) t8 = threading.Thread(target=geartest2.brk_open_option) t9 = threading.Thread(target=geartest2.can4b8) t10 = threading.Thread(target=geartest2.main) t11 = threading.Thread(target=geartest2.update) # 启动线程 t5.start() t6.start() t7.start() t1.start() t2.start() t3.start() t8.start() t9.start() t10.start() t11.start()