Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7752] pkg/trunk
|
From: <pof...@us...> - 2008-12-07 14:57:40
|
Revision: 7752
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7752&view=rev
Author: poftwaresatent
Date: 2008-12-07 14:57:36 +0000 (Sun, 07 Dec 2008)
Log Message:
-----------
sbpl_util:
* number of state expansions added to planner stats (resolve #672)
mp_benchmarks:
* no more Valgrind in mkhtml-inc.sh (resolve #671)
* measuring and summarizing the number of state expansions and their speed (resolve #672)
* storing stats of planning failures as well as successes
highlevel_controllers:
* adapted move_base_sbpl to changes in sbpl_util
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/mp_benchmarks/mkhtml-inc.sh
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-12-07 14:25:44 UTC (rev 7751)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-12-07 14:57:36 UTC (rev 7752)
@@ -345,6 +345,7 @@
&statsEntry.actual_time_wall_sec,
&statsEntry.actual_time_user_sec,
&statsEntry.actual_time_system_sec,
+ &statsEntry.number_of_expands,
&statsEntry.solution_cost,
&statsEntry.solution_epsilon,
&solutionStateIDs);
Modified: pkg/trunk/motion_planning/mp_benchmarks/mkhtml-inc.sh
===================================================================
--- pkg/trunk/motion_planning/mp_benchmarks/mkhtml-inc.sh 2008-12-07 14:25:44 UTC (rev 7751)
+++ pkg/trunk/motion_planning/mp_benchmarks/mkhtml-inc.sh 2008-12-07 14:57:36 UTC (rev 7752)
@@ -92,11 +92,13 @@
echo "extracting basename for $allopts"
basename=`$MPBENCH $allopts -X`
- echo "<a href=\" $basename.txt \"> log </a> <a href=\" cons-$basename.txt \"> cons </a> <a href=\" vg-$basename.txt \"> vg </a> <a href=\" $basename.png \"> png </a><br>" >> index.html
+ echo "<a href=\" $basename.txt \"> log </a> <a href=\" cons-$basename.txt \"> cons </a> <a href=\" $basename.png \"> png </a><br>" >> index.html
+## echo "<a href=\" $basename.txt \"> log </a> <a href=\" cons-$basename.txt \"> cons </a> <a href=\" vg-$basename.txt \"> vg </a> <a href=\" $basename.png \"> png </a><br>" >> index.html
echo "<a href=\" $basename.png \"><img src=\" small-$basename.png \" alt=\" $basename.png \"></a><br>" >> index.html
echo "running with $allopts -W"
- valgrind --log-file-exactly=vg-$basename.txt $MPBENCH $allopts -W 2>&1 | tee cons-$basename.txt
- chmod a+r vg-$basename.txt
+## valgrind --log-file-exactly=vg-$basename.txt $MPBENCH $allopts -W 2>&1 | tee cons-$basename.txt
+## chmod a+r vg-$basename.txt
+ $MPBENCH $allopts -W 2>&1 | tee cons-$basename.txt
cat $basename.html >> index.html
Modified: pkg/trunk/motion_planning/mp_benchmarks/src/mpbench-incremental.cpp
===================================================================
--- pkg/trunk/motion_planning/mp_benchmarks/src/mpbench-incremental.cpp 2008-12-07 14:25:44 UTC (rev 7751)
+++ pkg/trunk/motion_planning/mp_benchmarks/src/mpbench-incremental.cpp 2008-12-07 14:57:36 UTC (rev 7752)
@@ -81,11 +81,10 @@
static planList_t planList;
typedef map<waypoint_plan_t const *, SBPLPlannerStatsEntry> successStats_t;
-////typedef map<size_t, SBPLPlannerStatsEntry> failureStats_t;
+typedef map<size_t, SBPLPlannerStatsEntry> failureStats_t;
static successStats_t successStats;
-////static failureStats_t failureStats;
+static failureStats_t failureStats;
-
int main(int argc, char ** argv)
{
if (0 != atexit(cleanup))
@@ -588,6 +587,7 @@
double cumul_actual_time_wall(0);
double cumul_actual_time_user(0);
double cumul_actual_time_system(0);
+ ssize_t cumul_expands(0);
for (size_t jj(0); true; ++jj) {
// Handle the first iteration specially.
@@ -609,13 +609,24 @@
&statsEntry.actual_time_wall_sec,
&statsEntry.actual_time_user_sec,
&statsEntry.actual_time_system_sec,
+ &statsEntry.number_of_expands,
&statsEntry.solution_cost,
&statsEntry.solution_epsilon,
&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;
+ failureStats.insert(make_pair(ii, statsEntry));
+ break;
+ }
+
// A little hack to make the initial solution appear as if it
// had used exactly the allocated time. Actually, it gets
- // allocated a practically inifinite amount of time, which is
+ // allocated a practically infinite amount of time, which is
// not useful for cumulating in the statistics. Also, it seems
// that SBPLPlanner uses wall time, not user time, to limit its
// search.
@@ -623,20 +634,8 @@
if (0 == jj)
statsEntry.allocated_time_sec = statsEntry.actual_time_wall_sec;
- // 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.allocated_time_sec = cumul_allocated_time;
- statsEntry.actual_time_wall_sec = cumul_actual_time_wall;
- statsEntry.actual_time_user_sec = cumul_actual_time_user;
- statsEntry.actual_time_system_sec = cumul_actual_time_system;
- statsEntry.logStream(*logos, " FAILURE (below are cumulated times)", " ");
- *logos << flush;
- //// do NOT add to overall stats because of cumulated times
- // plannerStats.push_back(statsEntry);
- break;
- }
+ if (0 < statsEntry.number_of_expands)
+ cumul_expands += statsEntry.number_of_expands;
// detect whether we got the same result as before, in which
// case we're done
@@ -648,7 +647,8 @@
statsEntry.actual_time_wall_sec = cumul_actual_time_wall;
statsEntry.actual_time_user_sec = cumul_actual_time_user;
statsEntry.actual_time_system_sec = cumul_actual_time_system;
- statsEntry.logStream(*logos, " FINAL: epsilon did not change (below are cumulated times)", " ");
+ statsEntry.number_of_expands = cumul_expands;
+ statsEntry.logStream(*logos, " FINAL: epsilon did not change (below are cumulated times and expands)", " ");
*logos << flush;
//// do NOT add to overall stats because of cumulated times
// plannerStats.push_back(statsEntry);
@@ -669,7 +669,8 @@
statsEntry.actual_time_wall_sec = cumul_actual_time_wall;
statsEntry.actual_time_user_sec = cumul_actual_time_user;
statsEntry.actual_time_system_sec = cumul_actual_time_system;
- statsEntry.logStream(*logos, " FINAL: path states did not change (below are cumulated times)", " ");
+ statsEntry.number_of_expands = cumul_expands;
+ statsEntry.logStream(*logos, " FINAL: path states did not change (below are cumulated times and expands)", " ");
*logos << flush;
//// do NOT add to overall stats because of cumulated times
// plannerStats.push_back(statsEntry);
@@ -747,9 +748,12 @@
cout << "sorry, could not open file " << htmlFilename << "\n";
return;
}
+
+ //////////////////////////////////////////////////
+ // cumulated state expansions
htmlOs << "<table border=\"1\" cellpadding=\"2\">\n"
- << "<tr><th colspan=\"5\">consumed planning time (wall clock)</th></tr>\n"
+ << "<tr><th colspan=\"5\">cumulated state expansions</th></tr>\n"
<< "<tr><td>task</td><td>init</td><td>final</td><td>delta</td><td>% delta</td></tr>\n";
for (planList_t::const_iterator ip(planList.begin()); ip != planList.end(); ++ip) {
htmlOs << "<tr><td>" << ip->first << "</td>";
@@ -765,6 +769,103 @@
htmlOs << "<td colspan=\"4\"><em>error: no stats for initial solution</em></td></tr>\n";
continue;
}
+ if (1 == bundle.size()) {
+ htmlOs << "<td colspan=\"2\">" << initialStats->second.number_of_expands << "</td>"
+ << "<td colspan=\"2\">N/A</td></tr>\n";
+ continue;
+ }
+ ssize_t cumul_expands(initialStats->second.number_of_expands);
+ bool error(false);
+ for (++ib; ib != bundle.end(); ++ib) {
+ successStats_t::const_iterator stats(successStats.find(ib->get()));
+ if (successStats.end() == stats) {
+ error = true;
+ break;
+ }
+ cumul_expands += stats->second.number_of_expands;
+ }
+ if (error) {
+ htmlOs << "<td colspan=\"3\"><em>error: missing stats</em></td></tr>\n";
+ continue;
+ }
+ ssize_t const delta_expands(cumul_expands - initialStats->second.number_of_expands);
+ htmlOs << "<td>" << initialStats->second.number_of_expands << "</td>"
+ << "<td>" << cumul_expands << "</td>"
+ << "<td>" << delta_expands << "</td>"
+ << "<td>" << 1.0e2 * delta_expands / initialStats->second.number_of_expands
+ << "</td></tr>\n";
+ }
+
+ //////////////////////////////////////////////////
+ // expansions per second
+
+ htmlOs << "<table border=\"1\" cellpadding=\"2\">\n"
+ << "<tr><th colspan=\"5\">expansions speed [1/s] (wall clock)</th></tr>\n"
+ << "<tr><td>task</td><td>init</td><td>final</td><td>delta</td><td>% delta</td></tr>\n";
+ for (planList_t::const_iterator ip(planList.begin()); ip != planList.end(); ++ip) {
+ htmlOs << "<tr><td>" << ip->first << "</td>";
+ planBundle_t const & bundle(ip->second);
+ if (bundle.empty()) {
+ htmlOs << "<td colspan=\"4\"><em>no solution</em></td></tr>\n";
+ continue;
+ }
+ planBundle_t::const_iterator ib(bundle.begin());
+ waypoint_plan_t const * initialPlan(ib->get());
+ successStats_t::const_iterator initialStats(successStats.find(initialPlan));
+ if (successStats.end() == initialStats) {
+ htmlOs << "<td colspan=\"4\"><em>error: no stats for initial solution</em></td></tr>\n";
+ continue;
+ }
+ double cumul_time(initialStats->second.actual_time_wall_sec);
+ double cumul_expands(initialStats->second.number_of_expands);
+ double const init_speed(cumul_expands / cumul_time);
+ if (1 == bundle.size()) {
+ htmlOs << "<td colspan=\"2\">" << init_speed << "</td>"
+ << "<td colspan=\"2\">N/A</td></tr>\n";
+ continue;
+ }
+ bool error(false);
+ for (++ib; ib != bundle.end(); ++ib) {
+ successStats_t::const_iterator stats(successStats.find(ib->get()));
+ if (successStats.end() == stats) {
+ error = true;
+ break;
+ }
+ cumul_time += stats->second.actual_time_wall_sec;
+ cumul_expands += stats->second.number_of_expands;
+ }
+ if (error) {
+ htmlOs << "<td colspan=\"3\"><em>error: missing stats</em></td></tr>\n";
+ continue;
+ }
+ double const final_speed(cumul_expands / cumul_time);
+ double const delta_speed(final_speed - init_speed);
+ htmlOs << "<td>" << init_speed << "</td>"
+ << "<td>" << final_speed << "</td>"
+ << "<td>" << delta_speed << "</td>"
+ << "<td>" << 1.0e2 * delta_speed / init_speed
+ << "</td></tr>\n";
+ }
+
+ //////////////////////////////////////////////////
+ // consumed planning time
+
+ htmlOs << "<tr><th colspan=\"5\">consumed planning time (wall clock)</th></tr>\n"
+ << "<tr><td>task</td><td>init</td><td>final</td><td>delta</td><td>% delta</td></tr>\n";
+ for (planList_t::const_iterator ip(planList.begin()); ip != planList.end(); ++ip) {
+ htmlOs << "<tr><td>" << ip->first << "</td>";
+ planBundle_t const & bundle(ip->second);
+ if (bundle.empty()) {
+ htmlOs << "<td colspan=\"4\"><em>no solution</em></td></tr>\n";
+ continue;
+ }
+ planBundle_t::const_iterator ib(bundle.begin());
+ waypoint_plan_t const * initialPlan(ib->get());
+ successStats_t::const_iterator initialStats(successStats.find(initialPlan));
+ if (successStats.end() == initialStats) {
+ htmlOs << "<td colspan=\"4\"><em>error: no stats for initial solution</em></td></tr>\n";
+ continue;
+ }
double cumul(initialStats->second.actual_time_wall_sec);
if (1 == bundle.size()) {
htmlOs << "<td colspan=\"2\">" << timeStr(cumul) << "</td>"
@@ -792,6 +893,9 @@
<< "</td></tr>\n";
}
+ //////////////////////////////////////////////////
+ // epsilon
+
htmlOs << "<tr><th colspan=\"5\">epsilon</th></tr>\n"
<< "<tr><td>task</td><td>init</td><td>final</td><td>delta</td><td>% delta</td></tr>\n";
for (planList_t::const_iterator ip(planList.begin()); ip != planList.end(); ++ip) {
@@ -826,6 +930,9 @@
<< "<td>" << 1.0e2 * delta / initialStats->second.solution_epsilon << "</td></tr>\n";
}
+ //////////////////////////////////////////////////
+ // plan length
+
htmlOs << "<tr><th colspan=\"5\">plan length [m]</th></tr>\n"
<< "<tr><td>task</td><td>init</td><td>final</td><td>delta</td><td>% delta</td></tr>\n";
for (planList_t::const_iterator ip(planList.begin()); ip != planList.end(); ++ip) {
@@ -860,6 +967,9 @@
<< "<td>" << 1.0e2 * delta / initialStats->second.plan_length_m
<< "</td></tr>\n";
}
+
+ //////////////////////////////////////////////////
+ // plan tangent change
htmlOs << "<tr><th colspan=\"5\">plan tangent change [deg]</th></tr>\n"
<< "<tr><td>task</td><td>init</td><td>final</td><td>delta</td><td>% delta</td></tr>\n";
Modified: pkg/trunk/motion_planning/mp_benchmarks/src/mpbench.cpp
===================================================================
--- pkg/trunk/motion_planning/mp_benchmarks/src/mpbench.cpp 2008-12-07 14:25:44 UTC (rev 7751)
+++ pkg/trunk/motion_planning/mp_benchmarks/src/mpbench.cpp 2008-12-07 14:57:36 UTC (rev 7752)
@@ -606,6 +606,7 @@
&statsEntry.actual_time_wall_sec,
&statsEntry.actual_time_user_sec,
&statsEntry.actual_time_system_sec,
+ &statsEntry.number_of_expands,
&statsEntry.solution_cost,
&statsEntry.solution_epsilon,
&solutionStateIDs);
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-12-07 14:25:44 UTC (rev 7751)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-12-07 14:57:36 UTC (rev 7752)
@@ -163,6 +163,7 @@
double * actual_time_wall_sec,
double * actual_time_user_sec,
double * actual_time_system_sec,
+ ssize_t * number_of_expands,
int * solution_cost,
double * solution_epsilon,
vector<int>* solution_stateIDs_V) throw(no_planner_selected)
@@ -184,12 +185,13 @@
gettimeofday(&t_started, 0);
int const status(planner_->replan(allocated_time_sec, solution_stateIDs_V, solution_cost));
-
+
struct rusage ru_finished;
struct timeval t_finished;
getrusage(RUSAGE_SELF, &ru_finished);
gettimeofday(&t_finished, 0);
+ *number_of_expands = planner_->get_n_expands();
*solution_epsilon = planner_->get_solution_eps();
*actual_time_wall_sec =
@@ -299,6 +301,7 @@
<< prefix << "time actual (wall) [ms]: " << 1.0e3 * actual_time_wall_sec << "\n"
<< prefix << "time actual (user) [ms]: " << 1.0e3 * actual_time_user_sec << "\n"
<< prefix << "time actual (system) [ms]: " << 1.0e3 * actual_time_system_sec << "\n"
+ << prefix << "number of expands: " << number_of_expands << "\n"
<< prefix << "solution cost: " << solution_cost << "\n"
<< prefix << "solution epsilon: " << solution_epsilon << "\n"
<< prefix << "status (1 == SUCCESS): " << status << "\n"
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-12-07 14:25:44 UTC (rev 7751)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-12-07 14:57:36 UTC (rev 7752)
@@ -113,6 +113,7 @@
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) */
+ ssize_t number_of_expands; /**< number of state expansions, or -1 if not available */
int solution_cost; /**< cost of the solution, as given by replan() */
double solution_epsilon; /**< the "epsilon" value used to compute the solution */
double plan_length_m; /**< cumulated Euclidean distance between planned waypoints */
@@ -194,6 +195,8 @@
double * actual_time_user_sec,
/** out: how much time was actually used by the planner (system time) */
double * actual_time_system_sec,
+ /** out: number of state expansions (or -1 if not available) */
+ ssize_t * number_of_expands,
/** out: the cost of the planned path */
int * solution_cost,
/** out: the epsilon-value of the planned path */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
