コード例 #1
0
ファイル: eyes3.py プロジェクト: arcosaka/arcosaka
def eyes_py(id):
    global polePosX, polePosY, dispar, sadws

    # 初期化宣言 : このソフトウェアは"eyes_py_node"という名前
    rospy.init_node('eyes_py_node', anonymous=True)

    # インスタンスの作成 
    eyes = Eyes(id)

    # 処理周期の設定
    r = rospy.Rate(PUB_RATE)

    print("[eyes3] start")
    # ctl + Cで終了しない限りwhileループで処理し続ける
    while not rospy.is_shutdown():

        #==================================================
        # Loop
        ret0, imgL = RP3_Cap_L.read()
        ret1, imgR = RP3_Cap_R.read()

        if dispMode == 1:
            cv2.imshow( "Left  Camera", imgL )
            cv2.imshow( "Right Camera", imgR )

            #-- Create DiparityMap --
            disparity = GetDisparityMap()

            #-- Cal Pole Distance --
            capL_target = GetBlueDetect( imgL )
            Lx, Ly, Lw, Lh = cv2.boundingRect( capL_target )

            capR_target = GetBlueDetect( imgR )
            Rx, Ry, Rw, Rh = cv2.boundingRect( capR_target )

            # Xは左右中心の更に中心
            Lpos = Lx + (Lw / 2)
            Rpos = Rx + (Rw / 2)
            polePosX = (Lpos + Rpos) / 2
            # Yは片側基準でOK
            polePosY = Ly + (Lh / 2)

            # from disparity to dist
            # Z = f x T / um / disparity
            poleDist = 6.6 * 62 / 0.003 / disparity[polePosY][polePosX]
            print polePosX, polePosY, poleDist

            if dispMode == 1:
                cv2.rectangle( imgL, (Lx, Ly), (Lx + Lw, Ly + Lh), (0, 0, 255), 1 )
                cv2.imshow( "Left Pole", imgL )
                cv2.rectangle( imgR, (Rx, Ry), (Rx + Rw, Ry + Rh), (0, 0, 255), 1 )
                cv2.imshow( "Right Pole", imgR )

        #==================================================
        # メイン処理
        eyes.main()

        # Sleep処理
        r.sleep()
コード例 #2
0
def eyes_py(id):
    global sidebudsPosX, sidebudsPosY, dispar, sadws

    # 初期化宣言 : このソフトウェアは"eyes_py_node"という名前
    rospy.init_node('eyes_py_node', anonymous=True)

    # インスタンスの作成
    eyes = Eyes(id)

    # 処理周期の設定
    r = rospy.Rate(PUB_RATE)

    print("[eyes2] start")
    # ctl + Cで終了しない限りwhileループで処理し続ける
    while not rospy.is_shutdown():

        #==================================================
        # Loop
        ret0, imgL = RP3_Cap_L.read()
        ret1, imgR = RP3_Cap_R.read()

        if dispMode == 1:
            cv2.imshow("Left  Camera", imgL)
            cv2.imshow("Right Camera", imgR)

        #-- Create DiparityMap --
        disparity = GetDisparityMap()

        ###################################################
        #-- Look for SideBuds --
        sbsJpg = cv2.imread('./SideBuds_1.JPG', 0)

        grayImg = cv2.cvtColor(imgL, cv2.COLOR_BGR2GRAY)

        result = cv2.matchTemplate(grayImg, sbsJpg, cv2.TM_CCOEFF_NORMED)
        unused_minVal, unused_maxVal, unused_minLoc, maxLoc = cv2.minMaxLoc(
            result)
        top_left = maxLoc
        w, h = sbsJpg.shape[::-1]
        bottom_right = (top_left[0] + w, top_left[1] + h)

        if dispMode == 1:
            cv2.rectangle(imgL, top_left, bottom_right, (0, 255, 0), 1)
            cv2.imshow("SideBuds", imgL)

        sidebudsPosX = top_left[0] + (w / 2)
        sidebudsPosY = top_left[1] + (h / 2)

        #-- Cal SideBuds Distance --
        sidebudsPosDist = 6.6 * 62 / 0.003 / disparity[sidebudsPosY][
            sidebudsPosX]
        print sidebudsPosX, sidebudsPosY, sidebudsPosDist

        ###################################################
        #-- Cal weed Center --
        targetL = GetGreenDetect(imgL)
        targetR = GetGreenDetect(imgR)

        # target Position
        Lx, Ly, Lw, Lh = cv2.boundingRect(targetL)
        Rx, Ry, Rw, Rh = cv2.boundingRect(targetR)

        if dispMode == 1:
            cv2.rectangle(imgL, (Lx, Ly), (Lx + Lw, Ly + Lh), (0, 0, 255), 1)
            cv2.imshow("Weed L", imgL)
            cv2.rectangle(imgR, (Rx, Ry), (Rx + Rw, Ry + Rh), (0, 0, 255), 1)
            cv2.imshow("Weed R", imgR)

        # Xは左右中心の更に中心
        Lpos = Lx + (Lw / 2)
        Rpos = Rx + (Rw / 2)
        weedPosX = (Lpos + Rpos) / 2
        # Yは片側基準でOK
        weedPosY = Ly + (Lh / 2)

        # from disparity to dist
        # Z = f x T / um / disparity
        weedPosDist = 6.6 * 62 / 0.003 / disparity[weedPosY][weedPosX]
        print weedPosX, weedPosY, weedPosDist

        #==================================================
        # メイン処理
        eyes.main()

        # Sleep処理
        r.sleep()