Model-informed path planning and control for autonomous vessels

Project description

This repository is a part of the diploma thesis at the Faculty of Electrical Engineering and Computing, University of Zagreb, Laboratory for Underwater Systems and Technologies. The main goal of the thesis is to develop a model-informed path planning and control for autonomous vessels (Croatian: Modelski informirano globalno planiranje putanje i upravljanje autonomnoga plovila). The project is based on the PythonRobotics repository and it is used for educational purposes only. Model-informed path planning consists of the following steps:

1. Data extraction and processing from OpenStreetMap
2. Testing of the path planning algorithms from the PythonRobotics repository on the extracted OpenStreetMap data
3. Cost map generation
4. Testing of the path planning algorithms on the generated cost map
5. Path interpolation and optimization
6. Publishing data to the ROS2 environment and testing the path planning algorithms on the real vessel or in the simulation environment

Table of Contents

• Installing

• Requirements

• Executing program

  • Folder structure
  • Step 1 : Download data from OpenStreetMap
  • Step 2 : Data extraction and processing from OpenStreetMap
  • Step 3 : Path planning algorithms testing
  • Step 4 : Cost map generation
  • Step 5 : Testing of the path planning algorithms on the generated cost map
  • Step 6 : Path interpolation and optimization
  • Step 7 : Publishing data to the ROS2 environment and testing the path planning algorithms on the real vessel or in the simulation environment

• Credits

• Acknowledgments

Installing

git clone https://github.com/kr1zzo/Model-informed-path-planning.git

Requirements

pip install -r requirements/requirements.txt

Executing program

Folder structure

Model-informed-path-planning
    ├── assets
    │   └── ...
    ├── input_data            # Step 1
    │   └── ...
    ├── osm_data_processing   # Step 2 and Step 4
    │   └── ...
    ├── test_algorithm        # Step 3
    │   └── ...
    ├── results
    │   └── ...
    ├── requirements
    │   └── requirements.txt
    ├── config.yaml
    ├── README.md
    ├── .gitignore
    └── ...

• Steps are marked in config.yaml file with ## Step n : ... and can be changed according to the user's needs.

• After first run, the results and binary_dump folders are created for saving the results and binary files for the path planning algorithms

Step 1 : Download data from OpenStreetMap (optional)

Step 1. is optional and can be skipped if the default data is used. Feel free to download your data from OpenStreetMap and follow the instructions below!

Folder name : input_data

In the folder input_data are provided default data for the following locations:

• Voz, island of Krk, Croatia
• Jadranovo, Croatia

1.1. Go to OpenStreetMap and select the area you want to download

1.2. Make new folder geolocation_name (for example Rijeka) in the input_data folder and put osm_data.txt file inside

1.3. Click on the Download button and download the data in .png format

Figure 1: Download data from OpenStreetMap

1.4. Copy the downloaded .png file to the input_data/geolocation_name folder as geolocation_name.png

1.5. Copy HTML string from the OpenStreetMap website and paste it into the input_data/geolocation_name folder in the osm_data.txt file for extraction of the coordinates

Figure 2: Copy HTML string from OpenStreetMap

1.6. Update config.yaml file

Example of the config.yaml file for the location voz:

• location folder from the input_data folder in step 1.2 and location_image is the name of the .png file from step 1.3

## Step 1: Download data from OpenStreetMap (optional)
# Process OSM data
location_folder : "voz"
location_image: "voz.png"

1.7. Check the results

Folder input_data should have the following structure:

    input_data
    ├── voz                  
    │   ├── osm_input.txt       # HTML string from the OpenStreetMa
    │   └── voz.png             # .png file from the OpenStreetMap
    ├── voz2                  
    │   ├── osm_input.txt       # HTML string from the OpenStreetMa
    │   └── voz2.png             # .png file from the OpenStreetMap
    ├── jadranovo
    │   ├── osm_input.txt       # HTML string from the OpenStreetMap
    │   └── jadranovo.png       # .png file from the OpenStreetMap
    ├── location_1
    │   ├── osm_input.txt       # HTML string from the OpenStreetMa
    │   └── location_1.png      # .png file from the OpenStreetMap
    ├── location_2
    │   └── ...
    └── ...

• location_1, location_2, ..., location_n are the names of the locations added by the user in the previous steps

Step 2 : Data extraction and processing from OpenStreetMap

In this step, the OpenStreetMap data is processed and the map is resized to 1 pixel per 1 meter. Coastline is detected and start and goal points are set on the map.

Folder name: osm_data_processing

  osm_data_processing
    ├── detect_coast.py
    ├── process_osm_data.py
    ├── set_start_goal.py
    └── main.py

• main.py - main file for detecting coastline, setting start and goal points on the map and generating the cost map. Results are saved in the binary_dump folder for further steps and in the results folder for visualization

• process_osm_data.py:

  • Processing the OpenStreetMap data from osm_data.txt file
  • Converting GPS coordinates to the meters
  • Converting GPS coordinates to the pixels
  • Converting GPS coordinates to the DMS format
  • Resize the map to 1 pixel per 1 meter
  • Prepare image with geolocation data for plotting

• detect_coast.py - file for detecting coastline and coast points and marking them on the map

  • Detecting the coastline from the image
  • Detecting if (x, y) point is on the coast or at the sea
  • Detecting zones based on the distance from the coast (red, yellow, green) - cost map generation

• set_start_goal.py - file for setting start and goal points on the map and plotting them on the map

  • Setting start and goal points on the map
    • Option 1: Set start and goal points with a mouse click on the map
    • Option 2: Set start and goal points manually in the config.yaml file
  • Plotting start and goal points on the map

2.1. Update config.yaml file

• Set resized_location_image name for the resized map from the previous step. It is saved in the results folder for further steps
• Set custom_start_goal to True if you want to choose the start and goal points with a mouse click on the map or False if you want to add start and goal points manually in the config.yaml file
  • If custom_start_goal is set to False, set the start_latitude, start_longitude, goal_latitude, and goal_longitude in the config.yaml file. You can get the coordinates from the OpenStreetMap, Google Maps, etc.

Disable the cost map generation by setting cost_map to False in the config.yaml file. This step is used only for the path planning algorithms performance testing. The cost map is generated in Step 4 by setting cost_map to True and it is used for the path interpolation and optimization from Step 5!

The example of the config.yaml file for the location voz:

## Step 2 : Data extraction and processing from OpenStreetMap 
## Step 4: Cost map generation

resized_location_image : "voz_resized2.png"
result_costmap_name : "voz_costmap.png"

# Step 2 : False, Step 4 : True
cost_map : False

custom_start_goal : False
# This is hardcoded start and goal position and applied only if custom_start_goal is False
start_latitude : 45.234781
start_longitude: 14.577538

goal_latitude : 45.230241
goal_longitude: 14.583173

2.2. Run the following command

Run the following command in the folder osm_data_processing to generate a map, detect the coastline and set the start and goal points on the map

python3 main.py

2.2.1. Check the results

• the map is resized to 1 $pixel$ per 1 $meter^2$
• coastline is marked with a light blue color
• the start is marked with a blue circle and the goal with a blue X
• axis are displayed in geographical coordinates in format dms (degrees, minutes, seconds)
• longitude and latitude representation of the start and goal points are displayed in legend
• If the cost_map is set to False, the cost map is not generated and the zones are not marked on the map
• geolocation data marked in the map should be the same as geolocation data in the OpenStreetMap, Google Maps, etc

Example of the map with start and goal points hardcoded in the config.yaml file:

Figure 3: Example of the map with start and goal points

Step 3 : Path planning algorithms testing

In this step, path planning algorithms are tested on the map with the start and goal points set in the previous step. This step is used for testing path planning algorithms from the PythonRobotics repository and comparing the results in order to choose the best path planning algorithm for the further steps

Folder name: test_algorithm

  test_algorithm
    ├── algorithms
    │   ├── a_star.py
    │   ├── bidirectional_a_star.py
    │   ├── dijkstra.py
    │   ├── dstar.py
    │   ├── d_star_lite.py
    │   ├── breadth_first_search.py
    │   ├── bidirectional_breadth_first_search.py
    │   ├── depth_first_search.py
    │   └── greedy_best_first_search.py
    ├── algorithms_class.py
    ├── plot.py
    ├── test.py
    └── main.py

• main.py - main file for testing path planning algorithms and runs following files:
• test.py - file for testing path planning algorithms and runs the algorithms from the algorithms folder
• algorithms_class.py - class for path planning algorithms
• plot.py - file for plotting the results of the path planning algorithms on the map and table with the result runtime and path length in meters

3.1. Update config.yaml file

• Set result_image name for the map with the path planning algorithm results and table_name for the table with the runtime results and path length
• Set grid_size and robot_radius for the path planning algorithms, recommended values are boat length and width as grid size and robot radius as 1/2 of the boat length
• Set thread_enable if runtime results are not needed and thread_enable map if runtime results are needed for algorithm runtime comparison
  • thread_enable : True - overall runtime is lower, but runtime of each algorithm is higher
• Set the path planning algorithm you want to test in the test_algorithm variable
• Set the path planning algorithm you want to plot in the plot_algorithm variable

## Step 3: Path planning algorithms testing

result_image : "voz_result.png"
table_name : "runtime_results.png"
result_image_name : "result_image.png"

grid_size : 10.0  # [m]
robot_radius : 5.0  # [m]

# thread_enable : True - thread for plotting map
# thread_enable map : False - for algorithm runtime calculation
thread_enable : False

test_algorithm :  #plot_algorithm
- "a_star"
- "bidirectional_a_star"
- "dijkstra"
- "d_star"
- "d_star_lite"
- "breadth_first_search"
- "bidirectional_breadth_first_search"
- "greedy_best_first_search"

3.2. Run the following command to test path-planning algorithms

Run for testing and plotting algorithms:

 python3 main.py

Run for testing algorithms runtime:

 python3 test.py

Run for plotting the results:

 python3 plot.py

3.3. Check the results

• Results are saved in the results folder with the name result_image and table_name

Figure 4: Example of the map with the path planning algorithm results

Figure 5: Example of the table with the runtime results and path length

Step 4 : Cost map generation

Folder name: osm_data_processing

• cost map generation is needed for the path interpolation and optimization in Step 5
• the current cost map is based on the distance from the coast
• TODO: Implement costmap based on the depth, obstacles, etc.
• TODO: develop the solution for the bridge detections and elimination from the provided data

4.1. Update config.yaml file

• Set cost_map to True in the config.yaml file and set the result_costmap_name for the cost map figure name
• Set cost_map_show to True if you want to show the cost map on the plots or False if you want to skip the cost map visualization

## Step 4: Cost map generation
result_costmap_name : "voz_costmap.png"
cost_map : True
cost_map_show : True

4.2. Run the following command to generate cost map

• Same as in the Step 2.2. Run the following command in the folder osm_data_processing to generate a cost map, but set generate_costmap to True in the config.yaml file

 python3 main.py

4.3. Select start and goal points on the cost map

• Step 2.1. Choose start and goal points on the map

4.4. Check the results

• Zones are marked with different colors based on the distance from the coast and determined according to the prescribed safety regulations in Croatia
• Zone under 50 meters is marked with red color, zone from 50 to 150 meters is marked with yellow color, and zone from 150 to 300 meters is marked with green color

Figure 6: Example of the cost map

Step 5 : Testing of the path planning algorithms on the generated cost map

Folder name: test_algorithm

• From step 3, the path planning algorithm with the best results is chosen for the path interpolation and optimization (D* lite),and code is modified to fit the cost map generated in the previous step

5.1. Update config.yaml file

• Path optimization is based on the cost map generated in the previous step and zone cost values are set in the config.yaml file

red_cost : 10
yellow_cost : 5
green_cost : 2

5.n. Check the results

• Results are saved in the results folder with the name result_costmap_name and table_costmap_table

Figure 7: Example of the cost map with the D* lite results

Figure 8: Example of the table with the cost values, runtime results and path length

Step 6 : Path interpolation and optimization

• From the results of the path planning algorithms, path interpolation and optimization are needed for the path planning algorithms to be used in the real environment

//TODO

Step 7 : Publishing data to the ROS2 environment and testing the path planning algorithms on the real vessel or in the simulation environment

//TODO

Credits

© Faculty of Electrical Engineering and Computing, University of Zagreb, 2024

© Laboratory for Underwater Systems and Technologies (LABUST)

Contributors names and contact info

Author	GitHub	e-mail
Enio Krizman	@kr1zzo	enio.krizman@fer.hr

Mentors	e-mail
Doc. Dr. Sc. Đula Nađ	dula.nad@fer.hr
Dr. Sc. Nadir Kapetanović	nadir.kapetanovi@fer.hr

Acknowledgments

This repository is built using the following resources and it is used only for educational purposes:

• PythonRobotics repository
• PythonRobotics documentation