Mobile Robot Control 2023 R2-D2

Group members:

Name	student ID
Ismail Elmasry	1430807
Carolina Vissers	1415557
John Assad	1415654
Jelle Cruijsen	1369261
Florian Geister	1964429
Omar Elba	1492071

General Requirements of the wiki page

Is there an introduction/conclusion? (yes, at least there's a heading for that)
Is there a logical ordering storywise? (must be checked afterwards, need to have all the components of the functionalities, and everything we have done so far written down to not miss anything, then we can order)
Interpretability/readability: proper grammar, use of figures etc. (check afterwards, did we check comments in the code so it can be read and understood easily?

Videos:

Good test without doors: https://drive.google.com/file/d/1QjjkPH-98M7pvCE11sMnp0RJUNlhwpEW/view?usp=sharing

Perfect test with doors: https://drive.google.com/file/d/1QjmRv4MDE3_WtiGM_gy8g43pHbXBmKKp/view?usp=sharing

Plan around obstacle: https://drive.google.com/file/d/1Qlq6RXElBdS-wRzYQZ-_ZTRHnaEJYHck/view?usp=sharing

Wrong initial position to wall bump: https://drive.google.com/file/d/1Qk02ZMIf5oth2nHf3aLz32Aem26hoPw4/view?usp=sharing

Design Presentation

File:MRC Design.pdf Presentation pdf:

-- About the design presentation: Possible Extension Algorithms and why we don't use them any more

Introduction

This report presents an in-depth analysis of the Restaurant Challenge, focusing on the system description, software architecture, strategy employed, robustness considerations, and evaluation of the challenge. The Restaurant Challenge is a challenge that assesses the capabilities of an autonomous robot to navigate and deliver orders within a restaurant environment. The challenge requires teams to develop software and algorithms for their robot, known as Hero, enabling it to autonomously navigate through the restaurant, avoiding obstacles and humans, while accurately delivering orders to designated tables.

The report begins by introducing the challenge overview as well as the system description then follows by outlining the state flow and data flow created for the challenge. The state flow details how Hero progresses from the start area to the designated tables, while the data flow explains how information is exchanged and processed within the system.

Furthermore, the report covers the strategy employed by the team to tackle the Restaurant Challenge. This includes a discussion of the global planner responsible for determining the overall path, the local planner handling immediate obstacles and route adjustments, the particle filter aiding in localization, and the main algorithm that orchestrates the entire delivery process.

A crucial aspect of the challenge is the software architecture developed. This section of the report explores the architecture's design, how it was organized and its effectiveness in enabling Hero to perceive and navigate the environment, handle object detection, and execute the delivery task.

The report also addresses the robustness considerations involved in the challenge, ensuring that Hero can handle unexpected situations, such as collisions, stalls, or navigational uncertainties. Therefore, the software needs to be designed with this in mind, and this section evaluates the robustness of the implementation created.

Finally, the report discusses the results of the Restaurant Challenge. The performance of Hero will be assessed based on criteria such as successful delivery completion, adherence to the correct table order, object detection capabilities, and overall navigation efficiency. The evaluation section will include a discussion of the results, lessons learned throughout the challenge, things that could have been improved and future considerations. The report will then be concluded with a brief conclusion that describes the main achievements that were successfully completed.

Through this report, we aim to provide a comprehensive analysis of the Restaurant Challenge, shedding light on the technical aspects, strategies employed, software architectures developed, and evaluation outcomes. This knowledge contributes to the advancement of autonomous navigation and delivery systems, fostering innovation in real-world scenarios where robots can assist in restaurant operations and similar tasks.

Challenge Overview

The Restaurant Challenge is designed to test the navigation and delivery capabilities of Hero, an autonomous robot. The objective of the challenge is to deliver orders to designated tables in the correct order provided, following a specific set of rules and conditions. The challenge environment consists of a restaurant setup, including walls, tables, doors, and both static and dynamic objects.

At the start of the challenge, Hero is placed in a designated start area with an unknown orientation. The list of tables to be visited in the correct order is provided just before the challenge begins. Hero's task is to navigate through the restaurant, avoiding obstacles and humans, and reach each table to deliver the orders.

The delivery of an order is defined by driving up to the table, positioning near the table while facing towards it, and giving a clear sound signal to indicate Hero's arrival. The exact positioning next to the table is not critical as long as the robot is close enough for a customer to comfortably take their order from the tray.

The challenge environment specifications state that the walls are approximately straight, and tables are solid objects represented as rectangles in the LiDAR measurements such that it is not necessary to detect the legs of the tables. Doors may be open or closed, and there can be multiple routes to a given goal. Furthermore, the presence of dynamic objects in the form of humans adds complexity to the navigation task. Additionally, static obstacles such as chairs that are unknown to the robot and not present in the map given may be placed throughout the restaurant. Their detection and influence on the path taken is an additional challenge that needs to be overcome.

The challenge is subject to specific rules, including restrictions on touching walls or objects, limitations on robot speed, and time limits for each trial. Two trials are allowed, with a maximum of one restart per trial. A trial ends if Hero bumps into a wall or object, remains stationary or makes non sensible movements for 30 seconds, exceeds the total time limit of 10 minutes, or if a restart is requested. A restart allows Hero to return to the start position while resetting the trial time but not the total time.

The overall goal of the challenge is to demonstrate the robot's ability to autonomously navigate a complex environment, accurately deliver orders to designated tables, and effectively handle dynamic obstacles. The teams' performance will be evaluated based on their ability to complete the task within the given constraints.

System Description

This section provides a comprehensive overview of Hero, which is the robot's name that is being used for this challenge, its limitations, and functionalities.

Hero has certain limitations on its maximum speed. Its translational speed is limited to 0.5 m/s, while its rotational speed is limited to 1.2 rad/s. To establish communication with the robot, two types of inputs can be utilized: laser data from the laser range finder and odometry data from the wheels.

The laser range finder provides information about the distances between the robot and its surrounding objects. The range_min and range_max values define the minimum and maximum measurable distances, respectively. Any distance reading below range_min or above range_max is considered invalid. Furthermore, the angle_min and angle_max values determine the angles of the first and last beams in the measurement. The angle_increment value is also provided as an input metric from the laser data. The actual laser sensor readings are stored in a vector of floats of which each reading is associated with the ranges. Each element in the vector corresponds to a measured distance at a specific angle, which can be calculated using angle_min, angle_increment, and the index of the element in the vector. Additionally, the timestamp associated with the laser data indicates the time at which the measurements were taken.

Moreover, odometry provides information about how far the robot has traveled and how much it has rotated. Each wheel and the rotational joint are equipped with encoders that keep track of their respective rotations. By utilizing the encoder data from all sources and considering the wheel configuration, it is possible to calculate the displacement and rotation of the robot since its initial position. However, it is important to note that odometry data is susceptible to drift due to small measurement errors and wheel slip, which accumulate over time. Therefore, relying solely on odometry data for longer periods is not recommended. Additionally, it is essential to be aware that the odometry data does not start at the coordinates (0,0), indicating that the robot's initial position may be different from the origin. Thus, the utilization of a particle filter will be needed.

Apart from the laser and odometry data, the Hero robot is equipped with two separate bumpers, each acting as an individual sensor. Therefore, to obtain bumper information, the data from each bumper should be retrieved individually which would provide information about whether the robot made contact with an obstacle, from the front or the back.

Furthermore, the velocity command given to the robot can only realize movement in the x-direction (which is forward and backwards), and rotate with a given input rotational velocity. Thus, by understanding these different types of input data, the robot can be controlled, and made to navigate without collision.

From Desires to Specs

#####Add updated image from design presentation and some text#####(Jelle)

State Flow

Figure 1: State flow of Hero's algorithm

###### Add text in this section (ismail will be done tomorrow)#####

Data Flow

Figure 2: Data flow of Hero's algorithm

###### Add text in this section#####

Strategy Description Restaurant Challenge

Global Planner

The chosen global planner for the Hero robot is the A* algorithm. The A* algorithm is a widely used path planning algorithm that efficiently computes an optimal path from a starting point to a goal point. In the context of this assignment, the robot receives a specified order of tables as input, and the A* algorithm is responsible for generating a sequential path that visits these tables in the given order.

The A* algorithm combines the strengths of both Dijkstra's algorithm and greedy best-first search. It intelligently explores the search space by considering both the cost of reaching a particular point and an estimate of the remaining cost to reach the goal. One of the key advantages of the A* algorithm is its ability to find the shortest path while minimizing unnecessary backtracking or inefficient movements. By planning the path to the tables in the given order, the algorithm ensures that the robot follows a coherent and streamlined delivery process. This leads to efficient navigation, reducing both the time and distance traveled.

In this challenge, the optimal path, which is computed by the A* algorithm, is given as a sequence of nodes. The map is discretized into a grid representation, whereas nodes have been chosen to mark locations on the map that the robot be positioned at. The robot progresses the map by moving sequentially from one node to another, following a predetermined set of connections between adjacent nodes.

However, it is important to consider scenarios where unexpected obstacles may interfere with the computed path. In such cases, when an obstacle that was not pre-defined appears along the planned path, the local planner is activated. The local planner utilizes obstacle avoidance techniques to dynamically adjust the robot's trajectory, allowing it to safely navigate around the obstacle. Once the local planner successfully avoids the obstacle, the global planner, using the A* algorithm once again, is invoked to recompute an optimal path to the table goal.

By seamlessly integrating the A* algorithm as the global planner and incorporating the local planner for obstacle avoidance, Hero can effectively navigate the environment, efficiently delivering orders to the specified tables. The A* algorithm's ability to find optimal paths, combined with the adaptability of the local planner, ensures robust and intelligent decision-making during the robot's navigation process.

Local Planner

##Todo mention that the open space is activated once an obstacle is encountered. Reference the obstacle scanning figure.

Figure 3: Hero's obstacle detection scanning cone

Open Space

In the implemented open space approach, the robot utilizes laser data to navigate through the environment by identifying and traversing through open spaces or gaps between obstacles. This approach enables the robot to safely move without colliding with objects in its path. The algorithm consists of several key steps:

1. Gap Detection: The algorithm analyzes the laser scan data to identify gaps or open spaces. It examines consecutive laser points within a specified range (2π/3 cone in front of robot) and checks if their distances exceed a certain threshold, look ahead distance that begins at 0.8 meters, as seen in Figure 3. If a sequence of laser points with distances greater than the threshold is found, it indicates the presence of a gap.

2. Gap Width Evaluation: After detecting a potential gap, the algorithm evaluates its width to determine if it is wide enough to be considered a significant open space. A minimum gap width criterion is set to 100 laser pointers to ensure that the detected gap is large enough to fit the width of the robot such that it can safely drive within this gap.

3. Open Space Search: The algorithm searches for open spaces within the laser scan data by iteratively applying the gap detection process. It gradually adjusts the look ahead distance parameter by decreasing it by 10% each iteration to find wider gaps if no suitable gap is found initially. However, it should be noted that this look ahead distance should never decrease below the obstacle distance defined. This adaptive mechanism allows the robot to explore different distances and adapt to the environment's characteristics.

4. Open Space Calculation: Once a significant gap is detected, the algorithm calculates the distance and rotation required for the robot to navigate through the open space. It determines the start and end indices of the laser scan range representing the open space and uses this information to calculate the desired rotation angle. The algorithm also accounts for small adjustments to the rotation angle to ensure smooth and precise navigation.

By implementing the open space approach, the robot can effectively plan its trajectory by identifying safe areas to move through. This strategy minimizes the risk of collisions and enables efficient navigation in complex environments.

Re-planning Path

Once an obstacle is detected and open space is called, Hero finds the gap using the algorithm described above and then moves a distance equal to the obstacle distance defined. The obstacle distance is the minimum distance the robot can come close to an object before calling open space and is set to 0.4 meters in this case. Following the adjustment of position to circumvent the obstacle, if open space is not called once more, Hero initiates a re-computation of the A* algorithm to re-plan its path. This ensures that the robot can adapt to the new environment and dynamically generate an optimal trajectory towards the next table on the delivery list depending on its new location on the map.

The process of activating open spaces, moving a distance corresponding to the obstacle distance, and recalculating the A* algorithm enables Hero to navigate efficiently and effectively within the restaurant environment. This capability showcases the robot's adaptability and problem-solving skills, as it can respond to unexpected obstacles and make the necessary adjustments to ensure successful delivery completion.

Doors

Particle Filter

Localization is a crucial task in robotics, involving the estimation of a robot's pose (position and orientation) relative to a known map or reference frame within its environment. It enables effective navigation, perception, and autonomous task execution. For this restaurant navigation challenge, localization is a must since relying on the odometry information is not optimum due to wheel slippage. Thus, the implementation of a particle filter is needed.

A particle filter is a probabilistic localization technique widely employed in robotics. It utilizes an estimation algorithm that maintains a set of particles, each representing a hypothesis of the robot's pose. These particles are distributed across the map and updated based on sensor measurements and control inputs. This allows for robust and accurate localization, particularly in dynamic and uncertain environments.

The functioning of a particle filter involves iterative updates of the particles, guided by sensor measurements and control inputs. Initially, the particles are uniformly distributed across the map, encompassing a broad range of potential robot poses. As sensor measurements, such as odometry or perception data, are acquired, the particles undergo resampling and weighting based on their likelihood of generating the observed data. Particles with higher weights, indicating better agreement with the measured data, have a greater probability of being selected during the resampling process. This iterative process allows the particle filter to converge towards the true robot pose. This is seen, in this implementation, as a group of red particles that are relatively close to the robot's actual position on the map.

Furthermore, the particle filter also handles the coordination between different frames used in robot localization. There are three frames which are being utilized, and that are being coordinated with each other by the particle filter. The map frame, robot frame, and odometry frame are those three frames. The map frame represents the global reference frame, in this case, it is defined by the map of the restaurant which was given. The robot frame denotes the robot's pose within the map frame, encompassing both its position and orientation. The odometry frame tracks the robot's motion through wheel encoders or other motion sensors. The particle filter leverages information from the odometry frame to update and propagate the particles, accounting for the robot's movement and estimating its pose relative to the map frame. By integrating measurements from the sensor frame, which is obtained from the laser data, the particle filter refines the particles' weights and adjusts their positions, resulting in an accurate estimate of the robot's localization.

Finally, there is a tradeoff between maximizing the particle filter performance by increasing the number of particles utilized and the computational load. So, a tradeoff has to be made between how accurate the robot's pose is estimated, and how fast it is estimated. For this implementation, the optimum number of particles was determined to be 400 particles. This number was found through a trial and error basis.

By utilizing the particle filter, alongside the laser, odometry and bumper data, the robot should be able to navigate the restaurant.

Announcing Arrival and Celebration

The algorithm ensures that announcing the robot's arrival at each table and celebrating the completion of all table visits is incorporated. This section outlines the approach used to announce the arrival and trigger a celebratory response.

To begin with, the nodes that encompass each table are organized into separate vectors based on their relative positions. These vectors serve as reference points for determining the desired orientation of the robot when facing the table. Depending on the specific table being visited, the appropriate vector is selected.

Upon reaching a goal node, the algorithm identifies which vector the node belongs to, thereby determining the desired angle for the robot's orientation. To ensure alignment with the desired angle, the robot's pose is requested through the particle filter. If the robot's current facing direction differs from the desired angle, the robot performs a turn to face the table.

Once the robot is properly oriented towards the table, it announces its arrival by vocalizing the phrase, "Hello, I have arrived with your order." This vocalization ensures that the customers are aware of the robot's presence and the delivery of their order.

Furthermore, once all tables have been visited, and this is verified by checking whether the table list is empty, the algorithm triggers a celebratory response from the robot. The robot proclaims its achievement of visiting all tables, signaling the successful completion of its task.

Creating Table List and Inputting Table Order from User

When calling the executable from the terminal, arguments can be supplied to the system. These arguments are the table numbers that Hero should visit. These table numbers are taken and converted to node numbers. From the node map in Figure 4, nodes were identified per table as an option to visit. Each possible table node was also assigned a direction to turn to, as discussed in the Announcing Arrival and Celebration section. When converting table numbers to node numbers, two options were created.

Originally, the best nodes to reach were placed first in the list. The robot would only visit another node other if this original node is blocked. The table number to node translation only happens at the start of the program, and if a node is blocked. Therefore, if no obstacles are placed in the restaurant, the same node will always be visited per table. This was tested prior to the challenge and used in the challenge as well.

A second option was also created to optimize for the shorted path based on the current node number. The list of final goal nodes to visit was updated every time a table was reached, or a node was blocked. The list of possible nodes to visit is re-ordered based on which node the robot is in, and the closest node (by numerical number) is chosen as the node to visit. If this node is blocked, the next closest node is used instead. This implementation was chosen due to unoptimized paths. For example: the long path followed if table 0 is visited before table 1, causing the robot to drive all the way back around the table to node 46. This could be better chosen to be node 36. This version was only tested in simulation due to time. Within simulation, the recalculation of the nodes based on shortest distance was confirmed. However, due to the preference of different nodes that had not been tested in real life, as well as unexpected simulation behavior of open space, it was decided to not use this during the challenge.

Figure 4: R2D2 generated map with nodes and connections in defined grid

Map Generation

The map plays a critical role in the project. The map provided by the lecturers comprises an environment represented as a pixel image and a corresponding coordinate JSON file describing the corner nodes of tables, walls, and obstacles. Additionally, a config file is included, containing information about the resolution of the pixelated image, enabling the establishment of a scale for projecting the real-life environment onto the image. It is important to note that the relative sizes of objects, tables, and walls in the image are consistent with those in the actual environment.

The robot operates by navigating through nodes that it can visit. Consequently, the map generation process involves translating the given information, particularly the relative size information of walls, tables, and obstacles, into a file that can be interpreted by the A* algorithm. This file allows the algorithm to effectively plan the robot's path, ensuring that it visits all the tables. To achieve this, the strategy entails defining a grid with a chosen grid size, identifying the nodes that the robot can visit, and establishing connections between these nodes.

########### I'd say we immediately mention that this approach wasn't taken. So we should have 2 subsections under map generation, where we clearly mention in the section title, that this approach was discontinued The provided coordinate file contains specific point numbers and coordinates in both 2D directions, representing a coordinate system measured in meters that originates from the bottom left corner of the map. The process of generating the node and connection list involves developing an algorithm that can convert these given points and connections into a functional grid consisting of nodes that the robot can visit, each with their respective coordinates.

##### This paragraph is hard to understand as a reader, try to reformulate this information through pseudocode, or probably better, a (draw.io) flowchart. Initially, the total size of the map is determined based on the provided map coordinates. For instance, in the case of the restaurant challenge map, the width of the map, including the outer walls, was found to be 6 meters, while the height was 5 meters. Subsequently, a grid size of 0.2 meters was selected, based on the given wall thickness. The algorithm entails processing the corner points of obstacles to identify regions that the robot cannot access. By dividing each pair of connected points by the grid size, all points falling within the defined grid are recorded and added to a list of non-visitable points. Next, an exhaustive examination of every possible point on the grid, using the 0.2-meter grid size, takes place. During this process, the algorithm checks whether the current point is included in the non-passable node list and, if not, adds it to the list of visitable nodes. Following the execution of this algorithm, two lists are generated, encompassing all possible node points on the map. One list comprises the nodes that the robot can visit, while the other contains the remaining obstacle points. Subsequently, the algorithm is expanded to include an assessment of each node's neighboring nodes in the x and y directions. If these neighboring nodes are included in the passable node list, the corresponding connections are added to the connections list, which exclusively consists of connections between passable nodes. The resulting information is then stored in a JSON file format, facilitating reading by the program and utilization by the A* algorithm for path computation towards the respective tables.

In order to further enhance the algorithm, it is imperative to ensure that only the feasible nodes are included in the output JSON file, reflecting the nodes that the robot can actually visit. Considering the robot's size, which is half a meter, and the relatively smaller grid size, certain nodes must be excluded from the final node list, taking into account the distance from walls. Moreover, nodes enclosed within tables, for example, must also be excluded from the final node list as they are surrounded by a closed loop of table nodes that the robot cannot traverse. Additional modifications aim to reduce the number of generated and stored nodes, enabling the robot to visit a fewer number of nodes and avoiding unnecessary stop-and-go movements.

#####The 'several days' statement provides no information imo, also why is this sentence a contratiction (despite), what does it contradict Despite formulating an algorithm over several days that successfully stores all feasible nodes and their corresponding connections, it remains limited to the inner table nodes, nodes in proximity to unreachable areas due to the robot's size relative to the distance from walls, and a reduced number of nodes to avoid storing every individual node along a straight path defined by the grid size.

##### We shouldn't give the reason of the time constraints, also I feel like this wasn't the reason we dropped the map generating algorithm. I remember us having gained the insight that it would be complete overkill to map every node on a 0.2m grid (even with the removed straight line nodes), and most importantly we felt like it would be inferior to hand placed nodes, due to no thought going into it. When we received the actual map of the restaurant we saw quite quickly that we could grid it in its entirely by placing 50 nodes or so. This way we also could add additional diagonal connections, or cut connections that we deemed unsafe, resulting in a more robust nodelist. #### Btw don't place the sentences above in the wiki like this, they are just my thoughts and should be reformulated#### Due to time constraints imposed by other courses and personal circumstances, the decision was made to discontinue efforts in programming the algorithm to generate the node list. However, the program for the map generation was able to store every node of the defined grid in either the visitable or non-visitable node list. Instead, the node and connection lists were manually created, offering a significant time-saving advantage. Considering the map's size, the number of nodes and connections that needed to be stored, and the limited time available to the team, this approach was deemed more efficient for the restaurant challenge within the scope of this course. However, for larger projects involving more detailed or larger maps, an automated program must be developed to execute this concept efficiently within a short timeframe.

To manually construct the node and connection lists, a grid with a grid size of 0.2 meters was superimposed onto the image file, taking into account the image's relative size. Subsequently, a 2D coordinate system originating from the bottom left corner of the map was introduced. Within this coordinate system, it was established that one unit in the x or y direction would correspond to the grid size of 0.2 meters. The initial step involved marking the robot's starting area on the map, which entailed designating a single node at the center of the area. The subsequent initiation algorithm, to be programmed as part of the overall project, would enable the robot to locate itself within this area using localization methods such as the particle filter, regardless of its initial position within the starting area. Following this, additional points were defined, ensuring a distance of 10 cm from walls (equivalent to half a coordinate unit) and 20 cm from the door at all times. During the node selection process, careful consideration was given to the robot's size, with a circle representing the robot's size superimposed around each chosen node on the map to visually depict its dimensions. Furthermore, a deliberate effort was made to reduce the number of nodes through a selective process. In the attached map, you can observe the randomly numbered node points #### replace 'you', and the nodes aren't really in a random order### , the robot's size circle, the starting area depicted in red, and the corresponding coordinates. The positive x-axis is oriented upwards in the image, while the positive y-axis extends to the left. This choice aligns with the coordinate system used by the provided particle filter, enhancing programming convenience and code comprehension. #### This doesn't align with the PF coordinate frame, which was a major pain (the PF coordinate system was y-up x-right) However, I don't think that should be mentioned in this chapter, so maybe just remove this statement

Connections are depicted in blue on the map. The dotted connection represents passage through the gray door in the environment, although it is included in the regular connections list. It was crucial to ensure that the robot can traverse all these connections without violating any requirements, such as maintaining a safe distance from walls. The complete map is included as an attachment to this text. #### don't include it as an attachment, just place the image directly on this wiki#### The white space signifies the theoretically accessible area, while the black pixels represent obstacles, walls, or tables. The gray areas indicate doors. #### This last sentence seems kind of redundant and implied####

Following the manual definition of all connections and nodes, it was necessary to transcribe them into a compatible JSON file format, allowing the program to read and utilize the provided information. For this purpose, the nodes were recorded in a list, with each node assigned an index and associated with its respective 2D coordinates. Similarly, a connection list was created within the same JSON file, specifying the connections between nodes based on their corresponding indices. #### This doesn't really have to do with JSON files, just with the way that the provided A* source code in tools.cpp read the files. I think this short paragraph is unnecessary either way.

/*Image of the map..........*/

Main Algorithm

The main algorithm for this challenge is designed to facilitate efficient and obstacle-free movement towards reaching all the tables required and delivering the food order. It consists of several key steps and considerations that ensure the robot's safe and successful navigation through its environment through a combination of the global planner, local planner, and the particle filter.

Bumper Data and Movement The algorithm begins by checking the bumper data to determine if there are any immediate obstacles that are in contact with the robot. If no obstacles are detected, the robot continues with its regular movements. However, if obstacles are present, the algorithm utilizes bumper movements to navigate around them, depending on whether this data is received from the front or back bumper.
Laser and Odometry Updates The algorithm then verifies whether updated data from the laser and odometry sensors are received, if they are updated, then that means that the robot has moved, thus moving onto the next step in the algorithm.
Start Position Adjustment At the start position, the algorithm incorporates an additional step where the robot initially turns towards the main opening through the use of the particle filter. This ensures that the robot is properly aligned, preventing unwanted activation of open space functionality.
Initialization and Path Planning After data verification, the algorithm proceeds with the main code execution. It initializes the robot's orientation by checking the particle filter and ensures that the robot is in the correct starting pose. Following that, the algorithm plans the path to the goal location using the A* algorithm, as discussed previously.
Particle Filter and Node Propagation Once the robot starts moving, the particle filter continues to update the robot's pose to ensure accurate localization and proper arrival at the desired node and position. The algorithm propagates through the nodes along the planned path until it reaches the target table.
Obstacle Evasion If an obstacle is encountered during traversal, the algorithm activates the open space feature, which is the local planner, enabling the robot to evade the obstacle. However, if the robot fails to reach the next node successfully for two consecutive attempts, the algorithm removes this problematic node from future path planning to avoid further obstacles.
A* Path Recomputation In cases where the A* path computation fails entirely, for example a node in that path is unaccessible and deemed to be blocked, the algorithm then proceeds to recompute a different path that has the same goal node in the end as previously.
Door Handling When a door is encountered along the A* path, the algorithm sends a door open request and waits until the door is opened before proceeding.
Blocked Nodes and Goal Reassignment If the goal node at the table is blocked, the algorithm chooses an alternative node at the same table as the new goal, then progresses towards it.
Arrival at Table and Next Table Visit Once the robot reaches the goal node, the algorithm activates the arrival at table function which ensures proper orientation towards the table, and makes the robot announce its arrival to the table. It then removes the reached table from the table list and then sets the next table as the following goal.
Completion and Celebration Finally, if all the tables have been visited and the table list is empty, the algorithm triggers a celebration to signify the completion of the challenge.

By following this comprehensive main algorithm, the autonomous robot can successfully navigate its environment, avoid obstacles, and reach its designated goals efficiently.

Software Architecture Restaurant Challenge

Software Architecture

Logical interaction between software components (states, calling of functions, actively activating and disabling of functions)
Are the components implemented correctly (any errors seen?, how about any issues that still persist?) ######### Idk if I wrote about everything, so make sure that if there's something missing, you add it ########

Despite the overall progress and functionality of the main algorithm, there are certain challenges and issues that have been identified during the testing phase leading to the final challenge. This section aims to address some of the encountered difficulties and highlight the persisting issues that require further attention and resolution but could not be realized within the given time frame.

Open Space Interaction with Humans: One notable issue is that the open space feature, that is activated for obstacle evasion, does not work effectively in the presence of humans. The robot has a tendency to collide with human legs, potentially causing safety concerns. The root of this issue is yet to be identified, but is crucial if this robot is to be utilized in an actual restaurant setting.
Particle Filter Localization Errors: It has been observed that once the particle filter is in a wrong position, it struggles to converge back to the correct position. This issue becomes more pronounced when the robot encounters unknown obstacles, particularly large ones, leading to incorrect pose estimation. This issue might stem from the usage of few particles, but due to the computational speed at hand, increasing the particles is not favorable.

Robustness Restaurant Challenge

The performance verification of the system was conducted through a sequential approach, with examining each component of the algorithm. Initially, the A* algorithm was simulated to determine its effectiveness in pathfinding. It was found that A* functioned adequately, ensuring that the primary objective of efficient route planning was fulfilled. However due to no localization, Hero tended to veer from the real life path. Within Hero's odomerty it perfomed well, so this test was passed.

Next, the particle filter was integrated into the system, in conjunction with A* algorithm. This integration proved to be seamless, as the particle filter successfully operated without any complications when there were no unknown obstacles present in the environment, and fixed the poor localizatio issues from A*. However, challenges arose when incorporating the open space module back into the system. To address this, a careful calibration of the look-ahead distance was necessary to enable the system to identify unknown obstacles while excluding non-obstacle elements such as tables. Considerable effort was dedicated to fine-tuning the open space module, which ultimately did not perform up to the expected standard.

Furthermore, comprehensive testing was conducted to assess additional functionalities and ensure that all objectives were fulfilled. Tests encompassed scenarios involving door opening, blocking of a node, as well as functionalities like table identification and celebratory responses. These assessments served to validate the overall performance and understand the system's proficiency in executing its intended tasks.

To assess the robustness of the system, a series of tests were conducted to examine its performance under various challenging conditions. The tests encompassed different aspects, including obstacle avoidance, accuracy of walls, simulations, visualization of the particle filter, pathfinding, and verification of specific algorithm components.

One critical test involved evaluating the robot's ability to navigate in the presence of a moving human. Human movement was introduced as an obstacle, simulating real-world scenarios. However, it should be noted that the approach for handling human movement was not yet finalized, and further refinement was required to address the challenges posed by humans in the environment. The issues related to open space and the localization error of the particle filter were particularly relevant in this context.
Simulations played a crucial role in assessing the system's robustness. This allowed for extensive testing in a controlled environment before deployment in real-world settings.
Visualization of the particle filter was utilized to evaluate its performance and effectiveness in localizing the robot within the environment accurately. By examining the particle filter map, it was observed that removing the door completely from the map was necessary. Failure to do so resulted in localization errors, as the robot would incorrectly perceive its location once it passes the door, and assumes that the robot is still on the first end of the door.

The system's robustness was evaluated through tests and observations, revealing important insights. The performance of the particle filter was found to be influenced by the robot's interactions, with Define agressive aggressive behaviors resulting in decreased accuracy. Tuning the particle filter parameters and optimizing recovery from incorrect laser data were measures taken to improve robustness. The open space module, however, exhibited limitations and required further improvement as robustness was poor. Enhancing robustness requires addressing these factors which impact performance.

Evaluation Restaurant Challenge

The challenge, as expected, consisted of the map provided, in addition to, several static and dynamic objects. There was a static object completely closing off a hallway leading to table 4, and another one constricting the access to table 0. Furthermore, a dynamic object, in this case, a moving person, was also an added obstacle to the map. The observed behavior of the robot verified that there are several aspects of the algorithm that work, and several others that do not. The particle filter worked as expected. Localization was on point, and the robot did not lose its pose throughout the challenge. Also, the global planner was also working correctly. The path to the tables required to visit was generated correctly, and the robot ensured to follow that path.

However, the local planner was the downfall. The local planner utilized the open space approach, as mentioned before. The open space is initialized once an obstacle is detected in that cone shape scan showed in the figure but this meant that there are blind spots in this detection method. This issue was not detected during the testing phase of the project but became prevalent during the challenge due to the added obstacles. Thus, when the robot had to move through the constricted turn near table 0, the robot bumped into the obstacle, since this obstacle was in the detection blind spot.

Furthermore, when the robot met the static object blocking table 4, the A* algorithm managed to re-plan a different path, and this was verified through the output of the terminal. However, the robot was not able to follow said path, due to open space kicking in, and not being able to exit it.

Finally, when the robot was tasked to request the door to open, it succeeded. Just like in simulation and testing, the robot recognized the door as a door and not an obstacle, and then requested that the door be opened.

In conclusion, all aspects functioned as expected except for the open space. Further testing should have been conducted such that the open space issue would have been caught earlier, and the blind spots of the cone shaped detection would be identified. If the open space functioned properly, then the challenge would have been completed. Also, the detection of humans was not tested nor seen during the challenge but from testing, human detection was not working properly, and that should be considered as a point of improvement for the future.

Figure 5: Hero's obstacle detection blind spots

Figure 6: Hero's improved obstacle detection

Discussion

The previous section elaborated on the performance of the algorithm in the final challenge. The algorithm showed a shortfall in the implementation of the open space approach used as a local planner. As mentioned, the obstacle detection cone used to instigate the local planner had a blind spot, as seen in Figure 5. As an improvement, a new detection shape should be used. A potential shape is displayed in the Figure 6. This new and improved shape should ensure that the robot sees obstacles when cutting close corners, or navigating an environment with tight turns and obstacles.

Furthermore, the detection of humans should also be further tested and improved. Even though human detection was not observed through out the challenge but during testing, human detection was not functioning properly. This could also be due to open space detection issue, or perhaps an issue with recognizing that two legs are an obstacle. But due to time restrictions, human detection was not tweaked nor tested thoroughly.

Conclusion

In conclusion, the implementation of the system has demonstrated great promise, with several working corners of the algorithm functioning properly. The revised approach to the open space module and improving the code structure will induce enhancements to functionality and optimizing performance. The fine-tuning of the measurement model in the Particle Filter is a step yet to be taken to improve accuracy and reliability.

Despite the considerable progress achieved, further refinements are required to fully optimize the algorithm. Addressing the identified limitations of the open space module and fine-tuning specific components are crucial next steps towards achieving optimal performance.

Overall, this implementation has shown tremendous potential and marks a significant stride towards meeting the desired objectives. By incorporating the suggested improvements, the system can be further enhanced, boosting its robustness, accuracy, and overall effectiveness in real-world scenarios.