Related to https://github.com/roboskel/quadcopter_motion_planner/issues/4 and https://github.com/roboskel/quadcopter_ompl_planner/issues/3