(ROS) Bebop

とりあえず公式の説明に従ってインストールしておく。

離陸・制御・着陸

ドローンが飛び放しになって非常に危険なので、この説明を一通り読んでから実行すること

操作はドキュメントの通り。離陸と着陸はEmptyを送れば良い。

離陸

rostopic pub --once /bebop/takeoff std_msgs/Empty

制御

linearとangularを送る。linearの方は豪快に値を入れると凶器と化すので要注意

rostopic pub /bebop/cmd_vel geometry_msgs/Twist "{linear: {x: 0.0, y: 0.0, z0.0}, angular: {x: 0.0, y: 0.0, z: 0.1}}"

でz軸について回転(Yaw)。全部0を送ればホバリングになる。

rostopic pub /bebop/cmd_vel geometry_msgs/Twist "{linear: {x:.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

着陸

rostopic pub --once /bebop/land std_msgs/Empty

操作のやりかた

ひとまずコマンドで制御しようとする場合は4つのターミナルを開き、上の4コマンドをEnterだけで送れる状態にしておく(特にland)。

Bebopのカメラ映像

topicを/bebop/image_rawにするだけで良い。

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
import cv2, cv_bridge

class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        self.image_sub = rospy.Subscriber('bebop/image_raw', Image, self.image_callback, queue_size=1)
        self.fdet = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        faces = self.fdet.detectMultiScale(image[:,:,1])
        for (x, y, w, h) in faces:
            cv2.rectangle(image, (x, y), (x+w, y+h), (0, 0, 255), 2)
        cv2.imshow("window", image)
        cv2.waitKey(3)

rospy.init_node('follower')
follower = Follower()
rospy.spin()

あとは画像に基いて/bebop/cmd_velへpublishすれば良いのだが、画像の情報で追尾する場合には外乱対策が必要なので、テスト中はドローンを手で持って試すのが安全。

顔検出

基本的にPCでやる場合と同様。処理が重いのでオプションでどうにかする。

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
import cv2, cv_bridge

class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        self.image_sub = rospy.Subscriber('bebop/image_raw', Image, self.image_callback, queue_size=1)
        self.fdet = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        faces = self.fdet.detectMultiScale(image[:,:,1], scaleFactor=1.11, minNeighbors=1, minSize=(100, 100))
        for (x, y, w, h) in faces:
            cv2.rectangle(image, (x, y), (x+w, y+h), (0, 0, 255), 2)
        cv2.imshow("window", image)
        cv2.waitKey(3)

rospy.init_node('follower')
follower = Follower()
rospy.spin()

Publisherになってimage_viewerで見ることもできる。

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
import cv2, cv_bridge

class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        self.image_pub = rospy.Publisher("image_face", Image, queue_size=1)
        self.image_sub = rospy.Subscriber('bebop/image_raw', Image, self.image_callback, queue_size=1)
        self.fdet = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        faces = self.fdet.detectMultiScale(image[:,:,1], scaleFactor=1.11, minNeighbors=1, minSize=(100, 100))
        for (x, y, w, h) in faces:
            cv2.rectangle(image, (x, y), (x+w, y+h), (0, 0, 255), 2)
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(image, "bgr8"))

rospy.init_node('follower')
follower = Follower()
rospy.spin()

人検出

処理が重いので半分にリサイズしてる。

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
import cv2, cv_bridge

class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        self.image_sub = rospy.Subscriber('bebop/image_raw', Image, self.image_callback, queue_size=1)
        self.hog = cv2.HOGDescriptor()
        self.hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())

    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        image = cv2.resize(image, (428, 240))
        hogParams = {'winStride': (8, 8), 'scale': 1.5}
        people, r = self.hog.detectMultiScale(image[:, :, 1], **hogParams)
        for (x, y, w, h) in people:
            cv2.rectangle(image, (x, y), (x+w, y+h), (0, 0, 255), 2)
        cv2.imshow("window", image)
        cv2.waitKey(1)

rospy.init_node('follower')
follower = Follower()
rospy.spin()