Embedded Motion Control 2018 Group 1

Group Members

TU/e Number	Name	E-mail
1279602	Ahmed Hizar (A.H.) Ahmed Ajmal	a.h.ahmed.ajmal@student.tue.nl
0914013	Jari (J.J.) van Steen	j.j.v.steen@student.tue.nl
0924842	Nazar (N.) Rozkvas	n.rozkvas@student.tue.nl
1279491	Arpit (A.) Aggarwal	a.aggarwal@student.tue.nl
1031018	Willem (W.) Verhoeven	w.b.verhoeven@student.tue.nl

Escape room

This part describes all of our progress in the first challenge of the course, which is the Escape Room challenge.

Initial design

Before the software could be created that makes an escape possible, an initial design report was created to assist in the design of the software. This initial design report can be found here: Initial design report group 1. The most important points from this report will be given on the wiki page.

Requirements & Specifications

The first step in the design process is the creation of requirements for the software, which gives a way to express PICO's required behaviour in the Escape room challenge. To translate this into software, the requirements have to be specified, which is done via specifications. These specifications are used in the software design. Both the requirements and corresponding specifications are given here:

Requirements	Specifications
PICO has to avoid all walls at any time.	PICO should always stay always stay away from walls at least 15 centimeters.
	PICO should aim for an equal distance to the right and left when moving through a corridor between 0.5 and 1.5 meters.
PICO should complete its task well independent of the initial conditions.	PICO has to scan its surroundings with a frequency of at least 5 Hz while moving to prevent bumping into a wall.
PICO should operate fully autonomously.	The final software should not make use of teleoperation.
PICO cannot translate or rotate faster than prescribed.	The maximum translational velocity of PICO is 0.5 m/s and the maximum rotational velocity of PICO is 1.2 rad/s.
PICO has to deal with small imperfections in the real world it operates in.	An uncertainty margin of 0.05 meter should be present when building a world model to deal with imperfections in the real world.
The software should be easy to set up.	The software should be set up according to the information on the wiki page.
PICO has to cross the finish line as fast as possible within the maximum time of 5 minutes.
PICO needs to stop when it crosses the finish line.	PICO needs to stop after it has entered the corridor and there is more than 1 meter distance to the left and to the right
PICO has to identify the exit of the escape room.	An algorithm is created which ensures that PICO identifies an exit between 0.5 and 1.5 meters wide when facing the exit under an angle of 60 degrees or less.
PICO has to keep looking for the exit if it cannot find the exit.	PICO will drive towards the point furthest away from at that moment if the exit is not detected.

Escape strategy

To escape the room as fast as possible for this challenge, a plan has been created on geometric level that makes PICO escape as fast as possible. This plan will be described here by first explaining the plan at the highest level and explaining some of the steps in more detail afterwards.

Overlaying plan

The highest level of this plan will first be described by a simple step-by-step algorithm.

1. Initialize the sensors and actuators.
2. Scan surroundings from the given position and orientation.
3. Try detecting the exit.
4. IF exit is found: Drive towards exit and skip to step 8. ELSE: Turn 180 degrees and perform step 2 and 3 once again.
5. Try detecting the exit.
6. IF exit is found: Drive towards exit and skip to step 8. ELSE: Move into the direction of the furthest point for a quarter of this distance.
7. Return to step 2.
8. Identify the 2 corridor walls and orient the robot to face the exit, then drive forward while remaining in the center.
9. Stop once the finish line has been crossed.

The details of the steps performed in this plan are written below.

Detection plan

Some addition explanation on steps described in the overlaying plan will be described now, starting with the explanation on how to detect the exit and the data in the corridor of the maze.

Line fitting algorithm

As a groundwork of the detection methods, a line fitting algorithm has been created. This is required to localize the exit as well as detecting the walls of the corridor of the maze. This line fit will be based on saved data from the LRF that has been transformed to cartesian coordinates. The line fit algorithm tries to fit a line between 2 different points of the LRF data. To do this, the data between the first and last point is averaged into a smaller number of points. Between the first and last of these averaged points, a line is fitted, which is easy to do for 2 points. By checking that the root mean square of the distances from the other averaged points is smaller than a certain threshold, it is determined whether the initial points form a line. This threshold value is based on the total number of points and the number of averaged points. If this linefit does work, the mathematical formula of the line as y = ax + b can be passed.

Exit detection

A very important task in the overlaying plan is to detect the exit from a certain given orientation. The algorithm on how to do that, using the previously described line fitting algorithm, is described now. The exit detection starts by trying to make a line fit between the first LRF data point and around 30 points away. This number of 30 has been determined by looking at the size of the room and the total amount of LRF data points. If a line can be fit correctly between these points, it means that a wall is detected. If it cannot create a good line fit, no wall is detected.

When a wall has been found, the next step is to extrapolate this line, and check if the distance between this line and the points of the LRF data exceeds a certain threshold at some point for a minimum amount of points in a row. If this is the case, there is either a corner or the exit located here. If the distance from PICO to these points increases, this means that the exit is found, while a corner is found if the distance from PICO to these points decreases. If the exit is located, only one point of the exit is known. Therefore, the line will be extrapolated even more, until the LRF data gives new value that fit on the same line. When this happens, the second exit point has been found, and the location of the exit is fully known. The knowledge of finding a corner is largely ignored for this challenge, the only benefit of knowing the location of the corner is that a new linefit can be made starting at this corner point.

Corridor detection

After detection of the exit and being close enough to this detected exit, the algorithm for detection of the corridor walls is used to determine what the distance to the right and left walls is in the corridor. To do this, a line fit is created with LRF data on either side of PICO with the algorithm mentioned before. This line data is passed to the planning algorithm, which will determine what the distance of PICO to either side of the wall is. Once there are no more lines detected on either side of PICO, PICO has escaped the corridor and will automatically stop.

Furthest point detection

If PICO is not located in the corridor and cannot detect an exit, PICO should drive towards the furthest point. In order to do this, this furthest point is detected based of the values of the LRF with the additional requirement that there should be at least 5 values of the LRF data in a row that do not deviate too much from each other. This is done to prevent small openings or a single incorrect LRF measurement influencing the detection method.

Driving plan

Once the exit has been detected, a plan for driving towards the target needs to be made, this plan depends on the required task. The different driving tasks will be expanded upon here.

Driving towards exit

In order to drive towards the exit, the coordinate in the center of the two corner points of the corridor is used as a setpoint. PICO will drive towards this setpoint by turning until the exit is faced and after that, continue to drive forward in an open-loop controlled way. However, since errors can be made in this way, this process is continuously executed when finding the exit. This means that a loop will be executed in which the exit is continuously detected, after which PICO faces the exit and drives forward. This process is executed until PICO has reached a certain distance (20 centimeters) of the exit.

Driving through corridor

If the corridor is entered, a different driving plan applies, where the aim is to drive through the center of the wall. In this case, the detection algorithm provides the data of the two lines corresponding to the walls where PICO has to drive through. The next setpoint that PICO drives towards is a small distance (20 centimeters) in front of the current location, and is located in the center of the two walls. Apart from driving towards this point, PICO also aligns its orientation with the two walls. This process is looped until it has been detected that PICO escaped the corridor.

Driving towards furthest point

When no exit can be detected from the current position, PICO has to move towards the furthest point for one quarter of the total distance. This means PICO drives towards the point that is furthest away from the current position based on 2 scans, one from the initial orientation and one from the orientation after a turn of 180 degrees. This point is supplied as a setpoint to which PICO will drive in an open-loop controlled way. Opposite to the other two driving methods, this setpoint is not continuously updated, as the accuracy of moving exactly towards this setpoint is much less important. This means one simple setpoint is supplied, and the task is finished once driving towards this setpoint is finished.

Program structure

The program has been built according to the Initial Design (Initial design report group 1) having the following 3 structural blocks: Detection, Planning and Control. Each of these correspond to a separate class, connected via an additional class 'main'. The functionality of each class is described in more detail below.

State flags

Execution of the program is sequential, such that the methods are called one-by-one depending on the state of the robot. The states are controlled by boolean flags, which include:

• turn - a flag set to "true" by the planning block when no exit is detected and we wish to turn around to examine the other half of the room. It is unchecked when the control block completes the turn;

• drive_frw - a flag set to "true" by the planning block when exit is detected a PICO has to move in that direction or when no exit is detected and the robot should move in the direction of the furthest point detected;

• turned_once - a flag set to true by the planning block after the first turn in the case exit is not detected form the original orientation. This flag ensures that no continuous turning occurs. The value is set to "false" when drive_frw is set to "true";

• in_corridor - a flag set in the main function when the execution cycle for moving towards the exit it completed. This assumes that PICO is at the exit and a different algorithm for setpoints generation is executed in the planning block;

• escaped - a flag set by the detection block, when no reasonable data is detected after passing through the corridor. As this flag is turned to true, the program should stop executing.

Program classes

The whole program consisted of 4 C++ classes:

Main

• This is the main function where the program starts.
• It initializes objects that are required for the hardware components interaction and program execution (e.g. emc::Rate, emc::IO).
• It initializes the objects storing the sensor data, setpoint for the control loop and the furthest detected point. The later one has the same type as setpoints, however initialized separately since the furthest point is updated every iteration of the main while loop.
• Inside the while loop, the three classes are executed and are constantly updated (Detection, Planning, Control).

Detection

• This is the class that reads data from the sensors, filters it and does detection of primitive shapes and their interpretation. Output is wall line equation, corner points of the exit and one point for the furthest detected distance, with corresponding boolean flags
• Detection file includes methods to detect the walls, find exit and furthest point via line-fitting algorithm.
• The line fitting algorithm provides the equation of a line.
• Method to find exit compares the fitted lines with individual measurements, looking for large neighboring differences in the laser data.
• Method to find walls assumes that the data from the front and the sides of the robot should represent lines. This is verified by searching for laser data with distance closer than a specified value. It then fits the line over those points and spits them to the planning block.
• Method for the furthest point searches for the laser scan with the largest distance data.

Planning

• This is the class that interprets the data and sets the setpoints for movement. The output is relative angle to the setpoint and distance to it.
• It fully exploits the flag commands mentioned earlier and bases its decision on them.
• There are two algorithms between which, the distinction is done depending on the phase that the PICO is in.
• Room logic method checks all the data provided by the detection class and interprets it in destination points.
• A series of corridor following commands access the corridor data when the flag in_corridor is checked. It sets a setpoint based on the two parallel lines describing the walls, keeping the robot in the middle of the corridor

DriveControl

• This is the class that converts the setpoints to the commands to the actuators and tracks their execution.
• It heavily relies on the odometry data.
• Based on the flags turn or drive_frw it executes the functions for speed regulation till the odometry data states that PICO is at the desired position.

Simulation

With the program described above, PICO was able to escape the room in the simulation.

Escape room challenge

Unfortunately, the program for the Escape room challenge was not written perfectly and PICO have failed the challenge, not being able to find the exit. As it was found later, the reason for that is the different execution of the drive controls in the simulator and on the robot. The simulator was built in such a way, that PICO does not stop moving in a given direction till a command is changed. However, the real robot executed the drive commands only for a certain period of time and then it stops.

As a result, during the first operation stage facing the wall, it did not complete the full turn to scan the surroundings and proceeded to the next stage, moving to the furthest point. The furthest point was not selected properly and PICO have hit the wall.

Another conclusion that our group has made is that there should be a direct repulsion algorithm implemented such that in case PICO is located too close to the wall, the first command would be to move from the wall to the safe (predefined) distance and maintain it during the whole challenge. Only if this condition is achieved, the regular program can be run.

Hospital Room

For the final challenge, the task is to explore a Hospital, which consists of corridor and few rooms. PICO has to explore the rooms, return to the initial position, park backwards to the wall and as a second phase of the challenge, find an object that is located somewhere in the Hospital. It is given that the object is placed in a room, for which the most doors have to be entered from the initial position.

We expected a low of bugs when developing the program, therefore, it was decided to program our own visualizer. In the visualizer it is possible to display any data flowing in the program. It was proven to be especially useful for the developments of detection and planning algorithms.

I was proven to be a useful tool for debugging, since any unclear data structure could be displayed.

It is also used to show the working pieces of the program in the latter sections. It can be easily noticed by the LFR data represented with small red dots and large dots for point objects.

Requirements & Specifications

For the hospital room challenge, new requirements and specifications have to be set.

Requirements	Specifications
PICO has to avoid all walls at any time.	PICO should always stay always stay away from walls at least 15 centimeters.
PICO should operate fully autonomously.	The final software should not make use of teleoperation.
PICO cannot translate or rotate faster than prescribed.	The maximum translational velocity of PICO is 0.5 m/s and the maximum rotational velocity of PICO is 1.2 rad/s.
PICO has to deal with small imperfections in the real world it operates in.	An uncertainty margin of 0.05 meter should be present when building a world model to deal with imperfections in the real world.
	All detected features will be averaged over 10 scans to prevent false positives in the detection.
The software should be easy to set up.	The software should be set up according to the information on the wiki page.
PICO has to map the hospital and find the object in 10 minutes.
PICO needs to park backwards.	To park, PICO will turn around 0.3 meters before the wall and drive backwards until the control effort increases without any movement
PICO has to identify the openings (called: exits) and corners of the escape room.	An algorithm is created which ensures that PICO identifies corners and exits when facing the corner or exit under an angle of 60 degrees or less.
PICO has to map the hospital.	PICO will convert corners and exits that are locally detected to a global map.
PICO has to determine its own absolute position.	PICO will keep track of its global position based on odometry data, and tries to update the position using LRF data after a maximum of 20 centimeters that are driven.
The information in the worldmodel needs to be saved after finishing mapping.	A JSON file is created that saves the worldmodel.
PICO needs to find all rooms when mapping.	PICO will first drive through the corridor to identify all rooms, then inspects these rooms one by one and investigate all possible nested rooms.

Challenge strategy

Overlaying strategy

The Hospital challenge is going to be completed with the following strategy:

Exploration:

1. Move to the end of the corridor, meanwhile counting the number of doors in the corridor.
2. When reached the end of the corridor, enter the closest room.
   1. In the room (not in the entrance of the room), scan the surroundings with the laser.
   2. If 2 corners of the room are identified, turn approximately 180 degrees and scan the environment again. Use few scans to precisely identify the features of the room and their coordinates. Disregard features that are identified only once or twice over all the scans. (When scanning surroundings, PICO should be stationary). When scanning and identifying features for the room corners, also search for the doors.
   3. When the room is scanned, go to the unvisited exit (if such exists). In case there are few exits that are not leading to the hallway, go to the first unvisited room on the right from PICO.
   4. If there is only one exit, go through the exit.
3. Repeat the procedure for each room connected to the corridor. Write the data into the JSON file storing the information about the explored section of the hospital every time PICO returns to the hallway.
4. As all the rooms are visited, return to the initial position.
5. Park backwards. Park "blindly", based on the distance to the back wall, obtained before turning around. Also, implement control effort algorithm to prevent PICO from continuously driving into the wall.

Finding object:

1. Compute the destination to the furthest room as a stack of door coordinates that the robot has to enter. Use the known data about the hospital for accurate localization
2. When in the room with the object, search for any unreasonably close laser scan data and consider that as the object.
3. Move to the object and stop near it. Distance to the object is the same as the distance margin to the wall.

Detection

The detection strategy for the Hospital challenge was elaborated from the Escape Room challenge to account for the added complexity of the second challenge. As a result, some components of the detection strategy from the Escape room challenge were retained and improved, while the rest of the components were discarded due to their redundancy. The follow sections elaborate the new components employed in the detection strategy and the retained components from the previous challenge that were improved.

Retained and Improved Components

• Line fitting algorithm: The functioning of the line fit algorithm has remained the same from the Escape Room challenge, the description of the algorithm can be found Frog#Line fitting algorithm. The algorithm is still used to fit a line from the laser data, this line represents a wall of the hospital. The fitted line is then used as a base of comparison for laser data points which deviate from the fitted line, thereby indicating the presence of environment features.

• Closest point: The closest point has been reused from the Escape Room challenge. The closest point to PICO in relation to the environment is determined based on the shortest distance measurements across all the obtained laser data points. The closest point is then used to navigate PICO away from it's close proximity to the environment. This functions as an environment collision avoidance system.

• Exit detection: The exit detection algorithm still works in parallel with the line fitting algorithm but has been modified to account for multiple room entrances and exits in the Hospital challenge.

• Corner detection: The corner detection algorithm also still works in parallel with the line fitting algorithm but has been modified to save the corners instead of using it only to proceed with the next line fit.

New Components

The overall approach in detection for the Hospital challenge was to ensure the accuracy in the presence of environment features of the Hospital. Therefore, the aim was to eliminate the occurrence of false positive environment features detected by PICO. This was achieved by employing two stages given below:

• Right-Left (R-L) and Left-Right (L-R) Scanning: The laser range finder data is interpreted in two ways. Firstly, the data is interpreted starting from the first data point from the right to the last data point present on the left. Secondly, the data is interpreted starting from the first data point on the left to the last data point present on the right. Due to the functioning of our feature detection, this was done to ensure that features that could not detected from the R-L scan would be detected from the L-R scan and vice-versa.

• Feature averaging: Building on the base of R-L and L-R scanning, it was important to make feature detection more robust and reduce the presence of false-positive detected features. This was done by averaging the properties of features over multiple scans and comparing them to accurately ensure the presence of features.

Strategy

Corner Detection Algorithm

Corner Detection Algorithm

1. Filtering Laser Data: The laser data is filtered so as to remove erroneous data points that are highly inaccurate like PICO detecting itself.
2. Closest-point: The laser data point(s) with the shortest distance to PICO is obtained so that it can be used to avoid collisions with the environment.
3. Line-fitting: The line fitting algorithm takes data points at specified intervals and evaluates if the points can be fitted within a line within a distance threshold; if the points are within the threshold, a line is fitted between the set of data points which indicates the presence of a wall in the environment.
4. Left-Right (L-R) Corner-detection: The L-R corner-detection scan takes laser data points starting from the left most data point and ending with the right most data point from the available laser data and evaluates the distance of data points from the line fitted using the line-fitting algorithm. If the distances of a certain number of points deviate from the fitted line, the corner is detected. This can be visualized in the GIF image given within the section.
5. Right-Left (R-L) Corner-detection: The R-L corner-detection scan takes laser data points starting from the right most data point and ending with the left most data point from the available laser data and follows an identical procedure of the L-R corner detection given above.
6. Corners-averaging: To more accurately determine the position of a corner in the environment, an averaging algorithm is used. The averaging algorithm takes the corners detected over multiple scans and evaluates points within a specified distance threshold of each other. If multiple points are within the specified distance threshold of each other, these points are averaged to produce a even more accurate position of the actual corner point. This can be visualized in the GIF image provided within this section.
7. Left-Right (L-R) Exit/Entrance-detection: The L-R exit/entrance-detection scan takes laser data points starting from the left most data point and ending with the right most data point from the available laser data. It then evaluates the distance of the points from the line fitted if a line can be fitted. If in a fitted line, there are a certain number of laser points that deviate the fitted line and later continue to be in the vicinity of the fitted line, the first exit/entrance point is determined between where the first few points where the deviations begin and the second point is determined where the few points return to the vicinity of the fitted line.
8. Right-Left (R-L) Exit/Entrance-detection: The R-L exit/entrance-detection scan takes laser data points starting from the right most data point and ending with the left most data point from the available laser data and follows an identical procedure of the L-R exit/entrance-detection given above.
9. Exits-averaging: Similar to the corner averaging, to more accurately determine the position of exit points in the environment, an averaging algorithm is used. The algorithm determines the mid-point of two exit points detected in a scan; then calculates the mid-points of exit points from a number of following scans. If the mid-points of the exit points from the multiple scans are within a certain threshold distance of each other, an averaged mid-point is created. Using the averaged mid-point, the final averaged exit points are determined. This can be visualized in the GIF images provided within this section.
10. Averaging Left-Right and Right-Left Coincident Corners: In the event that corner points are detected in the same vicinity via both L-R and R-L methods, the distance average is taken between both the corner points and the resultant averaged corner point is determined. This is done to further increase the accuracy of the position of the corner point and to remove redundant corner points.
11. Averaging Left-Right and Right-Left Coincident Exits/Entrance: In the event that two exit/entrance points are detected in the vicinity of each other via both the L-R and R-L methods, the exit/entrance points are averaged and the resultant exit/entrance points are determined. This is done to further increase the accuracy of the position of the exit/entrance points and to remove redundant exit/entrance points.

Corner Point Averaging

Exit/Entrance Points Averaging

Mapping strategy

For the mapping strategy, it was decided not to use gmapping or an external library, but to program our own mapping algorithm. This strategy makes use of both odometry and LRF data, as odometry data on its own is too unreliable to trust. The map is a semantic map, consisting of a vector of rooms, where a room is defined as having 4 corners and 1 exit, which is the entrance of the room. The step by step strategy will be described now:

1. Initialize map with global PICO position in (0,0).
2. Perform detection algorithm to find corners and exits in local coordinates.
3. Convert these local coordinates into global coordinates using the global position and orientation of PICO.
4. Compare these global exit points and corner points to the points and exits that are already defined in the map.
   1. Update global location of PICO based on corners and exits close to previously found corners and exits.
   2. The remaining exits and corners not close to a previously defined exit or corner are added to the worldmodel based on PICO's global location.
5. Update map based on global position and remaining exits and corners.
   1. If a new exit has been found, this belongs to a new room, which means a room is added to the map.
   2. If a corner is found of which the angle does not lie between the 2 points of an exit, this corner belongs to the current room, which means that this corner point is added to the map as a part of the current room.
6. After updating the map is completed, PICO will drive towards a new setpoint based on the planning algorithm. Use the difference in odometry data before and after driving to calculate the estimated global position of PICO after driving.
7. Repeat from step 2.

It can be concluded from this description that indeed, both odometry data and LRF data are used to generate the map. An initial guess of the global position is determined through the old global position and the difference in odometry data between 2 consecutive points. This guess is updated with LRF data as described, a clearification of this method is shown in the gif-file below.

In this mapping strategy, it was chosen not to update the position of previously defined corners and exits, but only to use these points to update PICO. This was chosen because this is the most simple solution to fix the inaccuracies of the odometry data. If more time was available for the project, a different algorithm would have been chosen, which also updates the location of the previous points by keeping track of the certainty of the location of an exit/corner instead of just setting the exit/corner as a fixed value. This happens when an algorithm is used that makes use of a Kalman filter to identify the location of an object. However, this was too complex to program ourselves. To make sure that the current strategy can work, the detection mapping has been expanded with feature averaging, which filters all incorrectly identified errors and corners. This has been tested and worked very well, which makes it possible to map the hospitol according to this fairly simple strategy.

Mapping algorithm explanation

Driving/Planning strategy

The planning algorithm uses multiple setpoints where PICO drives to. The method to calculate a new setpoint depends on the phase of the 'overlaying strategy' where PICO is in, for example, when exploring the room low level detection data is used, while when moving through the known rooms, the known information from the previously created map is used. The strategy is fairly simple: first, one of the methods to determine the ultimate local destination is executed, producing the final setpoint for the current high strategy phase. Then, the local destination for PICO to travel for the current execution cycle is set. It always has a fixed distance with the angle in the direction to the high state phase setpoint. The new setpoint of PICO is always set to a distance of 0.2 meters away from the current position of PICO.

Assumptions

It is important to explicitly state the assumptions to the problem to define the working space of the object. From the description of the challenge the following assumptions were made:

• PICO always starts in the hallway
• Rooms might have nested rooms (entrance to a room is via another room)

Next, from the Exploration strategy described in one of the lower sections, the following assumptions are added:

Comparison with the Escape room challenge

The project in general consists of two challenges to test the PICO. Our group considered the challenges as highly interconnected in terms of developments sequence. The Escape room is viewed as a playground for the basic functions that are to be incorporated in the final Hospital Room challenge. It is clear, that in the Hospital challenge, some of the tasks are the same as in the Escape room challenge. Namely, the robot will have to identify the room properties (some of them are the same as in the Escape room challenge) and exit or "escape" the room, just as it have done in the first challenge. Therefore, it was decided to reuse as many developments from the previous part, as possible.

In this section, we define what are the changes comparing to the Escape room and what algorithms are reused.

Reused methods from the Escape room challenge

First of all, we maintain the detection algorithms for lines and objects such as exits of the room. The detection methods were proven to work for the previous challenge, however, needed more calibration for certain conditions. We also reuse the objects that are passed around the program that concern detection and setpoint planning. These are structures Exit, Point, Destination. The functions for the planning are also kept, however, the logic is reorganized for clearer code. Finally, the Control module has remained unchanged, since it was proven to work and direct the robot to a defined setpoint expressed as a turn angle and covered distance.

Innovations relative to the Escape room challenge

Comparing to the intermediate challenge (Escape room), there are few innovations that are employed in the program. These include both: new methods required to complete the challenge and new structure of the program execution. The structural changes mostly relate to the execution order and switching phases of the program.

First of all, since PICO has to perform repetitive tasks such as room exploration, passing through the doors in corridor or in the separate rooms, a State machine was added to the program. The State machine has 2-level states: high states for execution of general high tasks such as exploring the hospital, returning to the initial position, etc and low states that concern action planning within rooms or the corridor.

High states are:

• EXPLORE_HOSPITAL - high state that allows execution of all the low states for exploration phase (identification of the rooms, corridor).
• RETURN_TO_INIT - high state that changes the objective of the first part of the challenge from exploration to returning to the initial position.
• GO_TO_ROOM - the state that symbolizes execution of the second part of the challenge .

Low states are corridor-related and room-related:

• EXPLORE_CORRIDOR - this is the command that is first executed to go to the end of the hallway, counting the number of doors in the corridor. As it is mentioned previously, it is assumed that PICO always starts there. Therefore, this is also the initialized state.
• GO_TO_START - state that is active after all the rooms in the corridor (and, respectively, nested rooms) are explored and stored and the robot has to return to the starting point.
• PARKING - state to park backwards at the end of the first part of the challenge.

• EXIT_TO_PREV_ROOM - state that commands to go to the room one nesting level lower, e.g. exiting a nested room.

• EXPLORE_ROOM - state that enables execution of the room identification, which includes detection of features and mapping of the environment.
• GO_TO_NEXT_ROOM - state that is activated if there are more than one exit in a room, since according to the challenge strategy, we do not return to the hallway until all the nested rooms are fully explored.
• GO_INSIDE_ROOM - active when the actual movement through a door is happening. Initially, it was mean for the entrance based on the walls defining the exit, but later it was changed to just movement straight, parallel to the exit corners. This change was made because the walls are not thick enough to implement entrance algorithm similar to the one used in the Escape room.

• STAND_NEXT_TO_OBJECT - the state that commands PICO to go to the object if one is detected in the second part of the challenge.

Program structure

The structure of the program was changed comparing to the escape room challenge. As it is shown on the picture on the right, there are few structural important blocks added. These are State machine, Monitoring, Interpretation, World model and JSON file block. Below we explain the role and the importance of each block.

• State machine - the block that regulates the progress of the PICO in the challenge. Based on the states defined earlier, the activity in all other blocks is regulated.
• Monitoring - it is the block that incorporates all the logic within the program. It is operated based on the state that the robot is in and decides for the routine to be called in order to complete the task.
• Interpretation/Mapping. Since the detection is outputting labeled features such as corners or exits, it is required to group those appropriately in order for other block to use the data. The Interpretation block is responsible for creation of the objects and their conversion to the absolute coordinates.
• World model - it represents a large database with the information about the environment. The World model includes information both, about the progress of the challenge (how many rooms have been visited, how many nested rooms there are, location of the PICO in general terms like room, corridor, nested room, current state of the State machine, etc.) and about the features of the place that it is currently in (room or corridor features). All the other blocks are communicating with the World model to retrieve information or to write it. The World model does not include any logic regarding the operations. It also stores the sensor data (laser and odometry) as these are called by Detection, Mapping, Control and the Visualizer. Although it was advised not to do so, but to reduce redundancy, this seemed the best fit. Also, it made the collective {World Model, JSON file, Sensor Data} into the main database of the software.
• Planning - the block which generates a set point where PICO should drive to according to the STATE it is in and the data from the world model.
• Control - it gives a control input to PICO.
• JSON file - it is chosen to use the JSON file to store the map in. It makes the program variable storage clear by separating the unused or irrelevant information from the main section of the program. It is also may be used to view the progress of the mapping over time. The structure of the JSON file can be seen in the Code Snippets.

The rest of the blocks keep their role in the software architecture from the Escape room challenge, but are extended with new functionalities. It was made sure that each of the components only communicate with the World Model while exposing only one of their methods to the main program of the software. The rest of the functionalities were always private, except for the World Model which had the most exposed functionalities to communicate with all the blocks.

Software Architecture

Program details

Main files

Since the Hospital challenge consisted out of 2 parts: exploration and object search, 2 different main functions were defined. These are main_exploration.cpp and main_search.cpp The general structure of the two main functions was the same, except of the initial conditions that are set in the beginning of the program. The initialization for the exploration part stated that the first state is EXPLORE_CORRIDOR and set the necessary components of the World Model. The main_search.cpp, in turn, first parsed the JSON file to fill in the information based on the previous exploration and defined the Initial state and EXIT_CORRIDOR.

For the rest, both main functions have the following execution order in a while loop:

1. Update the detection information.
2. If there is a point detected that is too close to PICO, a method from planning is called to get away from the wall.
3. If no such point detected, proceed to monitoring function.
4. Update the states in the State Machine.
5. Call drive function
6. Check whether the program has to be stopped if the part of the challenge is completed (based on state machine)

World model

The WorldModel is used to store all data from the world which are used by other classes of the program. All the data which is stored inside the WorldModel is initialized when the program starts running. While running the program the data of the WorldModel is updated by different classes of different intervals. Only the data is saved inside the WorldModel which is necessary for other functions to work properly. A summary of the data which is stored inside the WorldModel is given below:

• globalRooms - This is a vector of all the rooms which are explored or unexplored. The Mapping class can add new rooms to this vector when there is an 'new' exit detected. An exit is determined to be 'new' when there is no room in the vector which has this exit as an property. The globalRooms vector is used to determine to which room PICO has to drive, depending on the State.

• allDetectedExits - This is a vector of exits which are currently detected by PICO. The detection class can set the vector of AllDetectedExits every time it performs a detection-scan. The data from the vector is used by the Mapping class to add exit properties to the rooms and the Planning class, when moving through an exit.

• allDetectedCorners - This is a vector of corners which are currenly detected by PICO. The detection class can set the vector of AllDetectedCorners every time it performs a detection-scan. The data from the vector is used by the Mapping class to add corner properties to the rooms.

• closestPointWall - This is a point, which consist of a (x,y) coordinate, an angle and a distance to the closest point from PICO. This point is set by the Detection class. The closestPointWall is used by the overruling WallDetection function and also the Planning class.

• destination - This is a point where PICO should drive to. The destination is set by the Planning class. It depends on the High/Low State what the destination will be. The destination is used by the DriveControl class, which let PICO move to the destination point.

• globalPosition - This is the current global location of PICO inside the hospital. The current location of PICO is set by the Mapping class. The Planning class uses the current location of PICO.

• currenHighState - This is the current High State where PICO is in. The High State is set by the State Machine class. The Monitoring class is using the High State to determine which planning functions needs to be executed.

• currenLowState - This is the current Low State where PICO is in. The Low State is set by the State Machine class. The Monitoring class is using the Low State to determine which planning functions needs to be executed.

• currentRoom - This is the room where PICO is currently in. The current room is changed by the StateMachine, when exiting or entering a certain room. The data of the currentRoom is used by the Planning class.

• jsonObject - This is the Json file which consists the map of the hospital. The Json file is updated by the Mapping class. The Json file is saved and shows a map of the whole hospital.

State machine

The state machine is one of the crucial components that define the execution of the program. It includes the states evolution and location changes. Based on these parameters, other classes of the program can regulate their activity, for example, decision for a planning algorithm can be made (move further in a room/corridor or go through a door to enter another room).

The transition of the states is the following:

When exploring the Hospital:

• If the location of PICO is IN_CORRIDOR:
  • Check whether the end of the corridor was reached once. If it was, never return to the EXPLORE_CORRIDOR state, needed to count the number of doors in the corridor.
  • When reached the end of the corridor once, GO_TO_NEXT_ROOM.
  • When at the entrance of the room, change to GO_INSIDE_ROOM for one cycle (given that the robot moves the same distance every time, it can be assumed that one cycle is enough to enter the room). Then change the location to IN_ROOM and state to EXPLORE_ROOM.
  • If number of explored rooms, connected to the corridor is the same as number of doors n the corridor, then change the state to RETURN_TO_INIT.

• If the location of PICO is IN_ROOM:
  • If the number of the detected room corners (besides exits) is 4, stop exploration and move to a different room/corridor.
  • If there are more doors besides the entrance it came from, GO_TO_NEXT_ROOM; otherwise, EXIT_TO_PREV_ROOM.
  • Similar to corridor exiting procedure with GO_INSIDE_ROOM and GO_TO_NEXT_ROOM (now also EXIT_TO_PREV_ROOM) applies for decision-making inside the room.

When returning to initial position:

• PICO is always located in the corridor when the state switched to RETURN_TO_INIT and GO_TO_START.
• When the global position of PICO is (0,0) with the margin of distance iteration set-point, switch to PARKING.
• PARKING state is executed for one cycle. Also the boolean to end the program is set to be true while setting to the state.

When searching for the object:

• First, GO_TO_NEXT_ROOM. When the distance to the entrance to the first room is closer than the constant set-point distance, switch to the GO_INSIDE_ROOM.
• When GO_INSIDE_ROOM is completed for one iteration, change the state to GO_INSIDE_ROOM.
• Every time entering a room, scan for an object. If an object is found, change the state to STAND_NEXT_TO_OBJECT. If close enough, also set the end of the program to true.

For an interested one, a clean snippet of the state switching is provided in the end of the page.

Monitoring

Monitoring is the function that chooses between the Planning block methods to execute based on the current state. This class does not introduce any new information, but rather serves as an structuring block for the planning methods.

Detection

The detection program follows the strategy that has been described previously. This is all programmed in 1 class, which consists all of the methods to find the averaged corners and exits as previously described, as well as a method to locally detect the closest point, such that a collision with the walls can be prevented at all times. This low-level detection is necessary apart from the high-level detection and mapping, since the high-level map and detection cannot be trusted to identify the walls accurately all the time.

The code is built upon a few structs, the main ones being Exit and Corner, which describe the position of the corners and exits in local coordinates. One main function can be called, which executes the entire detection procedure including feature averaging as described earlier, while a separate function can be called for the low-level detection to stay away from walls.

In the first version of the detection file, a lot of magic numbers have been used. For example, the amount of points needed for a line fit, the threshold of the RMS error when a line fit is seen as a line, the maximum distance of a point to a line before it is considered not to be on that line and many more. At a certain point, it was noticed that this may not have been the right thing to do, as the code became unscalable in the end, where there were a lot of values without any meaning that were obtained merely by tuning. An effort has been made to improve this. However, in the final code, there are still a lot of parameters that have been tuned and have values which are not purely based on properties of PICO or the environment. For later projects, it may be smart to avoid these situations as much as possible from the start.

Detection Simulation.

Mapping

The mapping procedure is performed as described in the mapping strategy section. The map is defined as a vector of rooms, where a room is a struct containing one exit (the entrance to the room), a vector of corners, the room ID and the ID of the previous room, so that it is known which rooms connect to which rooms, which can be used by the planning algorithm. The use of vectors is preferred over the use of arrays since this allows for dynamic allocation of the amount of rooms. The mapping procedure is performed as described in the mapping strategy section. The map is defined as a vector of rooms, where a room is a struct containing one exit (the entrance to the room), a vector of corners, the room ID and the ID of the previous room, so that it is known which rooms connect to which rooms, which can be used by the planning algorithm. The use of vectors is preferred over the use of arrays since this allows for dynamic allocation of the amount of rooms. Apart from this map, the other thing that mapping does is updating the position of PICO that has been stored in the worldmodel as a Position struct. This is done through an update of the odometry data and the new detection scan as described previously.

Planning

The planning algorithm generates an angle and a distance to the new set-point, this is also the input for the Drive Control class. Depending on the current High- or Low State of PICO, information from the WorldModel is used to generate a new setpoint. When a wall which is too close to PICO is detected an overruling function is executed, which is the following

• getAwayFromWall - In case PICO is in the Low State EXPLORE_CORRIDOR the wall is avoided by letting PICO side-drive away from the wall. Using this method PICO will always face the end of the corridor in this state. Also when the wall is at the side of PICO the side-drive method is used to avoid the wall. When the wall is at the front of PICO he turn away from the wall, without driving.

The overrulling function 'getAwayFromWall' is always run if there is a object/wall detected which is too close, independent in which State PICO is currently. After the 'getAwayFromWall' function is executed PICO continues creating setpoints for the State where PICO was in before the 'getAwallFromWall' function was excecuted. Below is described how the setpoints of each State are generated by the Planning block

EXPLORE_HOSPITAL

• EXPLORE_CORRIDOR - The new setpoint is always placed in front of PICO, without letting PICO turn. The overrulling 'getAwayFromWall' makes sure PICO stays inside the corridor and does not hit any walls.

• GO_TO_NEXT_ROOM - First the closest unexplored room is obtained from the WorldModel. This room has two exits. The middle point of the two exit points is calculated. Since the mapping data is used to obtain this information it can be possible the exit points are not accurate. That is why the final setpoint is placed in front of the exit (according to the mapping data).

• GO_INSIDE_ROOM - The real-time detection data is used to detect exit points of the room. These exit points are compared to the exit points of the closest unexplored room (according to the mapping data). When there is verified whether the real-time exit point is of the closest unexplored room PICO drives through the exit. The final setpoint is in the middle of the two exit points (according to real-time data).

• EXPLORE_ROOM - PICO drives forward until there is a wall in front of PICO which is too close, then PICO turns around.

• EXIT_PREV_ROOM - First the room where PICO is currently in is obtained from the WorldModel. This room has two exits. The middle point of the two exit points is calculated. Since the mapping data is used to obtain this information it can be possible the exit points are not accurate. That is why the final setpoint is placed in front of the exit (according to the mapping data).

RETURN_TO_INIT

• EXIT_TO_PREV_ROOM - First the room where PICO is currently in is obtained from the WorldModel. This room has two exits. The middle point of the two exit points is calculated. Since the mapping data is used to obtain this information it can be possible the exit points are not accurate. That is why the final setpoint is placed in front of the exit (according to the mapping data).

• GO_TO_START - Since the starting position of PICO was (0,0), PICO drives to this point.

• PARKING - PICO turns around, when slowly drives backwards until he hits the back wall of the corridor.

GO_TO_ROOM

• GO_TO_NEXT_ROOM - The next room where PICO has to drive to is obtained from the WorldModel. Since it is known in which room the object is placed, also the next room where PICO has to drive to is known. This room has two exits. The middle point of the two exit points is calculated. Since the mapping data is used to obtain this information it can be possible the exit points are not accurate. That is why the final setpoint is placed in front of the exit (according to the mapping data).

• GO_INSIDE_ROOM - The real-time detection data is used to detect exit points of the room. These exit points are compared to the exit points of the closest unexplored room (according to the mapping data). When there is verified whether the real-time exit point is of the closest unexplored room PICO drives through the exit. The final setpoint is in the middle of the two exit points (according to real-time data).

• STAND_NEXT_TO_OBJECT - The setpoint is set to the middle of the room, according to the mapping data. In case there is a closest point (according to real-time data) which is closer to PICO then where the walls should be, this must be the object. PICO drives to this point.

Drive control

Drive control class remained the same functionality as for the Escape room challenge. It is still getting a destination point in terms of distance and angle to it and drives for a fixed pre-defined distance in the direction of the point. It was improved, though, comparing to the Escape room in terms of reference tracking.

Previously, PICO could have exceeded the reference estpoint distance and angle parameters, since reference tracking was based on global coordinates. For the Escape room challenge, PICO first checked the odometry before moving and defined the desired final angle/distance in these global coordinates. When it reached the value, it stopped the movement. Such an approach worked well only for the first one or two destination commands, while making large errors later due to the global coordinates.

For the Hospital challenge, PICO's drive control class changed the reference tracking method, reseting the position and angle before the movement to 0. Again, the robot was moving till the reference is reached, however, now in local coordinates with 0 initial parameters. This resulted in accurate movement at any iteration of the program.

A simulation of the above mentioned method is provided in the animation below. The yellow dot represents a destination setpoint given to the drive control functions. It is shown how the PICO follows the commands one after another.

Drive Control Simulation.

Testing against critical situations

While deciding on the strategy to complete the challenge, a brainstorm session was organized to think through practical aspects that might prevent us from completing the challenge. A some of these and their solutions are presented below.

Starting position

---

Problem 1: If PICO is exiting a nested room and comes back to the main room, how to ensure that it does not explore the room the second time and does not create a new object?

Solution: Among the other features of the room, it should also include a flag whether the room was previously explored or not.

Problem 2: How to ensure that the PICO is driving to the end of the corridor in the very first phase? What is the target point to move to?

Solution: The detection class also should output the point that is straight ahead of PICO.

Identification of rooms and corridor

---

Problem 1: A complication that could have caused a failure in our program is the initial location of the PICO in the Hospital. It is, indeed, given that PICO is located in the corridor at the start of the challenge, however, it is not specified whether the corridor has all the doors along a straight line from the initial position. For example, the robot might start in the appendix of the corridor, that has a only one "door" according to our conventions (corridor is the room that we start in and it ends in the opposite wall from the initial position). Since write-to JSON file only happens after we return to the corridor, how to store the data about the visited rooms efficiently without overwriting it?

Solution: Use a stack feature to store the properties of the "main room" (which is meant to be a corridor) such as number of doors. Visit each nested room and use a separate stack for those.

Problem 2: How to deal with T-junctions in the corridors?

Solution: Same approach as with the previous problem can be applied. The corridor is considered to be a room and the actual rooms become nested rooms in this interpretation. If by the end of the exploration, PICO has 2 rooms with the same number of doors to be passed to reach that room, then it has to choose one and if the object is not found, a new path is constructed to the correct room.

Problem 3: Should rooms and corridor have different structures? Or is there a way to merge both of them into a single structure to simplify the software architecture?

Solution: Initialize the corridor as a room with Room ID as 0 and Previous Room as -1. Define it's Exits as (0,0) and (0,0) as after the completion of all exploration, PICO must return to its starting position, i.e., exit the corridor.

Detection

---

Problem 1: In an earlier version of the detection algorithm, exits were checked by fitting lines and searching for the deviation from right to left. However, for some exits, this algorithm cannot work, as the exit is a deviation in a line from left to right, while a perpendicular line is present to mark the other exit point. For other situations, not enough points are present to fit a good line from right to left.

Solution: The exits and corners should not only be detected from right to left, but also from left to right to ensure that all exits and corners are found. If an exit or corner detected from right to left is close enough to an exit or corner detected from left to right, the average of these two exits/corners is saved inside the worldmodel.

Problem 2: The detection algorithm was not always correct and sometimes gave false positives in detection of exits and corners. These false positives should not be saved in the worldmodel to prevent an erroneous map.

Solution: Instead of immediately saving the data of 1 scan into the worldmodel, PICO comes to a complete stop to do 10 scans in a row and evaluate the corners and exits of each scan. If at least 5 corners or 5 exits are located close enough to each other, these corners/exits are averaged and are saved in the worldmodel.

Mapping

---

Problem 1: The mapping algorithm should keep track of PICO's position while mapping, since this is needed to determine convert the globally defined exits into local exits, but the odometry data is too unreliable to do so.

Solution: An algorithm was used that updates PICO's location based on previously found features, LRF data and odometry data.

Problem 2: When a corner is detected, it is not necessarily a corner that belongs to the room PICO is currently in.

Solution: Check if the local angle of the corner point is located between the angle of the 2 points of an exit. If this is not the case, the corner belongs to the room PICO is currently in.

Movement

---

Problem 1: Is it going to be a deadlock, when the destination point distance is smaller than the pre-set distance that PICO drives each iteration? (Referring to the drive control block description)

Solution: It will never happen, since critical decision-making functions incorporate this value in computations (e.g. change of low-state from EXPLORE_CORRIDOR to GO_TO_NEXT_ROOM, when moving through the corridor for the first time, is dependent on the pre-defined moving distance: the state changes when the corners of the corridor are closer than the pre-defined moving distance).

Final challenge

For the final Hospital challenge, where PICO had to first explore the hospital and then find an object placed inside, our program was not ready to be executed. By the time of the competition, almost all the separate program structure blocks were working and tested separately. However, it was not enough time to integrate the program into one. By the final challenge, the following components were completed:

• Detection algorithm was able to recognize the corners and exits with high accuracy;
• The planning block had full functionality for the first part of the challenge;
• The world model was interacting with all the blocks, storing the necessary data and reading it from the JSON file. However, the structs used to store the data had lots of redundancies, which made the data flow inside inefficient;
• The state machine was functioning;
• Drive control was tested and proven to work.

The only problem, therefore, was incompatibility of the classes between each other. The program was successfully building, however was not able to execute as expected and crashed in the first iteration of the program loop. Instead, we showed that our detection algorithm performs very consistently in the local detection of corners and exits by using tele-operation through the hospital.

Later on, most of the bugs were identified as solved. The final version of the program by the deadline is as it is described in this Wiki page.

Lessons learned

Reflecting on the project as a whole, our team have identified the reasons of the failure in the final Hospital challenge. These are:

1. When the functionality of the classes become too complex and/or demanding in terms of computational effort, there most likely exists a library that is provides this functionality. It is easier to use the library than create all the necessary methods ourselves.
2. Try to stay away from the use of magic numbers. Although this is a way to quickly get code working, it will not be easy to re-use that code later on in the project.
3. Divide the tasks between the members of the group, assigning more experienced with programming members of the group with the ones who have less experience. This way the code would be more readable and clean. As the practice have shown, people with little C++ experience are able to deliver a working solution, however, it becomes harder to understand it for the rest of the group.
4. Do ask for an advice from your mentor often and exchange information with other groups.
5. When there is a large data flow in a program, dedicate a separate class for it's storage (like a world model). It makes exchange of variables easier and the code cleaner.
6. Working with sensor data requires extra patience and lots of filtering. Data is not always reliable.

Conclusions

During the project, we have learnt a lot about programming, interfaces with robotic systems and programming them. We have experienced many successes but even more failures and mistakes.

Since we have decided to create the full program ourselves, we can explain every bit of the program and any decision that is made in every function. We are proud of the eventual result despite the failure to create the program that enables PICO to complete the final challenge properly. Special attention can draw the fact that none of the members of the group are experienced programmers and almost none of us have worked with C++ enough to be able to create a nice and well-structured code straight away. This project will serve us as a reference for any following programming assignment.

Code Snippets

Destination point in front of the exit: Code snippet: Planning::getNearbyExitPoint

State Machine switching: Code snippet: bool stateMachine

JSON file creation: Code snippet: void WorldModel::createJson()

Data structures used throughout the software: Code snippet: helper.hpp