Robot control with face tracking

Get iRobot Roomba to rotate and track an individual's face

Follow the below steps:

Step 1: Create a node that would use HAAR cascade detector to detect an individual's face and publish those values

• Create a python file in your scripts folder and name it detect_face.py.
• Save the script in your ros_package/scripts folder. Alternatively, download the ROS package called irobot_vision_tutorial supplied with this tutorial.
• Add the following code to this script. We detail each segment of the code below.

#!/usr/bin/env python
import cv2
import numpy as np
import rospy
from std_msgs.msg import Float32
from sensor_msgs.msg import CompressedImage
import sys
sys.path.append('/opt/ros/indigo/lib/python2.7/dist-packages')

• This code will import all the necessary packages into your script. Note, we used Python 2.7.

• The following code will initiate the detect_face node and begin publishing a topic rotation_angle. Since we will be using the USB camera we will be obtaining the video frames from a topic called /usb_cam/image_raw/compressed which we will use for detecting a person's face. The subscriber will initiate a callback function image_callback every time it gets a new message.

if __name__ == '__main__':
    try:
	rospy.init_node('detect_face', anonymous=True)
	pub=rospy.Publisher('rotation_angle',Float32,queue_size=1)
	#rate = rospy.Rate(1) # 10hz
	face_cascade=cv2.CascadeClassifier('/home/arabinda/catkin_ws/src/ros_seminar/scripts/haarcascade_frontalface_default.xml')
	sub = rospy.Subscriber("/usb_cam/image_raw/compressed",CompressedImage, image_callback,  queue_size = 1)
	rospy.spin()
        #talker()
    except rospy.ROSInterruptException:
	pass

• The image_callback function code will utilise the haarcascade_frontalface_default.xml file to detect the individuals face on the compressed image it obtains as ros_data. Subsequently, we will calculate the coordinates of the center of the bounding box and evaluate its distance from the center x-coordinate of the window frame (in my case it is 320 as the x-coordinate of window frame ranges from 0-640). Then we will convert this distance into the degree value the iroomba should rotate. This degree value is published over the topic rotation angle.

def image_callback(ros_data):
       
        np_arr = np.fromstring(ros_data.data, np.uint8)
        image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
	gray=cv2.cvtColor(image_np,cv2.COLOR_BGR2GRAY)
	faces=face_cascade.detectMultiScale(gray,scaleFactor=1.1,minNeighbors=5,minSize=(30, 30),flags=cv2.CASCADE_SCALE_IMAGE)
	for (x,y,w,h) in faces:
		cv2.rectangle(gray,(x,y),(x+w,y+h),(255,0,0),2)
		distance_from_center=((2*x+w)/2)-320
		distance_per_degree=4
		rotation_angle=distance_from_center/distance_per_degree
		pub.publish(rotation_angle)
		rospy.loginfo(rotation_angle)
		#rate.sleep()		
		cv2.imshow('Video',gray)
		cv2.waitKey(2)

Step 2: Create a new node that subscribes to rotation_angle, convert's the angle value into radians and publishes it as the "angular value for the z-axis" over the /cmd_vel topic.

• Create a new python file in your ros_package/scripts folder and name it rotate_robot.py.

• Import the necessary packages, initiate the python inverter and create an object of the Twist class.

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist
msg=Twist()

• Initiate the python script by running the __main__ function and calling the listener function

if __name__ == '__main__':
    listener()

• The listener() is responsible for initializing the rotate_robot node and subscribing to the rotation_angle topic. It also passes the rotation angle data obtained from the topic to the callback function

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('rotate_robot', anonymous=True)

    rospy.Subscriber('rotation_angle', Float32, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

• The callback function converts the angle value passed into it using data and converts it into radians. This value is then published turtle1/cmd_vel

def callback(data):
    
    #rospy.loginfo(rospy.get_caller_id() + "I heard %f", data.data)
    pub=rospy.Publisher('turtle1/cmd_vel',Twist,queue_size=10)
    if(data.data>30):
	msg.angular.z=30*0.0174533
    elif(data.data<-30):
	msg.angular.z=-30*0.0174533
    else:
    	msg.angular.z=(data.data)*0.0174533
    pub.publish(msg)

Step 3: Make iRobot rotate based on the location of person's face on the camera image.

• If you have the robot installed with a camera, then run the above nodes and launch the create2 driver roslaunch ca_driver create_2.launch drivers and usb_cam node rosrun usb_cam usb_node

• A demonstration video for this section is available here: https://youtu.be/ZyD-bbF6ts4

Step 4: Make turtlesim rotate

• If you do not have an iRobot Roomba, launch the turtlesim by using the command $ rosrun turtlesim turtlesim_node

• When you move face in front of the webcam the turtlesim/irobot will rotate