Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6531] pkg/trunk
|
From: <pof...@us...> - 2008-11-11 20:13:07
|
Revision: 6531
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6531&view=rev
Author: poftwaresatent
Date: 2008-11-11 20:13:02 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
Introduced planner-type aliases (you can say e.g. "arastar" to mean
"ARAPlanner") and optional dumping to std::ostream of available names
and aliases in case the name you specified was not recognized.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-11 20:03:10 UTC (rev 6530)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-11 20:13:02 UTC (rev 6531)
@@ -229,7 +229,7 @@
string plannerType;
local_param("plannerType", plannerType, string("ARAPlanner"));
pMgr_ = new ompl::SBPLPlannerManager(env_->getDSI(), false, &mdpCfg_);
- if ( ! pMgr_->select(plannerType, false)) {
+ if ( ! pMgr_->select(plannerType, false, 0)) {
ROS_ERROR("in MoveBaseSBPL ctor: pMgr_->select(%s) failed", plannerType.c_str());
throw int(5);
}
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-11 20:03:10 UTC (rev 6530)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-11 20:13:02 UTC (rev 6531)
@@ -47,36 +47,73 @@
#include <errno.h>
#include <cstring>
+using namespace std;
+
namespace {
- std::string mk_invalid_state_str(std::string const & method, int state) {
- std::ostringstream os;
+ string mk_invalid_state_str(string const & method, int state) {
+ ostringstream os;
os << "EnvironmentWrapper::invalid_state in method " << method << ": state = " << state;
return os.str();
}
+ static map<string, string> planner_alias;
+
}
namespace ompl {
+ std::string canonicalPlannerName(std::string const & name_or_alias)
+ {
+ if (planner_alias.empty()) {
+ planner_alias.insert(make_pair("ARAPlanner", "ARAPlanner"));
+ planner_alias.insert(make_pair("ara", "ARAPlanner"));
+ planner_alias.insert(make_pair("ARA", "ARAPlanner"));
+ planner_alias.insert(make_pair("arastar", "ARAPlanner"));
+ planner_alias.insert(make_pair("ARAStar", "ARAPlanner"));
+
+ planner_alias.insert(make_pair("ADPlanner", "ADPlanner"));
+ planner_alias.insert(make_pair("ad", "ADPlanner"));
+ planner_alias.insert(make_pair("AD", "ADPlanner"));
+ planner_alias.insert(make_pair("adstar", "ADPlanner"));
+ planner_alias.insert(make_pair("ADStar", "ADPlanner"));
+ }
+
+ map<string, string>::const_iterator is(planner_alias.find(name_or_alias));
+ if (planner_alias.end() == is)
+ return "";
+ return is->second;
+ }
+
+
SBPLPlanner * createSBPLPlanner(std::string const & name,
DiscreteSpaceInformation* environment,
bool bforwardsearch,
- MDPConfig* mdpCfg)
+ MDPConfig* mdpCfg,
+ std::ostream * opt_err_os)
{
- if ("ARAPlanner" == name)
+ string const canonical_name(canonicalPlannerName(name));
+ if ("ARAPlanner" == canonical_name)
return new ARAPlanner(environment, bforwardsearch);
- if ("ADPlanner" == name)
+ if ("ADPlanner" == canonical_name)
return new ADPlanner(environment, bforwardsearch);
// VIPlanner not instantiable... work in progress by Max
#ifdef UNDEFINED
- if ("VIPlanner" == name)
+ if ("VIPlanner" == canonical_name)
return new VIPlanner(environment, mdpCfg);
#endif // UNDEFINED
-
+
+ if (opt_err_os) {
+ *opt_err_os << "ompl::createSBPLPlanner(): no planner called \"name\"\n"
+ << " use \"ARAPlanner\" or \"ADPlanner\"\n"
+ << " or one of the registered aliases:\n";
+ for (map<string, string>::const_iterator is(planner_alias.begin()); is != planner_alias.end(); ++is)
+ *opt_err_os << " " << is->first << " --> " << is->second << "\n";
+ }
+
return 0;
}
@@ -109,11 +146,11 @@
bool SBPLPlannerManager::
- select(std::string const & name, bool recycle)
+ select(std::string const & name, bool recycle, std::ostream * opt_err_os)
{
if (recycle && (name == name_))
return true;
- SBPLPlanner * new_planner(createSBPLPlanner(name, environment_, bforwardsearch_, mdpCfg_));
+ SBPLPlanner * new_planner(createSBPLPlanner(name, environment_, bforwardsearch_, mdpCfg_, opt_err_os));
if ( ! new_planner)
return false;
delete planner_;
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-11 20:03:10 UTC (rev 6530)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-11 20:13:02 UTC (rev 6531)
@@ -58,6 +58,13 @@
/**
+ Translate a name or alias into a string that createSBPLPlanner()
+ understands.
+ */
+ std::string canonicalPlannerName(std::string const & name_or_alias);
+
+
+ /**
Create a planner subclass based on its name.
\todo Use some sort of registry instead of hard-coded
@@ -73,7 +80,9 @@
/** Required by some planners (ARAPlanner, ADPlanner). */
bool bforwardsearch,
/** Required by some planners (VIPlanner). */
- MDPConfig* mdpCfg);
+ MDPConfig* mdpCfg,
+ /** optional stream to which error messages get written */
+ std::ostream * opt_err_os);
/**
@@ -171,7 +180,9 @@
\return true if createSBPLPlanner() succeeded OR the old instance was recycled.
*/
- bool select(std::string const & name, bool recycle);
+ bool select(std::string const & name, bool recycle,
+ /** optional stream to which error messages get written */
+ std::ostream * opt_err_os);
/** Dispatch to the currently select()-ed planner's
SBPLPlanner::replan(), measuring the time it actually takes to
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
