MAV planning tools using voxblox as the map representation.

NOTE: THIS PACKAGE IS UNDER ACTIVE DEVELOPMENT! Things are subject to change at any time.

Table of Contents

• Contents and Future Work
  • Included
  • To Come (by end of January 2019)
• Papers and References
• Getting Started
  • Installation
  • Download maps
  • Try out RRT and Skeleton planning
    • Get the planning panel
    • Using RRT voxblox planner:
    • Using the Skeleton planner:
  • Try out Local Planning
    • Install Rotors Simulator
    • Install the planning pannel (if you haven't yet)
    • Run the simulation
    • Try out global + local planning
• Advanced
  • Skeletonize your own maps

Contents and Future Work

Included

• Global Planning
  • RRT*, RRT Connect, BIT*, PRM... (OMPL interface) voxblox_rrt_planner
  • Planning on skeleton sparse graphs voxblox_skeleton_planner
  • Skeleton sparse graphs (topology) generation from ESDF voxblox maps voxblox_skeleton
  • Randomized benchmark on a variety of real maps mav_planning_benchmark
• Path Smoothing (between waypoints)
  • Velocity ramp mav_path_smoothing
  • Polynomial with adding additional vertices at collisions mav_path_smoothing
  • Local polynomial optimization (Loco), unconstraining waypoints and minimizing collisions mav_path_smoothing, loco_planner
• Local Planning -- mav_local_planner
  • Interface to MAV controller, sending timed, dynamically feasible trajectories and replanning online mav_local_planner
  • Planning in a short horizon in unknown or partially unknown spaces, using trajectory optimization in ESDFs voxblox_loco_planner
  • Path smoothing between waypoints if all waypoints are in known free space mav_path_smoothing
• RVIZ Planning Plugin -- mav_planning_rviz
  • Allows dragging around start and goal markers, sending planning requests to global planners mav_planning_rviz
  • Allows sending goal points either directly to controller or to the local planner mav_local_planner

To Come (by end of January 2019)

• Local Planning
  • Improved goal selection
  • Support for global and local coordinate frames
• RVIZ Planning Plugin
  • Support for entering multiple waypoints

Papers and References

If using these, please cite:

voxblox

Helen Oleynikova, Zachary Taylor, Marius Fehr, Roland Siegwart, and Juan Nieto, “Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning”. In IEEE Int. Conf. on Intelligent Robots and Systems (IROS), October 2017.
[pdf | bibtex | video | arxiv ]

loco planning

Helen Oleynikova, Michael Burri, Zachary Taylor, Juan Nieto, Roland Siegwart, and Enric Galceran, “Continuous-Time Trajectory Optimization for Online UAV Replanning”. In IEEE Int. Conf. on Intelligent Robots and Systems (IROS), October 2016.
[pdf | bibtex | video]

loco planning with voxblox

Helen Oleynikova, Zachary Taylor, Roland Siegwart, and Juan Nieto, “Safe Local Exploration for Replanning in Cluttered Unknown Environments for Micro-Aerial Vehicles”. IEEE Robotics and Automation Letters, 2018.
[pdf | bibtex | video | arxiv ]

voxblox skeleton and skeleton planning

Helen Oleynikova, Zachary Taylor, Roland Siegwart, and Juan Nieto, “Sparse 3D Topological Graphs for Micro-Aerial Vehicle Planning”. In IEEE Int. Conf. on Intelligent Robots and Systems (IROS), October 2018.
[pdf | bibtex | video | arxiv ]

Getting Started

Installation

This package is intended to be used with Ubuntu 16.04 and ROS kinetic or above. After installing ROS, install some extra dependencies, substituting kinetic for your ROS version as necessary:

sudo apt-get install ros-kinetic-cmake-modules python-wstool python-catkin-tools libyaml-cpp-dev protobuf-compiler autoconf

Then if not already done so, set up a new catkin workspace:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
catkin config --extend /opt/ros/kinetic
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin config --merge-devel

If using SSH keys for github (recommended):

cd ~/catkin_ws/src/
git clone [email protected]:ethz-asl/mav_voxblox_planning.git
wstool init . ./mav_voxblox_planning/install/install_ssh.rosinstall
wstool update

If not using SSH keys but using https instead:

cd ~/catkin_ws/src/
git clone https://github.com/ethz-asl/mav_voxblox_planning.git
wstool init . ./mav_voxblox_planning/install/install_https.rosinstall
wstool update

If you have already initalized wstool replace the above wstool init with wstool merge -t

Compile:

cd ~/catkin_ws/src/
catkin build mav_voxblox_planning

Download maps

We've prepared a number of maps for you to try out our planning on. The archive is 260 MB big and available here.

It contains 6 maps from 3 scenarios: machine hall (indoor), shed (mixed indoor and outdoor area), and rubble (outdoor), each with Intel Realsense D400-series scans and with grayscale stereo matching scans. Each map features 3 files: esdf, skeleton (esdf + skeleton diagram), and sparse graph, which contains just the sparse graph generated using skeletonization.

Dataset	Realsense	Stereo
Machine Hall
Rubble
Shed

Try out RRT and Skeleton planning

Get the planning panel

Make sure all the packages have built successfully! Re-source your workspace (source ~/catkin_ws/devel/setup.bash) and start up rviz (rviz). In rviz, select Panels -> Add New Panel and select Planning Panel:

Next, under Displays, add an InteractiveMarkers display with the topic /planning_markers/update:

You should now see both a path panel and start and goal arrows. You can select Edit on either the start or the goal to drag it around as an interactive marker:

You can also edit the numbers in the x, y, z, yaw fields manually; the markers and the numbers will update automatically to match.

Using RRT voxblox planner:

In ~/catkin_ws/src/mav_voxblox_planning/voxblox_rrt_planner/launch/rrt_saved_map.launch, open the file and replace the voxblox_path to the path of one of the esdf maps you downloaded above. Then run the file:

roslaunch voxblox_rrt_planner rrt_saved_map.launch

Note that the Fixed Frame in RVIZ should match the value for the frame_id parameter in the launch file, in this case map. Using the another value for the Fixed Frame will result in no mesh display.

In the planning panel, enter voxblox_rrt_planner as the planner name, and add a VoxbloxMesh display with the topic /voxblox_rrt_planner/mesh and a MarkerArray display with the topic /voxblox_rrt_planner/path. You can now press the "Planner Service" button to plan! In green is the RRT output path, and the other colors show different types of smoothing through these waypoints.

Using the Skeleton planner:

Very similar to above... Open ~/catkin_ws/src/mav_voxblox_planning/voxblox_skeleton_planner/launch/skeleton_saved_map.launch and update the paths to point to matching skeleton and sparse graph files from the maps above. Run it with:

roslaunch voxblox_skeleton_planner skeleton_saved_map.launch

In the planning panel, enter voxblox_skeleton_planner as the planner name, and add a VoxbloxMesh display with the topic /voxblox_skeleton_planner/mesh and a MarkerArray display with the topic /voxblox_skeleton_planner/path. Additionally you can add a MarkerArray with topic /voxblox_skeleton_planner/sparse_graph You can now press the "Planner Service" button to plan! Pink is the shortened path from the sparse graph, and teal is smoothed using loco through it.

Try out Local Planning

This demo is about using the mav_local_planner to do live replanning at 4 Hz in a simulation environment. The local planner uses loco to locally track a waypoint, or if given a list of waypoints, plan a smooth path through them.

Install Rotors Simulator

Follow the installation instructions here to install Rotors Simulator, which is an MAV simulator built on top of gazebo. This will allow us to fully simulate a typical MAV, with a visual-inertial sensor mounted on it.

Install the planning pannel (if you haven't yet)

See instructions above: here.

Run the simulation

After rotors is up and running, in a new terminal, launch the firefly sim: roslaunch mav_local_planner firefly_sim.launch

A gazebo window will come up, showing something similar to this:

You can then control the MAV using the planning panel. Enter firefly as the Namespace in the planning panel, then either type a position for the goal or edit the goal to drag it around. To send it to the local planner, press the Send Waypoint button.

The trajectory will be visualized as a visualization_msgs/MarkerArray with the topic /firefly/mav_local_planner/local_path and you can view the explored mesh as a voxblox_msgs/Mesh message with the topic /firefly/voxblox_node/mesh. The complete setup is shown below:

In case the MAV gets stuck, you can use Send to Controller, which will directly send the pose command to the controller -- with no collision avoidance or trajectory planning.

Try out global + local planning

Once you've explored some of the map in simulation, you can also use the global planner to plan longer paths through known space. First, start the simulation global RRT* planner (this is in addition to the firefly_sim.launch file above):

roslaunch voxblox_rrt_planner firefly_rrt.launch

Local planning only uses the goal point, but global planning requires a start point as well (as the global planner does not know the odometry of the MAV). For this, we can use the Set start to odom option in the planning panel. To do this, in the Odometry Topic field, enter ground_truth/odometry and check the Set start to odom check-box. Now the start location will automatically track the odometry of the Firefly as it moves.

Once the start and goal poses are set as you want, press Planner Service. In rviz, add a MarkerArray display with the topic /firefly/voxblox_rrt_planner/path to visualize what the planner has planned. Once you are happy with this path, press Publish Path to send it to the local planner, which should start tracking the path immediately.

Advanced

Skeletonize your own maps

TODO! Instructions coming soon. See the voxblox_skeleton/launch/skeletonize_map.launch file for reference, please make an issue if there are any questions or problems.