Raspberry Pi上でリアルタイムで指文字を認識させた後に2つのサーボモータを動作させようとしています。

Question

前提・実現したいこと

Raspberry Pi上でカメラにリアルタイムで指の本数を数えさせ、それに合わせてサーボモータを動かそうとしています。
指文字の本数を数えさせるプログラム、サーボモータの動作のプログラムはそれぞれうまくいっているのですが、これらを組み合わせるところで苦戦しています。
指文字の認識のプログラムは下に貼っています。
while文がうまく回っていないのではないかと考えていますが、ここ一週間くらい調べてもどうにもならずここへ質問に来ました。

発生している問題・エラーメッセージ

一瞬でtry-except文を抜けてしまい、動作しません。
print(l)によって最初に読み込まれた画像の指の本数を出し続けてしまいます。

Traceback (most recent call last):
  File "/usr/lib/python3.7/ast.py", line 35, in parse
    return compile(source, filename, mode, PyCF_ONLY_AST)
  File "/home/pi/Desktop/HandServo2.py", line 177
    except KeyboardInterrupt:                   #Ctrl+Cキーが押された
                                                             ^

本来指文字を数えるプログラムではframeというカメラに入力されている映像とmaskという入力映像をハイ、ローレベルに置き換えた映像の二つが映るはずなのですがどちらも動作しません。tryとwhileを入れ替えて実行してみたところカメラが起動した瞬間のmaskの画像だけ映ります。

また、このプログラムを走らせるとサーボモータがとても痙攣したような動きをします。
何度も制御信号(l)が送られてくるためだと考えているので、lが10回入ったらモータを動かすというようなプログラムに書き換える予定です。

該当のソースコード

import cv2
import numpy as np
import math                         #必要なモジュールをインポート
import RPi.GPIO as GPIO             #GPIO用のモジュールをインポート
import time                         #時間制御用のモジュールをインポート
import sys                          #sysモジュールをインポート
cap = cv2.VideoCapture(0)


servo_pin1 = 17                      #変数"Servo_pin1"に17を格納
GPIO.setmode(GPIO.BCM)              #GPIOのモードを"GPIO.BCM"に設定
GPIO.setup(servo_pin1, GPIO.OUT)     #GPIO17を出力モードに設定

servo_pin2 = 27                      #変数"Servo_pin2"に27を格納
GPIO.setup(servo_pin2, GPIO.OUT)     #GPIO27を出力モードに設定

#PWMの設定
servo1 = GPIO.PWM(servo_pin1, 50)     #GPIO.PWM1(ポート番号, 周波数[Hz])
servo2 = GPIO.PWM(servo_pin2, 50)     #GPIO.PWM2(ポート番号, 周波数[Hz])

servo1.start(0)                     
print('servo1_setup')
servo2.start(0)                     
print('servo2_setup')

while(1):
    try:

    #an error comes if it does not find anything in window as it cannot find contour of max area
            #therefore this try error statement
        ret, frame = cap.read()
        frame=cv2.flip(frame,1)
        kernel = np.ones((3,3),np.uint8)

        #define roi which is a small square on screen
        roi=frame[100:300, 100:300]


        cv2.rectangle(frame,(100,100),(300,300),(0,255,0),0)
        hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)



    # range of the skin colour is defined
        lower_skin = np.array([0,20,70], dtype=np.uint8)
        upper_skin = np.array([20,255,255], dtype=np.uint8)

        #extract skin colur image
        mask = cv2.inRange(hsv, lower_skin, upper_skin)



    #extrapolate the hand to fill dark spots within
        mask = cv2.dilate(mask,kernel,iterations = 4)

    #image is blurred using GBlur
        mask = cv2.GaussianBlur(mask,(5,5),100)



    #find contours

    #If the version is "open cv4", the number of returns is 2. image,contents..." I changed it from
        contours,hierarchy= cv2.findContours(mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

    #find contour of max area(hand)
        cnt = max(contours, key = lambda x: cv2.contourArea(x))

    #approx the contour a little
        epsilon = 0.0005*cv2.arcLength(cnt,True)
        approx= cv2.approxPolyDP(cnt,epsilon,True)


    #make convex hull around hand
        hull = cv2.convexHull(cnt)

        #define area of hull and area of hand
        areahull = cv2.contourArea(hull)
        areacnt = cv2.contourArea(cnt)

    #find the percentage of area not covered by hand in convex hull
        arearatio=((areahull-areacnt)/areacnt)*100

        #find the defects in convex hull with respect to hand
        hull = cv2.convexHull(approx, returnPoints=False)
        defects = cv2.convexityDefects(approx, hull)

    # l = no. of defects
        l=0

    #code for finding no. of defects due to fingers
        for i in range(defects.shape[0]):
            s,e,f,d = defects[i,0]
            start = tuple(approx[s][0])
            end = tuple(approx[e][0])
            far = tuple(approx[f][0])
            pt= (100,180)


            # find length of all sides of triangle
            a = math.sqrt((end[0] - start[0])**2 + (end[1] - start[1])**2)
            b = math.sqrt((far[0] - start[0])**2 + (far[1] - start[1])**2)
            c = math.sqrt((end[0] - far[0])**2 + (end[1] - far[1])**2)
            s = (a+b+c)/2
            ar = math.sqrt(s*(s-a)*(s-b)*(s-c))

            #distance between point and convex hull
            d=(2*ar)/a

            # apply cosine rule here
            angle = math.acos((b**2 + c**2 - a**2)/(2*b*c)) * 57


            # ignore angles > 90 and ignore points very close to convex hull(they generally come due to noise)
            if angle <= 90 and d>30:
                l += 1
                cv2.circle(roi, far, 3, [255,0,0], -1)

            #draw lines around hand
            cv2.line(roi,start, end, [0,255,0], 2)


        l+=1

        #display corresponding gestures which are in their ranges
        font = cv2.FONT_HERSHEY_SIMPLEX

        if l==1:
            if areacnt<2000:
                cv2.putText(frame,'Put hand in the box',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)
            else:
                if arearatio<12:
                    cv2.putText(frame,'0',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)
                
                else:
                    cv2.putText(frame,'1',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)
                    servo1.ChangeDutyCycle(2.0)
                    time.sleep(0.5)
                    servo1.ChangeDutyCycle(7.0)      #水平方向になる
                    time.sleep(3)

        elif l==2:
            cv2.putText(frame,'2',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)
            servo1.ChangeDutyCycle(12.0)
            time.sleep(0.5)
            servo1.ChangeDutyCycle(7.0)
            time.sleep(0.5)


        elif l==3:
            if arearatio<27:
                cv2.putText(frame,'3',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)
                servo2.ChangeDutyCycle(2.0)
                time.sleep(0.5)
                servo2.ChangeDutyCycle(7.0)
                time.sleep(0.5)
            else:
                cv2.putText(frame,'ok',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)

        elif l==4:
            cv2.putText(frame,'4',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)
            servo2.ChangeDutyCycle(12.0)
            time.sleep(0.5)
            servo2.ChangeDutyCycle(7.0)
            time.sleep(0.5)

        elif l==5:
            cv2.putText(frame,'5',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)

        elif l==6:
            cv2.putText(frame,'reposition',(0,50), font, 2, (0,0,255), 3, cv2.LINE_AA)

        else :
            cv2.putText(frame,'reposition',(10,50), font, 2, (0,0,255), 3, cv2.LINE_AA)

        cv2.imshow('mask',mask)
        cv2.imshow('frame',frame)

    except KeyboardInterrupt:                   #Ctrl+Cキーが押された
        servo1.stop()                           #サーボモータ1を止める
        servo2.stop()                           #サーボモータ2を止める
        GPIO.cleanup()                          #GPIOをクリーンアップ
        sys.exit()                              #プログラムを終了
        cv2.destroyAllWindows()
        cap.release() 

試したこと

while文の手前でGPIOのセットアップ、try文の終盤のif, elif分の中にサーボモータの動作設定を行うと動かなくなります。
servo1.start の下にprintを入れてみたところ、startは一度のみしか適用されていないことが分かりました。
やはりカメラの映像が連続で取り込まれず最初の一瞬のみしか入っていないのが問題です。
whileがうまくループできていないのではないかと考えています。

字数制限にかかったので必要でしたらコメントの返信で指の本数を数えるプログラムを記載します。

補足情報（FW/ツールのバージョンなど）

ここにより詳細な情報を記載してください。

Answer 1

except KeyboardInterrupt: #Ctrl+Cキーが押された

                                                         ^

IndentationError: unexpected unindent

このエラーは、内容にあるとおり、except文のインデントがtryと合っていないために起きているエラーです。
少なくとも文法的に正しくなければ動作しません。

これを直しても、なお動作しないということであれば、修正後のソースと問題内容で質問を修正しましょう。

---

「指文字の本数を数えさせるプログラム、サーボモータの動作のプログラムはそれぞれうまくいっている」ということですが、それらを混ぜてしまっていることでうまく動かなくなってしまっているのではないでしょうか。
自分でやるのであれば、

• 指を数える処理を関数化する。
  例えば、本数が判定できたら本数を返すようにする
• 引数によってサーボの位置を変更する関数を作る。
• それらを適切につなぎ合わせる処理をつくる

のようにします。こうすることでどこに問題があるのかわかりやすくなります。
また、サーボのような外部のデバイスの制御では、適切にスリープを入れることも必要です。高速にON/OFFをしてしまうと結局動いていないように見えるなどです。
コードにはスリープが入っているようですがらそのあたりは考慮しているのかもしれませんが、原因かもしれません。

Comments

[nyororo]

本当に情けないミスをしていましたね、、ありがとうございます。
ただいま帰宅中で学校にラズパイを置いてきてしまったので、明日インデントを修正し直して実行します。

[nyororo]

インデントがずれていた部分を直してきました。プログラムも修正しています。
動作はするのですが、カメラの映像が連続で読み込まれず、最初に一瞬入力された画像の処理しか行ってくれません。

[nyororo]

指を数える処理を関数化、してみようと思います。
本数が判定できると指の本数がlに格納され、print(l)で出力されるようにプログラムを書いています。因数(l)によりサーボの位置を変更するプログラムを入れたところ正常に動作しなくなったので今困っているところでした。
サーボモータはあまりスリープを長くすると動作が不安定になるので、因数が一回で動くのではなく、10回入力されたら動かすというような変更を加えたいと考えています。ご指摘いただいたようにこれを改善すればサーボモータが痙攣したような動きをしているのは解決できるのではないかと考えています。
ですが、やはりそれ以前にカメラの映像が入力されなくなったことが一番気になっています。。。

[TakaiY]

> サーボモータはあまりスリープを長くすると動作が不安定になる
そうですね。 サーボは入力値を常に参照してそれに合せて位置を決めるようにできているので、制御側は定期的に参照位置を送信している必要がありますね。

ということを考えると、サーボの制御以外のことをするのであれば、サーボの制御は別スレッドなどにして、定常的に情報を流せるようにする必要があるかもしれません。 上手く情報が取れるようになってからの話ではありますが。

[nyororo]

すいません、送信したつもりができていませんでした、、、
TakaiY様がおっしゃったとおりサーボの制御を別枠にする方法でうまくいきました。具体的には今までＣＰＵでＰＷＭ制御を行うソフトウェアＰＷＭで制御していましたがＣＰＵから独立したクロックで制御するハードウェアＰＷＭを利用したところかなり動作が安定するようになりました。これに加えて因数が複数回入力されたのちにモーターを動かす、というようにカウンタを設けてプログラムを変更しました。

また、プログラムが一瞬で抜けてしまう理由として、モータの制御をプログラムに組み込んだ際にwaitkeyをつけ忘れていたことが問題でした。

この二点で問題が解決しました。アドバイスありがとうございました。