Click here to Skip to main content
13,706,648 members
Click here to Skip to main content
Add your own
alternative version

Stats

3.1K views
84 downloads
7 bookmarked
Posted 18 Aug 2018
Licenced CPOL

Rodney - A long time coming autonomous robot (Part 2)

, 18 Aug 2018
Rate this:
Please Sign up or sign in to vote.
Second part in a series on a ROS (Robot Operating System) House Bot.

Editorial Note

Having trouble writing or posting articles? These articles aim to gather together tips and tricks from authors and mentors to help you write great articles.

Introduction

The Rodney Robot project is a hobbyist robotic project to design and build an autonomous house-bot. This article is the second in the series describing the project.

Background

In part 1 to help define the requirements for our robot we selected our first mission and split it down into a number of Design Goals to make it more manageable.

The mission was taken from the article Let's build a robot! and was: Take a message to... - Since the robot will [have] the ability to recognize family members, how about the ability to make it the 'message taker and reminder'. I could say 'Robot, remind (PersonName) to pick me up from the station at 6pm'. Then, even if that household member had their phone turned on silent, or were listening to loud music or (insert reason to NOT pick me up at the station), the robot could wander through the house, find the person, and give them the message.

The design goals for this mission were:

  • Design Goal 1: To be able to look around using the camera, search for faces, attempt to identify any people seen and display a message for any identified
  • Design Goal 2: Facial expressions and speech synthesis. Rodney will need to be able to deliver the message
  • Design Goal 3: Locomotion controlled by a remote keyboard and/or joystick
  • Design Goal 4: Addition of a laser ranger finder or similar ranging sensor used to aid navigation
  • Design Goal 5: Autonomous locomotion
  • Design Goal 6: Task assignment and completion notification

In the first part we used ROS (Robot Operating System) to add pan/tilt functionality to move the head and camera. In this part I'll add the face recognition and control node to complete Design Goal 1.

Mission 1, Design Goal 1 continued

Access images from the Raspberry Pi Camera

As stated in part 1 we can make use of work already carried out by the ROS community to make our life easier. The Raspberry Pi Ubuntu image I have installed includes a ROS package raspicam_node, we will make use of this package to access the camera. If you are using a different OS image on your Raspberry Pi you can still install the node from the GitHub site.

To add the node to our system we just need to include one of the supplied ROS launch files in our launch file. I'm going to use an image resolution of 1280 x 960 pixels, so adding the following to our launch file will start the raspicam_node in the required resolution.

<include file="$(find raspicam_node)/launch/camerav2_1280x960.launch" />

ROS uses its own image format to pass images between nodes. In the case of raspicam_node this image is in a compressed format. Now when we come to use the image to detect and recognise a face we will be using OpenCV to process the image. We therefore need a method to convert ROS images to OpenCV images and back again. Not a problem, those nice people in the ROS community have produced a package called cv_bridge which will do the work for us. There is however a slight fly in the ointment. There are two versions of cv_bridge, one for C++ and one for Python. The C++ version works with both compressed and non-compressed ROS images but the Python version only works with non-compressed images. Now I would like to write the face recognition node in Python since 1) the pan_tilt node was written in C++ and writing the next node in Python will give us examples in both languages and 2) I already have a Python face recognition library I would like to make use of.

So that we can use Python for the face recognition node we can use another available node called republish. We will use this node to convert from a compressed image to a raw image. To start this node and attach it to the compressed image from the camera will include the following line in our launch file.

<node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/raspicam_node/image/ raw out:=/camera/image/raw" />

This will start the node republish, which is part of the image_transport package, and subscribe it to the /raspicam_node/image/ topic. It will also name the published topic /camera/image/raw, which we will subscribe to in our face recognition node. 

Now I'm using the Raspberry Pi camera but you can use a different camera. You may find a ROS node is already available for your camera or you may need to wrap the camera library in a ROS node of your own. Although the topic name our face recognition node subscribes to is called /camera/image/raw, you can always remap your topic name to match this name. We did this in the launch file used in part 1 to remap pan_tilt_node/index0_position to pan_tilt_node/head_position.

<remap from="pan_tilt_node/index0_position" to="pan_tilt_node/head_position" />

Detect and recognise faces

Before the system can attempt to recognise a face we need to train it with the subjects we wish to recognise. We will write two non ROS Python scripts to train our system. The first, data_set_generator.py, will use the camera to capture facial images of each of our subjects. The second, training.py, uses the images collected by the first script to do the actual training. The output of this second script is a yaml file containing the training data, the ROS node will load this training data during its initialisation. If you wish to add new people to your list of subjects you will need to rerun each script.

Our ROS package for the node is called face_recognition and is available in the face_recognition folder. The sub folder scripts contains our two training scripts.

Each of the scripts makes use of face detection and face recognition built in to OpenCV. If you wish to fully understand how this works then may I suggest you read some of the many articles available on the internet. One of these can be found here. Not one article I came across had the code for exactly what I wanted so I have borrowed code from a number of articles. Here I'll give a high level description of each of the scripts starting with data_set_generator.py.

After the required imports we load the classifier using the OpenCV library, declare a helper function which we use to ensure that folders we require exist. The folder dataset will hold all the captured images and trainer will hold both the yaml file with the training data and a file containing names and ids of our subjects.

import cv2
import os
import io
import numpy
import yaml
import picamera

# Detect object in video stream using Haarcascade Frontal Face
face_detector = cv2.CascadeClassifier('../classifiers/haarcascade_frontalface_default.xml')

def assure_path_exists(path):
  dir = os.path.dirname(path)
  if not os.path.exists(dir):
    os.makedirs(dir)

assure_path_exists("dataset/")
assure_path_exists("../trainer/")

Next we set the camera resolution, set up some variables including the file name which holds our list of subjects and open the file.

We then create a window to display the image read from the camera. This will enable the subject to position themselves within the camera field of view.

The script will then prompt the user for the subjects unique ID, the subjects name and whether it is low light conditions or not. The unique IDs should start at 1 and be incremented by 1 each time you add a new subject and the name is the name that you wish the robot to use for this subject. You should ideally run this script twice per subject, once in bright light and a second time in low light conditions. This will improve the chances of the recognition algorithm having success. Each run of the script will take 100 images of the subject, the file name of each image is constructed from the subject id and an image number. Image numbers are numbered 0 to 99 for those taken in bright light and 100 to 199 for those taken in low light.

The next step is to add the subject to the names file if they don't already exist.

with picamera.PiCamera() as camera:
  camera.resolution = (1280, 960)

  looping = True
  count = 0
  end = 99
  names_dict = {}
  name_file = '../trainer/names.yml'

  # Open the file of IDs and names to append the new one to
  if os.path.exists(name_file):
    with open(name_file, 'r') as stream:
      names_dict = yaml.load(stream)

  cv2.namedWindow('frame', cv2.WINDOW_NORMAL)

  face_id = raw_input("What is this persons ID number? ")
  name = raw_input("What is this persons name? ")
  low_light = raw_input("Low light Y/N?" )

  if low_light == 'Y' or low_light == 'y':
    count = 100
    end = 199

  # If not already in the dictionary add details
  if not face_id in names_dict:
    names_dict[int(face_id)]=name

  with open(name_file, 'w') as outfile:
    yaml.dump(names_dict, outfile, default_flow_style=False)

A loop is then entered to capture the images. Each pass of the loop captures an image from the camera and converts it to a numpy array for the OpenCV calls.

Using OpenCV we then convert the image to a grey scale image and attempt to detect a face in the image. If a face is detected the image is cropped around the face, the number of image samples is incremented and the cropped grey scale image is stored in the dataset folder. The original image from the camera along with a superimposed frame around the face is displayed to the user.

A check is then made to see if the user has pressed the 'q' key on the keyboard which is used to exit the program early. Otherwise a check is made to see if we have captured the one hundred images of the face, if so this main loop is exited.

while(looping):
  # Create a memory stream so image doesn't need to be saved to a file
  stream = io.BytesIO()

  camera.capture(stream, format='jpeg')

  #Convert picture to numpy array
  buff = numpy.fromstring(stream.getvalue(), dtype=numpy.uint8)

  # Now create an OpenCV image
  image_frame = cv2.imdecode(buff, 1)

  # Convert frame to grayscale
  gray = cv2.cvtColor(image_frame, cv2.COLOR_BGR2GRAY)

  # Detect frames of different sizes, list of faces rectangles
  faces = face_detector.detectMultiScale(gray, 1.3, 5)

  # Although faces could contain more than one face we only expect one
  # person to be in the data set image otherwise it would confuse
  # the whole thing
  if (len(faces) != 0):
    # Expecting one face only on the data set image
    (x, y, w, h) = faces[0]

    # Crop the image frame into rectangle
    cv2.rectangle(image_frame, (x,y), (x+w,y+h), (255,0,0), 4)

    # Increment sample face image
    count += 1

    # Save the captured image into the datasets folder
    cv2.imwrite("dataset/User." + str(face_id) + '.' + str(count) + ".jpg", gray[y:y+h,x:x+w])

    # Display the video frame, with bounded rectangle on the person's face
    cv2.imshow('frame', image_frame)

  # To stop taking video, press 'q' for at least 100ms
  if cv2.waitKey(100) & 0xFF == ord('q'):
    looping = False

  # If image taken reach 100, stop taking video
  elif count>end:
    looping = False

The last two lines close the window displaying the image and prints a message indicating the process is complete.

# Close all started windows
cv2.destroyAllWindows()

print("Data prepared")

Once you have run the script for each subject, or if you have rerun the script for a new subject, you then run the training.py script. 

The training.py script starts with the imports and the assure_path_exists function definition, it then creates an instance of the OpenCV classes LBPHFaceRecognizer_create and CascadeClassifier using the same classifier file.

import cv2
import os
import numpy as np

def assure_path_exists(path):
  dir = os.path.dirname(path)
  if not os.path.exists(dir):
    os.makedirs(dir)

# Create Local Binary Patterns Histograms for face recognization
recognizer = cv2.face.LBPHFaceRecognizer_create()

# Using prebuilt frontal face training model, for face detection
detector = cv2.CascadeClassifier("../classifiers/haarcascade_frontalface_default.xml");

The get_images_and_labels function reads in each stored image, detects the face and obtains the id from the file name.

# Create method to get the images and label data
def get_images_and_labels(path):

  # Get all file path
  image_paths = [os.path.join(path,f) for f in os.listdir(path)] 

  # Initialize empty face sample
  face_samples=[]

  # Initialize empty id
  ids = []

  # Loop all the file path
  for image_path in image_paths:

    # The stored image is grayscale so read in in gray scale
    gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)

    # Get the image id
    id = int(os.path.split(image_path)[-1].split(".")[1])

    # Get the face from the training images
    # Don't need any scaling as these images already full face
    faces = detector.detectMultiScale(gray);

    # During testing not always detected face on image, which
    # is odd as it should be just an image that was saved
    if (len(faces) == 0):
      print "No face on " + image_path

    else:
      # We know each image is only of one face
      (x, y, w, h) = faces[0]

      # Add the image to face samples
      face_samples.append(gray[y:y+h,x:x+w])

      # Add the ID to IDs
      ids.append(id)

  # Pass the face array and IDs array
  return face_samples,ids

Once all the faces and ids are obtained they are passed to the OpenCV face recognizer and the data from the recognizer is saved to disk. The face recognition library that will be used by our node will later load this data to train the recognizer.

# Get the faces and IDs
faces,ids = get_images_and_labels('dataset')

# Train the model using the faces and IDs
recognizer.train(faces, np.array(ids))

# Save the model into trainer.yml
assure_path_exists('../trainer/')
recognizer.save('../trainer/trainer.yml')

print("Done")

The code for the ROS node itself is in the sub folder src in the file face_recognition_node.py. The code makes use of a library file, face_recognition_lib.py, which contains the class FaceRecognition. This file is in the sub folder src/face_recognition_lib.

Before describing the code for the node I'll discus the FaceRecognition class. After the required imports and the declaration of the class it defines a number of functions.

The class constructor creates the OpenCV face recognizer and then reads the training file created by the training script. It then opens the file containing the list of names and the ids, and creates the classifier. It finally stores a confidence value passed to it. This value will be used to determine if the suggested ID for the face is accepted.

def __init__(self, path, confidence):
  # Create Local Binary Patterns Histograms for face recognization
  self.__face_recognizer = cv2.face.LBPHFaceRecognizer_create()

  # Load the trained mode
  self.__face_recognizer.read(path + '/trainer/trainer.yml')

  # Load the names file
  with open(path + '/trainer/names.yml', 'r') as stream:
    self.__names_dict = yaml.load(stream)

  # Detect object in image using Haarcascade Frontal Face
  self.__face_detector = cv2.CascadeClassifier(path + '/classifiers/haarcascade_frontalface_default.xml')

  # Confidence level, the confidence of the system in recognising a face must be greater than
  # this level to be accepted by the system as a recognised face.
  self.__confidence_level = confidence

Two functions are declared which will be used to modify the captured image if a face is detected. The first will draw a rectangle on the image and the second will draw the supplied text on the image.

# Function to draw rectangle on image according to given (x, y) coordinates
# and the given width and height
def draw_rectangle(self, img, rect, bgr):
  (x, y, w, h) = rect
  cv2.rectangle(img, (x, y), (x+w, y+h), bgr, 4)

# Function to draw text on give image starting at the passed (x, y) coordinates.
def draw_text(self, img, text, x, y, bgr):
  cv2.putText(img, text, (x, y), cv2.FONT_HERSHEY_PLAIN, 3.0, bgr, 4)

The next function detect_faces, is used to detect any faces in the supplied image. It converts the image to grey scale so that OpenCV can be used to detect any faces. If faces are detected then the data for each face and a location of the face in the image is returned. Note that this part of the code is written to allow more than one face to be seen in the image.

# This function detects any faces using OpenCV from the supplied image
def detect_faces(self, img):
  face_data = []

  #convert the test image to gray image as opencv face detector expects gray images
  gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

  #let's detect multiscale (some images may be closer to camera than others) images
  #result is a list of faces
  faces_detected = self.__face_detector.detectMultiScale(gray, 1.3, 5);

  #if no faces are detected then return None
  if (len(faces_detected) == 0):
    return None, None

  #return only the face part of the image
  for face in faces_detected:
    (x, y, w, h) = face
    face_data.append(gray[y:y+w, x:x+h])

  # faces_detected is a list of rectangles where faces have been detected.
  # face_data is a list of the data for the faces detected

  return face_data, faces_detected

The final function in the class, scan_for_faces, is the one which will be called by the node to do the face detecting and recognition on the supplied image. This function  calls the detect_faces function and if any faces are detected it loops through each face calling the OpenCV face predictor which returns the id of the recognised face and a value indicating the confidence in the prediction. This value is converted to a percentage of confidence. From the supplied id a name is obtained. A rectangle is drawn around the face along with the name of the subject and the confidence level. If the confidence level of the prediction exceeds the stored threshold value the box and text drawn on the image will be in the colour green, otherwise in the colour red. Also if the confidence threshold is exceeded a dictionary entry of the id and name will be entered. Once all the faces detected have been analysed this dictionary is returned to the caller.

# This class function will be called from outside to scan the supplied img.
# First it attempts to detect faces in the image and then if any are found
# it attempts for recognise them against know subjects. It will adjust the
# supplied image.
def scan_for_faces(self, img):
  # First do the face detection, returned faces is a list of the faces detected
  # and rects is a list of rectangles of where the faces are in the image
  faces, rects = self.detect_faces(img)

  # Create a dictionary of IDs and Names of those detected in the image
  detected_dict = {}

  # If we detected faces then process each one
  if(faces != None):
    for index in range(len(faces)):
      # Predict the image using our face recognizer
      label, confidence = self.__face_recognizer.predict(faces[index])

      our_confidence = round(100 - confidence, 2)

      # Get name of respective label returned by face recognizer
      name_text = self.__names_dict[label]
      name_text_confidence = name_text + " {0:.2f}%".format(our_confidence)

      if(our_confidence > self.__confidence_level):
        colour = (0, 255, 0)
      else:
        colour = (0, 0, 255)

      #draw a rectangle around face(s) detected
      self.draw_rectangle(img, rects[index], colour)
      #draw name of predicted person(s) and the confidence value
      self.draw_text(img, name_text_confidence, rects[index,0], rects[index,1]-5, colour)

      if(our_confidence > self.__confidence_level):
        # Add details to the dictionary to be returned
        detected_dict[label]=name_text

  return detected_dict

Next I'll describe the ROS node itself.

The main routine initialises ROS for the node and creates an instance of FaceRecognitionNode and then calls spin to process the topics subscribed to and published.

def main(args):
    rospy.init_node('face_recognition_node', anonymous=False)
    frn = FaceRecognitionNode()
    rospy.loginfo("Face recognition node started")
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)

The rest of the file contains the FaceRecognitionNode class.

The constructor for FaceRecognitionNode registers that it will publish the topic face_recognition_node/adjusted_image which will be used to send the image with a box drawn around any faces recognised with the subject name and confidence level also printed on the image. This topic is used for testing the node. 

An instance of CVBridge is created, as previously stated, this will be used to convert the ROS image to an OpenCV image.

The node subscribes to the topic camera/image/raw which will contain the latest image from the camera, the function callback is registered to be called when a new image is received. 

Next the node subscribes and publishes two more topics. We could have used an action or even a service here but I opted to use two topics. We subscribe to face_recogntion_node/start which is an empty message type and will be used as a trigger to run the face recognition operation on the next image received from the camera. When the message arrives on the topic the function StartCallback will be called. The second topic, face_recognition_node/result, will contain the result of the attempt at face recognition. This topic contains a user defined message which is described later in this section.

The confidence threshold is read from the parameter server and is set to the default value of 20% if a value is not held in the parameter server.

Finally the constructor creates an instance of the FaceRecognition class described above.

class FaceRecognitionNode:

    def __init__(self):
        self.__image_pub = rospy.Publisher("face_recognition_node/adjusted_image", Image, queue_size=1)

        self.__bridge = CvBridge()
        self.__image_sub = rospy.Subscriber("camera/image/raw",Image, self.callback)

        # The calling node is probably on an action. Therefore we are going to use messages, one to
        # start the process and another to pass on the result.
        self.__command_sub = rospy.Subscriber("face_recognition_node/start",Empty, self.StartCallback)
        self.__response_pub = rospy.Publisher("face_recognition_node/result",face_recognition, queue_size=1)

        # Flag to indicate that we have been requested to use the next image
        self.__scan_next = False
        
        confidence_level = rospy.get_param('/face_rec_python/confidence_level', 20)
        rospy.loginfo("FaceRecognitionNode: Confidence level %s", str(confidence_level))
    
        # Create the face_recognition_lib class instance
        self.__frc = face_recognition_lib.FaceRecognition(roslib.packages.get_pkg_dir("face_recognition", required=True), confidence_level)

The function StartCallback is called when a message is received on the face_recogntion_node/start topic. The function simply sets a flag indicating that a request to start a face recognition operation has been requested.

# Callback for start command message
def StartCallback(self, data):
    # Indicate to use the next image for the scan
    self.__scan_next = True

The function callback is called each time a message is received on the camera/image/raw topic.

If the flag is set indicating that a request for a face recognition operation was made then the received image is converted from a ROS image to an OpenCV image. A call to the scan_for_faces is made to check the image for known faces. The adjusted image, which may now contain superimposed boxes and names is converted back to a ROS image and published on the face_recognition_node/adjusted_image topic.

The names and ids returned from the call to scan_for_faces is then used to create the message for the topic face_recognition_node/result, which is then published. If no faces were recognised the data in the message will be empty.

# Callback for new image received
def callback(self, data):
    if self.__scan_next == True:
        self.__scan_next = False
        # The image may show more than one face. Note that the supplied image
        # will be modified if faces are detected. The returned dictionary
        # will contain the unique IDs and Names of any subjects recognised.
        # If no detection/recognition dictionary will be empty
        image = self.__bridge.imgmsg_to_cv2(data, "bgr8")

        detected_dict = self.__frc.scan_for_faces(image)

        try:
            self.__image_pub.publish(self.__bridge.cv2_to_imgmsg(image, "bgr8"))
        except CvBridgeError as e:
            print(e)

        # Now post a message with the list of IDs and names
        ids = []
        names = []
        for k, v in detected_dict.items():
            ids.append(k)
            names.append(v)
        # Set the message to publish our custom message
        result = face_recognition()
        result.ids_detected = ids
        result.names_detected = names
        self.__response_pub.publish(result)

The node package also contains a config.yaml file which can be used to set the confidence level without having to recompile the code. The package also contains a launch file, test.launch, which can be used to test the node. As well as launching this node, it will launch the camera node and the node to republish the camera image in raw format.

Face recognition message and action

As stated above the face_recognition package uses a user defined message to return the results of an operation looking for known faces. The package face_recognition_msgs contains a message, face_recognition.msg, and our first ROS action scan_for_faces.action.

The files for the package are available in the face_recognition_msgs folder.

We covered the creation of user messages in part 1 of this article. As usual the message file is stored in the sub folder msg.

The message contains an array of ids and an array of names for any recognised faces.

uint16[] ids_detected
string[] names_detected

The file for the action is stored in the action sub folder. An action is used when a node is required to perform a non-blocking task which may take some time. A request is sent to perform the task and a reply is received once the task is complete. Feedback during the task can also be received. The task request can also be cancelled. Here I'll just describe the action specification for the task held in this package, the actual task client/server is described later in the article. That said it is worth noting that this action will be responsible for positioning the camera and requesting the face recognition operation on the current image.

More information on ROS actions can be found here.

An action specification contains a goal, result and feedback section. It looks similar to a message definition file except each of these parts is divided by the three dashes (---).

# This action scans by moving the head/camera for faces that are recognised
# There are no parameters to start the action
---
# Results of the final image/position scanned
face_recognition detected
---
# Percentage complete and result of the latest image/position scanned
float32 progress
face_recognition detected

Above the first three dashes is the goal. In our case we don't have any parameters for the goal, just the receipt of the goal will start our action.

The result parameters are below the first three dashes and in our case it is of type face_recognition which is defined in our  face_recognition.msg file. When the result of the action is returned all the faces detected will be contained in this parameter.

Below the second three dashes is the feedback parameters. In our case we will return a percentage complete of the number of face recognition operations we intend to run for the robots head movement range and the faces detected on the last individual scan.

We will run all the code together towards the end of the article but we can test our face detection node first.

Create a catkin workspace with the following terminal commands.

$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make

Copy the two package folders face_recognition and face_recognition_msgs into the ~/rodney_ws/src folder and then build the code. As a little tip I don't copy the code into the src folder but create a symbolic link in the src folder to the code location. That way I can have a number of workspaces using the same code files.

$ cd ~/rodney_ws/ 
$ catkin_make

Check that the build completes without any errors and then run the code.

$ cd ~/rodney_ws/
$ source devel/setup.bash
$ roslaunch face_recognition test.launch

With the nodes running on the Raspberry Pi I'm going to use a Linux workstation on the same network to run some test. Note: as we will use our user defined topics the code also needs to be built on this workstation. You can if you wish run the tests on the same Raspberry Pi running the system nodes.

At the workstation run the following to check that the nodes are running and connected to the correct topics. You can see the name of master in the output from running roslaunch. As I'm using the Ubiquity ROS Ubuntu image and have not changed the name my master is ubiquityrobot.

$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rqt_graph

If any topics have been misspelt in one part of the code then it will be obvious from the graph as the nodes will not be joined by the topics.

In another terminal enter the following in order to be able to view the images.

$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rqt_image_view

In the Image View GUI you can select the topic /camera/image/raw to view the current camera image. For the test I'm going to select the topic /face_recognition_node/adjusted_image, the image will currently be blank but when we request a face recognition operation we will be able to view the result.

In yet another terminal we will monitor the result topic with the following:

$ cd ~/rodney_ws/
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ source devel/setup.bash
$ rostopic echo /face_recognition_node/result

In the final terminal we will send a message on the /face_recognition_node/start topic to kick off a face recognition operation.

$ cd ~/rodney_ws/
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ source devel/setup.bash
$ rostopic pub -1 /face_recognition_node/start std_msgs/Empty

When I published the topic without anyone in view of the camera the image viewer displayed an image of the room and the rostopic echo terminal reported empty messages with:

ids_detected: []
names_detected: []
---

When published with myself in view of the camera the rostopic echo terminal and the image viewer displayed the following:

ids_detected: [1]
names_detected: [Phil]
---

When testing with two people in the image, it's trained for both these subjects, I got the following results.

ids_detected: [1, 2]
names_detected: [Phil, Dave]
---

Controlling the head

We now have a node to carry out the facial recognition operation on an image from the camera and from part 1 of this article we have the pan/tilt functionality to move the head/camera. The next step is to put the two together so that the robot can scan a room within its head movement range looking for faces it recognises.

Our ROS package for the node is called head_control and is available in the head_control folder. The package contains all the usual ROS files and folders.

The config folder contains a config.yaml file which can be used to override some of the default configuration values. You can configure:

  • the maximum and minimum range of movement in both pan and tilt directions
  • the angle that the pan and tilt device should move between image grabs
  • the step range the pan and tilt servos should move per request. This is to stop the servo moving a requested angle in one jump and causing the head to shudder
  • the pan/tilt position that the head should return to once a complete scan of the area is complete

The include/head_control and src folders contain the C++ code for the package. For this package we have one C++ class, HeadControlNode and a main routine contained within the head_control_node.cpp file.

The main routine informs ROS of our node, creates an instance of the class for the node and passes it the node handle and node name. For the first time in the project we are not going to hand total control of the node to ROS. In all our other nodes so far we have handed off control to ROS with a call to ros::spin in C++ or rospy.spin in our Python code. These calls hand control to ROS and our code only gets to run when ROS calls one of our callback functions when a message is received. In this node I want to retain control as we want to move the servos in small incremental steps to a target position. If we allow the servos to move a large angle in one go the head comes to a shuddering stop when the target position is reached. The way that we retain control is to call ros::spinOnce. In this function ROS will publish any topics we have requested and process any incoming topics by calling our callbacks but will then return control to main.

Before we enter the while loop we create an instance of ros::Rate and pass it the timing we would like to maintain, in our case I'm setting the rate to 10Hz. When inside the loop we call r.sleep, this Rate instance will attempt to keep the loop at 10Hz by accounting for the time used to complete the work in the loop.

Our loop will continue while the call to ros::ok returns true, it will return false when the node has finished shutting down e.g. when you press Ctrl-c on the keyboard.

In the loop we will call moveServo which is described later in the article.

int main(int argc, char **argv),
{
    ros::init(argc, argv, "head_control_node");
    ros::NodeHandle n;
    std::string node_name = ros::this_node::getName();
    HeadControlNode head_control(n, node_name);	
    ROS_INFO("%s started", node_name.c_str());
    
    // We need control of the node so that we can step the servos to the target 
    // position in small steps to stop the head shuddering if it was to move in one big step
    
    ros::Rate r(10); // 10Hz
    
    while(ros::ok())
    {
        // See if the servos need moving
        head_control.moveServo();
        
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}

The constructor for our class contains a little bit more than our previous node constructors, this is because we are going to make use of the scan_for_faces action we defined in the face_recognition_msgs package. In fact our node will include the action server for this action.

The call to the constructor also creates an instance of Server, as_ and a string, action_name_, to hold the name of the action. We then register our callback function, scanForFacesCallback, with the action server. This callback will be called when the action server receives the action goal, this in affect kicks off the action.

We then start the server running with the as_.start call.

The rest of the constructor is similar to our previous nodes. It subscribes to the topic /face_recognition_node/result in order to receive the result from the face recognition node, it advertises that it will publish the topic face_recognition_node/start which is used to instruct the face recognition node to start a face recognition operation. It also advertises that it will publish the topic pan_tilt_node/head_position which will be used to pass the required pan/tilt position to the pan/tilt node.

We then set some configuration defaults and read any overrides from the parameter server should they be available.

The action server we created sends feedback during the action which includes the percentage complete, in order to determine this we calculate the number of positions that will be used to scan the range of movement of the head and assign it to total_indv_scans_.

Finally we publish a message to move the head to a known starting point. We can't move to this position in small steps as we don't know our starting position after power up.

// Constructor 
HeadControlNode::HeadControlNode(ros::NodeHandle n, std::string name) : as_(n, name, false), action_name_(name)
{	
    nh_ = n;
	
    as_.registerGoalCallback(boost::bind(&HeadControlNode::scanForFacesCallback, this));
    as_.start();
  
    individual_scan_finished_sub_ =
        nh_.subscribe("/face_recognition_node/result", 1, &HeadControlNode::individualScanFinishedCallback, this);          
		
    // Topic to instruct the scan of the current image
    start_individual_scan_pub_ = nh_.advertise<std_msgs::Empty>("face_recognition_node/start", 1);
	
    // Topic to move head
    move_head_pub_ = nh_.advertise<servo_msgs::pan_tilt>("pan_tilt_node/head_position", 10);	
	
    pan_min_ = 0;       // Smallest servo angle for pan
    pan_max_ = 180;     // Maximum servo angle for pan
    tilt_min_ = 0;      // Smallest servo angle for tilt
    tilt_max_ = 180;    // Maximum servo angle for tilt  
    pan_step_ = 10;     // Maximum step movment for pan servo 
    tilt_step_ = 10;    // Maximum step movment for tilt servo 
    pan_view_step_ = 10;     // Angle to increase/decrease pan position by when scanning 
    tilt_view_step_ = 10;    // Angle to increase tilt position by when scanning 
	
    // Obtain any configuration values from the parameter server. If they don't exist use the defaults above
    nh_.param("/servo/index0/pan_min", pan_min_, pan_min_);
    nh_.param("/servo/index0/pan_max", pan_max_, pan_max_);
    nh_.param("/servo/index0/tilt_min", tilt_min_, tilt_min_);
    nh_.param("/servo/index0/tilt_max", tilt_max_, tilt_max_);
    nh_.param("/head/view_step/pan", pan_view_step_, pan_view_step_);
    nh_.param("/head/view_step/tilt", tilt_view_step_, tilt_view_step_);
    nh_.param("/head/max_step/pan", pan_step_, pan_step_);
    nh_.param("/head/max_step/tilt", tilt_step_, tilt_step_);
        
    int pan = 90;      // Pan position to return to once a task is complete
    int tilt = 45;     // Tilt position to return to once a task is complete
    nh_.param("/head/position/pan", pan, pan);    
    nh_.param("/head/position/tilt", tilt, tilt);
    default_position_.pan = (int)pan;
    default_position_.tilt = (int)tilt;
    
    // Calculate the total number of individual scans for % complete
    total_indv_scans_ = (((pan_max_ - pan_min_) / pan_view_step_) + 1) * (((tilt_max_ - tilt_min_) / tilt_view_step_) + 1) - 1;
	
    // This start position may be so the user can access the on screen keyboard. 
    // We will often return to this position when a task is completed	
    current_pan_tilt_ = default_position_;
    // We don't know where the servo starts from so just jump to the required position    
    // Publish a start position to get the head in a known position.
    move_head_pub_.publish(current_pan_tilt_);
    
    move_head_ = false;
    movement_complete_ = false;
}

I'll now briefly describe the functions that make up the class.

The scanForFacesCallback is called by the ROS when the action server receives a goal message. This message can contain data but in our case does not and is used to start a scan of the full range of movement of the head checking for faces that are recognised at each position.

The function calls the action server to inform it that the goal has been accepted, it clears a list that will be used to store the ids and names of people recognised and sets scans_complete_ to zero, this will be used in the percentage complete calculation. A starting position of the pan/tilt device is set and the variables used to control the smooth movement of the head are set and process_when_moved_ is set to indicate that an individual scan for faces should be conducted when the head reaches its target position.

// This callback is used to kick off the action to scan all positions
void HeadControlNode::scanForFacesCallback()
{
    face_recognition_msgs::scan_for_facesGoal::ConstPtr goal;
	
    goal = as_.acceptNewGoal();
    ROS_DEBUG("HeadControlNode: Got action goal");

    // Clear the list of those seen
    seen_list_.clear();
    
    scans_complete_ = 0;

    // Here we set up to move the head to the start position and then to request an 
    // individual scan when servo reaches that position
    
    // Set the target position to pan min and tilt min. 
    target_pan_tilt_.pan = pan_min_;
    target_pan_tilt_.tilt = tilt_min_;
	
    // Set the variable that says which direction the pan is going. Start by incrementing
    increase_pan_ = true;
    
    // State what we want to occur when the servo reaches the target position
    process_when_moved_ = requestScan;
	
    // Indicate that the servos should be moved
    move_head_ = true;
}

The individualScanFinishedCallback is called when the message with the result of an individual scan is received from the face recognition node.

An action can be preempted in order to cancel it during an operation. Our code checks to see if the action has been preempted and if so acknowledges the fact and request that the head be moved back to the default position.

If the action was not preempted it checks to see if all the range has been scanned or calculates the next position of the head.

We then add any people detected on the last individual scan to our list of seen individuals first ensuing that we have not already seen them before.

If all positions have been scanned the people seen are added to the result and sent to the action server with an indication that the action was completed successfully by the call to as_.setSucceeded. A request is then set up to move the head back to the default position.

If all the positions are not compete then the percentage complete is calculated and along with the result of the last individual scan is passed to the function publishFeedback. We then request that the head is moved and that another individual scan is conducted when the movement of the head is complete.

// This callback is used when the face recognition node sends the result back for an individual scan		
void HeadControlNode::individualScanFinishedCallback(const face_recognition_msgs::face_recognition& msg)
{
    bool all_areas_scanned = false;
    float percentage_complete;    

    scans_complete_++;
	
    if(as_.isPreemptRequested() || !ros::ok())
    {
        // Here we set up to move the head to the default position and then
        // to request preempted when servo reaches that position
        
        ROS_DEBUG("HeadControlNode: %s Preempted", action_name_.c_str());
        
        as_.setPreempted();
        
        // preempted, move to the neutral position
        target_pan_tilt_ = default_position_;
                    
        // State what we want to occur when the servo reaches the target position
        process_when_moved_ = nothing;
	
        // Indicate that the servos should be moved
        move_head_ = true;
    }
    else
    {
        // Scan of individual image complete
        // Calculate the next position of the head/camera
        if(increase_pan_ == true)
        {           
            if(target_pan_tilt_.pan == pan_max_)
            {
                // Last scan was at the edge, move tilt up and then pan the other way
                increase_pan_ = false;
                                	            
	            target_pan_tilt_.tilt += tilt_view_step_;
			
	            if(target_pan_tilt_.tilt > tilt_max_)
	            {
	                all_areas_scanned = true;                			
	            }
	        }                                
            else
            {            
	            target_pan_tilt_.pan = current_pan_tilt_.pan + pan_view_step_;
		
	            if(target_pan_tilt_.pan > pan_max_)
	            {          
	                // Moved out of range, put back on max                   
	                target_pan_tilt_.pan = pan_max_;
                }
	        }
        }
        else
        {
            if(target_pan_tilt_.pan == pan_min_)        
            {
                // Last scan was at the edge, move tilt up and then pan the other way
                increase_pan_ = true;
                
                target_pan_tilt_.tilt += tilt_view_step_;
			
	            if(target_pan_tilt_.tilt > tilt_max_)
	            {
	                all_areas_scanned = true;                			
	            }
	        }
	        else
	        {	                     
                target_pan_tilt_.pan = current_pan_tilt_.pan - pan_view_step_;
		
	            if(target_pan_tilt_.pan < pan_min_)
	            {
	                // Moved out of range, put back on min
        	        target_pan_tilt_.pan = pan_min_;
        	    }
	        }
        }
		   
	    // Add any faces seen to the stored list
	    if(msg.ids_detected.size() > 0)
        {  
            for(unsigned long x = 0; x < msg.ids_detected.size(); x++)
            {
                FaceSeen face_detected;
                face_detected.id = msg.ids_detected[x];
                face_detected.name = msg.names_detected[x];             

                if(haveWeSeenThisPerson(face_detected) == false)
                {                                    
                    ROS_DEBUG("HeadControlNode: I have seen %s", msg.names_detected[x].c_str());
                }
            }          
        }
	    
        if(all_areas_scanned == true)
        {              		    
		    // Note here in the result we send a list of all those people seen
		    
	        // Send action result, the seen list
	        face_recognition_msgs::scan_for_facesResult result;	        
	          	        
	        // Iterate through the faces seen adding the the result
	        for (std::list<FaceSeen>::const_iterator iterator = seen_list_.begin(), end = seen_list_.end(); iterator != end; ++iterator)
	        {	            	           	            
	            result.detected.ids_detected.push_back(iterator->id);
	            result.detected.names_detected.push_back(iterator->name);	            	       
	        }
		
            as_.setSucceeded(result);
            
            // Move the head/camera back to default position             
	        target_pan_tilt_ = default_position_;
	            
	        // State what we want to occur when the servo reaches the target position
            process_when_moved_ = nothing;
	
            // Indicate that the servos should be moved
            move_head_ = true;
        }
        else
        {        
	        // Calculate percentage complete
        	percentage_complete = ((float)scans_complete_ / (float)total_indv_scans_) * 100.0;
				    
		    // Note that here in the feedback we only send those just seen, even if seen before
		        
        	// Send feedback which includes the result of the last individual scan
	        publishFeedback(percentage_complete, msg); 	        
		
            // target pan/tilt position set above            	        
	            
	        // State what we want to occur when the servo reaches the target position (i.e. next scan)
            process_when_moved_ = requestScan;
	
            // Indicate that the servos should be moved
            move_head_ = true;
        }	
    }
}

The publishFeedback function is used to post feedback to the action server. Our feedback includes a percentage complete figure and the result of the last individual scan.

// This will be called after we receive the result of each individual scan except for the final scan 
void HeadControlNode::publishFeedback(float progress, face_recognition_msgs::face_recognition msg)
{
    face_recognition_msgs::scan_for_facesFeedback feedback;
    feedback.progress = progress;
    feedback.detected = msg;    
    as_.publishFeedback(feedback);
}

The haveWeSeenThisPerson function is used to check to see if any people recognised in the last individual scan have been seen already. If not they are added to the list of seen people.

// Function used to keep track of who has been seen
bool HeadControlNode::haveWeSeenThisPerson(FaceSeen face_detected)
{
    bool ret_val = true;

    // Is this person already in our list of people seen
    std::list<FaceSeen>::iterator it = std::find_if(seen_list_.begin(), seen_list_.end(),
    boost::bind(&FaceSeen::id, _1) == face_detected.id);

    if(it == seen_list_.end())
    {
        // Not seen before, add to seen list
        seen_list_.insert(it, face_detected);

        ret_val = false;
    }

    return ret_val;
}

The final function, moveServo, is the function called by main in our loop. It checks to see if a request to move the head was made.

If the target position was reached a check is made to see if a request for an individual scan should be published.

If the target position is not yet reached it calculates what movement of the servos should be made to move towards the target position and publishes a message to move the head.

// Function to move the servos if required by a step amount. This is to stop the head shuddering if the servo
// is moved to the target position in one movement. The function also carries out any process required when
// the servo reaches the target.
void HeadControlNode::moveServo()
{
    if(move_head_ == true)
    {
        // Still processing servo movement
        if(movement_complete_ == true)
        {
            // We have reached the target but may be giving time to settle
            loop_count_down_--;
            
            if(loop_count_down_ <= 0)
            {
                movement_complete_ = false;
                move_head_ = false;
                
                if(process_when_moved_ == requestScan)
                {
                    // Request the next individual scan for this position
                    std_msgs::Empty mt;
                    start_individual_scan_pub_.publish(mt);
                }
            }
        }
        else
        {
            if((target_pan_tilt_.pan == current_pan_tilt_.pan) && (target_pan_tilt_.tilt == current_pan_tilt_.tilt))
            {
                // Last time around we must have requested the final move
                movement_complete_ = true;
                if(process_when_moved_ == requestScan)
                {
                    // Allow time for head to steady before requesting scan of image
                    loop_count_down_ = 5;
                }
                else
                {
                    loop_count_down_ = 1;
                }
            }        
            else
            {                
                // Still moving, calculate pan movement
                if(std::abs(target_pan_tilt_.pan - current_pan_tilt_.pan) > pan_step_)
                {
                    // Distance to target to great to move in one go
                    if(target_pan_tilt_.pan > current_pan_tilt_.pan)
                    {
                        // Add the step to current
                        current_pan_tilt_.pan += pan_step_;
                    }
                    else
                    {
                        // Subtract step from current 
                        current_pan_tilt_.pan -= pan_step_;
                     }
                }
                else 
                {
                    // Can move to the target position in one go (or pan is in fact already there but tilt is not)
                    current_pan_tilt_.pan = target_pan_tilt_.pan;                                                            
                }
        
                // Calculate tilt movement
                if(std::abs(target_pan_tilt_.tilt - current_pan_tilt_.tilt) > tilt_step_)
                {
                    // Distance to target to great to move in one go
                    if(target_pan_tilt_.tilt > current_pan_tilt_.tilt)
                    {
                        // Add the step to current
                        current_pan_tilt_.tilt += tilt_step_;
                    }
                    else
                    {
                        // Subtract step from current 
                        current_pan_tilt_.tilt -= tilt_step_;
                     }
                }
                else 
                {
                    // Can move to the target position in one go (or tilt is in fact already there but pan is not)
                    current_pan_tilt_.tilt = target_pan_tilt_.tilt;                                                            
                }
                
                
                // Publish the movement
                move_head_pub_.publish(current_pan_tilt_);
            }
        }
    }
}

The package also includes a launch file in the launch folder. This file, test.launch, will launch all the nodes discussed in these first two parts of this article in order for us to test the system so far developed.

<?xml version="1.0" ?>
<launch>

  <rosparam command="load" file="$(find pan_tilt)/config/config.yaml" />
  <rosparam command="load" file="$(find face_recognition)/config/config.yaml" />
  <rosparam command="load" file="$(find head_control)/config/config.yaml" />
  
  <include file="$(find raspicam_node)/launch/camerav2_1280x960.launch" /> 
  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/raspicam_node/image/ raw out:=/camera/image/raw" />
  
  <node pkg="pan_tilt" type="pan_tilt_node" name="pan_tilt_node" output="screen">
    <remap from="pan_tilt_node/index0_position" to="pan_tilt_node/head_position"/>
  </node>
  <node pkg="face_recognition" type="face_recognition_node.py" name="face_recognition_node" output="screen"/>
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen" args="/dev/ttyUSB0"/>
  <node pkg="head_control" type="head_control_node" name="head_control_node" output="screen"/>
</launch>

Action client

Having included an action server in our node you would expect to have an action client to connect with it, this is definitely one method of communicating with the server. In the next part of the article I'm going to introduce a ROS package which will allow us to create state machines and sub-state machines to control our robot missions. Using this package it is possible to assign an individual state to be the action client and all the communication is done behind the scenes for us.

In order to test the system we have developed so far and to show how to write an action client we will write a test node here which will include an action client for our scan_for_faces action.

Our ROS package for the test node is called rodney_recognition_test and is available in the rodney_recognition_test folder. The package contains all the usual ROS files and folders.

The include/rodney_recognition_test and src folders contain the C++ code for the package. For this package we have one C++ class, RodneyRecognitionTestNode and a main routine contained within the rodney_recognition_test_node.cpp file.

The main routine informs ROS of our node, creates an instance of the class for the node and passes it the node handle, logs that the node has started and hands control to ROS with the call to ros::spin.

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rodney_test");
    ros::NodeHandle n;    
    RodneyRecognitionTestNode rodney_test_node(n);   
    std::string node_name = ros::this_node::getName();
	ROS_INFO("%s started", node_name.c_str());
    ros::spin();
    return 0;
}

The constructor creates an instance of our action client, ac_, and passes it the name of the action server which in our case is head_control_node. This must match the name we gave to our action server when we created it in the HeadControlNode constructor.

We are going to use a keyboard node which is available from https://github.com/lrse/ros-keyboard, to interact with the system. In the constructor we subscribe to the topic keyboard/keydown and call the function keyboardCallBack when a message is received on that topic.

The call ac_.waitForServer will wait in the constructor until our action server is running.

// Constructor 
RodneyRecognitionTestNode::RodneyRecognitionTestNode(ros::NodeHandle n) : ac_("head_control_node", true)
{
    nh_ = n;
    
    // Subscribe to receive keyboard input
    key_sub_ = nh_.subscribe("keyboard/keydown", 100, &RodneyRecognitionTestNode::keyboardCallBack, this);

    ROS_INFO("RodneyRecognitionTestNode: Waiting for action server to start");

    // wait for the action server to start
    ac_.waitForServer(); //will wait for infinite time
    
    scanning_ = false;

    ROS_INFO("RodneyRecognitionTestNode: Action server started"); 
}

The function keyboardCallBack checks the received message for one of two keys. If the lower case 's' is pressed we will start a complete scan using the robot head range of movement looking for faces recognised. It does this by creating an instance of our action goal and passes it to the action server with a call to ac_.sendGoal. With the call we pass three callback functions, 1) doneCB which is called when the action is completed 2) activeCB which is called when the action goes active and 3) feedbackCB which is called when the feedback on the progress of the action is received.

The action can be preempted, so if the lower case 'c' is pressed and scanning is in progress we will cancel the action with a call to ac_.cancelGoal.

void RodneyRecognitionTestNode::keyboardCallBack(const keyboard::Key::ConstPtr& msg)
{        
    // Check no modifiers apart from num lock is excepted
    if((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0)
    {
        // Lower case
        if(msg->code == keyboard::Key::KEY_s)
        {            
            // Lower case 's', start a complete scan looking for faces
            // Send a goal to the action
            face_recognition_msgs::scan_for_facesGoal goal;
        
            // Need boost::bind to pass in the 'this' pointer
            ac_.sendGoal(goal,
                boost::bind(&RodneyRecognitionTestNode::doneCB, this, _1, _2),
                boost::bind(&RodneyRecognitionTestNode::activeCB, this),                
                boost::bind(&RodneyRecognitionTestNode::feedbackCB, this, _1));                                   
        }
        else if(msg->code == keyboard::Key::KEY_c)
        {          
            // Lower case 'c', cancel scan if one is running
            if(scanning_ == true)
            {
                ac_.cancelGoal();
            }        
        }
        else
        {
            ;
        }
    }
}

The callback function activeCB is called when the action goes active, here we log the fact and set a member variable indicating that scanning is taking place.

// Called once when the goal becomes active
void RodneyRecognitionTestNode::activeCB()
{
    ROS_DEBUG("RodneyRecognitionTestNode: Goal just went active");
    
    scanning_ = true;
}

The callback function feedbackCB is called when feedback on the progress of the action is received. If you remember our feedback includes a percentage complete value and any faces seen on the last individual scan. Here we log the percentage complete and log any names of people just seen.

// Called every time feedback is received for the goal
void RodneyRecognitionTestNode::feedbackCB(const face_recognition_msgs::scan_for_facesFeedbackConstPtr& feedback)
{
    ROS_DEBUG("Got Feedback percentage complete %f", feedback->progress);    
    
    if(feedback->detected.ids_detected.size() > 0)
    {  
        for(unsigned long x = 0; x < feedback->detected.ids_detected.size(); x++)
        {
            // Log just seen
            ROS_INFO("RodneyRecognitionTestNode: Just seen %s", feedback->detected.names_detected[x].c_str());          
        }          
    }
}

The callback function donCB is called when the action is completed. The result data contains all the people seen during the complete scan of the head movement. Here we log a greeting to anyone seen.

// Called once when the goal completes
void RodneyRecognitionTestNode::doneCB(const actionlib::SimpleClientGoalState& state,
                        const face_recognition_msgs::scan_for_facesResultConstPtr& result)                 
{
    ROS_DEBUG("RodneyRecognitionTestNode: Finished in state [%s]", state.toString().c_str());
    scanning_ = false;    

    if(result->detected.ids_detected.size() > 0)
    {  
        for(unsigned long x = 0; x < result->detected.ids_detected.size(); x++)
        {
            // Log we have seen you now!
            ROS_INFO("RodneyRecognitionTestNode: Hello %s, how are you?", result->detected.names_detected[x].c_str());            
        }            
    }
}

Using the code

As previously when testing the code, I'm going to run the system code on the Raspberry Pi and the test code on a separate Linux workstation. The Raspberry Pi will also be connected to the Arduino nano which in turn is connected to the servos and running the sketch from part one of the article.

Building the ROS packages on the Pi

If not already done create a catkin workspace on the Raspberry Pi and initialise it with the following commands:

$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make

Copy the packages face_recognition, face_recognition_msgs, head_control, pan_tilt and servo_msgs into the ~/rodney_ws/src folder and then build the code with the following commands:

$ cd ~/rodney_ws/ 
$ catkin_make

Check that the build completes without any errors.

Building the ROS test packages on the workstation

You can build and run the test packages on the Raspberry Pi but I'm going to use a Linux workstation which is on the same network as the Pi.

Create a workspace with the following commands:

$ mkdir -p ~/test_ws/src 
$ cd ~/test_ws/ 
$ catkin_make

Copy the packages face_recognition_msgs, rodney_recognition_test and ros-keyboard  (from https://github.com/lrse/ros-keyboard) into the ~/test_ws/src folder and then build the code with the following commands:

$ cd ~/test_ws/ 
$ catkin_make

Check that the build completes without any errors.

Running the code

Now we are ready to run our code. With the Arduino connected to a USB port use the launch file to start the nodes with the following commands. If no master node is running in a system the launch command will also launch the master node, roscore:

$ cd ~/rodney_ws/
$ source devel/setup.bash
$ roslaunch head_control test.launch

In the terminal you should see:

  • a list of parameters now in the parameter server
  • a list of our nodes
  • the address of the master
  • log information from our code

Next I'm going to use rqt_graph and our test code to test the system. I'm going to run the code on the separate workstation which needs to know the address of the machine running the ROS master, if you are running the test code on the Raspberry Pi you can ignore the command about the location of the ROS master.

On the workstation run the following commands to start the keyboard node:

$ cd ~/test_ws 
$ source devel/setup.bash 
$ export ROS_MASTER_URI=http://ubiquityrobot:11311 
$ rosrun keyboard keyboard

On the workstation in a second terminal run the following commands to start our test node:

$ cd ~/test_ws 
$ source devel/setup.bash 
$ export ROS_MASTER_URI=http://ubiquityrobot:11311 
$ rosrun rodney_recognition_test rodney_recognition_test_node

In a third terminal run the following commands to start rqt_graph:

$ cd ~/test_ws
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rqt_graph

From the graph you should see the nodes for both the system and the test code running. You should also see the nodes linked by the topics. Any broken links is an indication of misspelt topics in the code.

The workstation should also be running a small window whose title is "ROS keyboard input". Make sure this window has the focus and then press the lower case 's' key. The head should start moving and scanning for faces it recognises as it goes. The terminal which you started the rodney_recognition_test_node in should report any faces it recognised for each individual scan. When all the scans are complete the head should return to the default position and a greeting for each individual seen will be displayed in the same terminal.

During a scan press the lower case 'c' key to cancel the action, the head should return to the default position.

Points of Interest

In this part of the article we have added the face recognition and overall control of the head to the code from part 1 to complete our Design Goal 1.

In the next article I'll give Rodney facial expressions, speech and introduce the state machine package that will be used to control the robot missions. This will complete Design Goal 2.

A short video showing Rodney running a test for Design Goal 1 and 2.

History

  • Initial Release: 2018/08/18

License

This article, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

Share

About the Author

Phil Hopley
Software Developer (Senior)
United Kingdom United Kingdom
Having spent the last 40 years as an engineer, 19 years as a test and commissioning engineer and 21 years as a software engineer, I have now retired to walk the Cumbrian fells and paddle the coast and lakes. When the weather is inclement I keep my hand in with robotic software and hardware. Over the years I have used Code Project to help me solve some programming issues so now with time on my hands it’s time to put something back into the Code Project.

You may also be interested in...

Pro

Comments and Discussions

 
PraiseAwesome :) Pin
AJSON22-Sep-18 3:31
mvpAJSON22-Sep-18 3:31 
GeneralRe: Awesome :) Pin
Phil Hopley20hrs 5mins ago
professionalPhil Hopley20hrs 5mins ago 

General General    News News    Suggestion Suggestion    Question Question    Bug Bug    Answer Answer    Joke Joke    Praise Praise    Rant Rant    Admin Admin   

Use Ctrl+Left/Right to switch messages, Ctrl+Up/Down to switch threads, Ctrl+Shift+Left/Right to switch pages.

Permalink | Advertise | Privacy | Cookies | Terms of Use | Mobile
Web06-2016 | 2.8.180920.1 | Last Updated 18 Aug 2018
Article Copyright 2018 by Phil Hopley
Everything else Copyright © CodeProject, 1999-2018
Layout: fixed | fluid