ROS-手势控制小海龟移动

手势控制小海龟移动

这是很早之前学习ros做的一个小demo，适合初学者学习。
一.要求
要求通过电脑自带摄像头检测手势动作控制小海龟移动
二.问题分析
本设计需要运行三个节点，第一个节点为reconition（发布），通过采用传统的视觉算法检测手势动作，并将检测完后的数据传递到command（手势订阅）节点，command（海龟运动发布）节点订阅到手势信息后进行判断，并将相应指令传递给小海龟/turtle1/cmd_vel’，控制其运动。
note：其实也可以不必采用传统的视觉检测算法，也可以采用深度学习的方法，但是我感觉没有必要，所以采取了简单的方法来做。
三.运行环境
需要安装opencv2或者以上的cv库，numpy库
四.代码参考

recognition

#!/usr/bin/env python

# organize imports

import cv2
import imutils
import numpy as np
from std_msgs.msg import String
import math
import rospy
# global variables

#Based on skin detection
def getContours(img):
    kernel = np.ones((5, 5), np.uint8)
    closed = cv2.morphologyEx(img, cv2.MORPH_OPEN, kernel)
    closed = cv2.morphologyEx(closed, cv2.MORPH_CLOSE, kernel)
    _,contours, h = cv2.findContours(closed, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    vaildContours = []
    for cont in contours:
        if cv2.contourArea(cont) > 9000:
            # x,y,w,h = cv2.boundingRect(cont)
            # if h/w >0.75:
            # filter face failed
            #print(cv2.contourArea(cont))
            # time.sleep(1)
            vaildContours.append(cv2.convexHull(cont))
            # print(cv2.convexHull(cont))
            # rect = cv2.minAreaRect(cont)
            # box = cv2.cv.BoxPoint(rect)
            # vaildContours.append(np.int0(box))
    return  vaildContours


def HSVBin(img):
    hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)

    lower_skin = np.array([100, 50, 0])
    upper_skin = np.array([125, 255, 255])

    mask = cv2.inRange(hsv, lower_skin, upper_skin)
    # res = cv2.bitwise_and(img,img,mask=mask)
    return mask
def cal_distance(p1, p2):
    return math.sqrt(math.pow((p2[0] - p1[0]), 2) + math.pow((p2[1] - p1[1]), 2))

if __name__ == '__main__':
     
     rospy.init_node('recognition', anonymous=True)
     pub = rospy.Publisher('gesture', String, queue_size=10)
     rate = rospy.Rate(10)  # 10hz

     cap = cv2.VideoCapture(0)
     a=1
     while (cap.isOpened()):
        while not rospy.is_shutdown():
            
            #x = []
            #y = []
            ret, img = cap.read()
            skinMask = HSVBin(img)
            contours = getContours(skinMask)

            for c in contours:

                M = cv2.moments(c)
                cX = int(M["m10"] / M["m00"])
                cY = int(M["m01"] / M["m00"])

                #cv2.drawContours(img, [c], -1, (0, 255, 0), 2)
                #cv2.circle(img, (cX, cY), 7, (255, 255, 255), -1)
                #cv2.putText(img, "center", (cX - 20, cY - 20),
                 #           cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
                distance=[]


                for i in c:
                    #print(i[0])
                    point1 = tuple(i[0])
                    point2 = (cX, cY)
                    distance.append(cal_distance(point1, point2))

                Direction_point=tuple(c[distance.index(max(distance))][0])
                #cv2.circle(img, Direction_point, 7, (255, 255, 255), -1)
                #cv2.line(img, Direction_point,(cX, cY), (255, 0, 0))


                if cal_distance(Direction_point, (cX,cY))>=100:
                    if abs(Direction_point[0]-cX)>abs(Direction_point[1]-cY):
                        if Direction_point[0]-cX>0:
                            print('Left')
                            a=3
                            cv2.putText(img, "Left", ( 200, 20),
                                        cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 4)
                        elif Direction_point[0]-cX<=0:
                            print('Right')
                            a=4
                            cv2.putText(img, "Right", (200, 20),
                                        cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 4)
                    elif abs(Direction_point[0]-cX)<abs(Direction_point[1]-cY):
                        if Direction_point[1]-cY>0:
                            print('Down')
                            a=5
                            cv2.putText(img, "Down", (200, 20),
                                         cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 4)
                        elif Direction_point[1]-cY<=0:
                            print('Up')
                            a=2
                            cv2.putText(img, "Up", (200, 20),
                                        cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255,0), 4)

                else:
                    print('stop')
                    a=1
                    cv2.putText(img, "Stop", (200, 20),
                                cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 4)



                pub.publish(str(a))

                cv2.imshow("hold the turtle", img)

               

            rate.sleep()
            k = cv2.waitKey(10)
            if k == 27:
                break

        k = cv2.waitKey(10)
        if k == 27:
            break

command.py

#!/usr/bin/env python

import rospy
#!/usr/bin/env python

from geometry_msgs.msg import Twist
from std_msgs.msg import String

fingers = 0
def callback(data):
	global fingers
	fingers = data.data

rospy.init_node('command')
rospy.Subscriber("gesture", String, callback)
cmd_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(10)


while not rospy.is_shutdown():
	twist = Twist()
	if fingers == '2':
		twist.linear.x = 0.6
		rospy.loginfo("driving forward")
	elif fingers == '3':
		twist.angular.z = -1
		rospy.loginfo("turning left")
	elif fingers == '4':
		twist.angular.z = 1
		rospy.loginfo("turning right")
	elif fingers == '5':
		twist.linear.x = -0.6
		rospy.loginfo("turning right")
	else:
		twist.linear.x = 0.0
		twist.angular.z = 0.0
		rospy.loginfo("stoped")
	cmd_vel_pub.publish(twist)
	rate.sleep()

这是核心代码，读者可以根据此代码自己编写完整。最后附上工程链接，有兴趣的可以去下载。
https://download.csdn.net/download/weixin_39591533/34379935