Merge pull request #24 from eiyooooo/master

提升可用性

Author: Lyric0620

.gitignore

@@ -0,0 +1,3 @@
+engines/
+.vscode/settings.json
+*.pyc

Control/__pycache__/controller.cpython-38.pyc

@@ -5,7 +5,7 @@
 def Purepursuit(truck, nav_line):
     robot_state = np.zeros(2)  # 定义车辆位置
     robot_state[0] = truck.x
-    robot_state[1] = truck.y+60  # 后轴中心位置
+    robot_state[1] = truck.y + truck.bev_l  # 后轴中心位置
     dx, dy = np.average(nav_line[-12:], axis=0) - robot_state
     alpha = np.arctan(-dx/dy)  # dx, dy夹角
     ang = math.atan2(2.0 * truck.bev_l * np.sin(alpha), truck.ld) + 0.5  # pure pursuit controller

Control/__pycache__/drive.cpython-38.pyc

@@ -15,14 +15,13 @@ def __init__(self):  # L:wheel base
         self.dtheta = 0
         self.ang = 0
         self.acc = 0
-        self.l = 6.0  # 轴距
-        self.bev_l = 60  # bev下轴距
+        self.l = 12.0  # 轴距
+        self.bev_l = 120  # bev下轴距
+        self.ld = 120  # 预瞄距离
+        self.lf = 160  # 预瞄距离补偿
         self.speed = 0  # 速度
         self.bev_speed = 0  # bev下的速度
         self.dv = 0  # 速度变量
-        self.L = 6.0  # 车辆全长
-        self.k = 1.0
-        self.ld = self.k * self.bev_speed + self.L  # 后轮到观察点距离
         self.dt = 0.2  # 决策间隔时间
         self.delta = 0   # 车辆与轨迹夹角
 
@@ -32,13 +31,46 @@ def update(self, ang, acc, refer_time, speed):  # update ugv's state
         self.dv = speed - self.speed
         self.speed = speed
         self.bev_speed = self.speed / 3.6 * 6
-        self.ld = self.k * self.bev_speed + self.bev_l
+        self.ld = 1.0 * self.bev_speed + self.lf
         self.dt = refer_time  # 更新推理时间
         self.dx = self.speed * np.cos(self.theta) * self.dt
         self.dy = self.speed * np.sin(self.theta) * self.dt
         self.dtheta = self.speed * np.tan(self.ang) / self.l * self.dt
 
 
+class Info:
+    def __init__(self):
+        self.activeAP = False  # 是否激活自动驾驶
+        self.roads_type = 0  # 0：普通道路 1：高速公路 2：高速公路超车速度
+        self.road_speed = [50, 50, 45, 40, 30]  # 道路速度
+        self.AP_exit_reason = 0  # 自动驾驶退出原因 0：正常退出 1：地图导航获取路线出错
+        self.direction = 0  # -2: 左转 -1：左变道 0：直行 1：右变道 2：右转
+        self.change_lane = 0  # <0：左变道 0：不变道 >0：右变道
+        self.change_lane_dest = 0  # 变道目标车道
+
+    def update(self, roads_type):  # 更新道路类型
+        self.roads_type = roads_type
+        self.AP_exit_reason = 0
+        if self.roads_type == 0:
+            self.road_speed[0] = 50
+            self.road_speed[1] = 50
+            self.road_speed[2] = 45
+            self.road_speed[3] = 40
+            self.road_speed[4] = 30
+        elif self.roads_type == 1:
+            self.road_speed[0] = 90
+            self.road_speed[1] = 90
+            self.road_speed[2] = 80
+            self.road_speed[3] = 70
+            self.road_speed[4] = 60
+        elif self.roads_type == 2:
+            self.road_speed[0] = 95
+            self.road_speed[1] = 95
+            self.road_speed[2] = 80
+            self.road_speed[3] = 70
+            self.road_speed[4] = 60
+
+
 def driver(ang, acc):
     j.data.wAxisX = int(ang * 32767)
     j.data.wAxisY = int(acc * 32767)

Control/controllers/__pycache__/PID_controller.cpython-38.pyc

@@ -1,3 +1,4 @@
+import time
 import cv2
 import numpy as np
 
@@ -93,7 +94,7 @@ def display_bev_lanes(bevmap, bev_lanes, stop_line, width=2):
 
     return bevmap
 
-def print_info(bevmap, refer_time, truck, speed_limit, state, weather):
+def print_info(bevmap, refer_time, truck, speed_limit, state, weather, info, planetrigger):
     cv2.putText(bevmap, 'Infer time: {:.4f}s'.format(refer_time), (10, 15), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
                 fontScale=0.5, color=(200, 200, 200), thickness=1)
     cv2.putText(bevmap, "Steer angle: {:.2f}".format((truck.ang - 0.5) * 180), (10, 30), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
@@ -104,10 +105,46 @@ def print_info(bevmap, refer_time, truck, speed_limit, state, weather):
                 fontScale=0.5, color=(255, 255, 255), thickness=1)
     cv2.putText(bevmap, "power: {:.1f}".format((1-truck.acc)*10), (10, 75), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
                 fontScale=0.5, color=(255, 255, 255), thickness=1)
+    
+
+    if not info.activeAP:
+        cv2.putText(bevmap, "AP: inactive", (10, 120), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)
+        if info.AP_exit_reason == 1:
+            cv2.putText(bevmap, "AP exited, please take control immediately!!!", (10, 332), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                        fontScale=0.5, color=(0, 0, 255), thickness=1)
+    elif info.roads_type == 0:
+        cv2.putText(bevmap, "AP: road", (10, 120), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)
+    elif info.roads_type == 1 or info.roads_type == 2:
+        cv2.putText(bevmap, "AP: highway", (10, 120), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)       
+     
+     
+    if info.direction == 0:
+        cv2.putText(bevmap, "straight", (10, 135), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)
+    elif info.direction < 0:
+        cv2.putText(bevmap, "left", (10, 135), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)
+    elif info.direction > 0:  
+        cv2.putText(bevmap, "right", (10, 135), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)
+            
 
     if state is not None:
         cv2.putText(bevmap, state, (10, 90), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
                     fontScale=0.5, color=(255, 255, 255), thickness=1)
+        
+    if state == 'Passing' or state == 'Follow':
+        t = int(time.time() - planetrigger.t)
+        if t > 45 or t < 0:
+            t = 0
+        cv2.putText(bevmap, "time: {}".format(t), (10, 150), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)
+    if state == 'Passing':
+        cv2.putText(bevmap, "passing: {}".format(planetrigger.change_lane_state), (10, 165), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
+                    fontScale=0.5, color=(255, 255, 255), thickness=1)
 
     cv2.putText(bevmap, "weather: {}".format(weather), (10, 105), fontFace=cv2.FONT_HERSHEY_SIMPLEX,
                 fontScale=0.5, color=(255, 255, 255), thickness=1)