## 3 Robot Functionalities: People Perception, Human Following and Pick and Placing
### Person Following  
The Person Following module requires that the People Perception component, described in Section 3.2, is working correctly and a person can be correctly detected, identified and his/her location can be accurately estimated. Our people following method is built on top of an autonomous waypoint navigation method previously developed for our robot [45]. The base navigation is the ROS navigation stack ([[http://wiki.ros.org/navigation](http://wiki.ros.org/navigation)]([http://wiki.ros.org/navigation](http://wiki.ros.org/navigation)), (accessed on 1 September 2020)) with AMCL ([[http://wiki.ros.org/amcl](http://wiki.ros.org/amcl)]([http://wiki.ros.org/amcl](http://wiki.ros.org/amcl)), (accessed on 15 September 2020)), a particle filter based localization implementation. For motion planning, a Dijkstra based global planner is used as well as the Dynamic Window Approach used as the local planner.  
Algorithm 2 describes the Person Following routine. After obtaining the position of the target person relative to the robot's base and, consequently, relative to the map's frame, the algorithm calculates the desired goal pose where the robot should be located at. This goal pose is then fed to the autonomous navigation stack that guides the robot to the target position taking the shortest path while also avoiding obstacles. If the target person moves positions, then the robot will track the movement and reapply the algorithm.  
Steps for calculating the goal pose ( lines 5 to 17 of the Algorithm 2) can be summarized in the following manner, where \(r\) is the desired distance that the robot should keep from the person:  
1. A circle of radius \(r\), number of points \(n_{pc}\) and density of points \(d_{p}\) is created around the target (\(n_{pc}=d_{p}r\)).  
Figure 2: Face recognition of an encountered person (on the **right**) from a pool of 6 known faces (on the **left**).  
All the points are ordered by the distance to the robot.  
The points are tested to check if they are free in the costmap.  
Check if there is any obstacle between the robot and the target.  
If there are points in this circle that satisfy these conditions, the closest point to the robot is used. If there is not, \(r\) increases and the algorithm returns to step 1.  
The orientation of the robot is chosen from the calculated position towards the person.  
```
Result: The robot follows a certain person. input:target_person_pose, robot_pose, map, r, \(d_{p}\)
1 begin
2 whilepeople_following is truedo
3 ifperson_moved(target_person_pose, old_person_pose)then
4 target_pose = o
5 whiletarget_pose = odo
6 circle[] = circle_creaton(target_person_pose, r, \(d_{p}\));
7 ordered_circle[] = order(circle[], robot_pose);
8 ordered_circle[] = check_availability(ordered_circle[], map);
9 ifordered_circle is not emptythen
10 target_pose = ordered_circle[first_element];
11 orientation = get_orientation(target_pose, target_person_pose);
12 break;
13
14
15
16
17 else
18r = r + increase_constant;
19
20
21 end if
22
23
24 end if
25 Move_To(target_pose, orientation);
26 old_person_pose = target_person_pose;
27
28 end if
29 rotate_head_towards(target_person_pose);
30
31 end if
32
33 end if
```  
**Algorithm 2**Person following algorithm  
Figure 3 illustrates an example of people following from an actual trial where the robot selects the best possible goal position while navigating through a narrow corridor.  
Figure 3: Person following example. The yellow sphere represents the person position, blue spheres represent the possible positions the robot could be at the desired distance to the person and the purple line is the path the robot chose from its position to the chosen target position.