日韩性视频-久久久蜜桃-www中文字幕-在线中文字幕av-亚洲欧美一区二区三区四区-撸久久-香蕉视频一区-久久无码精品丰满人妻-国产高潮av-激情福利社-日韩av网址大全-国产精品久久999-日本五十路在线-性欧美在线-久久99精品波多结衣一区-男女午夜免费视频-黑人极品ⅴideos精品欧美棵-人人妻人人澡人人爽精品欧美一区-日韩一区在线看-欧美a级在线免费观看

歡迎訪問 生活随笔！

生活随笔

當前位置： 首頁 > 编程资源 > 综合教程 >内容正文

综合教程

TB3_Autorace之交通杆检测

發布時間：2023/12/15 综合教程 30 生活家
生活随笔 收集整理的這篇文章主要介紹了 TB3_Autorace之交通杆检测 小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

利用blob檢測算法識別交通桿，控制TB3機器人完成對交通桿的起停動作！

上一篇博文中《TB3_Autorace之路標檢測》訂閱了原始圖像信息，經過SIFT檢測識別出道路交通標志，這里我們同樣訂閱樹莓派攝像頭的原始圖像信息對交通桿進行識別，同時我們還訂閱了交通桿的狀態信息以及任務完成信息，實現桿落即停，桿起即過的功能。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
 
self.sub_image_type = "raw"  
self.pub_image_type = "raw"  

if self.sub_image_type == "compressed":
    
    self.sub_image_original = rospy.Subscriber('/detect/image_input/compressed', CompressedImage,
                                               self.cbGetImage, queue_size=1)
elif self.sub_image_type == "raw":
    
    self.sub_image_original = rospy.Subscriber('/detect/image_input', Image, self.cbGetImage, queue_size=1)
self.sub_level_crossing_order = rospy.Subscriber('/detect/level_crossing_order', UInt8,
                                                 self.cbLevelCrossingOrder, queue_size=1)
self.sub_level_crossing_finished = rospy.Subscriber('/control/level_crossing_finished', UInt8,
                                                    self.cbLevelCrossingFinished, queue_size=1)

發布話題

發布圖像信息，交通桿的狀態信息/detect/level_crossing_stamped、任務開始信息/control/level_crossing_start以及速度控制信息/control/max_vel。

1
2
3
4
5
6
7
8
9
10
 
if self.pub_image_type == "compressed":
    # publishes level image in compressed type
    self.pub_image_level = rospy.Publisher('/detect/image_output/compressed', CompressedImage, queue_size=1)
elif self.pub_image_type == "raw":
    # publishes level image in raw type
    self.pub_image_level = rosppy.Publisher('/detect/image_output', Image, queue_size=1)

self.pub_level_crossing_return = rospy.Publisher('/detect/level_crossing_stamped', UInt8, queue_size=1)
self.pub_parking_start = rospy.Publisher('/control/level_crossing_start', UInt8, queue_size=1)
self.pub_max_vel = rospy.Publisher('/control/max_vel', Float64, queue_size=1)

設定檢測狀態

這里我們利用python的枚舉操作將路標檢測封裝為幾個不同狀態，包括識別標志、檢測交通桿、識別交通桿、停車、通過等狀態。根據不同的識別狀態執行相應的控制命令。

1
2
3
4
5
6
 
self.StepOfLevelCrossing = Enum('StepOfLevelCrossing',
                                    'searching_stop_sign '
                                    'searching_level '
                                    'watching_level '
                                    'stop '
                                    'pass_level')

距離計算

計算點到直線的距離，代碼封裝如下：

1
2
 
distance = abs(x0 * a + y0 * b + c) / math.sqrt(a * a + b * b)
return distance

計算點到點的距離（歐式），代碼封裝如下：

1
2
 
distance = math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1))
return distance

線性判斷

利用blob算法檢測出的紅色塊，得出色塊的坐標對，根據三點呈一線的原理，計算直線方程并判斷檢測出的點是否呈線性，代碼封裝如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
 
def (point1, point2, point3):
    # 判斷是否呈線性
    threshold_linearity = 50
    x1, y1 = point1
    x2, y2 = point3
    if x2 - x1 != 0:
        a = (y2 - y1) / (x2 - x1)
    else:
        a = 1000
    b = -1
    c = y1 - a * x1
    err = fnCalcDistanceDot2Line(a, b, c, point2[0], point2[1])

    if err < threshold_linearity:
        return True
    else:
        return False

距離判斷

利用blob算法檢測出的三個紅色塊，得出三個色塊的坐標對并計算相互之間的距離，判斷距離是否相等，代碼封裝如下：

1
2
3
4
5
6
7
8
9
10
11
 
def (point1, point2, point3):
    # 判斷距離是否相等
    threshold_distance_equality = 3
    distance1 = fnCalcDistanceDot2Dot(point1[0], point1[1], point2[0], point2[1])
    distance2 = fnCalcDistanceDot2Dot(point2[0], point2[1], point3[0], point3[1])
    std = np.std([distance1, distance2])

    if std < threshold_distance_equality:
        return True
    else:
        return False

格式轉換

設定合適的幀率，將訂閱到的原始圖像信息格式轉換成OpenCV庫能夠處理的信息格式，代碼封裝如下：

1
2
3
4
5
6
7
8
9
10
11
12
 
def cbGetImage(self, image_msg):
    if self.counter % 3 != 0:
        self.counter += 1
        return
    else:
        self.counter = 1

    if self.sub_image_type == "compressed":
        np_arr = np.fromstring(image_msg.data, np.uint8)
        self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
    else:
        self.cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")

注:根據計算機處理能力設定幀率，這里設置成10fps，不合適的幀率設置容易產生掉幀的現象，會影響最終的檢測。

提取色塊

利用掩膜操作，設定顏色閾值，將RGB圖像轉換成HSV格式圖像，根據顏色閾值提取出交通桿上的紅色色塊，返回掩膜后的0-1圖像。代碼封裝如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
 
def fnMaskRedOfLevel(self):
       image = np.copy(self.cv_image)

       # Convert BGR to HSV
       hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
       Hue_l = self.hue_red_l
       Hue_h = self.hue_red_h
       Saturation_l = self.saturation_red_l
       Saturation_h = self.saturation_red_h
       Lightness_l = self.lightness_red_l
       Lightness_h = self.lightness_red_h

       # define range of red color in HSV
       lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
       upper_red = np.array([Hue_h, Saturation_h, Lightness_h])

       # Threshold the HSV image to get only red colors
       mask = cv2.inRange(hsv, lower_red, upper_red)

       if self.is_calibration_mode == True:
           if self.pub_image_type == "compressed":
               self.pub_image_color_filtered.publish(self.cvBridge.cv2_to_compressed_imgmsg(mask, "jpg"))

           elif self.pub_image_type == "raw":
               self.pub_image_color_filtered.publish(self.cvBridge.cv2_to_imgmsg(mask, "mono8"))

       mask = cv2.bitwise_not(mask)
       return mask

速度控制

根據訂閱到的交通桿狀態信息/detect/level_crossing_order，實時發布目前的檢測狀態以及速度命令控制小車由減速到停車再到啟動的全過程。代碼封裝如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
大專欄%20TB3_Autorace之交通桿檢測e">39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
 
def cbLevelCrossingOrder(self, order):
    pub_level_crossing_return = UInt8()

    if order.data == self.StepOfLevelCrossing.searching_stop_sign.value:
        rospy.loginfo("Now lane_following")

        pub_level_crossing_return.data = self.StepOfLevelCrossing.searching_stop_sign.value

    elif order.data == self.StepOfLevelCrossing.searching_level.value:
        rospy.loginfo("Now searching_level")

        msg_pub_max_vel = Float64()
        msg_pub_max_vel.data = 0.10
        self.pub_max_vel.publish(msg_pub_max_vel)

        while True:
            is_level_detected, _, _ = self.fnFindLevel()
            if is_level_detected == True:
                break
            else:
                pass

        rospy.loginfo("SLOWDOWN!!")

        msg_pub_max_vel.data = 0.04
        self.pub_max_vel.publish(msg_pub_max_vel)

        pub_level_crossing_return.data = self.StepOfLevelCrossing.searching_level.value

    elif order.data == self.StepOfLevelCrossing.watching_level.value:
        rospy.loginfo("Now watching_level")

        while True:
            _, is_level_close, _ = self.fnFindLevel()
            if is_level_close == True:
                break
            else:
                pass

        rospy.loginfo("STOP~~")

        msg_pub_max_vel = Float64()
        msg_pub_max_vel.data = 0.0
        self.pub_max_vel.publish(msg_pub_max_vel)

        pub_level_crossing_return.data = self.StepOfLevelCrossing.watching_level.value

    elif order.data == self.StepOfLevelCrossing.stop.value:
        rospy.loginfo("Now stop")

        while True:
            _, _, is_level_opened = self.fnFindLevel()
            if is_level_opened == True:
                break
            else:
                pass

        rospy.loginfo("GO~~")

        msg_pub_max_vel = Float64()
        msg_pub_max_vel.data = 0.04
        self.pub_max_vel.publish(msg_pub_max_vel)

        pub_level_crossing_return.data = self.StepOfLevelCrossing.stop.value

    elif order.data == self.StepOfLevelCrossing.pass_level.value:
        rospy.loginfo("Now pass_level")

        pub_level_crossing_return.data = self.StepOfLevelCrossing.pass_level.value

    self.pub_level_crossing_return.publish(pub_level_crossing_return)

交通桿檢測

這里主要利用blob斑點檢測算法，在上一篇文章中《TB3_Autorace之路標檢測》提到的通過設定幾個檢測指標對掩膜后的圖像進行關鍵點檢測，將識別出的色塊用黃色圓標識。當檢測到3個紅色矩形框時，計算關鍵點的平均坐標以及關鍵點到平均點的距離列表，通過計算三個紅色塊之間的距離以及校驗三個色塊的線性情況判斷是否檢測到交通桿，將斑點用藍色線連接標識，計算交通桿的斜率，根據斜率值的大小說明到攔路桿的距離，判斷攔路桿的狀態;通過計算self.stop_bar_count的值判斷交通桿已經收起，小車可以通過，返回交通桿的三個狀態（小車減速，停車，通過），代碼封裝如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
 
def fnFindRectOfLevel(self, mask):
    is_level_detected = False
    is_level_close = False
    is_level_opened = False

    params = cv2.SimpleBlobDetector_Params()
    # Change thresholds
    params.minThreshold = 0
    params.maxThreshold = 255

    # Filter by Area.
    params.filterByArea = True
    params.minArea = 200
    params.maxArea = 3000

    # Filter by Circularity
    params.filterByCircularity = True
    params.minCircularity = 0.5

    # Filter by Convexity
    params.filterByConvexity = True
    params.minConvexity = 0.9

    det = cv2.SimpleBlobDetector_create(params)
    keypts = det.detect(mask)
    frame = cv2.drawKeypoints(self.cv_image, keypts, np.array([]), (0, 255, 255),
                              cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

    mean_x = 0.0
    mean_y = 0.0

    if len(keypts) == 3:
        for i in range(3):
            mean_x = mean_x + keypts[i].pt[0] / 3
            mean_y = mean_y + keypts[i].pt[1] / 3
        arr_distances = [0] * 3
        for i in range(3):
            arr_distances[i] = fnCalcDistanceDot2Dot(mean_x, mean_y, keypts[i].pt[0], keypts[i].pt[1])

        idx1, idx2, idx3 = fnArrangeIndexOfPoint(arr_distances)
        frame = cv2.line(frame, (int(keypts[idx1].pt[0]), int(keypts[idx1].pt[1])),
                         (int(keypts[idx2].pt[0]), int(keypts[idx2].pt[1])), (255, 0, 0), 5)
        frame = cv2.line(frame, (int(mean_x), int(mean_y)), (int(mean_x), int(mean_y)), (255, 255, 0), 5)

        point1 = [int(keypts[idx1].pt[0]), int(keypts[idx1].pt[1] - 1)]
        point2 = [int(keypts[idx3].pt[0]), int(keypts[idx3].pt[1] - 1)]
        point3 = [int(keypts[idx2].pt[0]), int(keypts[idx2].pt[1] - 1)]

        is_rects_linear = fnCheckLinearity(point1, point2, point3)
        is_rects_dist_equal = fnCheckDistanceIsEqual(point1, point2, point3)

        if is_rects_linear == True or is_rects_dist_equal == True:
            # finding the angle of line
            distance_bar2car = 100 / fnCalcDistanceDot2Dot(point1[0], point1[1], point2[0], point2[1])

            # publishing topic
            self.stop_bar_count = 40
            if distance_bar2car > 0.8:
                is_level_detected = True
                self.stop_bar_state = 'slowdown'
                self.state = "detected"
            else:
                is_level_close = True
                self.stop_bar_state = 'stop'

    if self.stop_bar_count > 0:
        self.stop_bar_count -= 1
    if self.stop_bar_count == 0:
        is_level_opened = True
        self.stop_bar_state = 'go'

    if self.pub_image_type == "compressed":
        self.pub_image_level.publish(self.cvBridge.cv2_to_compressed_imgmsg(frame, "jpg"))

    elif self.pub_image_type == "raw":
        self.pub_image_level.publish(self.cvBridge.cv2_to_imgmsg(frame, "bgr8"))

    return is_level_detected, is_level_close, is_level_opened

def cbLevelCrossingFinished(self, level_crossing_finished_msg):
    self.is_level_crossing_finished = True

def main(self):
    rospy.spin()


至此，已經完成對交通桿的檢測工作，根據交通桿下放過程中實時計算的桿子斜率指標給小車發布速度控制命令，實現了減速，停車，通過全過程。

附

完整進程源代碼：https://github.com/sun-coke/TB3_Autorace

總結

以上是生活随笔為你收集整理的TB3_Autorace之交通杆检测的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。