Merge pull request #24 from eiyooooo/master

提升可用性

.gitignore

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

Control/controllers/purepursuit_controller.py

@@ -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/drive.py

@@ -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)

Navigation/BevDraw.py

@@ -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)

Navigation/Navigation_Process.py

@@ -8,20 +8,20 @@ def __init__(self, fit, pts, pts_x, pts_y):
         self.pts_x = pts_x
         self.pts_y = pts_y
 
-def nav_process(nav):
+def nav_process(nav, truck, info, cipv):
     bev_nav = nav2bev(nav)
-    nav_line = get_nav_line(bev_nav)
+    nav_line = get_nav_line(bev_nav, truck, info, cipv)
     curve = np.abs(nav_line.fit[0])
     if curve <= 0.0005:
-        curve_speed_limit = 80
+        curve_speed_limit = info.road_speed[0]
     elif curve > 0.0005 and curve <= 0.001:
-        curve_speed_limit = 75
+        curve_speed_limit = info.road_speed[1]
     elif curve > 0.001 and curve <= 0.002:
-        curve_speed_limit = 65
+        curve_speed_limit = info.road_speed[2]
     elif curve > 0.002 and curve <= 0.004:
-        curve_speed_limit = 55
+        curve_speed_limit = info.road_speed[3]
     elif curve > 0.004 and curve <= 0.005:
-        curve_speed_limit = 40
+        curve_speed_limit = info.road_speed[4]
     elif curve > 0.005:
         curve_speed_limit = 20
     elif curve == 0:
@@ -57,8 +57,9 @@ def filter_out_red(img):
     mask = mask+75
     return mask
 
-def get_nav_line(img):
+def get_nav_line(img, truck, info, cipv):
     middle_pts = []
+    turn = []
     x = 400
     for i in range(540, 340, -5):
         for j in range(x, 0, -1):
@@ -70,7 +71,28 @@ def get_nav_line(img):
         if j == 0 or j == 799 or k == 0 or k == 799:
             break
         x = int((j+k)/2)
-        middle_pts.append([x, i])
+        middle_pts.append([x + info.change_lane, i])
+
+    #     #直角弯道判断
+    #     if len(middle_pts) >= 12 and i < 435:
+    #         far = middle_pts[-6:]
+    #         near = middle_pts[-12:-6]
+    #         far = (far[0][0]+far[1][0]+far[2][0]+far[3][0]+far[4][0]+far[5][0])/6
+    #         near = (near[0][0]+near[1][0]+near[2][0]+near[3][0]+near[4][0]+near[5][0])/6
+    #         if np.abs(near-far) > 30:
+    #             turn.append(near-far)
+
+    # if (len(turn) >0 and cipv is not None) or (len(turn) >0 and truck.speed > 25):
+    #     middle_pts = []
+    #     for i in range(540, 340, -5):
+    #         middle_pts.append([400, i])
+    #     if np.mean(turn) > 0:
+    #         info.direction = -2
+    #     else:
+    #         info.direction = 2
+    #     info.road_speed = [30, 30, 30, 30, 30]
+    #     turn = [0]
+
     middle_pts = np.array(middle_pts) # 路线中心线
     if len(middle_pts):
         fit = np.polyfit(np.array([i[1] for i in middle_pts]), np.array([i[0] for i in middle_pts]), 2)
@@ -80,6 +102,27 @@ def get_nav_line(img):
         pts_x = [400]
         pts_y = [470]
         fit = [0, 0, 0]
+
+    # #直角弯道左转
+    # if len(turn) >0 and np.mean(turn) > 0:
+    #     info.direction = -2
+    #     info.road_speed = [30, 30, 30, 30, 30]
+    #     fit = [0.0003552, -0.304186, 463.918]
+    #     pts_y = np.linspace(300, 470, 50)
+    #     pts_x = fit[0] * pts_y ** 2 + fit[1] * pts_y + fit[2]
+    # #直角弯道右转
+    # if len(turn) >0 and np.mean(turn) < 0:
+    #     info.direction = 2
+    #     info.road_speed = [30, 30, 30, 30, 30]
+    #     fit = [-0.00021, 0.246, 328.3826]
+    #     pts_y = np.linspace(300, 470, 50)
+    #     pts_x = fit[0] * pts_y ** 2 + fit[1] * pts_y + fit[2]
+    # #无直角弯道或直角弯道结束
+    # if len(turn) == 0:
+    #     if info.direction != -1 and info.direction != 1:
+    #         info.direction = 0
+    #     info.update(info.roads_type)
+
     nav_pts = []
     if len(pts_y):
         for pt_x, pt_y in zip(pts_x, pts_y):

Perception/LaneDetection/FCW.py

@@ -0,0 +1,47 @@
+import numpy as np
+
+class Bev_Lane:  # 封装车道线类
+    def __init__(self, cls, position_type, fit, endpoint, startpoint, lanes_pts):
+        self.cls = cls  # 车道线线型 虚线：0， 实线：1
+        self.position_type = position_type
+        ###############车道线类型################
+        # 左边界： -4
+        # 左边第三根车道线：-3
+        # 左边第二根（左临近车道的左边界）：-2
+        # 当前车道左边界：-1
+        # 当前车道中心线：0
+        # 当前车道右边界：1
+        # 右边第二根（右临近车道的左边界）：2
+        # 右边第三根车道线：3
+        # 右边界：4
+        # 未知车道线：5 未定义前全部设置为5
+        #######################################
+        self.fit = fit  # 三次函数拟合车道线的参数 [a, b, c] 纵是X轴，横是Y轴
+        self.endpoint = endpoint  # 远离本车的端点坐标
+        self.startpoint = startpoint  # 接近本车的端点坐标
+        self.pts = lanes_pts  # 车道线上所有的点
+
+def FCW(nav_line, speed):
+    # 根据速度计算车辆前向物体检测线长度
+    long = 420 * (1 - speed / 100)
+    if long < 210:
+        long = 210
+
+    # 根据导航线确定车辆前向物体检测线
+    bev_lanes = []
+    pts_y = np.linspace(long, 470, 20)
+    pts_x_L10 = nav_line.fit[0] * pts_y ** 2 + nav_line.fit[1] * pts_y + nav_line.fit[2] - 10
+    pts_x_R10 = nav_line.fit[0] * pts_y ** 2 + nav_line.fit[1] * pts_y + nav_line.fit[2] + 10
+
+    L10_nav_pts = []
+    R10_nav_pts = []
+    for i in range(len(pts_y)):
+        L10_nav_pts.append([int(pts_x_L10[i]), int(pts_y[i])])
+        R10_nav_pts.append([int(pts_x_R10[i]), int(pts_y[i])])
+
+    fit_L10 = np.polyfit(np.array([i[1] for i in L10_nav_pts]), np.array([i[0] for i in L10_nav_pts]), 3)
+    fit_R10 = np.polyfit(np.array([i[1] for i in R10_nav_pts]), np.array([i[0] for i in R10_nav_pts]), 3)
+
+    bev_lanes.append(Bev_Lane(1, -1, fit_L10, (pts_x_L10, pts_y[-1]), (pts_x_L10, pts_y[0]), L10_nav_pts))
+    bev_lanes.append(Bev_Lane(1, 1, fit_R10, (pts_x_R10, pts_y[-1]), (pts_x_R10, pts_y[0]), R10_nav_pts))
+    return bev_lanes

Perception/ObjectDetection/cipv_notice.py

@@ -17,7 +17,7 @@ def cipv_notice(obstacles, bev_lanes):
         for obstacle in obstacles:
             x = obstacle.position[0] + obstacle.position[2] / 2
             y = obstacle.position[1] + obstacle.position[3]  # 计算尾部中心
-            if y >= 320:  # 过滤监控范围之外的物体
+            if y >= 300:  # 过滤监控范围之外的物体
                 if y <= 470:
                     if ego_left is not None and ego_right is not None:  # 前方车道的CIPV
                         if ego_left.fit[0]*y**3 + ego_left.fit[1]*y**2 + ego_left.fit[2]*y + ego_left.fit[3] < x and ego_right.fit[0]*y**3 + ego_right.fit[1]*y**2 + ego_right.fit[2]*y + ego_right.fit[3] > x:

Perception/ObjectDetection/postprocess.py

@@ -28,8 +28,8 @@ def __init__(self, id, position, cls, speed, track):
         self.cipv = None # 是否为前方最邻近车辆 是：True
 
 
-def postprocess(dets, CAM, CAM_BL, tracker, tracks, infer_time):  # 目标追踪后处理主函数
-    objs, traffic_lights_list = position(dets, CAM, CAM_BL)
+def postprocess(dets, CAM, CAM_BL, CAM_BR, tracker, tracks, infer_time):  # 目标追踪后处理主函数
+    objs, traffic_lights_list = position(dets, CAM, CAM_BL, CAM_BR)
     if len(objs) >= 1:
         objs = np.array(objs)
         targets = tracker.update(torch.tensor(objs), [600, 800], (600, 800))
@@ -44,7 +44,7 @@ def postprocess(dets, CAM, CAM_BL, tracker, tracks, infer_time):  # 目标追踪
     return obstacles, objs, tracks, traffic_light
 
 
-def position(dets, CAM, CAM_BL):  # 定位
+def position(dets, CAM, CAM_BL, CAM_BR):  # 定位
     objs = []
     traffic_lights_list = []
     for det in dets:
@@ -65,8 +65,8 @@ def position(dets, CAM, CAM_BL):  # 定位
                     box[1] = 470 + int(position[0, 1])
                     box[2] = 400 + int((position[0, 0]))
                     box[3] = 530 + int(position[0, 1])
-            elif (x0+x1)/2 < 1280 and (x0+x1)/2 > 1100 and y1 <= 240: # 处理左后视镜
-                position = CAM_BL.pixel_to_world([(x0 + x1) / 2, 240 - y1])[0]  # 相对距离
+            elif (x0+x1)/2 < 1280 and (x0+x1)/2 > 1100 and y1 <= 240: # 处理右后视镜
+                position = CAM_BR.pixel_to_world([(x0 + x1) / 2, 240 - y1])[0]  # 相对距离
                 if box[5] == 2:
                     box[0] = 400 + int((position[0, 0]))
                     box[1] = 470 + int(position[0, 1])

Perception/ObjectDetection/utils/utils.py

@@ -44,7 +44,7 @@ def infer(self, img):
         data = [out['host'] for out in self.outputs]
         return data
 
-    def inference(self, origin_img, CAM, CAM_BL, img_show, tracker, tracks, infer_time, conf=0.5):
+    def inference(self, origin_img, CAM, CAM_BL, CAM_BR, img_show, tracker, tracks, infer_time, conf=0.5):
         img, ratio = preproc(origin_img, self.imgsz)
         data = self.infer(img)
         num, final_boxes, final_scores, final_cls_inds = data
@@ -55,7 +55,7 @@ def inference(self, origin_img, CAM, CAM_BL, img_show, tracker, tracks, infer_ti
         obstacles = None
         traffic_light = None
         if dets is not None and len(dets):
-            obstacles, objs, tracks, traffic_light = postprocess(dets, CAM, CAM_BL, tracker, tracks, infer_time)
+            obstacles, objs, tracks, traffic_light = postprocess(dets, CAM, CAM_BL, CAM_BR, tracker, tracks, infer_time)
             final_boxes, final_scores, final_cls_inds = dets[:, :4], dets[:, 4], dets[:, 5]
             img_show = vis(img_show, final_boxes, final_scores, final_cls_inds, conf=conf,
                             class_names=self.class_names)

Planning/Cruise.py

@@ -8,12 +8,18 @@
 from controllers.purepursuit_controller import Purepursuit
 
 
-def Cruise(vertical_pid, horizontal_pid, truck, speed_limit, nav_line):
-    vertical_pid.update_e(speed_limit - truck.speed)
+def Cruise(vertical_pid, horizontal_pid, truck, speed_limit, nav_line, info):
+    vertical_pid.update_e(speed_limit + 4 - truck.speed)
     acc = vertical_pid.get_a() + 0.5
     # horizontal_pid.update_e(float(np.average(nav_line.pts_x[-5:])) - 400)
     # ang = horizontal_pid.get_u() / np.pi + 0.5
     ang = Purepursuit(truck, nav_line.pts)
+
+    # delta_ang = abs(ang - truck.ang)
+    # if delta_ang > 0.1 and info.activeAP:
+    #     info.AP_exit_reason = 1
+    #     info.activeAP = False
+
     if acc > 1: acc = 1
     elif acc < 0: acc = 0
     return acc, ang