00001 /* 00002 * dense_planner.h 00003 * 00004 * Created on: Feb 9, 2015 00005 * Author: ros developer 00006 */ 00007 00008 #ifndef DENSE_PLANNER_H_ 00009 #define DENSE_PLANNER_H_ 00010 00011 #include <descartes_core/path_planner_base.h> 00012 #include <descartes_planner/planning_graph.h> 00013 00014 namespace descartes_planner 00015 { 00016 class DensePlanner : public descartes_core::PathPlannerBase 00017 { 00018 public: 00019 DensePlanner(); 00020 00021 virtual ~DensePlanner(); 00022 00023 virtual bool initialize(descartes_core::RobotModelConstPtr model); 00024 virtual bool initialize(descartes_core::RobotModelConstPtr model, 00025 descartes_planner::CostFunction cost_function_callback); 00026 virtual bool setConfig(const descartes_core::PlannerConfig& config); 00027 virtual void getConfig(descartes_core::PlannerConfig& config) const; 00028 virtual bool planPath(const std::vector<descartes_core::TrajectoryPtPtr>& traj); 00029 virtual bool getPath(std::vector<descartes_core::TrajectoryPtPtr>& path) const; 00030 virtual bool addAfter(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); 00031 virtual bool addBefore(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); 00032 virtual bool remove(const descartes_core::TrajectoryPt::ID& ref_id); 00033 virtual bool modify(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); 00034 virtual int getErrorCode() const; 00035 virtual bool getErrorMessage(int error_code, std::string& msg) const; 00036 00037 // Helper functions meant to access the underlying graph structure 00038 00039 const PlanningGraph& getPlanningGraph() const 00040 { 00041 return *planning_graph_; 00042 } 00043 00044 protected: 00045 descartes_core::TrajectoryPt::ID getPrevious(const descartes_core::TrajectoryPt::ID& ref_id); 00046 descartes_core::TrajectoryPt::ID getNext(const descartes_core::TrajectoryPt::ID& ref_id); 00047 descartes_core::TrajectoryPtPtr get(const descartes_core::TrajectoryPt::ID& ref_id); 00048 bool updatePath(); 00049 00050 protected: 00051 boost::shared_ptr<descartes_planner::PlanningGraph> planning_graph_; 00052 int error_code_; 00053 descartes_core::PlannerConfig config_; 00054 std::vector<descartes_core::TrajectoryPtPtr> path_; 00055 std::map<int, std::string> error_map_; 00056 }; 00057 00058 } /* namespace descartes_core */ 00059 #endif /* DENSE_PLANNER_H_ */