As we've already seen, the face tracker controller node is responsible for controlling the Dynamixel servo according to the face centroid position. Let's understand the code of this node, which is placed at face_tracker_control/src/face_tracker_controller.cpp.

The main ROS headers included in this code are as follows. Here, the Float64 header is used to hold the position value message to the controller:

    #include "ros/ros.h" 
    #include "std_msgs/Float64.h" 
    #include <iostream> 

The following variables hold the parameter values from servo_param.yaml:

    int servomaxx, servomin,screenmaxx, center_offset, center_left, 
    center_right; 
    float servo_step_distancex, current_pos_x; 

The following message headers of std_msgs::Float64 are for holding the initial and current positions of the controller, respectively. The controller only accepts this message type:

    std_msgs::Float64 initial_pose; 
    std_msgs::Float64 current_pose; 

This is the publisher handler for publishing the position commands to the controller:

    ros::Publisher dynamixel_control; 

Switching to the main() function of the code, you can see following lines of code. The first line is the subscriber of /face_centroid, which has the centroid value, and when a value comes to the topic, it will call the face_callback() function:

    ros::Subscriber number_subscriber =    
    node_obj.subscribe("/face_centroid",10,face_callback);

The following line will initialize the publisher handle in which the values are going to be published through the /pan_controller/command topic:

    dynamixel_control = node_obj.advertise<std_msgs::Float64>   
    ("/pan_controller/command",10);

The following code creates new limits around the actual center of image. This will be helpful for getting an approximated center point of the image:

      center_left = (screenmaxx / 2) - center_offset; 
      center_right = (screenmaxx / 2) + center_offset; 

Here is the callback function executed while receiving the centroid value coming through the /face_centroid topic. This callback also has the logic for moving the Dynamixel for each centroid value.

In the first section, the x value in the centroid is checking against center_left, and if it is in the left, it just increments the servo controller position. It will publish the current value only if the current position is inside the limit. If it is in the limit, then it will publish the current position to the controller. The logic is the same for the right side: if the face is in the right side of the image, it will decrement the controller position.

When the camera reaches the center of image, it will pause there and do nothing, and that is the thing we want too. This loop is repeated, and we will get a continuous tracking:

    void track_face(int x,int y) 
    { 
   
        if (x < (center_left)){ 
          current_pos_x += servo_step_distancex; 
          current_pose.data = current_pos_x; 
        if (current_pos_x < servomaxx and current_pos_x > servomin ){ 
           dynamixel_control.publish(current_pose); 
        } 
 
        } 
 
     
       else if(x > center_right){ 
    current_pos_x -= servo_step_distancex; 
    current_pose.data = current_pos_x; 
      if (current_pos_x < servomaxx and current_pos_x > servomin ){ 
            dynamixel_control.publish(current_pose); 
    } 
 
 
        } 
 
       else if(x > center_left and x < center_right){ 
 
    ; 
    } 
 
    }