A subsumption layered control system for a mobile platform featuring collision avoidance, object avoidance and corridor following for multiple real-time processed RTIS sensors for usage in the RTIS Network software framework. The controller also has a full simulation in MATLAB. This can be found in the Matlab Simulator folder. The Python-based controller can be connected to the MATLAB simulator for testing where the simulator takes care of world, robot and sensor simulation.

Publication

This work is described in more detail in the following publication:

Title: Adaptive Acoustic Flow-Based Navigation with 3D Sonar Sensor Fusion.
Authors: Jansen, Wouter & Laurijssen, Dennis & Steckel, Jan.
Conference: 2021 International Conference on Indoor Positioning and Indoor Navigation (IPIN)
DOI: 10.1109/IPIN51156.2021.9662596. 
https://www.researchgate.net/publication/357601135_Adaptive_Acoustic_Flow-Based_Navigation_with_3D_Sonar_Sensor_Fusion
https://ieeexplore.ieee.org/document/9662596/

and

Title: Real-Time Sonar Fusion for Layered Navigation Controller.
Authors: Jansen, Wouter & Laurijssen, Dennis & Steckel, Jan.
Journal: MDPI Sensors 2022, Special Issue Advances in Indoor Positioning and Indoor Navigation****
DOI: https://doi.org/10.3390/s22093109
https://www.researchgate.net/publication/360034002_Real-Time_Sonar_Fusion_for_Layered_Navigation_Controller
https://www.mdpi.com/1424-8220/22/9/3109

Usage

Python Controller

The Python implementation of the controller can be used in the RTIS Network software framework or with the Matlab Simulation framework. For usage and setup of the eRTIS Network see here.

The main script is RTISRobotController.py. There are a few launch arguments that you likely need to use to correctly configure the script to work. You can use the launch argument --help to show the description of all launch arguments available.

For example:

python3 ./Python/RTISRobotController.py --help

if --keyboard is set to 1, it will overwrite the default input velocities when using the arrow keys.

With RTIS Network

When using the controller in combination with the RTIS Network, the sensors connected to the RTIS Server will be automatically parsed and retrieved by the controller. One should use a controller configuration file such as the example. The --sim argument should be set to 0. The --connect_data_ip argument should be set to the IP of the RTIS Server. Don't forget to update the RTIS Server configuration file to match the application IP to that running the RTIS Robot Controller. The --connect_command_ip and --connect_command_port arguments should match that of the application that should receive the output velocities of the controller.

For example:

python3 ./Python/RTISRobotController.py --log 3 --keyboard 1 --connect_data_ip 192.168.1.160 

With Simulator

When using the controller in combination with the Simulation environment, the sensors need to be manually configured. This is done in a separate configuration file such as the example. One should use a controller configuration file such as the example. The --sim argument should be set to 1. The --connect_data_ip argument should be set to the IP that is hosting the RTIS Simulator Connect. The --connect_command_ip and --connect_command_port should be set to the IP that is configured in RTIS Simulator Connect.

For example:

python3 ./Python/RTISRobotController.py --sim 1 --log 3 --keyboard 1 --connect_data_ip 192.168.1.160 

Furthermore, a separate script needs to be started to act as an intermediate between the Python Controller and the Matlab simulator. This script is called RTISSimulatorConnect.py. There are a few launch arguments that you likely need to use to correctly configure the script to work. You can use the launch argument --help to show the description of all launch arguments available.

For example:

python3 ./Python/RTISSimulatorConnect.py --help

When connecting to the Matlab Simulator, you must turn the external_controller in the simulation configuration to 1.

Simulator

There is also a full simulation in MATLAB in the Matlab Simulator folder. This can both simulate the robot's world and motion, the sonar energyscape generation as well as the controller. Furthermore, the Python controller can also be coupled to the simulator.

Run Simulation

The main script is called run_Simulation.m. This will create all Objects and run the simulation. The start of the script has a lot of configurable parameters. Please configure them before running. Further down a World Object and Robot Object are created. Next are the Sonar Objects. Please make sure to configure these to your liking. You can use as many as you want, but we recommend between 1 and 3. Make sure most of them are forward and side facing. Next up the Controller Object is made. You can configure at the start to show more debug plots of how it is configured.

All objects are MATLAB Classes and as such can be edited in a structured manner. For example if you want to change the range of a sonar sensor you run

MySonar.settings_ES.R_max = 10;

Please see the individual Classes in the Objects folder to see what else can be changed.

The Controller can also be disabled. This way it will simply drive straight between the waypoints.

Connect Python Controller

As described above when discussing the Python Controller, to connect to the external controller, you must turn the external_controller in the simulation configuration to 1.

Replay

There is also the functionality to replay saved simulations. Do note this only replays the Robot and its path, no energyscapes or additional Controller information is saved. First make sure you are running the Simulation with file saving turned on. Then you can run replay_Simulation.m with the correct filename in the settings. You can either plot the end frame of each replay or view them frame by frame. Once again all options are configurable at the top of the script.

Heatmap

There is also a script to create a heatmap of the trajectory distribution of all saved simulations. This is heatmap_Simulation.m. Once again all options are configurable at the top of the script.

Extras

There are also two additional scripts in the Extras folder to make some additional plots of the workings of the eRTIS Robot Controller. Notably the 2D velocity fields and the flow-lines it creates and one for generating a plot of the Masks the Controller uses.

RTIS ROS Robot Controller Publisher

The output of the Python Controller can be published on ROS as velocity commands for a differential drive robot. This is developed as ROS package that can be found in the rtisros_robot_controller folder. The launch file configuration should match that of the --connect_command_ip and --connect_command_port arguments of the Python script. It also has a custom message type RTISControllerAction which publishes the behaviour, controller output velocities and measurement index. It will also publish a Twist message with the output velocities of the controller which can be used to steer a mobile platform that supports ROS.

Furthermore, two Matlab scripts are available called rosbag_parse.m rosbag_parse_combined.m in the Matlab Rosbag Parser folder that can be used to plot the occupancy grid map of a SLAM system combined with the odometry output of a mobile platform and the output of the RTIS Controller to a figure or a combination of the trajectories of multiple rosbags respectively.