30 #include <ompl/geometric/planners/cforest/CForest.h> 31 #include <ompl/geometric/planners/est/EST.h> 32 #include <ompl/geometric/planners/kpiece/BKPIECE1.h> 33 #include <ompl/geometric/planners/kpiece/KPIECE1.h> 34 #include <ompl/geometric/planners/prm/LazyPRM.h> 35 #include <ompl/geometric/planners/prm/PRM.h> 36 #include <ompl/geometric/planners/prm/PRMstar.h> 37 #include <ompl/geometric/planners/rrt/LBTRRT.h> 38 #include <ompl/geometric/planners/rrt/RRT.h> 39 #include <ompl/geometric/planners/rrt/RRTConnect.h> 40 #include <ompl/geometric/planners/rrt/RRTstar.h> 45 REGISTER_MOTIONSOLVER_TYPE("RRTConnectSolver",
exotica::RRTConnectSolver)
46 REGISTER_MOTIONSOLVER_TYPE("PRMSolver",
exotica::PRMSolver)
47 REGISTER_MOTIONSOLVER_TYPE("LazyPRMSolver",
exotica::LazyPRMSolver)
48 REGISTER_MOTIONSOLVER_TYPE("ESTSolver",
exotica::ESTSolver)
49 REGISTER_MOTIONSOLVER_TYPE("KPIECESolver",
exotica::KPIECESolver)
50 REGISTER_MOTIONSOLVER_TYPE("BKPIECESolver",
exotica::BKPIECESolver)
51 REGISTER_MOTIONSOLVER_TYPE("RRTStarSolver",
exotica::RRTStarSolver)
52 REGISTER_MOTIONSOLVER_TYPE("LBTRRTSolver",
exotica::LBTRRTSolver)
56 RRTStarSolver::RRTStarSolver() =
default;
58 void RRTStarSolver::Instantiate(
const RRTStarSolverInitializer &init)
60 init_ = OMPLSolverInitializer(RRTStarSolverInitializer(init));
61 algorithm_ =
"Exotica_RRTStar";
62 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::RRTstar>, _1, _2);
65 LBTRRTSolver::LBTRRTSolver() =
default;
67 void LBTRRTSolver::Instantiate(
const LBTRRTSolverInitializer &init)
69 init_ = OMPLSolverInitializer(LBTRRTSolverInitializer(init));
70 algorithm_ =
"Exotica_LBTRRT";
71 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::LBTRRT>, _1, _2);
74 RRTSolver::RRTSolver() =
default;
76 void RRTSolver::Instantiate(
const RRTSolverInitializer &init)
78 init_ = OMPLSolverInitializer(RRTSolverInitializer(init));
79 algorithm_ =
"Exotica_RRT";
80 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::RRT>, _1, _2);
83 RRTConnectSolver::RRTConnectSolver() =
default;
85 void RRTConnectSolver::Instantiate(
const RRTConnectSolverInitializer &init)
87 init_ = OMPLSolverInitializer(RRTConnectSolverInitializer(init));
88 algorithm_ =
"Exotica_RRTConnect";
89 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::RRTConnect>, _1, _2);
92 void RRTConnectSolver::SetRange(
double range)
95 rrtcon->setRange(range);
98 double RRTConnectSolver::GetRange()
101 return rrtcon->getRange();
104 ESTSolver::ESTSolver() =
default;
106 void ESTSolver::Instantiate(
const ESTSolverInitializer &init)
108 init_ = OMPLSolverInitializer(ESTSolverInitializer(init));
109 algorithm_ =
"Exotica_EST";
110 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::EST>, _1, _2);
113 KPIECESolver::KPIECESolver() =
default;
115 void KPIECESolver::Instantiate(
const KPIECESolverInitializer &init)
117 init_ = OMPLSolverInitializer(KPIECESolverInitializer(init));
118 algorithm_ =
"Exotica_KPIECE";
119 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::KPIECE1>, _1, _2);
122 BKPIECESolver::BKPIECESolver() =
default;
124 void BKPIECESolver::Instantiate(
const BKPIECESolverInitializer &init)
126 init_ = OMPLSolverInitializer(BKPIECESolverInitializer(init));
127 algorithm_ =
"Exotica_BKPIECE";
128 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::BKPIECE1>, _1, _2);
131 PRMSolver::PRMSolver() =
default;
133 void PRMSolver::Instantiate(
const PRMSolverInitializer &init)
135 init_ = OMPLSolverInitializer(PRMSolverInitializer(init));
136 algorithm_ =
"Exotica_PRM";
137 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::PRM>, _1, _2);
138 multi_query_ = init.MultiQuery;
141 void PRMSolver::GrowRoadmap(
double t)
147 void PRMSolver::ExpandRoadmap(
double t)
150 prm->expandRoadmap(t);
153 void PRMSolver::Clear()
156 ompl_simple_setup_->getPlanner()->setProblemDefinition(ompl_simple_setup_->getProblemDefinition());
160 void PRMSolver::ClearQuery()
166 void PRMSolver::Setup()
172 int PRMSolver::EdgeCount()
175 return prm->edgeCount();
178 int PRMSolver::MilestoneCount()
181 return prm->milestoneCount();
184 bool PRMSolver::IsMultiQuery()
const 189 void PRMSolver::SetMultiQuery(
bool val)
194 LazyPRMSolver::LazyPRMSolver() =
default;
196 void LazyPRMSolver::Instantiate(
const LazyPRMSolverInitializer &init)
198 init_ = OMPLSolverInitializer(LazyPRMSolverInitializer(init));
199 algorithm_ =
"Exotica_LazyPRM";
200 planner_allocator_ = boost::bind(&AllocatePlanner<ompl::geometric::LazyPRM>, _1, _2);
201 multi_query_ = init.MultiQuery;
204 void LazyPRMSolver::Clear()
210 void LazyPRMSolver::ClearQuery()
216 void LazyPRMSolver::Setup()
222 int LazyPRMSolver::EdgeCount()
225 return prm->edgeCount();
228 int LazyPRMSolver::MilestoneCount()
231 return prm->milestoneCount();
234 bool LazyPRMSolver::IsMultiQuery()
const 239 void LazyPRMSolver::SetMultiQuery(
bool val)
#define REGISTER_MOTIONSOLVER_TYPE(TYPE, DERIV)
boost::shared_ptr< T > ompl_cast(boost::shared_ptr< T1 > ptr)