Mission Planning

Overview

This package contains the whole pipeline for global mission planning, specifically for vehiles in orchards and vineyards. However, it can also be used in other environmets, such as forest and search&rescue.

During the first traversal of the field, a map is created. This map is than used, to find the opitmal path through the field covering all or certain rows while taking the tractor dimensions into account. The final path is given as waypoints to the local planner.

The Misson Planning pipeline has been tested under ROS Noetic and Ubuntu 20.04.

Paper

This repository contains the implementation of the approach demonstrated in the following paper:

Schönegg, Timo & Tuna, Turcan & Yang, Fan & Waibel, Gabriel & Mattamala, Matías & Hutter, Marco. (2024). Global Path Planning for Autonomous Vehicles in Orchards and Vineyards. 1-8. 10.1109/RoMoCo60539.2024.10604355.

Link to project website with video: https://sites.google.com/leggedrobotics.com/global-orchard-vineyard

Direct link to paper: https://www.researchgate.net/publication/382673203_Global_Path_Planning_for_Autonomous_Vehicles_in_Orchards_and_Vineyards

Installation

Install python libraries

pip install pillow
pip install numpy
pip install matplotlib
pip install scipy
pip install pandas
pip install networkx
pip install tqdm
pip install graphviz

Ubuntu packages

sudo apt install ffmpeg

Install Processing (for path marking GUI)

Download latest version from https://processing.org/download.

cd ~/Downloads
sudo mkdir /usr/share/processing
sudo tar -xvf processing-[VERSION-NR]-linux64.tgz -C /usr/share/processing

Adjust the processing version in line 108 in global_planning.py

Run the pipeline: Occupancy Grid Map to Global Path

1. Create or use map and vehicle

Load an occupancy grid map into the folder data/maps and a vehicle shape into the folder data/vehicles, or use an existing file.

2. Create the configuration for the map and the vehicle.

Vehicle json:

In the data/vehicles create:

{
  "vehicle_center": [5, 2],
  "min_distance_to_object": 2
}

• vehicle_center [px]: The center coordinates of the vehicle in the image (down, right).
• min_distance_to_object [px]: By how much the regions will be enlarged at the beginning of the voronoi diagram. This should be usually half the vehicles width, as passages smaller than the vehicles width cannot be traversed and should not lead to an edge in the voronoi graph.

Map json:

In the data/maps create:

{
  "time_limit": 300,
  "fill_freespace_radius": 10,
  "truncated_distance": 7,
  "row_distance_m": 1.8,
  "pixel_in_m": 0.2
}

• time_limit [s]: The maximal time the A* planning is allowed to take.
• fill_freespace_radius [px]: The radius of the filling lattice in pixel. Must be larger than 2*min_distance_to_object of the vehicle parameter, as otherwise no lattice structure can be generated.
• truncated_distance [px]: The tsdf distance for the voronoi diagram.
• row_distance_m [m]: The distance between 2 rows.
• pixel_in_m [m]: The size of a map pixel in meters.
• skipp_small_edges: Optional: Whether to skipp small edges <10px or not. Default true. Disable in non-orchard and non-vineyard environments with no long rows.

3. Define which vehicle and map to use

In the main function of the global_planning.py, define the map, vehicle and the corresponding config files of the map and vehicle that should be used.

Make sure, that the processing version in the function select_edge_and_initial_position is correct.

4. Run the file global_planning.py.

python3 global_planning.py

5. Wait for the GUI to open

Select the edges of the graph that should be traversed.

Next, select the starting position and orientation of the vehicle. It must be in free white space.

Click "save" and close the window of the GUI.

Wait for the algorithm to finish. A video of the final path will open as soon as the algorithm finishes. The final result is in the folder data/paths.

Create an Occupancy Grid Map from a Point Cloud

To create a binary occupancy grid map, run the following.

1. Build the marker_publisher ROS package.

   cd ~/catkin_ws/src/
   ln -s ~/git/[path to this repo, e.g. coverage_planner] .
   cd coverage_planner
   
   catkin build global_mapping
   catkin build marker_publisher
   source ~/catkin_ws/devel/setup.zsh

2. Configure the params in forest_config.yaml or vineyard_config.yaml or create a new config file. Especially give the map a proper name by configuring the map_name parameter.

3. Example Rosbags can be found in this GDrive folder.

   3.1. Using a full point cloud map (Forest/Search&Rescue/RTC360 Vineyard):

   Run the map generation, in this order

   roslaunch global_mapping forest_map.launch # start the map generation algorithm
   rviz -d ~/git/[path to repo]/visualisation/final_path.rviz # start rviz
   rosbag play -k -d 10 timo_forest_map2.bag # run the rosbag

   This should generate you a .bmp file of the generated occupancy grid map in the /data/maps folder, together with a file of the coordinates of the origin of the map and sample json parameters. Update the parameters in the generated .json if necessary.

   

   3.2. Using a Lidar Scanner Point Cloud Stream from Robot (Orchard/Vineyard)

   Install the Open3DSlam repo:

   git clone git@github.com:leggedrobotics/open3d_slam.git

   and follow the steps in Open3D Slam Adaptions Readme.

   Now run the map generation, in this order

   roslaunch global_mapping vineyard_config.launch # start the map generation algorithm
   rosbag play waedenswil_whole_field_2022-11-21-14-16-03.bag # run the rosbag
   roslaunch open3d_slam_ros mapping.launch # run open3d slam and start rviz

   This should generate you a .bmp file of the generated occupancy grid map in the /data/maps folder, together with a file of the coordinates of the origin of the map and sample json parameters. Update the parameters in the generated .json if necessary.

   

4. Start the pipeline as described above in section "Run the pipeline: Occupancy Grid Map to Global Path". Go to the global_planning.py and configure in the main the params map_file and map_config.

   Then run:

   python3 global planning.py

5. After the final path was generated successfully, update in the file MarkerPublisher.py in the function get_marker() the map_name. Then run:

   cd visualisation
   rosrun marker_publisher MarkerPublisher.py # visualize final global path in rviz
   rosservice call /mapping_node/align_world "{}" # alighn the maps

   Now you should see the final path in the Rviz.

   

Pipline Structure

The pipeline is structured as follows:

1. global_mapping

   While the robot drives through the whole field once, a map is created.

2. voronoi_graph

   Using the created occupancy grid map, a topological graph is created.

3. local_planner

   For all the edges in the voronoi diagram, a local path is planned, taking the tractor dimensions and turning radius as well as the occupancy grid into account.

4. rpp_solver

   Using the topological graph created in step 2. with weights according to the path length of the local planning estimation from step 3., an optimal path is found. The graph needs to cover some previously definde edges of the topological graph (e.g. every row or every second row).

Author & Licence

Author: Timo Schönegg

Licence: MIT