coverage_3d_planning: NeighborhoodGraph.h Source File

Go to the documentation of this file.
00001 
00003 #ifndef NEIGHBORHOODGRAPH_H
00004 #define NEIGHBORHOODGRAPH_H
00005 
00006 #include<vector>
00007 #include<surfacelet/SurfacePatch.h>
00008 #include<coverage_3d_arm_navigation/CollisionCheck.h>
00009 #include <octomap_ros/OctomapROS.h>
00010 #include <octomap/octomap.h>
00011 
00012 using namespace std;
00013 
00014 class NeighborhoodGraph {
00015 
00016 public:
00017         NeighborhoodGraph(ros::NodeHandle& nh);
00018         NeighborhoodGraph(ros::NodeHandle& nh, bool coll_check);
00019         NeighborhoodGraph(ros::NodeHandle& nh, std::vector<SurfacePatch> patches,
00020                         octomap::OctomapROS<octomap::OcTree>* octomap);
00021         NeighborhoodGraph(ros::NodeHandle& nh, std::vector<SurfacePatch> patches,
00022                         octomap::OctomapROS<octomap::OcTree>* octomap, double dist_above_surface, int max_neigborhood,
00023                         double max_neigborhood_radius, int n_ik_samples, bool coll_check);
00024         ~NeighborhoodGraph();
00025         void setPatches(std::vector<SurfacePatch> patches);
00026         void setSurfaceDist(double dist);
00027         void setOctomap(octomap::OctomapROS<octomap::OcTree>* octomap);
00028         void getReachableVector(std::vector<bool>& reachable);
00029         void computeConnectedComponents();
00030 
00031         void getGraph(std::vector<SurfacePatch>& patches, std::vector<pcl::PointXYZ>&surface_points,
00032                         Eigen::MatrixXi& adjacency, std::vector<std::vector<tf::Pose> >&coll_free_poses,
00033                         std::vector<std::vector<std::vector<std::vector<double> > > >& coll_free_ik);
00034         void constructGraphMarker(const std::vector<pcl::PointXYZ>& surface_samples, const Eigen::MatrixXi& adjacency,
00035                         const std::string frame_id, visualization_msgs::Marker& marker_list) const;
00036         void constructGraphMarker(const std::vector<SurfacePatch>& surface, const Eigen::MatrixXi& adjacency,
00037                         const std::string frame_id, visualization_msgs::Marker& marker_list) const;
00038         void buildAdjacencyMatrixUsingVoronoi();
00039         void sampleSurfacePoints(const std::vector<SurfacePatch>& surface, std::vector<pcl::PointXYZ>& samples) const;
00040 
00041         void computeEndEffectorPose(const SurfacePatch p1, const SurfacePatch p2, const double dist_above_surface,
00042                         tf::Pose& pose) const;
00043         void
00044         computeEndEffectorPose(const SurfacePatch p1, const double dist_above_surface, std::vector<tf::Pose>& poses, bool single_pose =false) const;
00045         ros::NodeHandle nh_;
00046         double dist_above_surface_;
00047         //  double max_adjacency_;
00048         int max_neigborhood_;
00049         double max_neigborhood_radius_;
00050         octomap::OctomapROS<octomap::OcTree>* octomap_;
00051         CollisionCheck* coll_checker_;
00052 
00053         std::vector<SurfacePatch> surface_;
00054         std::vector<bool> reachable_surface_;
00055 
00056         std::vector<SurfacePatch> graph_;
00057         std::vector<std::set<int> > subgraphs_;
00058         Eigen::MatrixXi adjacency_;int n_ik_samples_;
00059         bool coll_check_;
00060 
00061 private:
00062 
00063         void sampleSurfacePoints(const std::vector<SurfacePatch>& surface, pcl::PointCloud<pcl::PointXYZ>& samples) const;
00064 
00065         void depthFirstConnectedComponentSearch(const int node, Eigen::MatrixXi& adjacency, std::set<int>& graph);
00066         void buildAdjacencyMatrixUsingOctomap();
00067 
00068         bool isPoseValid(const tf::Pose pose, const bool check_collision, std::vector<std::vector<double> >& ik_solutions);
00069         void getValidIKSolutions(const std::vector<tf::Pose>& poses, const bool check_collision, std::vector<bool>& valid,
00070                         std::vector<std::vector<std::vector<double> > >& ik_solutions);
00071         void getSubgraphAdjacency(const std::set<int>& subgraph, std::map<int, int>& index_map,
00072         Eigen::MatrixXi& subgraph_adj, const std::vector<SurfacePatch>& patches,
00073         std::vector<SurfacePatch>& subgraph_patches);
00074         void getIKSolutions(const tf::Pose p, std::vector<std::vector<double> >& ik_solutions) const;
00075 
00076 };
00077 
00078 #endif /* GRAPHCONSTRUCTION_H_ */