v0.2 p

pure5.1.py

@@ -173,7 +173,7 @@ def main():
         # logger.add('file.log')
         # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
         v.append(state.v)
-        print('v', v[-1])
+        # print('v', v[-1])
 
         t.append(time)
         # plt.cla()

pure_back_2.py

@@ -309,9 +309,246 @@ def main():
         # plt.pause(0.001)
 
 
-def car_back_right():
-
+def car_back_right(direction):
     print('进入右倒库')
+    a = 0
+    g1_trace = abs(math.sqrt((float(tcp4.east) - 67.97) ** 2 + (float(tcp4.north) - 45.75) ** 2))
+    g2_trace = abs(math.sqrt((float(tcp4.east) - 68.35) ** 2 + (float(tcp4.north) - 43.96) ** 2))
+    if g1_trace< a <g2_trace:
+        direction == "line"
+    else:
+        direction == "rightback"
+    if direction == "line":
+        def pure_pursuit_control(state, cx, cy, pind):
+            global fangxp
+            ind = calc_target_index(state, cx, cy)
+            # if pind >= ind:
+            #     ind = pind
+            if ind < len(cx):
+                tx = cx[ind]
+                print('tx', tx)
+                ty = cy[ind]
+                print('ty', ty)
+            else:
+                tx = cx[-1]
+                # print('tx1',tx)
+
+                ty = cy[-1]
+
+                # print('ty1',ty)
+
+                ind = len(cx) - 1
+
+            alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
+            # print('alpha',alpha)
+            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.04)
+            # print(delta)
+            return delta, ind
+
+        def calc_target_index(state, cx, cy):
+            # 搜索最临近的路点
+            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))
+            # print('ind',ind)
+            L = 0.0
+            Lf = k * state.v + Lfc
+            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
+                # print('ind1', ind)
+
+            return ind
+
+        def main():
+            #  设置目标路点
+            cx = []
+            cy = []
+            target_speed = 5 / 3.6  # [m/s]
+            T = 500.0  # 最大模拟时间
+            # 设置车辆的出事状态
+            state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
+            # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
+            lastIndex = len(cx) - 1
+            time = 0.0
+            x = [state.x]
+            # print('x',x)
+            y = [state.y]
+            # print('y',y)
+            yaw = [state.yaw]
+            v = [state.v]
+            t = [0.0]
+            target_ind = calc_target_index(state, cx, cy)
+            while True:
+                # print('heading',tcp4.heading)
+                ai = PControl(target_speed, state.v)
+                di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
+                state = update(state, ai, di)
+                time = time + dt
+                x.append(state.x)
+                x1 = x[-1]
+                print('x1', x[-1])
+                y.append(state.y)
+                y1 = y[-1]
+                print('y1', y[-1])
+                yaw.append(state.yaw)
+                yaw1 = y[-1] * 57.3
+                print('yaw', yaw[-1])
+                t.append(time)
+                # logger.add('file.log')
+                # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
+                v.append(state.v)
+                # print('v', v[-1])
+
+                t.append(time)
+                # plt.cla()
+                # plt.plot(cx, cy, ".r", label="course")
+                # plt.plot(x, y, "-b", label="trajectory")
+                # plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
+                # plt.axis("equal")
+                # plt.grid(True)
+                # plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
+                # plt.pause(0.001)
+
+        def pure_pursuit_control(state, cx, cy, pind):
+            global fangxp
+            ind = calc_target_index(state, cx, cy)
+            # if pind >= ind:
+            #     ind = pind
+            if ind < len(cx):
+                tx = cx[ind]
+                print('tx', tx)
+                ty = cy[ind]
+                print('ty', ty)
+            else:
+                tx = cx[-1]
+                # print('tx1',tx)
+
+                ty = cy[-1]
+
+                # print('ty1',ty)
+
+                ind = len(cx) - 1
+
+            alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
+            # print('alpha',alpha)
+            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.04)
+            # print(delta)
+            return delta, ind
+
+        def calc_target_index(state, cx, cy):
+            # 搜索最临近的路点
+            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))
+            # print('ind',ind)
+            L = 0.0
+            Lf = k * state.v + Lfc
+            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
+                # print('ind1', ind)
+
+            return ind
+
+        def main():
+            #  设置目标路点
+            cx = [67.982, 68.007, 68.028, 68.055, 68.077, 68.089, 68.114, 68.138, 68.158, 68.189, 68.208, 68.223, 68.24, 68.265, 68.29, 68.312, 68.338, 68.365]
+            cy = [45.775, 45.671, 45.569, 45.466, 45.368, 45.256, 45.156, 45.056, 44.945, 44.838, 44.739, 44.638, 44.533, 44.419, 44.314, 44.214, 44.101, 43.99]
+            target_speed = 5 / 3.6  # [m/s]
+            T = 500.0  # 最大模拟时间
+            # 设置车辆的出事状态
+            state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
+            # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
+            lastIndex = len(cx) - 1
+            time = 0.0
+            x = [state.x]
+            # print('x',x)
+            y = [state.y]
+            # print('y',y)
+            yaw = [state.yaw]
+            v = [state.v]
+            t = [0.0]
+            target_ind = calc_target_index(state, cx, cy)
+            while True:
+                # print('heading',tcp4.heading)
+                ai = PControl(target_speed, state.v)
+                di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
+                state = update(state, ai, di)
+                time = time + dt
+                x.append(state.x)
+                x1 = x[-1]
+                print('x1', x[-1])
+                y.append(state.y)
+                y1 = y[-1]
+                print('y1', y[-1])
+                yaw.append(state.yaw)
+                yaw1 = y[-1] * 57.3
+                print('yaw', yaw[-1])
+                t.append(time)
+                # logger.add('file.log')
+                # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
+                v.append(state.v)
+                # print('v', v[-1])
+
+                t.append(time)
+    if direction == "rightback":
         while True:
             global fangxp
             db_file = "turndb.db"
@@ -346,7 +583,7 @@ def car_back_right():
 
                 print('回正',fangxp)
 
-        elif 70 < float(tcp4.fang_xiang) < 110 and rc_trace<1.2:
+            elif 70 < float(tcp4.fang_xiang) < 110 and rc_trace<1.25:
                 fangxp = -540
                 print('右打死',fangxp)
 
@@ -499,8 +736,8 @@ if __name__ == '__main__':
     t9 = threading.Thread(target=car_back_lelf)
     t10 = threading.Thread(target=car_back_lelf_fly)
     # 启动线程
-    # t5.start()
-    t8.start()
+    t5.start()
+    # t8.start()
     # t9.start()
     # t10.start()
     t6.start()