Task management for aerial robots in missions with physical interaction with the environment

Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Trabajo Fin de Grado Ingeniería Electrónica, Robótica y Mecatrónica Task management for aerial robots in missions with physical interaction with the environment Autor: Iván Gutiérrez Rodríguez Tutores: José Ramiro Martínez de Dios, Aníbal Ollero Baturone Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2022 Trabajo Fin de Grado Ingeniería Electrónica, Robótica y Mecatrónica Task management for aerial robots in missions with physical interaction with the environment Autor: Iván Gutiérrez Rodríguez Tutores: José Ramiro Martínez de Dios Catedrático de Universidad Aníbal Ollero Baturone Catedrático de Universidad Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2022 Trabajo Fin de Grado: Task management for aerial robots in missions with physical interaction with the environment Autor: Iván Gutiérrez Rodríguez Tutores: José Ramiro Martínez de Dios, Aníbal Ollero Baturone El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: A mis padres, A Ramiro, A mis compañeros Agradecimientos Ag radecer en primer lugar todo el apoyo y cariño recibido por mis padres y por mi familia, que me han dado todo lo que ha estado en su mano para que pueda llegar hasta aquí. Agradecer también a Ramiro su dedicación y la confianza que ha depositado en mí dándome una gran oportunidad. Por último, agradecer el apoyo de mi pareja, mis amigos y en especial, de mis compañeros de clase, los cuales han sido un apoyo diario y una parte fundamental de mi vida estos últimos años. Iván Gutiérrez Rodríguez Sevilla, 2022 III XBrief Index 6 Conclusions and future work 63 6.1 Conclusions 63 6.2 Future work 63 Appendix A Tables with descriptions of different missions sent to the Parser and results of their check 65 Appendix B Messages printed by the mission manager and the action server during the execution of missions 67 Appendix C Planned positions to inspect bearings or to install a sensor box 71 List of Figures 73 List of Tables 75 List of Codes 77 Bibliography 79 Contents Resumen V Abstract VII Índice Abreviado IX 1 Introduction 1 1.1 Motivation and objectives 1 1.2 Context 1 1.3 Structure 2 2 Related work 3 2.1 Task planning 3 2.2 Software tools 4 2.2.1 ROS 4 2.2.2 UAL 5 2.2.3 PILOTING-MAVSDK library 6 3 Architecture 7 3.1 Introduction 7 3.2 Mission architecture in PILOTING project 7 3.3 VIAD-DRONE and TTDRONE architecture 9 3.3.1 Architecture 9 3.3.2 Communication with the gRCS 10 3.3.3 Communication with the UAL and the drivers 11 3.3.4 Communication with the perception module 13 3.3.5 gRCS 13 3.3.6 Mission Manager 15 3.4 Conclusion 19 4 Implementation 21 4.1 Introduction 21 4.2 Structure of nodes 21 4.3 MAVSDK-ROS node 23 4.4 Action server node 24 4.5 Mission manager node 25 4.5.1 Constraints 25 4.5.2 Overview of the operation 26 XI XII Contents 4.5.3 Alarms, checklist, and high-level actions 27 4.5.4 Telemetry and home position 28 4.5.5 Auxiliary functions 30 4.5.6 Parser 32 Bearing inspections 33 Box installation 34 Target installation 35 Tunnel inspections 36 4.5.7 Planner 37 Bearing inspection 38 Box installation 39 Target installation 39 Tunnel inspections 40 4.5.8 Command execution 40 4.5.9 Mission execution 41 4.6 Conclusion 43 5 Experimental results 45 5.1 Introduction 45 5.2 Alarms and checklist 47 5.3 Telemetry and home position 48 5.4 Parser 51 5.5 Planner and execution 52 5.6 High-level actions 58 5.7 Conclusion 61 6 Conclusions and future work 63 6.1 Conclusions 63 6.2 Future work 63 Appendix A Tables with descriptions of different missions sent to the Parser and results of their check 65 Appendix B Messages printed by the mission manager and the action server during the execution of missions 67 Appendix C Planned positions to inspect bearings or to install a sensor box 71 List of Figures 73 List of Tables 75 List of Codes 77 Bibliography 79 1 Introduction 1.1 Motivation and objectives The purpose of this bachelor’s thesis is to design, implement, and validate a mission manager for aerial robots. To begin with, this mission manager will be in charge of receiving a list of high-level tasks the robot has to perform and of designing a mission to fulfill the tasks. The mission designed will be a list of simple actions whose execution in single file would achieve all the high-level tasks. The available actions will be those related to the aerial robots (take off, go to a waypoint, land, etc.) in addition to other actions related with the high-level tasks to be done, which will involve interaction with the environment. Once the mission is planned, it will also be responsible for executing it by performing the planned actions. Furthermore, the mission manager will count on many safety functionalities such as the sending of information about the mission in execution and the state of the robot, or the management of requests to pause or abort the mission. 1.2 Context The mission manager developed in this thesis is designed to be used by the robots of the University of Seville for the PILOTING project, which is part of the Horizon 2020 program of the European Union [ 1 ]. The name of the project is an acronym of PILOTS for robotic Inspection and maintenance Grounded on advanced intelligent platforms and prototype applications, and as implied by its name, it consists of inspecting and maintaining civil infrastructures and refineries automatically by using robots. It is well known that refineries and infrastructures are gradually deteriorating and that the economic situation in Europe is very likely to prevent large investments in their renewal. Moreover, their manual inspection and maintenance usually involves working on elevated structures or dangerous locations, which in turn involves costs and slow progress in the operations. As a result, the aim of the PILOTING project is to increase the efficiency and quality, and to decrease cost and time of inspection and maintenance activities by the adaptation, integration, and demonstration of robotic solutions in an integrated platform. [5] Due to the extent of the project, there are many partners which collaborate in it. Some of them are the companies related to refineries, tunnels, or viaducts and, therefore, are the end-users of the system. The rest of the partners are in charge of developing the software applications and the robots to perform the operations. For instance, the University of Seville, which is developing two aerial robots to inspect tunnels and viaducts. The coordinator of the project is CATEC, the Advanced Center for Aerospace Technologies. As will be addressed afterwards, CATEC develops the platform to control the robots and as a consequence, the mission manager developed in this thesis has to be adapted to this platform. 1 2Chapter 1. Introduction In each of the pilots in which the system will be tested (refineries, viaducts, and tunnels), there are different use cases for which the system is designed. They are usually performed by a single robot, but however, many of them complement other use cases, taking advantage of the different characteristics of each robot. In the case of the robots developed by the University of Seville, they are in charge of performing 4 use cases: 3 in viaducts, and 1 in tunnels. The first robot for which the mission manager is designed is the VIAD-DRONE, which is used in viaducts. This robot is used in 3 different use cases, and consequently, it is a configurable robot which has to be mounted with the pertinent tool before its use for a specific use case. The first use case performed by this robot is the inspection of bearings. The bearings are elements of the viaducts that are between the pillars and the deck of the viaduct. They are used to cushion the viaduct when a vehicle crosses it, and therefore, they are very likely to become degraded. That is why the VIAD-DRONE will be used to fly to them to take pictures of the bearings using a gimbal. After that, these pictures will be sent to the end-user, allowing it to check the status of the bearings. The second use case is the installation of a sensor box, which has to be installed on the top of the pillars and which will be used by the end user to measure the vibration of the viaduct. Lastly, the VIAD-DRONE will stick surveying targets close to previously detected defects. After that, another robot developed by another partner will take a photo of the defect with the survey. These pictures are then processed using artificial intelligence, which uses the survey target to precisely obtain the size of the defect. The second robot, the TTDRONE, is used to inspect defects in tunnels. This robot goes along the tunnel with an UGV which performs a global inspection by browsing for possible defects. Then, the UAV fly to the possible defects and takes photos using a gimbal. After that, the pictures are sent and processed to identify the gravity of the defect. Figure 1.1 PILOTING project logo. 1.3 Structure This bachelor’s thesis is organized as follows. Firstly, an overview of the literature related to mission planning is addressed, as well as an overview of the software tools used for the implementation. This is followed by Chapter 3, which, at first, briefly describes the global architecture of the project presenting the different modules and the communication flow with the robots. After that, the architecture of the developed robots is addressed in detail. The next chapter focuses on the implementation of the system. Although this chapter only dealt in full with the implementation of the mission manager, it also gives an overview of the implementation of those modules which interact directly with the mission manager. Lastly, Chapter 5 is concerned with the validation of the system by simulated experiments. 2 Related work 2.1 Task planning In addition to path planning, motion planning, or kinematic planning, task planning plays a significant role in the field of robotics. The purpose of this type of planning, also known as automated planning, is to generate a sequence of actions to achieve a goal. These actions are understood as transitions between states, and the set of states and actions in a specific planning problem is known as the domain. Solving a task planning problem implies its previous description, which, in turn, implies the description of the possible states, as well as of the actions and transitions they cause. In the last century, STRIPS [ 24 ] was considered to be a pioneering method for describing (and solving) task planning problems. However, over the last 20 years, the focus has shifted to new languages, especially PDDL [16]. Regarding the planning itself, a variety of approaches have emerged over the last years to solve the problem. To begin with, as mentioned, STRIPS was one of the first automated planners and, in fact, it was the first one used in a real robot. This method receives the goal as a state that satisfies a condition or an attainment goal and to which it must bring the current state. To this end, it searches for an action which satisfies the condition and uses the prerequisites of this action as a sub-goal. A more recent approach are the Hierarchical Task Networks (HTN) planners, which instead of receiving the goal as a state, receive high-level tasks which they have to perform and generate a plan by task decomposition and conflict resolution [ 23 ]. Some examples of this type of planner are SHOP2 [ 31 ], SIPE-2 [ 35 ], or OPLAN [ 20 ]. Another extended option are planners that represent the domain using temporal logic such as TALPLANNER [22] or TLPLAN [17]. All the planners mentioned above use domain-dependent knowledge, which means that a human has to provide them with the heuristics to solve the problem. This might have a considerable difficulty in certain situations, and that is why planners that use domain-independent knowledge were developed. These planners are able to find these heuristics by themselves but, as a counterpoint, their performance is usually lower than that of the planners that use domain-dependent knowledge. Some examples of these planners are FF [ 28 ], LPG [ 26 ], and SGPlan [ 18 ]. There are also approaches that attempt to combine the advantages of both methods, such as the one found in [25]. The use of these planners to solve real-world problems is not trivial due to the need to describe action models and use domain-dependent knowledge to obtain acceptable performances in complex domains. The definition of this knowledge may be very tedious, and in addition, the planners are not compatible with changes in the domain. However, there are approaches which use machine learning to simplify the definition of planning knowledge and help with the adaptability of the system [29]. 3 4Chapter 2. Related work Although they have traditionally been treated in isolation in robotics, task planning, motion planning, and path planning require a good performance of the others to achieve the goals correctly. For example, if one of the actions automatically generated by the task planner requires a movement that cannot be performed in the current state, the execution would fail. Therefore, several studies have found the combination of the different planners to be very important for the correct functioning of the system [21]. The planning problem treated in this thesis consists of robots that perform only one type of high-level task in each mission. In addition, the computational complexity of the developed software have to be as low as possible due to the low payload aerial robots can carry limits the onboard computers output. Therefore, the complex methods previously mentioned are not suitable to achieve the purpose of this thesis. As a result, in order to simplify the system, the approach is strongly based on the use of the domain knowledge. The different high-level tasks will be associated with groups of actions, and the mission will be planned by the simple concatenation of the group of actions associated to the goals sent by the user of the system. To conclude, the approach might be seen as a simplified version of the HTN planners. 2.2 Software tools 2.2.1 ROS Robot Operating System (ROS) is a set of open source multi-language software elements that have been enhanced by the contributions of millions of developers and users for more than 10 years [ 12 ]. The software in ROS is organized into packages, which are supposed to constitute useful modules and which make collaborative development easier. The large number of packages available for ROS provides collections of hardware abstraction, device drivers, libraries, visualizers, and tools which facilitate the development of robust robotic systems with different robotic platforms [ 27 ]. For instance, the use of commercial sensors and actuators is usually trivial when programming with ROS due to most of them count on dedicated ROS packages developed by other users or even by the manufacturer itself. Figure 2.1 ROS logo. ROS-based systems consist of different processes connected in a peer-to-peer topology. The processes running in a ROS program are called nodes and communicate with each other by sending messages, which are predefined data structures based on standard types (integer, floating point, etc.). A node can send a message by its publication in a topic to which the other nodes are subscribed or not, depending on whether they need the data published in it. Furthermore, ROS provides a mechanism to perform synchronous transactions, the services. The services are provided by nodes acting as servers and are called by other nodes acting as clients. Each call to a service can send a message that is received by the server node. After processing the data, the server node can attach another message with the response that the client would receive. [32] 2.2 Software tools 5 In order to allow the easy simulation of robotic platforms, ROS count on packages which provide wrappers around Gazebo [ 2 ]. This simulator was designed to quickly develop and test robotic systems by accurately reproducing the dynamic environments a robot may encounter [ 30 ] and to date is one of the most popular simulators for robotics [ 19 ]. This tool is especially useful for this thesis since task management can be completely tested with simulations, avoiding the difficulties and risks which would imply the use of real aerial vehicles. 2.2.2 UAL Unmanned aerial vehicle Abstraction Layer (UAL) is a software framework for UAVs which provides an interface independent of the autopilot to perform a set of functionalities normally required by these systems. It is an open source tool developed in ROS C++ composed of 3 layers. The first of them is the UAL itself, which provides a double interface to request the execution of actions and to receive information. As shown in Figure 2.2, one of them is based on ROS topics and services, and the other one is accessible by creating an UAL class object and calling to its methods. Both of them can be used at the same time. The second layer is the Backend, which links the UAL with the last layer by a common interface with the UAL. Lastly, the third layer is a specific backend that connects to the autopilot taking into account the specific details of the communication with that autopilot. In this way, the high-level control programs developed using this tool can be used on different platforms with different autopilots after minimal changes in the software. [33] Figure 2.2 Interfaces provided by the UAL to request actions or receive information. The system developed in this thesis will use the UAL to receive information related to the pose, the velocity, and the state of the vehicle. Furthermore, every movement of the system will be executed by the UAL. For instance, the actions to take off, land and go to specific waypoints in addition to the inspections or installations which require moving the robot. 6Chapter 2. Related work 2.2.3 PILOTING-MAVSDK library The system proposed in the project has different robots which, as will be addressed afterwards, are controlled by a ground control station. To develop a functional system, both the robots and the station have to receive and send commands and information to each other. The communication between these terminals with the standard types of message (integer, float, etc.) would be not intuitive, and due to the different partners participating in the development which will use the communications, programming intuitively is almost obligatory. Therefore, the partner which has developed the ground station (CATEC) has also created a dedicated library, the PILOTING-MAVSDK library [ 15 ] to facilitate the development of the system. This library is an adaptation of another library to be used in the PILOTING project, the MAVSDK library [ 14 ]. The MAVSDK library is an Application Programming Interface (API) to perform communications based on the MAVLink messaging protocol [ 3 ], which is widely used in communications with drones and between onboard drone components. Therefore, the library offers a set of messages and interfaces to communicate different terminals adapted to the PILOTING project. Lastly, as most partners use ROS as the framework to develop the robots, CATEC also provides a ROS wrapper of the library. This wrapper will facilitate the communications with the ground station by providing a ROS node in charge of it. 3 Architecture 3.1 Introduction As previously mentioned, this bachelor’s thesis is involved in the PILOTING project in which there are many robots that have to gather information, to process it, to share it, etc. Furthermore, most of these robots and all the infrastructure necessary to manage the information flow with them are developed by diverse partners. As a result, there is a global architecture of the project related to the different robots and the infrastructure to share data with the robots, as well as a local architecture in each of the components of the project. The purpose of this chapter is to address not the architecture of the entire project but the architecture related to the communication with the robots and the local architecture of the VIAD-DRONE and the TTDRONE, the robots developed by the University of Seville and, as a consequence, the robots for which a mission manager has been elaborated in this thesis. Thus, the structure of this chapter will be as follows. Firstly, it starts with a brief overview of the way in which the missions are designed and sent to the robots, highlighting the tools developed in the project to face the different challenges. Then, it focuses on the VIAD-DRONE and the TTDRONE, addressing a detailed explanation of the high-level modules present in them, the information sent to each other, and how they have to act according to the received data. 3.2 Mission architecture in PILOTING project The main goal of the PILOTING project is to identify defects in constructions. Consequently, what is really important in the project is the data generated and the data available to plan effective missions and to draw conclusions, which requires the existence of a data server that allows the storage and the treatment of this data. In the project, it has been called Data Management System (DMS) and it is accessible by different Human Machine Interfaces (HMI), by the artificial intelligence services or by the installed sensors as shown in Figure 3.1. At first, inspection tasks are created and included in the DMS by one of the HMIs, the Visualization Portal. The inspection tasks consist of a point cloud of the environment where the robot will navigate and highlighted points to be inspected or where installations must be done. Most of the times, these highlighted points are specific zones, as it could be a bearing, which would be identified directly by a human, but in certain cases, the selection of the points will be based on the data obtained in previous missions and the treatment of this data by the artificial intelligence services. As a result, there are two common problems for each of the robotic systems in the project that are the sending of the inspection tasks to the robots and the generation of the post-mission data to be uploaded into the DMS. 7 14 Chapter 3. Architecture To have all the functionalities available in the gRCS after the connection with the robot, the downloading of the high-level actions, the alarms, and the checklist is required. In Figure 3.4, it is shown how the gRCS would receive and represent the localization of the robots, how high-level actions would be available, or how the status of the alarms would be received after the pertinent actions. Figure 3.4 gRCS interface after connecting to a robot. The main purpose of the gRCS is to design and send a mission to the robot. For that, it counts on another interface where the mission can be designed. This interface, which is shown in Figure 3.5, allows the user to create waypoints and change their fields, as well as to define the robot’s home position. Figure 3.5 gRCS interface to design a mission. 3.3 VIAD-DRONE and TTDRONE architecture 15 Once a mission is designed, all the worker would have to do is send the home position, upload the mission to the robot, download it from the robot to detect possible mistakes, and set the current waypoint to start the mission. During the execution of the mission, he will be able to command high-level actions and see its progress visually. 3.3.6 Mission Manager The MISSION MANAGER module is responsible for receiving the list of waypoints from the gRCS, for checking it, for planning the mission and for executing it by making calls to the action server. Each partner in the project has to adapt its robot to the gRCS and that is why when the gRCS sends a list of waypoints, they must have a specific configuration. This will allow the planner to identify from the list of waypoints the points where the inspections and the installations have to be done, apart from other details such as the surfaces to do the installations or the positions from where it must perform the inspections. When the bearing inspections, the bearings have to be inspected in pairs. They must be marked with ACTION waypoints and must be preceded by a POSE waypoint at which the attachment to the viaduct deck is supposed to be done. In order to differentiate this contact waypoint from other POSE waypoints, its z axis must point down. There can be as many inspections as the worker desires as long as each contact waypoint is followed by two ACTION waypoints. There can be intermediate POSE waypoints between the inspections through which the robot must pass. Something similar is required for the box installation. Whereas when the bearing inspections each contact waypoint must be followed by two ACTION waypoints associated with the bearings, in the use case of the sensor box, each contact waypoint has to be followed by a single ACTION waypoint related to the position where the box has to be installed. Unlike the bearing inspection, only one box can be transported by the robot, and consequently, only one installation can be done in a single mission. There can also be intermediate waypoints. About the target installation, just like with the box installation, only one installation can be done in a mission due to only one target can be carried with the stick. To define the installation surface, a POSE waypoint will be used since the ACTION waypoints do not provide the orientation, which is needed to define a surface with a single waypoint. In order to distinguish the installation waypoint from the intermediate waypoints, it must be preceded by an ACTION waypoint. If a POSE waypoint is preceded by an ACTION waypoint but it does not define a vertical surface, the mission will not be accepted. Furthermore, the pair of waypoints must be close to each other to avoid mistakes of the worker using the gRCS. There can be intermediate waypoints after and before the two waypoints associated with the installation, but not between them. Finally, the requirements for the tunnel inspection use case are similar to those for the target installation. As the UAV has to know from where it must take the pictures, the orientation of the waypoints is required. Therefore, each inspection will be marked with a POSE waypoint preceded by an ACTION waypoint. Nevertheless, in this case there is no limit in the number of inspections due to the fact that the camera can take as many pictures as necessary. There can also be intermediate waypoints between the inspections. When a mission is sent from the gRCS to the robotic system, the PARSER checks if its waypoints follow the rules for the current use case. If the check is successful, the planner will go through the received list of waypoints and for each one or for each group of them it will associate one or more actions which the EXECUTION submodule must perform. These actions will be inserted into a new list of waypoints, in which each waypoint will be related to a specific action. 16 Chapter 3. Architecture For all the use cases, if the planner receives an intermediate waypoint, it will push a GO_TO_- WAYPOINT action. Furthermore, each waypoint can be an autocontinue waypoint or not, which means that once a waypoint has been reached, the robot has to wait for a command to continue the mission. When one of these intermediate waypoints is not autocontinue, a waypoint to pause the mission will be added after the one to go. Whereas the intermediate waypoints are handled in the same way in all the use cases, the ACTION or contact waypoints will be associated to different groups of actions depending on the use case. If a contact waypoint is received when the bearings inspections, first of all, a GO_TO_WAYPOINT action will be added to a point at a configurable distance under the contact point. As shown in Figure 3.6, the pictures will be taken from a point in the line that joins the contact point and the point in the middle of the bearings at a predefined distance from this intermediate point. This calculation is done without considering the Z axis, meaning the problem is solved with the projection of the waypoints in an horizontal plane. As the caterpillar system is supposed to move the robot without changes of direction, the GO_TO_WAYPOINT action will place the robot with the proper orientation to allow the robot to reach the point from where the pictures have to be taken. Having made these calculations, the actions of attaching, crawling to the calculated point, inspecting both bearings, crawling back, and detaching are added to the list. Figure 3.6 Planned position to inspect bearings. All the pictures will not be taken properly and this might be detected with the extra ground station that is very likely to be used. As a result, if an ACTION waypoint is a non-autocontinue waypoint, the UAV will wait a command to continue the mission after taking the picture. Therefore, the worker will be able to watch the picture and repeat it if necessary before continuing the mission. If the contact point is not autocontinue, the UAV will wait for the command after performing the inspections and detaching from the viaduct deck. The planning of the missions at the box installation use case is very similar to the previous one. However, instead of crawling to the intermediate point between the two bearings, the robot will crawl to the point where the box has to be installed as shown in Figure 3.7. This point will be marked with an ACTION waypoint that the PARSER would have previously ensured it follows the contact waypoint. In contrast, as the installation cannot be repeated, if the contact or the ACTION waypoints are non-autocontinue, the robot will be paused after the installation and the detachment. 3.3 VIAD-DRONE and TTDRONE architecture 17 Figure 3.7 Planned position to install sensor box. About the target installation, while going through the waypoint list, the ACTION waypoints will indicate that the installation must be done there. Just like in the previous use cases, the first action to perform an installation will be a GO_TO_WAYPOINT action to a point with a specific orientation. In this case, the point will be situated in front of the surface on which the target has to be installed at a configurable distance and with the stick pointing to the surface as shown in Figure 3.8. As the ACTION waypoints do not provide orientation, they cannot define a plane, and consequently, to know the orientation of the wall and the side on which the action has to be performed, the POSE waypoint that follows the ACTION waypoint is used. This POSE waypoint will have its z axis perpendicular to the wall. If one of the waypoints associated to the installation is non-autocontinue, the command will be expected after the performance of the installation. Figure 3.8 Planned position to install target. 18 Chapter 3. Architecture Finally, the planner for the tunnel inspections will also detect the requirement of an inspection when it comes across an ACTION waypoint. In this use case, the planner will also start with a GO_TO_WAYPOINT action. As shown in Figure 3.9, the point to go will be situated in front of the surface of the tunnel at a configurable distance. The robot needs to know the orientation of the wall in order to go to a proper position before taking the picture. That is why the POSE waypoint after the ACTION waypoint is required. Furthermore, the robot will calculate the point to go taking into account that it is the camera that has to be positioned at the point. Finally, in this use case, the orientation of the vehicle must never change due to the wire connected to it. Figure 3.9 Planned position to inspect tunnel defects. Once a mission is accepted and planned, the gRCS can send a request to set the current waypoint, that is the starting point of the execution. It should be noted that not all the waypoints are acceptable to start a mission and not all of them can easily be matched with the planned actions. There is no problem with the intermediate waypoints which can be effortlessly associated to its GO_TO_- WAYPOINT action. In contrast, when the set waypoint is an ACTION or contact waypoint in the case of the bearings inspection or the box installation, the mission cannot start with an inspection, an installation, or an attachment directly. As the missions should start with a GO_TO_WAYPOINT action, in case of being set one of those waypoints, the execution will begin with the GO_TO_- WAYPOINT action previous to the attachment. Equally, for the target installation and the tunnel inspections, if the inspection is chosen to start with, the mission will change the first action to the previous GO_TO_WAYPOINT. Once the action to start with is selected, all the EXECUTION submodule has to do is to execute the planned actions in the correct arrangement. In addition, it has to manage the PAUSE/CONTINUE or STOP AND LAND high-level actions and to refuse incompatible requests with the execution of the mission. The mission manager is also responsible for the execution of the aforementioned extra functionalities. Firstly, the checklist, the list of available alarms, and high-level actions has to be sent to the gRCS. After that, the status of the alarms and the localization have to be sent periodically to the gRCS and the commands to execute a high-level action must be managed even if there is not a mission running. Lastly, the mission manager can receive a new home position of the UAV in the global reference frame and the mission manager will have to act accordingly. 3.4 Conclusion 19 3.4 Conclusion The architectures presented in this chapter contextualize the scope in which the mission manager will be used, which is essential to the design of its functioning. All the information sent to the mission manager comes from the gRCS, which works as an interface between the data server and the robots. This flow of communication is conditioned to the gRCS as it is used by all the robots and cannot be modified to meet the requirements of each one. Furthermore, the mission manager has to command actions to the robot which is done by the action server. As a consequence, the implementation of the mission manager will consist of providing all the desired functionalities to the robot by making calls to the action server and relieving the limitations imposed by the gRCS. 4 Implementation 4.1 Introduction The previous chapter has dealt with the different modules present in the software of the robots, the functionalities they perform, and the communication flow between them. Now, how these modules have been implemented in ROS C++ will be addressed emphasizing the nodes, algorithms, and tools used to develop a fast and robust program. As not all the modules are related to the mission management, the implementation of all the modules will not be completely described, focusing on those which are part of the scope of this thesis. The structure of this chapter is as follows. First of all, an overview of the different nodes and their functionality is given. Then, the nodes are explained individually in different sections, emphasizing the one for the mission manager. For this node, all its functionalities have been addressed after a brief summary of its required performance and its operation. 4.2 Structure of nodes Firstly, there is a node in charge of the communication between the gRCS and the robots which is related to the MAVSDK-ROS module, the MAVSDK-ROS node. As this node has been developed by another partner, a brief overview of its functioning will be addressed instead of a full explanation. What is important of this node is that all the messages sent by the gRCS through the IP connection are turned into ROS topics and services, and vice versa. The previous node is connected to the mission manager node, which is the most important node in this thesis. As implied by its name, this node is associated with the mission manager module. Thus, it will receive the mission to be executed and will check, plan, and execute it by making calls to the action server in addition to other functionalities such as the management of high-level actions, alarms, etc. The services and topics involved in the communication between these pair of nodes are shown in Table 4.1 and Table 4.2, respectively. The services to ”set upload” high-level actions, checklist or alarms are used to send to the gRCS the corresponding list of items, something required due to the aforementioned problem of the different robots with the same gRCS. Those to update the current or the reached waypoint refer to the progress of the missions. Before starting the performance of the actions related to a waypoint, this waypoint must be marked as the current waypoint by calling the pertinent service. In the same way, every time the actions related to a waypoint are finished, the waypoint must be marked as reached with the call to the corresponding service. Finally, the ”set_upload_waypoint_list” service allows the mission manager to send back the received waypoint list to the gRCS. 21 22 Chapter 4. Implementation Table 4.1 ROS services provided by the MAVSDK-ROS and the mission manager nodes. MAVSDK-ROS Mission manager mavsdk_ros/set_upload_waypoint_list inspection mavsdk_ros/update_current_waypoint_item command mavsdk_ros/update_reached_waypoint_item mavsdk_ros/set_upload_hl_action mavsdk_ros/set_upload_checklist mavsdk_ros/set_upload_alarm The ”inspection” service is the mechanism provided by the mission manager node to check and plan missions, whereas the ”command” service is used to send commands, which can request the execution of high-level actions or the update of the home position. Table 4.2 ROS topics published by the MAVSDK-ROS and the mission manager nodes. MAVSDK-ROS Mission manager /mavsdk_ros/inspection_set_current /mavsdk_ros/local_position/pose /mavsdk_ros/local_position/twist /mavsdk_ros/alarm_status /mavsdk_ros/text_status When starting a mission, the gRCS sends to the robot the current waypoint through the topic ”mavsdk_ros/inspection_set_current”, to which the mission manager is subscribed. The position and the linear velocities of the robotic system are transmitted to the gRCS through the ”/mavsdk_- ros/local_position/pose” topic and the orientation and angular velocities will be sent through the ”/mavsdk_ros/local_position/twist” topic. Equally, the status of the alarms will be updated periodically in the ”/mavsdk_ros/alarm_status” topic. Lastly, the ”/mavsdk_ros/text_status” topic allows the mission manager to show text messages at the gRCS, which will be used to give explanations about the refused requests. The action server is also implemented in a single node. The calls to execute actions are not implemented with ROS services but with the ROS package actionlib as will be discussed afterwards. Finally, as the mission manager has to access to the UAL state (landed, flying, manual, etc.), it is published by this node in the ”/ual/state” topic, to which the mission manager node will be subscribed. The other nodes are related to the communication with the autopilot which controls the robots, to the drivers of the add-ons, and to the localization. As they are not involved directly in the mission management, their implementation will not be discussed in this paper. However, it should be pointed out how the nodes for localization send the estimation to the node for the communication with the autopilot, and it is this node that publishes the localization on the topic ”/mavros/local_- position/odom”. 4.3 MAVSDK-ROS node 23 The communications between the different nodes and modules have been schematized in Figure 4.1. Figure 4.1 Diagram of the ROS nodes. 4.3 MAVSDK-ROS node This node has been developed by CATEC, and consequently, its implementation will not be discussed in full. Nevertheless, it is important to include a brief explanation of the way in which it communicates with the gRCS and with the mission manager in order to give a contextualized explanation about the implementation of the mission manager afterwards. All this node does is wait information from the gRCS or from the mission manager node and forward it to the other. The connection with the gRCS is an IP connection whereas the connection with the mission manager is done through the ROS topic and services previously mentioned. When the mission manager calls the services to upload the high-level actions, the checklist or the alarms, this node stores the received information until it is updated with a new call to the service. After that, the user of the gRCS can download the available high-level actions, the checklist or the list of alarms whenever he wants due to the gRCS would be able to download them through the IP connection at any time. Therefore, as these lists are static, the mission manager only has to call the service once, ideally at the beginning. In the same way, when the mission manager calls the service to upload the waypoint list, it is stored in the node and the gRCS can request it whenever the worker wants. However, in this case, the service has to be called every time a new mission is received. In contrast, if the mission manager calls the services to update the current or the reached waypoint, this node sends the request directly to the gRCS. Equally, when the mission manager publishes the localization, the status of an alarm or a text message on the corresponding topics, the node transmits the information to the gRCS as soon as it is received. Finally, when the user sets the current waypoint to start a mission, the request is sent to the node and it publishes it in the topic to set the current waypoint also at that time. 30 Chapter 4. Implementation In regards to the velocities, both angular and linear velocities are expressed with a vector. The vector of the linear velocity indicates the direction and the magnitude of the velocity of the robot and the vector of the angular velocity defines the axis of the rotation and the magnitude of the angular velocity. Therefore, neither of them has to be translated to change their reference frame, but of course, they have to be rotated as shown in Equation 4.3 and Equation 4.4. vw r=wqhvh r(wqh)−1 ,(4.3) ωw r=wqhωh r(wqh)−1 ,(4.4) where vb a is the vector of the linear velocity of a referred to b, ωb a is the angular velocity of a referred to b, w is the world reference frame, h is the reference frame of the home position and r is the reference frame of the robot. 4.5.5 Auxiliary functions In order to simplify the implementation of the Parser and the Planner, the auxiliary functions in Table 4.5 have been created. Table 4.5 Auxiliary functions. changeRS() Refers a list of waypoints in world to the reference frame of the home position translateWP() Applies a translation to a waypoint defined in its own reference frame checkProximity() Checks if two waypoints are close enough to associate them checkHorizontal() Checks if the Z axis of a waypoint is horizontal checkContactWP() Checks if the Z axis of a waypoint is pointing down createWaypointItem() Creates a waypoint item To begin with, in spite of the fact that the gRCS may update the home position of the robot, the gRCS always sends waypoints referred to the world reference frame. As the UAL needs to receive the waypoints referred to the home position, the corresponding transformation to each waypoint is performed prior to the planning of any mission. These transformations are done by the auxiliary function changeRS() which is also a method of the mission manager class because it has to access to the class attribute that stores the transformation from world to the home position. Thus, the changeRS() method will go through the received lists of waypoints referring each point initially referred to the world to the home position. The calculations to do the transformations are also done using vectors and quaternions as shown in Equation 4.5 and Equation 4.8. hqp= (wqh)−1wqp,(4.5) hth p=hth w+wth p,(4.6) hth p=−wth h+wth p,(4.7) hth p=−(hqwwtw h hqw)−1+hqwwtw phqw −1 ,(4.8) where atc b is the translation from a to b referred to c, aqb is the rotation from a to b as a quaternion, w is the world reference frame, h is the reference frame of the home position, and p is the reference frame of the waypoint. 4.5 Mission manager node 31 Apart from this, when planning missions, there are certain cases in which it is desirable to get a waypoint in front of another one. Concretely, when the target installation and the tunnel inspections, the Planner has to place a waypoint at a certain distance in front of the walls, whose normal vector would be defined by the Z axis of the POSE waypoint. As a result, the function translateWP() has been designed to do a translation of the waypoints referred to their own reference frame as can be seen in Figure 4.5. Figure 4.5 Translation of a waypoint on its z axis. The function receives as arguments the distances to translate the waypoint in each of its axes. These distances cannot be added to the vector of the waypoint directly due to the fact that it would be referred to the home position, whose orientation is unlikely to be the same as that of the waypoint. The addition has to be done after rotating the point with the rotation from home to the point itself so that the axes of both reference frames would have the same orientation. After reversing the rotation, the waypoint would have been translated properly. The operations to do this with vectors and quaternions are shown in Equation 4.9. hth p′=hqp(hqp −1hth phqp+ptp p′)hqp −1 ,(4.9) where atc b is the translation from a to b referred to c, aqb is the rotation from a to b as a quaternion, w is the world reference frame, h is the reference frame of the home position, p is the reference frame of the waypoint, and p′is the reference frame of the translated waypoint. The function checkProximity() is used when an ACTION waypoint ought to be followed by a POSE waypoint in order to check the distance between them is not very large, which could be associated with a mistake of the user of the gRCS forgetting to include the POSE waypoint. The function returns a boolean variable which would be true if the distance between the waypoints is less than a threshold or false otherwise. The calculation of the distance between the waypoints has been considered simple enough to be omitted. checkHorizontal() and checkContactWP() are used to check if the Z axis of a waypoints is horizontal to the ground plane and pointing downward, respectively. Both checks require knowledge of the Z axis of the reference frame associated to the waypoint, which is not known at first since the 32 Chapter 4. Implementation orientation is expressed with quaternions. However, it can be effortlessly obtained from the rotation matrix associated with the quaternion, which in turn can be calculated as shown in Equation 4.10. Once the rotation matrix is obtained, the z axis is associated with the third column of the rotation matrix. R(q) =   1−2(q2 y+q2 z)2(qxqy−qwqz)2(qxqz+qwqy) 2(qxqy+qwqz)1−2(q2 x+q2 z)2(qyqz−qwqx) 2(qxqz−qwqy)2(qyqz+qwqx)1−2(q2 x+q2 y)  ,(4.10) where R(q) is the rotation matrix associated with the quaternion q , which is defined as q=qw+ qxi+qyj+qzk After obtaining the vector of the axis, the checks can be easily done by checking its third component. When the vector is horizontal, this component has to be 0, so the function verifies that the condition is met and returns true or false depending on the result. Something similar is done by the function to check if the vector is pointing down, but in contrast, in this case the component has to be -1. Nevertheless, in most of the cases these values will not be exactly 0 or -1 because they are defined by a human in the gRCS and the orientation might be slightly different from the desired one. As a consequence, the functions use a threshold to avoid rejecting acceptable waypoints. Lastly, there is a function to create a waypoint item (createWaypointItem()) just as those to create alarms, checks, and high-level action items. All of them receive the desired content of the fields as arguments and return the properly initialized item. 4.5.6 Parser The purpose of the Parser is to ensure that the received waypoint list meets the restrictions imposed by the Planner, that requires the waypoints to have a specific configuration. That is why the method associated with the Parser,checkMissionCompatibility(), is run by inspectionMissionCallback() just before the planning of a mission every time the service inspection is called. The method receives the waypoint list to be checked as its argument and returns a boolean value according to the result of the check. This check will be completely different depending on the use case since a distinct configuration is required by each planner. However, all of them are based on going through the waypoint list checking that each type of waypoint is followed or preceded by the necessary type of waypoint and that each waypoint has a proper orientation or position if applicable. Furthermore, if the Parser rejects a mission, it has to send a message to the gRCS with the reason for which the waypoint list was refused. This message can be sent by publishing in the /mavsdk_ros/text_status topic after its definition. Every time a condition is not met, the message is updated with the explanation associated with that incompatibility. When the parser finishes the checks, the last detected mistake is sent to the gRCS if and when the waypoint list is not acceptable. Of course, if the waypoint list misses more than one requisite, only one of them will be notified, but nevertheless, the mission will not be accepted until the waypoint list is completely faultless and the user would receive the other messages when he repairs the notified mistake and he resends the mission. 4.5 Mission manager node 33 Bearing inspections In the case of the bearings inspections, the Planner needs at least two ACTION waypoints preceded by a contact waypoint. As mentioned, the contact waypoints are characterized by its Z axis, which must be pointing down. Although the identification of contact waypoints might muddle the implementation of this check, it can be done with a single call to the function checkContactWP(). As a result, the algorithm followed by the Parser in this use case is shown in Code 4.3. It is important to point out that any action is done when an intermediate waypoint is received as they are always accepted. Besides, despite skipping the ACTION waypoints when a contact waypoint is identified, when it comes across an ACTION waypoint, it has to check if it is preceded by a contact waypoint. This is due to the ACTION waypoints are exclusively skipped when the action server is strictly followed by two ACTION waypoints, meaning that if a contact waypoint is followed by only 1 of them, the mistake would be incorrectly associated with an ACTION waypoint without a previous CONTACT waypoint. Code 4.3 Pseudocode to check a waypoint list in the use case of the bearing inspections. compatible = true numero_insp = 0 FOR waypoints IN waypoint_list SWITCH <waypoint type> CASE POSE IF <contact waypoint> numero_insp = numero_insp + 1 IF <followed by two action waypoints> <skip action waypoints> ELSE <update error message> compatible = false END IF END IF BREAK CASE ACTION IF <not preceded by contact waypoint> <update error message> compatible = false END IF BREAK CASE OTHER <update error message> compatible = false BREAK END SWITCH END FOR IF compatible = true AND numero_insp > 0 RETURN true ELSE <send message to gRCS> RETURN false END IF 34 Chapter 4. Implementation Box installation The Planner for this use case is very similar to the previous one. Even so, whereas when the bearings inspection there must be at least one inspection, in this case there must be only one installation. Furthermore, the contact point has to be followed by only 1 ACTION waypoint. The algorithm followed by the Parser is as shown in Code 4.4. Unlike in bearing inspections, if an ACTION waypoint is received, it will be directly assumed as an error due to the contact waypoints always skip the ACTION waypoints. Code 4.4 Pseudocode to check a waypoint list in the use case of the box installation. compatible = true numero_inst = 0 FOR waypoints IN waypoint_list SWITCH <waypoint type> CASE POSE IF <contact waypoint> numero_inst = numero_inst + 1 <skip action waypoint> IF <not followed by an action waypoints> <update error message> compatible = false END IF END IF BREAK CASE ACTION <update error message> compatible = false BREAK CASE OTHER <update error message> compatible = false BREAK END SWITCH END FOR IF compatible = true AND numero_inst = 1 RETURN true ELSE <send message to gRCS> RETURN false END IF 4.5 Mission manager node 35 Target installation In this case, each ACTION waypoint has to be followed by a POSE waypoint to provide the orientation. In addition, the pair of ACTION and POSE waypoints have to be close to each other and the orientation of the POSE waypoint has to be with the Z axis horizontal. Both of these conditions can be checked with the aforementioned functions checkProximity() and checkHorizontal(), respectively. The algorithm followed by the Parser is shown in Code 4.5. Code 4.5 Pseudocode to check a waypoint list in the use case of the target installation. compatible = true numero_inst = 0 FOR waypoints IN waypoint_list SWITCH <waypoint type> CASE ACTION numero_inst = numero_inst + 1 <skip action waypoint> IF <followed by pose waypoint> IF <close waypoints> IF <pose waypoint not horizontal> <update error message> compatible = false END IF ELSE <update error message> compatible = false END IF ELSE <update error message> compatible = false END IF BREAK CASE POSE BREAK CASE OTHER <update error message> compatible = false BREAK END SWITCH END FOR IF compatible = true AND numero_inst = 1 RETURN true ELSE <send message to gRCS> RETURN false END IF 36 Chapter 4. Implementation Tunnel inspections In the same way as in the target installation, when the tunnel inspections, each ACTION waypoint has to be followed by a close POSE waypoint. In contrast, there is no need for the orientation to be horizontal. The algorithm followed by the Parser is shown in Code 4.6. Code 4.6 Pseudocode to check a waypoint list in the use case of the tunnel inspections. compatible = true numero_insp = 0 FOR waypoints IN waypoint_list SWITCH <waypoint type> CASE ACTION numero_insp = numero_insp + 1 <skip action waypoint> IF <followed by pose waypoint> IF <not close waypoints> <update error message> compatible = false ELSE <update error message> compatible = false END IF ELSE <update error message> compatible = false END IF BREAK CASE POSE BREAK CASE OTHER <update error message> compatible = false BREAK END SWITCH END FOR IF compatible = true AND numero_insp > 0 RETURN true ELSE <send message to gRCS> RETURN false END IF 4.5 Mission manager node 37 4.5.7 Planner In the same way as the Parser, the planner will perform a different planning for each use case just after the execution of checkMissionCompatibility(), if applicable. However, this planning is not performed by the callback of the inspection service since, as mentioned, the execution of the callbacks has to be done as fast as possible. Consequently, the callback stores the received waypoint list in a class attribute and starts the thread to do the planning. The method executed by the thread is the missionPlanning() method which not only does it call the method associated with the planner for the use case but also resends the received list of waypoints to the gRCS as required. It should also be noted that the waypoint list must be modified by the changeRS() function before the execution of the methods missionPlanningBearings(),missionPlanningBox(),missionPlanningTarget() or missionPlanningTunnel(). All the methods to do the planning itself consist of going through the received waypoint list while generating a new one in which each waypoint is associated to an action of the action server. The new waypoint list is stored in the same attribute in which it is received and which will be read while the execution to perform the actions in single line. As a result, the inspection service callback has to refuse any mission if there is a mission running at that moment because the execution requires the stored list and the planning would modify it before the end of the execution. The command field of the waypoints is filled with MAV_CMD messages, which are enumerations related to actions frequently used in ground control stations provided by the MAVSDK library. In the received waypoint list, this field distinguishes ACTION waypoints (MAV_CMD_NAV_INSP_- POINT) from POSE waypoints (MAV_CMD_NAV_WAYPOINT_QUATERNION) whereas after the planning of the mission, it is used to associate the waypoints with the available actions as shown in Table 4.6. Table 4.6 Correspondence between actions and MAV_CMDs. Action Command field TAKE_OFF MAV_CMD_NAV_TAKEOFF LAND MAV_CMD_NAV_LAND GO_TO_WAYPOINT MAV_CMD_NAV_WAYPOINT ATTACH MAV_CMD_USER_1 DETACH MAV_CMD_USER_2 CRAWL_TO_WAYPOINT MAV_CMD_USER_3 INSPECT_BEARING MAV_CMD_NAV_INSP_POINT INSTALL_SENSOR_BOX MAV_CMD_NAV_PAYLOAD_PLACE INSTALL_TARGET MAV_CMD_NAV_PAYLOAD_PLACE INSPECT_TUNNEL_DEFECT MAV_CMD_NAV_INSP_POINT TAKE_OFF_FROM_UGV MAV_CMD_NAV_TAKEOFF LAND_ON_UGV MAV_CMD_NAV_LAND PAUSE MAV_CMD_WAYPOINT_USER_1 38 Chapter 4. Implementation Something similar is done with the autocontinue field, which indicates if the mission has to be paused after reaching that waypoint in the received waypoint list, but in contrast, it is used to identify the received waypoints among the planned waypoints after the planning. On the one hand, during the planning, a PAUSE action is pushed every time a waypoint is non-autocontinue so that the execution would wait for a continue command at that point. On the other hand, the updating of the current and the reached waypoint has to be sent to the gRCS while the execution. The updates are done with calls to the mavsdk_ros/update_current_waypoint_item and mavsdk_ros/update_reached_waypoint_item services by sending as arguments the current or reached waypoint as its number in the received waypoint list. However, once the mission is planned, the received waypoint list is lost, and that is why the planner uses the autocontinue field to mark planned waypoints as waypoint related to the received waypoints. This association is not always trivial since there are waypoints and group of waypoints which generate group of waypoints. As the execution of a mission might start with any of the waypoints in the received list, the planner will not include the takeoff in the waypoint list. It is the execution that will call for an action to take off before the execution of the planned actions. As a result, the planner starts directly generating actions with the received waypoints that can be ACTION, POSE or CONTACT waypoints. POSE waypoints are processed equally by all the planners by generating a waypoint with the command MAV_CMD_NAV_WAYPOINT to go to that waypoint followed by a waypoint to pause the mission if it is non-autocontinue. In contrast, the actions generated for ACTION or CONTACT waypoints differ for each use case as it is addressed below. Bearing inspection In this case, the ACTION waypoints are associated with the bearings and are always in pairs and preceded by a CONTACT waypoint. Therefore, the planner searches for CONTACT waypoints, plans actions using the following ACTION waypoints, and skips the ACTION waypoints. The planning consists of the following actions which are added to the new waypoint list with the agreement in Table 4.6. •Go to a waypoint under the CONTACT waypoint •Attach to the viaduct deck •Crawl to a point in front of the bearing •Take a picture to the first bearing •Take a picture to the second bearing •Crawl to the CONTACT waypoint •Detach from the viaduct In order to calculate the point in front of the bearings, the planner calculates a vector from one bearing to another. Then, the vector is divided by 2 and used to calculate another vector from this intermediate point to the contact point. Finally, this last vector is normalized and multiplied by the configurable distance which is received as an argument with a yaml file. As can be seen in Figure 4.6, these calculations are performed without taking into account the z component, meaning that the calculations are done with a projection of the waypoints on the viaduct deck where the robot will be attached. Furthermore, the orientation before the attachment has to be established so that the caterpillar system is in line with the target point. 4.5 Mission manager node 39 Figure 4.6 Calculated vectors in planning for bearing inspections. About the correspondence between the planned actions and the received waypoints, the CONTACT waypoint will be associated with the action of attaching and the ACTION waypoints with the actions to take pictures. In this way, the reached waypoint progresses when the robot does the attachment or takes the pictures, which has been considered the most intuitive solution to the problem. Lastly, the PAUSE action for non-autocontinue waypoints is added after the pictures for ACTION waypoints or after the detachment for POSE waypoints. Box installation The planning for this use case is similar to the planning for the bearing inspections. However, in this case, there is only one ACTION waypoint where the box has to be released. As a result, the point to crawl to will be the ACTION waypoint and the only thing which has to be calculated is the orientation. The list of ACTIONS generated when the planner comes across a CONTACT waypoint would be as follows: •Go to a waypoint under the CONTACT waypoint •Attach to the viaduct deck •Crawl to the ACTION waypoint •Release the sensor box •Crawl to the CONTACT waypoint •Detach from the viaduct In the same way as when the bearing inspection, the contact waypoint is associated with the attachment and the action waypoint with the action to release the box. Finally, for the sake of simplicity, if any of the waypoints are not autocontinue, the PAUSE action will be included after the detachment since the installation cannot be repeated and it would make no sense pausing the execution of the mission there. Target installation When the target installation, there are no CONTACT waypoints so that the waypoints with their z axis pointing downward are also treated as intermediate waypoints. The only waypoints that receive a specific planning are the ACTION waypoints, which are always followed by a POSE waypoint in charge of providing the orientation of the surface. 46 Chapter 5. Experimental results Figure 5.2 Interface of the visualization portal to create inspection plans. Figure 5.3 Interface of the gRCS after the download of an inspection plan. Regarding the simulation world, a 3D model based on the point cloud of the inspection plans has been used. This point cloud is provided by Ferrovial, the partner of the project related to the viaducts, so that the point cloud is the same which will be used by the real robots in the future. As of today, there is no point cloud available for any tunnel but, nevertheless, the functionalities of the system for this use case can be checked in the world of the viaduct without problem. The structure of this chapter is as follows. To begin with, the download of the alarms and the checklist are tested. Then, the verification of the functionalities related to the localization and the home position is addressed. After this, the Parser,Planner and Execution sub-modules of the mission manager are tested. Lastly, the execution of high-level actions is checked. 5.2 Alarms and checklist 47 5.2 Alarms and checklist After the connection of the simulated robot to the gRCS, the download of the alarms and the updating of their state have been tested by changing the topic they use to check the condition and by manual publications in them. The reason why the alarms have not been tested with the real conditions is that in the simulation, the battery level is always correct and the manual mode can not be set. Figure 5.4 shows the information correctly displayed by the gRCS after the downloading of the alarms and the publication of some messages to generate true and false conditions. The publication of repeated false or positive conditions has been taken into account in order to check the correct functioning of the count of the warns. Figure 5.4 Alarms displayed by the gRCS after their download and update. The checklist download has also been tested and, as shown in Figure 5.5, is also sent and received without errors. Figure 5.5 Available checklist in the gRCS after its download. 48 Chapter 5. Experimental results 5.3 Telemetry and home position As shown in Figure 5.6, after the connection of the robot to the gRCS, the robot updates the localization with the data received from the autopilot. (a) Before the connection. (b) After the connection. Figure 5.6 Localization data received and displayed by the gRCS. In regards to the updating of the home position, it has been checked by sending a new one when the robot is positioned so that the robot’s reference frame is in the same place as the reference frame of the home position. Figure 5.7 shows arithmetically and visually how after updating the home position to the point x=1 , y=2 , z=0 , roll =0 , pitch =0 , and yaw =30 the robot position changes to the proper place. It should be noted that the gRCS uses the notation East, North, and Up instead of X, Y, and Z to name the axes. (a) Received data. (b) Visual representation. Figure 5.7 Result of the update of the home position. 5.3 Telemetry and home position 49 The transformations of the waypoints before the planning with new home positions and the sending of the localization in the global reference frame can be checked by the execution of a mission to follow known waypoints after the sending of a new home position. The waypoints used in this test define a square of four meters on each side at a constant height. The new home position is also that at x=1 , y=2 , z=0 , roll =0 , pitch =0 , and yaw =30 . Figure 5.8 and Figure 5.9 show the waypoints in conjunction with the localization sent and received by the mission manager. It should be noted how the robot is able to follow the waypoints referred to the world reference frame and how the received localization is correctly transformed with the transformation of the home position, proving the correct functioning of the system. Figure 5.8 Positions received and sent by the robot to the gRCS during the execution of a mission after the change of the home position. Figure 5.9 Top view of the positions received and sent by the robot to the gRCS during the execution of a mission after the change of the home position. Regarding the linear velocity, as the sides of the previous square are in line with the axes of the global reference frame, each movement of the robot implies velocity values in only one of the axes simultaneously. Figure 5.10 shows how, after performing the transformation correctly, the robot sends only the velocity values on one axis at the same time, although it does not receive velocities 50 Chapter 5. Experimental results in line with the axes. Figure 5.10 Velocities received and sent by the robot to the gRCS during the execution of a mission after the change of the home position. To verify the correct transformations of the orientation and the angular velocity, a mission has been created to take off the robot, rotate its yaw to 0º, rotate it to 90º, rotate it to 0ºagain, and repeat these actions with -90º. Before the execution, the home position has been moved to the same position used in the previous test. Figure 5.11 shows how the sent yaw follows the desired angles and how both yaws are 30ºapart from each other, which is the orientation of the home position. In regards to the pitch and the roll, the robot can not perform these rotations so that both values are always very close to 0º. In addition, as the yaw of the home position is not very large, the sent values are very similar to the received ones. Figure 5.11 Orientation received and sent by the robot during the execution of a mission to rotate the robot after the change of the home position. Finally, Figure 5.12 shows how the angular velocity sent by the robot is the same as that received. This is because the rotation of the home position is around the z axis, that is the only axis around which the robot can rotate. The velocities would have been different if the rotation of the home 5.4 Parser 51 position had been around a different axis but, however, the gRCS only allows the user to change the yaw of the home position as the home position should be in an horizontal plane. Figure 5.12 Angular velocities received and sent by the robot during the execution of a mission to rotate the robot after the change of the home position. 5.4 Parser Figure 5.13 shows the messages received and displayed by the gRCS when wrong missions are sent to the robot. Figure 5.13 Messages received by the gRCS after sending wrong missions to the robot in the bearing inspection use case. Once a mission is completely faultless, the mission manager accepts it as shown in Figure 5.14. Figure 5.14 Mission accepted by the parser. The communication flow between the gRCS and the mission manager during the two refusals and the acceptance of the mission is shown in Figure 5.15. 52 Chapter 5. Experimental results Figure 5.15 Communication flow between the gRCS and the mission manager during the sending of wrong and correct missions. In order to check the correct functioning of the parser for each use case, wrong missions have been created and sent to the robots. The distinctive features of the sent missions and the result of the checks for the different use cases are found in Appendix A. 5.5 Planner and execution Due to the complexity of proving the correct functioning of the planner in isolation, it has been tested in conjunction with the execution of the missions, which requires the correct functioning of the planner. After the acceptance of a mission by the parser, the mission manager plans the actions and after that, the gRCS can request its start by setting the current waypoint. Then, the mission manager start the execution of the actions planned for the different waypoints, updating the current and reached waypoints when it finished the execution of the actions of a specific waypoint. The communication flow between the gRCS and the mission manager during this process is shown in Figure 5.16. Figure 5.17 shows how after setting the current waypoint to 0, the mission starts going to the pertinent waypoint. In the gRCS, the current waypoint can be visually identified because its color changes to orange. Additionally, the current and reached waypoints are numerically indicated in the telemetry window. As shown in Figure 5.18, both current and reached waypoints progress properly once the robot has performed the actions associated with the waypoint. 5.5 Planner and execution 53 Figure 5.16 Communication flow between the gRCS and the mission manager during the start and execution of a mission. Figure 5.17 Mission execution before reaching the first waypoint. During the execution of the missions, the mission manager and the action server nodes print on the terminal the actions called and the actions in execution. Therefore, simple missions have been executed to test the ability of the mission manager to arrange actions based on the received waypoint lists. Each of the missions consists of a single inspection or installation, as well as intermediate waypoints to avoid obstacles if applicable. In this way, the printed messages must be associated with actions to take off, to go to waypoints, to perform the installation or the inspection, and to land. The results found in Appendix B show how the planner is able to perform the planning properly. 54 Chapter 5. Experimental results Figure 5.18 Mission execution after reaching the first waypoint. Despite the correct order of the planned actions, the points and orientations also have to be calculated correctly, which can be visually checked for the use cases of the target installation and the tunnel inspections during the execution of the missions. The configurable distances to plan the actions are 2 meters between the robot and the wall before the installation of a target and 1 meter from the camera to the wall to perform the inspections of tunnel defects. For the sake of simplicity, the offset of the camera with the TTDRONE has been considered null. Figure 5.19 and Figure 5.20 show how the robot goes to the correct place before the installation of a target or the inspection of a tunnel defect. (a) Gazebo. (b) gRCS. Figure 5.19 Positioning of the simulated robot before the installation of a target. It should be pointed out that there is a limit to the altitude at which the TTDRONE must fly, so that if the calculated waypoint implies going below that altitude, the robot will continue flying at the minimum altitude as shown in Figure 5.21. 5.5 Planner and execution 55 (a) Top view. (b) Front view. Figure 5.20 Positioning of the simulated robot before the inspection of a tunnel defect. (a) Top view. (b) Front view. Figure 5.21 Positioning of the simulated robot before the inspection of a tunnel defect at a low height. To test the calculations for the use cases of the bearing inspections and the box installation, the actions to attach, crawl, and detach have been replaced by actions to go to the calculated points. After that, the robot has executed simple missions without obstacles and the localization has been represented in conjunction with the ACTION and CONTACT waypoints. The configurable distance at which the robot must take the pictures in the bearing inspections use case has been set as 1.5 meters. As found in Appendix C, the robot goes to the correct positions before the inspection or the installation. 6 Conclusions and future work 6.1 Conclusions The aim of this thesis has been to connect a HMI developed by another partner with the robots developed by the University of Seville, providing the robots with autonomy to achieve the tasks sent by a human through the interface. The implementation of the link between these two elements has implied the understanding of the communications with the HMI and of the robotic vehicles. Moreover, the tools provided by the HMI to design the missions were very generic and, therefore, the use of them in an intuitive way to send the information to plan the missions was not trivial. In the end, the system bases the planning on its knowledge about the way in which the user will send the waypoints and their association with different parts of the environment. The developed system has been completely adapted to the HMI, meeting all the requisites and being able to perform the required functionalities. This work has presented an approach to manage the tasks a robot has to perform in a very simplified way. The method allows a human to control a robot by indicating only what it has to do and where. Although it is not a complex approach and it is not easily adaptable for different use cases, it might be interesting for systems with limited computational output, a common problem in the field of aerial robotics. 6.2 Future work The most important limitation of the developed system lies in the fact that the user has to send the waypoints following a specific format. Nevertheless, there is no alternative since the robot does not have the point cloud of the environment. If the robots had the point cloud, they would be able to plan the actions receiving only the points of the inspections or of the installations. For instance, if the robot received a point on a surface to install a target and was able to get the orientation of the surface with the point cloud, it would not need a POSE waypoint to know the orientation. Therefore, the use of the point cloud to increase the autonomy and the intelligence of the system might be an interesting area to be explored further. The implementation of this new system capable of finding the contact points or the installation surfaces by itself might be simplified by using the mission manager developed in this thesis. The new system may be designed to calculate the points which the mission manager of this thesis cannot find and prepare the waypoint list following the format. In this way, the human only would have to 63 64 Chapter 6. Conclusions and future work send ACTION waypoints at the inspection or installation points, obtaining a much more intuitive interface. Lastly, another possible improvement that could be made if the robot would count on the point cloud is the use of a path planner to avoid obstacles instead of the intermediate waypoints placed by the human. Appendix A Tables with descriptions of different missions sent to the Parser and results of their check A correct functioning of the Planner requires the waypoint list for each use case to be in a specific format. Therefore, the Parser has to reject any waypoint list that does not follow the rules imposed by the Planner. To ensure the correct acceptance or refusal of the Parser, missions with distinctive features have been sent to it. The tables in this appendix show a brief description of these missions and the results returned by the Parser, which has been the desirable one in all the cases. Table A.1 Missions sent to the robot and result of their check in the bearing inspections use case. Description Result Only intermediate waypoints Refusal Only 1 ACTION waypoint Refusal Only 2 ACTION waypoints Refusal Only 1 CONTACT waypoint Refusal 1 CONTACT waypoint followed by 1 ACTION waypoint Refusal 1 CONTACT waypoint followed by 2 ACTION waypoints Acceptance More than 1 group of CONTACT + ACTION waypoints Acceptance Table A.2 Missions sent to the robot and result of their check in the box installation use case. Description Result Only intermediate waypoints Refusal Only 1 ACTION waypoint Refusal Only 1 CONTACT waypoint Refusal 1 CONTACT waypoint followed by 1 ACTION waypoint Acceptance More than 1 pair of CONTACT + ACTION waypoint Refusal 65 66 Appendix A. Tables with descriptions of different missions sent to the Parser and results of their check Table A.3 Missions sent to the robot and result of their check in the target installation use case. Description Result Only intermediate waypoints Refusal Only 1 ACTION waypoint Refusal POSE waypoint followed by ACTION waypoint Refusal ACTION waypoint followed by POSE waypoint at long distance Refusal ACTION waypoint followed by POSE waypoint with z axis pointing to arbitrary direction Refusal ACTION waypoint followed by a close POSE waypoint with horizontal z axis Acceptance More than 1 valid pair of ACTION + POSE waypoints Refusal Table A.4 Missions sent to the robot and result of their check in the tunnel inspections use case. Description Result Only intermediate waypoints Refusal Only 1 ACTION waypoint Refusal POSE waypoint followed by ACTION waypoint Refusal ACTION waypoint followed by POSE waypoint at long distance Refusal ACTION waypoint followed by a close POSE waypoint Acceptance More than 1 valid pair of ACTION + POSE waypoints Acceptance Appendix B Messages printed by the mission manager and the action server during the execution of missions During the execution of missions, the mission manager and the action server print on the terminal messages with the actions called and in execution. In order to test the correct functioning of the Planner, simple missions composed of a single inspection or installation and intermediate waypoints have been executed. As shown in the figures of this appendix, the planner is able to plan the actions for each use case properly. Figure B.1 Messages printed by the mission manager node and the action server node during the execution of a mission in the bearing inspections use case. 67 68 Appendix B. Messages printed by the mission manager and the action server during the execution of missions Figure B.2 Messages printed by the mission manager node and the action server node during the execution of a mission in the box installation use case. 69 Figure B.3 Messages printed by the mission manager node and the action server node during the execution of a mission in the target installation use case. Figure B.4 Messages printed by the mission manager node and the action server node during the execution of a mission in the tunnel inspection use case. Appendix C Planned positions to inspect bearings or to install a sensor box The test of the correct positioning of the robot before the inspection of bearings or the installation of a box is not trivial as the actions of attach, detach, and crawl are not simulated. Consequently, this actions have been replaced by simple movements of the robot. In this way, the correct positioning of the robot can be checked visually as shown in the figures of this appendix. Figure C.1 Trajectory followed by the robot during the inspection of two bearings. 71 Bibliography [1] European commission, https://ec.europa.eu/, Accessed: 2022-05-28. [2] Gazebo, https://gazebosim.org/, Accessed: 2022-05-28. [3] Mavlink, https://mavlink.io/, Accessed: 2022-05-28. [4] Mavros, https://dev.px4.io/v1.10_noredirect/en/ros/mavros_installation.html, Accessed: 2022- 06-27. [5] Piloting, https://piloting-project.eu/, Accessed: 2022-05-28. [6] Px4 simulation, https://docs.px4.io/v1.12/en/simulation/, Accessed: 2022-06-27. [7] srv, https://wiki.ros.org/srv, 2017, Accessed: 2022-06-09. [8] actionlib, http://wiki.ros.org/actionlib, 2018, Accessed: 2022-06-07. [9] rosserial_arduino, http://wiki.ros.org/rosserial_arduino, 2018, Accessed: 2022-06-07. [10] What is arduino?, https://www.arduino.cc/en/Guide/Introduction, 2018, Accessed: 2022-06- 07. [11] msg, https://wiki.ros.org/msg, 2019, Accessed: 2022-06-09. [12] Ros, https://www.ros.org/, 2021, Accessed: 2022-05-28. [13] geometry_msgs, http://wiki.ros.org/geometry_msgs, 2022, Accessed: 2022-06-13. [14] Mavsdk, https://mavsdk.mavlink.io/, 2022, Accessed: 2022-05-28. [15] Piloting-mavsdk, https://github.com/fada-catec/piloting-mavsdk, 2022, Accessed: 2022-05-28. [16] Constructions Aeronautiques, Adele Howe, Craig Knoblock, ISI Drew McDermott, Ashwin Ram, Manuela Veloso, Daniel Weld, David Wilkins SRI, Anthony Barrett, Dave Christianson, et al., Pddl| the planning domain definition language, Technical Report, Tech. Rep. (1998). [17] Fahiem Bacchus and Froduald Kabanza, Using temporal logics to express search control knowledge for planning, Artificial intelligence 116 (2000), no. 1-2, 123–191. [18] Yixin Chen, Benjamin W Wah, and Chihwei Hsu, Temporal planning using subgoal partitioning and resolution in sgplan, Journal of Artificial Intelligence Research 26 (2006), 323–369. [19] Jack Collins, Shelvin Chand, Anthony Vanderkop, and David Howard, A review of physics simulators for robotic applications, IEEE Access 9(2021), 51416–51431. 79 80 Bibliography [20] Ken Currie and Austin Tate, O-plan: the open planning architecture, Artificial intelligence 52 (1991), no. 1, 49–86. [21] Neil T Dantam, Swarat Chaudhuri, and Lydia E Kavraki, The task-motion kit: An open source, general-purpose task and motion-planning framework, IEEE Robotics & Automation Magazine 25 (2018), no. 3, 61–70. [22] Patrick Doherty and Jonas Kvarnstram, Talplanner: A temporal logic-based planner, AI Magazine 22 (2001), no. 3, 95–95. [23] Kutluhan Erol, James A Hendler, and Dana S Nau, Semantics for hierarchical task-network planning, Tech. report, MARYLAND UNIV COLLEGE PARK INST FOR SYSTEMS RESEARCH, 1995. [24] Richard E Fikes and Nils J Nilsson, Strips: A new approach to the application of theorem proving to problem solving, Artificial intelligence 2(1971), no. 3-4, 189–208. [25] Alfonso Gerevini, Ugur Kuter, Dana Nau, Alessandro Saetti, and Nathaniel Waisbrot, Combining domain-independent planning and htn planning: The duet planner, ECAI 2008, IOS Press, 2008, pp. 573–577. [26] Alfonso Gerevini, Alessandro Saetti, and Ivan Serina, Planning through stochastic local search and temporal action graphs in lpg, Journal of Artificial Intelligence Research 20 (2003), 239–290. [27] Abdelfetah Hentout, Abderraouf Maoudj, and Brahim Bouzouia, A survey of development frameworks for robotics, 2016 8th International Conference on Modelling, Identification and Control (ICMIC), 2016, pp. 67–72. [28] Jörg Hoffmann and Bernhard Nebel, The ff planning system: Fast plan generation through heuristic search, Journal of Artificial Intelligence Research 14 (2001), 253–302. [29] Sergio Jiménez, Tomás De La Rosa, Susana Fernández, Fernando Fernández, and Daniel Borrajo, A review of machine learning for automated planning, The Knowledge Engineering Review 27 (2012), no. 4, 433–467. [30] N. Koenig and A. Howard, Design and use paradigms for gazebo, an open-source multi-robot simulator, 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566), vol. 3, 2004, pp. 2149–2154 vol.3. [31] Dana S Nau, Tsz-Chiu Au, Okhtay Ilghami, Ugur Kuter, J William Murdock, Dan Wu, and Fusun Yaman, Shop2: An htn planning system, Journal of artificial intelligence research 20 (2003), 379–404. [32] Morgan Quigley, Ken Conley, Brian Gerkey, Josh Faust, Tully Foote, Jeremy Leibs, Rob Wheeler, Andrew Y Ng, et al., Ros: an open-source robot operating system, ICRA workshop on open source software, vol. 3, Kobe, Japan, 2009, p. 5. [33] Fran Real, Arturo Torres-González, Pablo Ramón Soria, Jesús Capitán, and Anibal Ollero, Unmanned aerial vehicle abstraction layer: An abstraction layer to operate unmanned aerial vehicles, International Journal of Advanced Robotic Systems 17 (2020), no. 4, 1–13. [34] J. VINCE, Rotation transforms for computer graphics, Springer London, 2011. [35] David E Wilkins, Using the sipe-2 planning system, Artificial Intelligence Center, SRI International, Menlo Park, CA (1999).