|
From: <rph...@us...> - 2009-02-12 00:27:27
|
Revision: 11017
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11017&view=rev
Author: rphilippsen
Date: 2009-02-12 00:27:17 +0000 (Thu, 12 Feb 2009)
Log Message:
-----------
hooked 16-connected 2D grids into mpglue, mpbench, and highlevel_controllers (#925)
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/mpbench/mkhtml.sh
pkg/trunk/motion_planning/mpbench/src/setup.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h
pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -162,7 +162,9 @@
boost::shared_ptr<mpglue::IndexTransform> mit(mpglue::createIndexTransform(&getCostMap()));
if ("2D" == environmentType)
- env_.reset(mpglue::SBPLEnvironment::create2D(mcm, mit));
+ env_.reset(mpglue::SBPLEnvironment::create2D(mcm, mit, false));
+ if ("2D16" == environmentType)
+ env_.reset(mpglue::SBPLEnvironment::create2D(mcm, mit, true));
else if ("3DKIN" == environmentType) {
string const prefix("env3d/");
//// ignored by SBPL (at least in r9900).
@@ -179,7 +181,7 @@
timetoturn45degsinplace_secs, 0));
}
else {
- ROS_ERROR("in MoveBaseSBPL ctor: invalid environmentType \"%s\", use 2D or 3DKIN",
+ ROS_ERROR("in MoveBaseSBPL ctor: invalid environmentType \"%s\", use 2D, 2D16 or 3DKIN",
environmentType.c_str());
throw int(2);
}
Modified: pkg/trunk/motion_planning/mpbench/mkhtml.sh
===================================================================
--- pkg/trunk/motion_planning/mpbench/mkhtml.sh 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpbench/mkhtml.sh 2009-02-12 00:27:17 UTC (rev 11017)
@@ -2,7 +2,7 @@
MPBENCH=`rospack find mpbench`/mpbench-base
-CONSTANT_OPTS="-w hc:cubicle:1.2:3"
+CONSTANT_OPTS="-w xml:../data/scrape-corner.xml"
CONSTANT_HR=""
A_OPT="-p"
Modified: pkg/trunk/motion_planning/mpbench/src/setup.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/setup.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpbench/src/setup.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -911,10 +911,18 @@
shared_ptr<mpglue::SBPLEnvironment> sbpl_environment;
if ("2d" == envstr) {
if (progress_os)
- *progress_os << " creating 2DEnvironment\n" << flush;
+ *progress_os << " creating 8-connected 2DEnvironment\n" << flush;
sbpl_environment.reset(mpglue::SBPLEnvironment::create2D(setup->getCostmap(),
- setup->getIndexTransform()));
+ setup->getIndexTransform(),
+ false));
}
+ else if ("2d16" == envstr) {
+ if (progress_os)
+ *progress_os << " creating 16-connected 2DEnvironment\n" << flush;
+ sbpl_environment.reset(mpglue::SBPLEnvironment::create2D(setup->getCostmap(),
+ setup->getIndexTransform(),
+ true));
+ }
else if ("3dkin" == envstr) {
if (progress_os)
*progress_os << " creating 3DKINEnvironment\n" << flush;
@@ -930,7 +938,7 @@
}
else
throw runtime_error("mpbench::Setup::create(): invalid environment token \""
- + envstr + "\", must be \"2d\" or \"3dkin\"");
+ + envstr + "\", must be \"2d\", \"2d16\" or \"3dkin\"");
if ( ! sbpl_environment)
throw runtime_error("mpbench::Setup::create(): failed to create SBPLEnvironment from \""
+ envstr + "\"");
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -87,7 +87,8 @@
{
public:
SBPLEnvironment2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it);
+ boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV2D * env);
virtual ~SBPLEnvironment2D();
virtual DiscreteSpaceInformation * getDSI();
@@ -101,7 +102,6 @@
virtual void SetGoalTolerance(double tol_xy, double tol_th);
virtual deprecated_msgs::Pose2DFloat32 GetPoseFromState(int stateID) const throw(invalid_state);
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const;
- virtual std::string getName() const;
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost);
@@ -122,10 +122,10 @@
public:
SBPLEnvironment3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV3DKIN * env,
footprint_t const & footprint,
double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos);
+ double timetoturn45degsinplace_secs);
virtual ~SBPLEnvironment3DKIN();
virtual DiscreteSpaceInformation * getDSI();
@@ -139,7 +139,6 @@
virtual void SetGoalTolerance(double tol_xy, double tol_th);
virtual deprecated_msgs::Pose2DFloat32 GetPoseFromState(int stateID) const throw(invalid_state);
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const;
- virtual std::string getName() const;
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost);
@@ -219,40 +218,78 @@
SBPLEnvironment * SBPLEnvironment::
create2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it)
+ boost::shared_ptr<IndexTransform const> it,
+ bool is16connected) throw(std::exception)
{
- SBPLEnvironment2D * env(new SBPLEnvironment2D(cm, it));
- return env;
+ EnvironmentNAV2D * env(new EnvironmentNAV2D());
+ if ((is16connected) && ( ! env->SetEnvParameter("is16connected", 1))) {
+ delete env;
+ throw runtime_error("mpglue::SBPLEnvironment::create2D(): EnvironmentNAV2D::SetEnvParameter() failed for \"is16connected\"");
+ }
+
+ int const obst_cost_thresh(cm->getCSpaceObstacleCost());
+
+ // good: Take advantage of the fact that InitializeEnv() can take
+ // a NULL-pointer as mapdata in order to initialize to all
+ // freespace.
+ //
+ // bad: Most costmaps do not support negative grid indices, so the
+ // generic CostmapAccessor::getXBegin() and getYBegin() are ignored
+ // and simply assumed to always return 0 (which they won't if we
+ // use growable costmaps).
+ env->InitializeEnv(cm->getXEnd(), // width
+ cm->getYEnd(), // height
+ 0, // mapdata
+ obst_cost_thresh);
+
+ // as above, assume getXBegin() and getYBegin() are always zero
+ for (ssize_t ix(0); ix < cm->getXEnd(); ++ix)
+ for (ssize_t iy(0); iy < cm->getYEnd(); ++iy) {
+ int cost;
+ if (cm->getCost(ix, iy, &cost)) // "always" succeeds though
+ env->UpdateCost(ix, iy, cost);
+ }
+
+ return new SBPLEnvironment2D(cm, it, env);
}
SBPLEnvironment * SBPLEnvironment::
create3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ // bool is16connected,
footprint_t const & footprint,
double nominalvel_mpersecs,
double timetoturn45degsinplace_secs,
- ostream * dbgos)
+ ostream * dbgos) throw(std::exception)
{
- SBPLEnvironment3DKIN * env(new SBPLEnvironment3DKIN(cm, it,
- footprint, nominalvel_mpersecs,
- timetoturn45degsinplace_secs,
- dbgos));
- return env;
- }
-
-}
-
-namespace {
-
- SBPLEnvironment2D::
- SBPLEnvironment2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it)
- : SBPLEnvironment(cm, it),
- env_(new EnvironmentNAV2D())
- {
- int const obst_cost_thresh(cm->getCSpaceObstacleCost());
+ EnvironmentNAV3DKIN * env(new EnvironmentNAV3DKIN());
+// if ((is16connected) && ( ! env->SetEnvParameter("is16connected", 1))) {
+// delete env;
+// throw runtime_error("mpglue::SBPLEnvironment::create3DKIN(): EnvironmentNAV3DKIN::SetEnvParameter() failed for \"is16connected\"");
+// }
+ int const obst_cost_thresh(cm->getWSpaceObstacleCost());
+ vector<sbpl_2Dpt_t> perimeterptsV;
+ perimeterptsV.reserve(footprint.size());
+ for (size_t ii(0); ii < footprint.size(); ++ii) {
+ sbpl_2Dpt_t pt;
+ pt.x = footprint[ii].x;
+ pt.y = footprint[ii].y;
+ perimeterptsV.push_back(pt);
+ }
+
+ if (dbgos) {
+ *dbgos << "mpglue::SBPLEnvironment3DKIN:\n"
+ << " perimeterptsV =\n";
+ for (vector<sbpl_2Dpt_t>::const_iterator ip(perimeterptsV.begin());
+ ip != perimeterptsV.end(); ++ip)
+ *dbgos << " " << ip->x << "\t" << ip->y << "\n";
+ *dbgos << " nominalvel_mpersecs = " << nominalvel_mpersecs << "\n"
+ << " timetoturn45degsinplace_secs = " << timetoturn45degsinplace_secs << "\n"
+ << " obst_cost_thresh = " << obst_cost_thresh << "\n" << flush;
+ }
+
// good: Take advantage of the fact that InitializeEnv() can take
// a NULL-pointer as mapdata in order to initialize to all
// freespace.
@@ -261,22 +298,39 @@
// generic CostmapAccessor::getXBegin() and getYBegin() are ignored
// and simply assumed to always return 0 (which they won't if we
// use growable costmaps).
- env_->InitializeEnv(cm->getXEnd(), // width
- cm->getYEnd(), // height
- 0, // mapdata
- obst_cost_thresh);
+ env->InitializeEnv(cm->getXEnd(), // width
+ cm->getYEnd(), // height
+ 0, // mapdata
+ perimeterptsV, it->getResolution(), nominalvel_mpersecs,
+ timetoturn45degsinplace_secs, obst_cost_thresh);
// as above, assume getXBegin() and getYBegin() are always zero
for (ssize_t ix(0); ix < cm->getXEnd(); ++ix)
for (ssize_t iy(0); iy < cm->getYEnd(); ++iy) {
int cost;
if (cm->getCost(ix, iy, &cost)) // "always" succeeds though
- env_->UpdateCost(ix, iy, cost);
+ env->UpdateCost(ix, iy, cost);
}
+
+ return new SBPLEnvironment3DKIN(cm, it, env, footprint,
+ nominalvel_mpersecs, timetoturn45degsinplace_secs);
}
+}
+
+namespace {
SBPLEnvironment2D::
+ SBPLEnvironment2D(boost::shared_ptr<CostmapAccessor const> cm,
+ boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV2D * env)
+ : SBPLEnvironment(cm, it),
+ env_(env)
+ {
+ }
+
+
+ SBPLEnvironment2D::
~SBPLEnvironment2D()
{
delete env_;
@@ -396,70 +450,16 @@
}
- std::string SBPLEnvironment2D::
- getName() const
- {
- std::string name("2D");
- return name;
- }
-
-
SBPLEnvironment3DKIN::
SBPLEnvironment3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV3DKIN * env,
footprint_t const & footprint,
double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos)
+ double timetoturn45degsinplace_secs)
: SBPLEnvironment(cm, it),
- env_(new EnvironmentNAV3DKIN())
+ env_(env)
{
- int const obst_cost_thresh(cm->getWSpaceObstacleCost());
- vector<sbpl_2Dpt_t> perimeterptsV;
- perimeterptsV.reserve(footprint.size());
- for (size_t ii(0); ii < footprint.size(); ++ii) {
- sbpl_2Dpt_t pt;
- pt.x = footprint[ii].x;
- pt.y = footprint[ii].y;
- perimeterptsV.push_back(pt);
- }
-
- if (dbgos) {
- *dbgos << "mpglue::SBPLEnvironment3DKIN:\n"
- << " perimeterptsV =\n";
- for (vector<sbpl_2Dpt_t>::const_iterator ip(perimeterptsV.begin());
- ip != perimeterptsV.end(); ++ip)
- *dbgos << " " << ip->x << "\t" << ip->y << "\n";
- *dbgos << " nominalvel_mpersecs = " << nominalvel_mpersecs << "\n"
- << " timetoturn45degsinplace_secs = " << timetoturn45degsinplace_secs << "\n"
- << " obst_cost_thresh = " << obst_cost_thresh << "\n" << flush;
- }
-
- // good: Take advantage of the fact that InitializeEnv() can take
- // a NULL-pointer as mapdata in order to initialize to all
- // freespace.
- //
- // bad: Most costmaps do not support negative grid indices, so the
- // generic CostmapAccessor::getXBegin() and getYBegin() are ignored
- // and simply assumed to always return 0 (which they won't if we
- // use growable costmaps).
- //
- // also there is quite a bit of code duplication between this, the
- // SBPLEnvironment2D ctor, and
- // SBPLEnvironment3DKIN::DoUpdateCost()...
- env_->InitializeEnv(cm->getXEnd(), // width
- cm->getYEnd(), // height
- 0, // mapdata
- perimeterptsV, it->getResolution(), nominalvel_mpersecs,
- timetoturn45degsinplace_secs, obst_cost_thresh);
-
- // as above, assume getXBegin() and getYBegin() are always zero
- for (ssize_t ix(0); ix < cm->getXEnd(); ++ix)
- for (ssize_t iy(0); iy < cm->getYEnd(); ++iy) {
- int cost;
- if (cm->getCost(ix, iy, &cost)) // "always" succeeds though
- env_->UpdateCost(ix, iy, cost);
- }
}
@@ -591,12 +591,4 @@
return env_->GetStateFromCoord(ix, iy, ith);
}
-
- std::string SBPLEnvironment3DKIN::
- getName() const
- {
- std::string name("3DKIN");
- return name;
- }
-
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h 2009-02-12 00:27:17 UTC (rev 11017)
@@ -83,14 +83,16 @@
static SBPLEnvironment * create2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it);
+ boost::shared_ptr<IndexTransform const> it,
+ bool is16connected) throw(std::exception);
static SBPLEnvironment * create3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ //bool is16connected,
footprint_t const & footprint,
double nominalvel_mpersecs,
double timetoturn45degsinplace_secs,
- std::ostream * dbgos);
+ std::ostream * dbgos) throw(std::exception);
virtual DiscreteSpaceInformation * getDSI() = 0;
virtual bool InitializeMDPCfg(MDPConfig *MDPCfg) = 0;
@@ -147,8 +149,6 @@
of the map. */
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const = 0;
- virtual std::string getName() const = 0;
-
boost::shared_ptr<CostmapAccessor const> getCostmap() const { return cm_; }
boost::shared_ptr<IndexTransform const> getIndexTransform() const { return it_; }
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -194,65 +194,65 @@
}
- SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search)
- {
- shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
- shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search)
+// {
+// shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
+// shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
- SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search)
- {
- shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
- shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search)
+// {
+// shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
+// shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
- SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos)
- {
- shared_ptr<SBPLEnvironment>
- environment(SBPLEnvironment::create3DKIN(cm, it,
- footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs,
- dbgos));
- shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// ostream * dbgos)
+// {
+// shared_ptr<SBPLEnvironment>
+// environment(SBPLEnvironment::create3DKIN(cm, it,
+// footprint,
+// nominalvel_mpersecs,
+// timetoturn45degsinplace_secs,
+// dbgos));
+// shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
- SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos)
- {
- shared_ptr<SBPLEnvironment>
- environment(SBPLEnvironment::create3DKIN(cm, it,
- footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs,
- dbgos));
- shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// ostream * dbgos)
+// {
+// shared_ptr<SBPLEnvironment>
+// environment(SBPLEnvironment::create3DKIN(cm, it,
+// footprint,
+// nominalvel_mpersecs,
+// timetoturn45degsinplace_secs,
+// dbgos));
+// shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h 2009-02-12 00:27:17 UTC (rev 11017)
@@ -89,33 +89,33 @@
};
- SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh);
+// SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh);
- SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh);
+// SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh);
- SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- std::ostream * dbgos);
+// SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// std::ostream * dbgos);
- SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- std::ostream * dbgos);
+// SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// std::ostream * dbgos);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|