#include <limits.h>
#include <iostream>
#include <fstream>
#include <ros/ros.h>
#include <angles/angles.h>
#include <Eigen/Core>
#include <coverage_3d_tools/conversions.h>
#include <coverage_3d_arm_navigation/TrajectoryPlanner.h>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>