Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7351] pkg/trunk
|
From: <pof...@us...> - 2008-11-26 18:17:50
|
Revision: 7351
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7351&view=rev
Author: poftwaresatent
Date: 2008-11-26 18:17:44 +0000 (Wed, 26 Nov 2008)
Log Message:
-----------
fix mem problems, thanks Brian. (resolves #652)
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/mp_benchmarks/src/mpbench-incremental.cpp
pkg/trunk/motion_planning/mp_benchmarks/src/mpbench.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-26 17:17:45 UTC (rev 7350)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-26 18:17:44 UTC (rev 7351)
@@ -116,13 +116,14 @@
virtual void updateGoalMsg();
private:
+ typedef list<ompl::SBPLPlannerStatsEntry> plannerStats_t;
bool isMapDataOK();
MDPConfig mdpCfg_;
ompl::EnvironmentWrapper * env_;
ompl::SBPLPlannerManager * pMgr_;
- ompl::SBPLPlannerStatistics pStat_;
+ plannerStats_t pStat_;
double plannerTimeLimit_; /* The amount of time given to the planner to find a plan */
std::string planStatsFile_;
size_t goalCount_;
@@ -231,7 +232,6 @@
ROS_ERROR("in MoveBaseSBPL ctor: pMgr_->select(%s) failed", plannerType.c_str());
throw int(5);
}
- pStat_.pushBack(plannerType, environmentType);
}
catch (int ii) {
delete env_;
@@ -270,8 +270,7 @@
bool MoveBaseSBPL::makePlan(){
ROS_DEBUG("Planning for new goal...\n");
- ompl::SBPLPlannerStatistics::entry & statsEntry(pStat_.top());
-
+ ompl::SBPLPlannerStatsEntry statsEntry(pMgr_->getName(), env_->getName());
try {
// Update costs
lock();
@@ -374,7 +373,7 @@
statsEntry.logFile(planStatsFile_.c_str(), title, prefix_os.str().c_str());
}
//// statsEntry.logInfo("move_base_sbpl: ");
- pStat_.pushBack(pMgr_->getName(), env_->getName());
+ pStat_.push_back(statsEntry);
updatePlan(plan);
return true;
Modified: pkg/trunk/motion_planning/mp_benchmarks/src/mpbench-incremental.cpp
===================================================================
--- pkg/trunk/motion_planning/mp_benchmarks/src/mpbench-incremental.cpp 2008-11-26 17:17:45 UTC (rev 7350)
+++ pkg/trunk/motion_planning/mp_benchmarks/src/mpbench-incremental.cpp 2008-11-26 18:17:44 UTC (rev 7351)
@@ -74,14 +74,16 @@
static shared_ptr<SBPLBenchmarkSetup> setup;
static shared_ptr<EnvironmentWrapper> environment;
static shared_ptr<SBPLPlannerManager> plannerMgr;
-static shared_ptr<SBPLPlannerStatistics> plannerStats;
static shared_ptr<ostream> logos;
static shared_ptr<footprint_t> footprint;
static planList_t planList;
+typedef vector<SBPLPlannerStatsEntry> plannerStats_t;
+static plannerStats_t plannerStats;
+
int main(int argc, char ** argv)
{
if (0 != atexit(cleanup))
@@ -115,7 +117,6 @@
setup.reset();
environment.reset();
plannerMgr.reset();
- plannerStats.reset();
logos.reset();
planList.clear();
}
@@ -503,9 +504,6 @@
if ( ! plannerMgr->select(plannerType, false, logos.get()))
errx(EXIT_FAILURE, "plannerMgr->select(%s) failed", plannerType.c_str());
*logos << " planner name: " << plannerMgr->getName() << "\n" << flush;
-
- *logos << "creating planner stats\n" << flush;
- plannerStats.reset(new SBPLPlannerStatistics());
}
@@ -516,10 +514,9 @@
IndexTransformWrap const & it(*setup->getIndexTransform());
SBPLBenchmarkSetup::tasklist_t const & tasklist(setup->getTasks());
for (size_t ii(0); ii < tasklist.size(); ++ii) {
- plannerStats->pushBack(plannerMgr->getName(), environment->getName());
- SBPLPlannerStatistics::entry & statsEntry(plannerStats->top());
+ SBPLPlannerStatsEntry statsEntry(plannerMgr->getName(), environment->getName());
SBPLBenchmarkSetup::task const & task(tasklist[ii]);
-
+
*logos << "\n task " << ii << ": " << task.description << "\n" << flush;
// set start
@@ -558,9 +555,10 @@
// - initially just the 1st path we come across
// - subsequently with some allocated timeslice
// - until there is no change in the returned path
-
+
+ statsEntry.plan_length_m = 0; // just in case the first replan() fails
+ statsEntry.plan_angle_change_rad = 0;
vector<int> prevSolution;
- SBPLPlannerStatistics::entry & prevStatsEntry(statsEntry);
for (size_t jj(0); true; ++jj) {
// Handle the first iteration specially.
@@ -583,16 +581,17 @@
&statsEntry.actual_time_system_sec,
&statsEntry.solution_cost,
&solution);
-
+
// forget about this task if we got a planning failure
if ((1 != statsEntry.status) // planners should provide status
|| (1 >= solution.size()) // but sometimes they do not
) {
statsEntry.logStream(*logos, " FAILURE", " ");
*logos << flush;
+ plannerStats.push_back(statsEntry);
break;
}
-
+
// detect whether we got the same result as before, in which
// case we're done
if (prevSolution.size() == solution.size()) {
@@ -605,7 +604,7 @@
if (same) {
statsEntry.logStream(*logos, " FINAL", " ");
*logos << flush;
- statsEntry = prevStatsEntry;
+ plannerStats.push_back(statsEntry);
break;
}
}
@@ -625,11 +624,8 @@
else
statsEntry.logStream(*logos, " IMPROVED", " ");
*logos << flush;
-
- plannerStats->pushBack(plannerMgr->getName(), environment->getName());
- prevStatsEntry = statsEntry;
- statsEntry = plannerStats->top();
-
+ plannerStats.push_back(statsEntry);
+
prevSolution.swap(solution);
}
}
@@ -638,16 +634,15 @@
void print_summary()
{
- SBPLPlannerStatistics::stats_t const & stats(plannerStats->getAll());
size_t n_success(0);
size_t n_fail(0);
double t_success(0);
double t_fail(0);
double lplan(0);
double rplan(0);
- for (SBPLPlannerStatistics::stats_t::const_iterator ie(stats.begin()); ie != stats.end(); ++ie) {
- // cannot use status, some planners say SUCCESS even they fail
- if (ie->plan_length_m < 1e-3) {
+ for (plannerStats_t::const_iterator ie(plannerStats.begin()); ie != plannerStats.end(); ++ie) {
+ // cannot always use status, some planners say SUCCESS even they fail
+ if ((1 != ie->status) || (ie->plan_length_m < 1e-3)) {
++n_fail;
t_fail += ie->actual_time_user_sec;
}
Modified: pkg/trunk/motion_planning/mp_benchmarks/src/mpbench.cpp
===================================================================
--- pkg/trunk/motion_planning/mp_benchmarks/src/mpbench.cpp 2008-11-26 17:17:45 UTC (rev 7350)
+++ pkg/trunk/motion_planning/mp_benchmarks/src/mpbench.cpp 2008-11-26 18:17:44 UTC (rev 7351)
@@ -75,14 +75,16 @@
static shared_ptr<SBPLBenchmarkSetup> setup;
static shared_ptr<EnvironmentWrapper> environment;
static shared_ptr<SBPLPlannerManager> plannerMgr;
-static shared_ptr<SBPLPlannerStatistics> plannerStats;
static shared_ptr<ostream> logos;
static shared_ptr<footprint_t> footprint;
static planList_t planList;
+typedef vector<SBPLPlannerStatsEntry> plannerStats_t;
+static plannerStats_t plannerStats;
+
int main(int argc, char ** argv)
{
if (0 != atexit(cleanup))
@@ -116,7 +118,6 @@
setup.reset();
environment.reset();
plannerMgr.reset();
- plannerStats.reset();
logos.reset();
planList.clear();
}
@@ -547,9 +548,6 @@
if ( ! plannerMgr->select(plannerType, false, logos.get()))
errx(EXIT_FAILURE, "plannerMgr->select(%s) failed", plannerType.c_str());
*logos << " planner name: " << plannerMgr->getName() << "\n" << flush;
-
- *logos << "creating planner stats\n" << flush;
- plannerStats.reset(new SBPLPlannerStatistics());
}
@@ -560,8 +558,7 @@
IndexTransformWrap const & it(*setup->getIndexTransform());
SBPLBenchmarkSetup::tasklist_t const & tasklist(setup->getTasks());
for (size_t ii(0); ii < tasklist.size(); ++ii) {
- plannerStats->pushBack(plannerMgr->getName(), environment->getName());
- SBPLPlannerStatistics::entry & statsEntry(plannerStats->top());
+ SBPLPlannerStatsEntry statsEntry(plannerMgr->getName(), environment->getName());
SBPLBenchmarkSetup::task const & task(tasklist[ii]);
*logos << "\n task " << ii << ": " << task.description << "\n" << flush;
@@ -599,8 +596,6 @@
statsEntry.goalState, statsEntry.goalIx, statsEntry.goalIy, status);
// plan it
- if (task.from_scratch)
- plannerMgr->force_planning_from_scratch();
vector<int> solutionStateIDs;
statsEntry.stop_at_first_solution = false;
statsEntry.plan_from_scratch = task.from_scratch;
@@ -632,6 +627,8 @@
title = " FAILURE";
statsEntry.logStream(*logos, title, " ");
*logos << flush;
+
+ plannerStats.push_back(statsEntry);
}
}
}
@@ -639,16 +636,15 @@
void print_summary()
{
- SBPLPlannerStatistics::stats_t const & stats(plannerStats->getAll());
size_t n_success(0);
size_t n_fail(0);
double t_success(0);
double t_fail(0);
double lplan(0);
double rplan(0);
- for (SBPLPlannerStatistics::stats_t::const_iterator ie(stats.begin()); ie != stats.end(); ++ie) {
- // cannot use status, some planners say SUCCESS even they fail
- if (ie->plan_length_m < 1e-3) {
+ for (plannerStats_t::const_iterator ie(plannerStats.begin()); ie != plannerStats.end(); ++ie) {
+ // cannot always use status, some planners say SUCCESS even they fail
+ if ((ie->status != 1) || (ie->plan_length_m < 1e-3)) {
++n_fail;
t_fail += ie->actual_time_user_sec;
}
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-26 17:17:45 UTC (rev 7350)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-26 18:17:44 UTC (rev 7351)
@@ -239,8 +239,8 @@
}
- SBPLPlannerStatistics::entry::
- entry(std::string const & _plannerType, std::string const & _environmentType)
+ SBPLPlannerStatsEntry::
+ SBPLPlannerStatsEntry(std::string const & _plannerType, std::string const & _environmentType)
: plannerType(_plannerType),
environmentType(_environmentType),
goalState(-1),
@@ -250,28 +250,7 @@
}
- void SBPLPlannerStatistics::
- pushBack(std::string const & plannerType, std::string const & environmentType)
- {
- stats_.push_back(entry(plannerType, environmentType));
- }
-
-
- SBPLPlannerStatistics::entry & SBPLPlannerStatistics::
- top()
- {
- return stats_.back();
- }
-
-
- SBPLPlannerStatistics::stats_t const & SBPLPlannerStatistics::
- getAll() const
- {
- return stats_;
- }
-
-
- void SBPLPlannerStatistics::entry::
+ void SBPLPlannerStatsEntry::
logInfo(char const * prefix) const
{
ostringstream os;
@@ -280,12 +259,12 @@
}
- void SBPLPlannerStatistics::entry::
+ void SBPLPlannerStatsEntry::
logFile(char const * filename, char const * title, char const * prefix) const
{
FILE * ff(fopen(filename, "a"));
if (0 == ff) {
- ROS_WARN("SBPLPlannerStatistics::entry::logFile(): fopen(%s): %s",
+ ROS_WARN("SBPLPlannerStatsEntry::logFile(): fopen(%s): %s",
filename, strerror(errno));
return;
}
@@ -293,12 +272,12 @@
logStream(os, title, prefix);
fprintf(ff, "%s", os.str().c_str());
if (0 != fclose(ff))
- ROS_WARN("SBPLPlannerStatistics::entry::logFile(): fclose() on %s: %s",
+ ROS_WARN("SBPLPlannerStatsEntry::logFile(): fclose() on %s: %s",
filename, strerror(errno));
}
- void SBPLPlannerStatistics::entry::
+ void SBPLPlannerStatsEntry::
logStream(std::ostream & os, std::string const & title, std::string const & prefix) const
{
if ( ! title.empty())
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-26 17:17:45 UTC (rev 7350)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-26 18:17:44 UTC (rev 7351)
@@ -92,65 +92,41 @@
std::ostream * opt_err_os);
- /**
- Collection of statistic for SBPL planner runs.
- */
- class SBPLPlannerStatistics
- {
- public:
- /** One element of the statistics. Represents one SBPLPlanner::replan() cycle. */
- struct entry {
- entry(std::string const & plannerType, std::string const & environmentType);
-
- std::string plannerType; /**< name of the planner (an SBPLPlanner subclass) */
- std::string environmentType; /**< name of the environment type (2D, 3DKIN, ...) */
- std_msgs::Pose2DFloat32 goal; /**< from the std_msgs::Planner2DGoal we received (map frame) */
- unsigned int goalIx; /**< x-index of the goal in the costmap */
- unsigned int goalIy; /**< y-index of the goal in the costmap */
- int goalState; /**< stateID of the goal (from costmap indices) */
- std_msgs::Pose2DFloat32 start; /**< global pose (map frame) "just before" before planning */
- unsigned int startIx; /**< x-index of the start in the costmap */
- unsigned int startIy; /**< y-index of the start in the costmap */
- int startState; /**< stateID of the start (from costmap indices) */
- bool stop_at_first_solution; /**< whether to just plan until any plan is found */
- bool plan_from_scratch; /**< whether to discard any previous solutions */
- double allocated_time_sec; /**< the amount of time we had available for planning */
- double actual_time_wall_sec; /**< the amount of time actually used for planning (wallclock) */
- double actual_time_user_sec; /**< the amount of time actually used for planning (user time) */
- double actual_time_system_sec; /**< the amount of time actually used for planning (system time) */
-
- int status; /**< return value of replan() (i.e. success == 1, or -42 if replan() never got called) */
- int solution_cost; /**< cost of the solution, as given by replan() */
- double plan_length_m; /**< cumulated Euclidean distance between planned waypoints */
- double plan_angle_change_rad; /**< cumulated abs(delta(angle)) along planned waypoints */
-
- /** Use ROS_INFO() to log this entry to rosconsole.
- \todo needs to be unified with the other logXXX() methods
- */
- void logInfo(char const * prefix = "") const;
-
- /** Append this entry to a logfile (which is opened and closed each time). */
- void logFile(char const * filename, char const * title, char const * prefix) const;
-
- /** Append this entry to a stream. */
- void logStream(std::ostream & os, std::string const & title, std::string const & prefix) const;
- };
+ struct SBPLPlannerStatsEntry {
+ SBPLPlannerStatsEntry(std::string const & plannerType, std::string const & environmentType);
- typedef std::vector<entry> stats_t;
+ std::string plannerType; /**< name of the planner (an SBPLPlanner subclass) */
+ std::string environmentType; /**< name of the environment type (2D, 3DKIN, ...) */
+ std_msgs::Pose2DFloat32 goal; /**< from the std_msgs::Planner2DGoal we received (map frame) */
+ unsigned int goalIx; /**< x-index of the goal in the costmap */
+ unsigned int goalIy; /**< y-index of the goal in the costmap */
+ int goalState; /**< stateID of the goal (from costmap indices) */
+ std_msgs::Pose2DFloat32 start; /**< global pose (map frame) "just before" before planning */
+ unsigned int startIx; /**< x-index of the start in the costmap */
+ unsigned int startIy; /**< y-index of the start in the costmap */
+ int startState; /**< stateID of the start (from costmap indices) */
+ bool stop_at_first_solution; /**< whether to just plan until any plan is found */
+ bool plan_from_scratch; /**< whether to discard any previous solutions */
+ double allocated_time_sec; /**< the amount of time we had available for planning */
+ double actual_time_wall_sec; /**< the amount of time actually used for planning (wallclock) */
+ double actual_time_user_sec; /**< the amount of time actually used for planning (user time) */
+ double actual_time_system_sec; /**< the amount of time actually used for planning (system time) */
- /** Create a fresh entry and append it to the end of the
- accumulated statistics. */
- void pushBack(std::string const & plannerType, std::string const & environmentType);
+ int status; /**< return value of replan() (i.e. success == 1, or -42 if replan() never got called) */
+ int solution_cost; /**< cost of the solution, as given by replan() */
+ double plan_length_m; /**< cumulated Euclidean distance between planned waypoints */
+ double plan_angle_change_rad; /**< cumulated abs(delta(angle)) along planned waypoints */
- /** \return The topmost (latest) element, which is only defined if
- you called pushBack() at least once. */
- entry & top();
+ /** Use ROS_INFO() to log this entry to rosconsole.
+ \todo needs to be unified with the other logXXX() methods
+ */
+ void logInfo(char const * prefix = "") const;
- /** Read-only access to the entire history. */
- stats_t const & getAll() const;
+ /** Append this entry to a logfile (which is opened and closed each time). */
+ void logFile(char const * filename, char const * title, char const * prefix) const;
- protected:
- stats_t stats_;
+ /** Append this entry to a stream. */
+ void logStream(std::ostream & os, std::string const & title, std::string const & prefix) const;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
