Human-Aware Navigation (HAN) module for Thorvald robots

This repository contains a ROS package that allows the Thorvald robots to detect and safely interact with humans during logistic operations at polytunnels. The package includes a human detection system based on 2D LiDARs, RGB-D, and thermal cameras. The human information extracted from these sensors (humans position, motion, orientation, gesture) is used as input for a decision-making system that can perform safety actions such as 1) modify the robot's current goal (the goals are nodes in a topological map of the field) according to human commands (i.e. activated by gesture recognition), 2) controlling/modulating the robot velocities according to gesture recognition and/or the distance between the robot and human (when the robot is moving towards the human) 3) stop completely any operation when a human is detected within unsafe distance respect to the robot (in the case of UV-C treatment a safety distance is 7m, in the case of logistics it is 1.2m), 4) Activate audiovisual alerts in order to warn the human about the robot's current action and potential danger (critical during UV-C treatment). The components and overall system architecture are shown in the following scheme. A video with a demo can be found here.

How the Human sensing works:

• The human detection is based on information taken from two 2D LiDARs (the same used for robot localization), two RGBD cameras (realsense D455) one aligned to the robot's local x-axis (to detect a human in front of the robot), and the other one in the opposite direction (to detect a human in the back of the robot). Moreover, a thermal camera (FLIR Lepton 3.5) was mounted together with each RBGD camera using a 3D printed base in order to match the images extracted from both cameras.
• To start publishing human information, the detection system requires to be subscribed to topics published by 2D LiDARs nodes or RGBD cameras nodes, or both in the best case (for sensor fusion). When only LiDAR data is available, the human information contains only (x,y) position, a label with the motion estimation (if the human is static or not static), and the number of the area around the robot in which the human is being detected (there is a total of 10 areas around the robot, 5 correspond to the front of the robot and 5 to the back). When the RGBD data is available, then apart from the information above, the detection system delivers a human orientation label (if the human is facing or not the robot), a human posture label (if the human is performing a specific body gesture). If Thermal cameras are also publishing data (it is not mandatory), then, this information is used to robustify the human detection ( based only on RBD images) and remove false positives. The thermal information is also valuable for UV-C treatment scenarios when even if a human skeleton is not detected, but If a certain percentage of the pixel on the thermal image is higher than a threshold, then a thermal detection label is activated to alert the safety system that the robot must pause operation.
• The name of the labels corresponding to the human body gestures that can be interpreted by the robot are: "no gesture","left arm up","left hand front","left arm sideways","left forearm sideways","right arm up","right hand front","right arm sideways","right forearm sideways","both arms up","both hands front","crouched down","picking". At the moment only the following gestures are used to control the robot actions: "both hands front" to stop the robot, "left forearm sideways" to make the robot move away from the human, "right forearm sideways" to make the robot move towards the human, "left arm sideways" and "right arm sideways" to make the robot move sideways (only valid at footpaths) and "right arm up" and "left arm up" to make the robot rotate clockwise/counterclockwise (only valid at footpaths). The following figure shows examples of each gesture label.

• The name of the labels corresponding to the human motion are: "not defined","mostly static", "moving".
• The name of the labels corresponding to the human orientation are: "facing the robot", "giving the back", "left side", "right side".
• The following figures illustrate the distribution of the areas around the robot which are used for both sensor fusion (when LiDAR+RGD data is used) and safety purposes. The figure on the left illustrates a Thorvald robot moving along polytunnels where the areas from 0 to 4 correspond to frontal areas (x is positive) and from 5 to 9 correspond to back areas (x is negative). The angle a is a configuration parameter set by the user and the parameter w is chosen according to the distance between crop rows. Humans detected in green areas (pickers moving in other rows) are not considered critical, but humans in red areas (pickers working in the same row as the robot) are particularly critical. According to the figure on the right, a similar area distribution is used when a robot navigates outside polytunnels, however, there are two main differences. The first one is that more areas are considered critical (areas 1,3,6,8 are now red), and areas 2 and 7 now cover some additional space on the sides of the robot since the robot may perform sideways motion. Moreover, at footpaths, the parameters a and w can be scaled by using n,m factors in order to make the red areas to cover more space. Both distributions are valid only for logistics operations, in case of using the robot for UV-C treatment, all the areas are considered as critical. The risk level distributions illustrated in these two figures are valid only for logistics operations, in the case of using the robot for UV-C treatment, all the areas are considered critical (see safety policies subsection).

How the Decision Making works:

• The decision-making controls the behavior of the safety system based on safety policies (determined during a Hazard Analysis stage) and information delivered by the human detection system and the Thorvald navigation system.
• The safety system must always be publishing messages, even if no safety action is required. If the safety topics stop being published for a specific period of time (e.g. if the safety system node stopped suddenly), the decision-making makes the current robot action stop and activates audiovisual alerts to let the human know that the safety system is not running. When the safety system starts publishing again, the previous robot action is resumed.
• Like the safety system, the human detection system is always publishing messages, even if no human is detected. Thus, if human information is not being published for a while, the safety system makes the current robot action stop and activates audiovisual alerts to warn the human that the robot perception is not running. The robot can resume the previous action only when the human detection system is running again.
• If teleoperation mode is activated, then, any robot's autonomous action is stopped (as the standard behavior of Thorvald robots) and audiovisual alerts indicate to the human that the robot is being controlled by the joystick. When the autonomous mode is activated again, the previous action is not resumed and the robot keeps on pause, till the human gives a new command/goal.
• The information being published by the safety system include safety action label, voice message label, human command label, risk level label, operation mode label, action mode label, topological goal label, critical human detected index.
• The name of the labels corresponding to the safety actions published by the safety system are: "move to goal","reduced speed","move away from human","pause","wait for new human command","teleoperation","gesture control","no safety action".
• The name of the labels corresponding to the audio/voice messages are: "no message","alert UVC danger","ask for next action","ask for free space","moving towards you","moving away from you","moving to goal","safety system error","human sensing error","teleoperation mode","gesture control mode","collision detected".
• The name of the labels corresponding to human commands performed by body gesture are: "no command","move towards me","move away from me","stop","move forwards","move backwards","move right","move left","rotate clockwise","rotate counterclockwise".
• The name of the labels corresponding to the level of risk during the Human-Robot Interaction (HRI) are: "no human","safe","warning","danger". These labels are updated according to the safety policies (see subsection below).
• The operation mode labels can be only "logistics" or "UV-C treatment". Depending on which operation is selected, the safety system behavior will be different since the safety policies for each application are different (see subsection below).
• The action mode labels can be "polytunnel" or "footpath". This label is updated depending on robot localization, i.e. if robot is navigating outside the polytunnel or along a row inside the polytunnel. If the action mode is "polytunnel" it means that the human gestures are limited to commands that make the robot stop, approach him/her, or make the robot move away. If the action mode is "footpaths" it means that the human gestures can also control the robot's motion in any direction outside the polytunnel, including moving sideways. The robot action activated by a gesture outside the polytunnel is valid only while the gesture is being detected. On the other hand, any robot action activated by a gesture inside the polytunnel is still valid after the human stop performing that gesture.
• The robot topological goal can be set as in the standard Thorlvald navigation, i.e. by using rviz interface (clicking on a node). However, when the safety system is being used, this goal can be modified in some situations. For instance, if the robot is navigating in "polytunnel" mode, and the human performs gestures to make the robot move away or approach him/her, then the robot's goal is updated in order to make the robot move in the proper direction according to the human command.
• The human critical index corresponds to the index of the element in the human detected list which represent the most critical human to be tracked. This is critical when more than one human is detected at the same time. The selection of the most critical human to be tracked depends on the area in which the humans are located, the distance between them and the robot, and if they are performing body gestures or not.
• The audio alerts correspond to explicit voice messages which are reproduced by a speaker attached to the robot chassis. The voice messages were recorded in two languages and are reproduced in a loop till a new safety action updates the message (there is a specific message for each safety action and robot operation).
• The visual alerts are activated by an Arduino board connected to a colored beacon that changes the color or blinks according to the safety system updates. The visual alerts include: "green" when level of risk is "safe", "yellow" when level of risk is "warning", "red" when level of risk is "danger", and "none" when the robot is not active yet (no goal has been selected). The blinking states include: "green blink" when "teleoperation" mode is activated, "yellow blink" when no human is detected (but the human detection is working well), and "red blink" in case of detecting failures in the safety system or the human detection system.

HAN POLICIES:

To minimize the risk of getting human injuries during HRIs, the following safety policies were considered for the decision-making:

• If a human is detected in critical areas (red color areas) and he/she is occluding the robot path, then, the distances "d_log" used to classify the risk of collisions during "logistics" operations are: 0<d_log<1.2m ("danger"), 1.2m<d_log<3.6m ("warning"), d_log>3.6m ("safe").
• If the operation is "UV-C treatment", all the areas are considered critical, then, the distances "d_uvc" used to classify the risk of getting human injuries due to UV-C light are: 0<d_uvc<7m ("danger") , 7m<d_uvc<10m ("warning"), d_uvc>10m ("safe").
• If a human is detected in non critical areas (green color), then, the level of risk do not depend of the distance and it is always considered as "safe".
• The robot must always stop any action in any autonomous operation ("logistics" or "UV-C treatment") if the risk label is "danger".
• If the robot is in "logistics" operation, inside or outside polytunnels, and the human is performing gestures to command the robot to approach to him/her, then the robot speed must be reduced proportionally to the distance between them and stop completely when the risk becomes "danger".
• If the robot is in "logistics" operation, outside polytunnels ("footpahts"), and the human is performing gestures to command the robot to approach him/her, then before the robot starts moving towards the human, it must be reoriented and approach the human always facing it directly (this includes moving forwards or backward towards the human).
• If the robot is in "logistics" operation, inside or outside polytunnels, and moving to a current goal but a human is detected occluding the path, then the robot safety action becomes "pause" and stays like this till the human is no longer detected or when is no longer in "warning". It is important to notice that the "pause" action is only valid if the human is occluding the robot's path, i.e. a "pause" is not activated if the human is really close to the robot but is located on the back of the robot when the robot is moving forwards, or when the human is in front of the robot and the robot is moving backward. Moreover, If one or more humans are detected but they are not located at critical areas (red color areas), they do not activate "pause" action since humans walking in another row or at the side of the robot are not considered critical.
• If the robot is in "logistics" operation, inside or outside polytunnels, and a human detected is performing a body gesture which correspond to a command, such command is only valid if the human orientation label is "facing the robot".
• If the robot is in "logistics" operation, inside polytunnels, and is performing actions to move towards a human, the robot must stop immediately if the human motion label turns into "moving" or if the human orientation label is not "facing the robot".

How to use de MeSAPro code:

PREREQUISITES:

The mesapro package requires the following packages/dependencies in order to be used in the Thorvald robots. Make sure that all packages are cloned into the directory ~/<workspace name>/src where <workspace name> is your workspace name (the default name used in the Thorvald robots is rasberry_ws).

1. Install ROS Melodic following the steps shown here.
2. Clone the RASberry repository and install all the dependencies. This repository contains the necessary packages to interface with real or simulated Thorvald robots. Note that the RASberry repository is private, so make sure you have access to it. A detailed installation guideline can be found here.
3. Clone the polytunnel_navigation_actions repository. This repository contains the necessary scripts to make the Thorvald robots navigate and localize inside polytunnels. This is a forked version compatible with mesapro messages. This repository is private, so make sure you have access to it.
4. Clone the people-detection repository. This repository contatins a LiDAR based leg detector. This repository is private, so make sure you have access to it and follow the installation instructions shown in the README section.
5. Clone the flir_module_driver repository. This repository contains the drivers to interface with FLIR lepton cameras. This repository is private, so make sure you have access to it.
6. Clone the openpose repository. This repository contains the Openpose framework used to extract human skeleton features based on RGB images. Make sure to download and install the OpenPose prerequisites for your particular operating system (e.g. cuda, cuDNN, OpenCV, Caffe, Python). Follow the instructions shown here.
7. Clone the realsense-ros repository. This repository contains the necessary packages to use Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. Follow the installation instructions shown in the README section.
8. Clone the ds4_driver repository. This repository contains the drivers to interface with Ps4 joysticks (this is only needed for simulation purposes). To install it correctly, make sure you follow the instructions shown in the README section.
9. Clone the ros_numpy repository. This repository contains tools for converting ROS messages to and from numpy arrays.
10. Clone the topological_navigation repository. This repository contains the basic packages used for the topological navigation of Thorvald robots. This is a forked version compatible with mesapro messages.
11. Install rosserial_arduino to allows your Arduino (used to activate visual alerts with a 24V beacon) to be a full fledged ROS node which can directly publish and subscribe to ROS messages. Follow the installation instructions shown in the README section.
12. Clone the web_video_server repository. This repository is only neccesary for the visualization purposes.
13. Finally, clone the mesapro repository, and make sure to build and source the workspace.

HOW TO USE IT:

• There are 3 important config files (into the ~/mesapro/tmule/ directory) that must be launched in order to run everything shown on the system architecture scheme. If implementing it on the Thorvald-014 (the one used during the whole MeSAPro project), the rasberry-hri_navigation_<name of the field>.yaml is launched on the NUC (computer without GPU, used as master), the rasberry-hri_safety_perception.yaml is launched on the ZOTAC (computer with GPU).
• The config files into tmule folder have several parameters that can be modified in case some features of the human detection or safety system are not required for certain tests. For instance, the leg detection can be disabled to test only camera detections, the thermal information can be disable, audio or visual alerts can be disabled if necessary, etc. Moreover, directories of important files can be modified using these configuration parameters, e.g. the bag files directory, the workspace directory, the conda environment directory, etc. Make sure to modify the directories according to your system.
• To test the safety system in simulation (GAZEBO), you can launch the config file rasberry-hri_sim.yaml. In this simulation, the human perception system is replaced by a node that is publishing the human information of two simulated people commanded by a joystick.
• To test the human detection system (based only on camera data) using bag files, you can launch the config file rasberry-hri_camera_detector.yaml.
• To test the human detection system (based only on LiDAR data) using bag files, you can launch the config file rasberry-hri_leg_detector.yaml.
• To use the human gesture recognition feature for first time, it is necessary to uncompress the file which contains the trained model. This file is located in the ~/mesapro/config/ directory. In the config folder, you will also find a global config file named global_config.yaml which contains important parameters, directories, and dictionaries used for the human detection system and safety system. Make sure to modify the directories according to your system.

To launch any config file into the ~/mesapro/tmule/ directory, it is necessary to first install Tmule-TMux Launch Engine with pip install tmule (source code here), and execute the following commands in terminal:

roscd mesapro/tmule
tmule -c <config_file_name>.yaml launch

To terminate the execution of a specific tmule session:

tmule -c <config_file_name>.yaml terminate

To monitor the state of every panel launched for the current active tmule sessions:

tmux a

Notes:

• The creation of this package was motivated by the MeSAPro project which aims to ensure the autonomy of agricultural robots in scenarios that involve human-robot interactions. The decision-making and safety policies of the safety system were designed to be implemented mainly during logistics operations at polytunnels (especially the gesture control features), however, some of the safety features (audiovisual alerts and safety stops) are still relevant during UV-C treatment operations.