Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13000] pkg/trunk
|
From: <rph...@us...> - 2009-03-26 02:26:55
|
Revision: 13000
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13000&view=rev
Author: rphilippsen
Date: 2009-03-26 02:26:50 +0000 (Thu, 26 Mar 2009)
Log Message:
-----------
Support more cost thresholds in XYTHETALAT planning.
* new: setting the "cost_inscribed" and "cost_possibly_circumscribed"
parameters if SBPL's EnvironmentNAVXYTHETALAT
* new: libsunflower now allows us to compute the cost at the circumscribed
radius, updated mpglue wrappers accordingly
* fix: plotting of the non-lethal but inscribed region was broken
* fix: mpbench wrongly initialized cost "fade-out" to less than
inflation radius
* fix: dump of command-line args was missing a space
Modified Paths:
--------------
pkg/trunk/3rdparty/libsunflower/Makefile
pkg/trunk/motion_planning/mpbench/mkhtml.py
pkg/trunk/motion_planning/mpbench/src/gfx.cpp
pkg/trunk/motion_planning/mpbench/src/world.cpp
pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h
pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h
pkg/trunk/motion_planning/mpglue/src/costmap.cpp
pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
Modified: pkg/trunk/3rdparty/libsunflower/Makefile
===================================================================
--- pkg/trunk/3rdparty/libsunflower/Makefile 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/3rdparty/libsunflower/Makefile 2009-03-26 02:26:50 UTC (rev 13000)
@@ -19,7 +19,7 @@
all: installed
TARBALL_VERSION= r910
-SVN_REVISION= -r 967
+SVN_REVISION= -r 971
TARBALL= build/libsunflower-$(TARBALL_VERSION).tar.gz
TARBALL_URL= http://pr.willowgarage.com/downloads/libsunflower-$(TARBALL_VERSION).tar.gz
Modified: pkg/trunk/motion_planning/mpbench/mkhtml.py
===================================================================
--- pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-03-26 02:26:50 UTC (rev 13000)
@@ -82,7 +82,7 @@
print >>out, ' <td>mpbenchmark failed with code %d, see <a href="%s">console output</a></td>' % (retcode, conslogname)
else:
print >>out, ' <td><table border="0" cellpadding="1">'
- print >>out, ' <tr><td colspan="3">./mpbenchmark' + args + '</td></tr>'
+ print >>out, ' <tr><td colspan="3">./mpbenchmark ' + args + '</td></tr>'
print >>out, ' <tr>'
print >>out, ' <td><a href="' + basename + '.txt">log</a></td>'
print >>out, ' <td><a href="' + conslogname + '">console</a></td>'
Modified: pkg/trunk/motion_planning/mpbench/src/gfx.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -198,7 +198,7 @@
virtual double GetTheta() const { return 0; }
virtual double GetDelta() const { return gframe.Delta(); }
virtual sfl::GridFrame const * GetGridFrame() { return &gframe; }
- virtual int GetObstacle() const { return costmap->getLethalCost(); }
+ virtual int GetObstacle() const { return costmap->getInscribedCost(); }
virtual int GetFreespace() const { return 0; }
virtual ssize_t GetXBegin() const { return costmap->getXBegin(); }
virtual ssize_t GetXEnd() const { return costmap->getXEnd(); }
Modified: pkg/trunk/motion_planning/mpbench/src/world.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/world.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpbench/src/world.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -240,7 +240,7 @@
<< " (currently hardcoded to sfl::Mapper2d)\n";
boost::shared_ptr<sfl::Mapper2d::travmap_grow_strategy>
growstrategy(new sfl::Mapper2d::always_grow());
- double const buffer_zone(opt_.costmap_circumscribed_radius - opt_.costmap_inscribed_radius);
+ double const buffer_zone(opt_.costmap_inflation_radius - opt_.costmap_inscribed_radius);
double const padding_factor(0);
shared_ptr<sfl::Mapper2d> m2d(new sfl::Mapper2d(gridframe_,
0, 0, // grid_xbegin, grid_xend
@@ -252,12 +252,13 @@
opt_.costmap_obstacle_cost,
// costmap_2d seems to use
// a quadratic decay in r7215
- sfl::exponential_travmap_cost_decay(2),
+ shared_ptr<sfl::exponential_travmap_cost_decay>(new sfl::exponential_travmap_cost_decay(2)),
"m2d",
sfl::RWlock::Create("m2d"),
growstrategy));
- cm = mpglue::createCostmapper(m2d);
+ cm = mpglue::createCostmapper(m2d, m2d->ComputeCost(opt_.costmap_circumscribed_radius));
+
return cm;
}
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h 2009-03-26 02:26:50 UTC (rev 13000)
@@ -138,10 +138,15 @@
CostmapAccessor * createCostmapAccessor(costmap_2d::ObstacleMapAccessor const * cm);
- CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt);
- CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt);
+
+ CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt,
+ int possibly_circumscribed_cost);
+
+ CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt,
+ int possibly_circumscribed_cost);
IndexTransform * createIndexTransform(costmap_2d::ObstacleMapAccessor const * cm);
+
IndexTransform * createIndexTransform(sfl::GridFrame const * gf);
}
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h 2009-03-26 02:26:50 UTC (rev 13000)
@@ -124,7 +124,8 @@
};
- boost::shared_ptr<Costmapper> createCostmapper(boost::shared_ptr<sfl::Mapper2d> m2d);
+ boost::shared_ptr<Costmapper> createCostmapper(boost::shared_ptr<sfl::Mapper2d> m2d,
+ int possibly_circumscribed_cost);
}
Modified: pkg/trunk/motion_planning/mpglue/src/costmap.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -130,22 +130,20 @@
sfl::RDTravmap const * rdt_;
int const wobstCost_;
int const cobstCost_;
- int const cobstCostMinusOne_;
+ int const possibly_circumscribed_cost_;
- sflRDTAccessor(sfl::RDTravmap const * rdt)
+ sflRDTAccessor(sfl::RDTravmap const * rdt,
+ int possibly_circumscribed_cost)
: rdt_(rdt),
// Beware: W-space means "non-inflated", which is tagged by
// making the cost higher than necessary in sfl::Mapper2d.
wobstCost_(rdt->GetObstacle() + 1),
cobstCost_(rdt->GetObstacle()),
- // This is a bit of a hack to work around the fact that
- // sfl::Mapper2d does not distinguish between inscribed and
- // circumscribed robot radii.
- cobstCostMinusOne_(rdt->GetObstacle() - 1) {}
+ possibly_circumscribed_cost_(possibly_circumscribed_cost) {}
virtual mpglue::cost_t getLethalCost() const { return wobstCost_; }
virtual mpglue::cost_t getInscribedCost() const { return cobstCost_; }
- virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return cobstCostMinusOne_; }
+ virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return possibly_circumscribed_cost_; }
virtual ssize_t getXBegin() const { return rdt_->GetXBegin(); }
virtual ssize_t getXEnd() const { return rdt_->GetXEnd(); }
@@ -171,20 +169,9 @@
virtual bool isPossiblyCircumcribed(ssize_t index_x, ssize_t index_y,
bool out_of_bounds_reply) const {
- // Problem: sfl::Mapper2d() does not distinguish between
- // inscribed and circumscribed radii, it simply assumes the
- // whole robot fits in the radius, period. On the one hand
- // that's like saying the inscribed radius is the same as the
- // circumscribed radius, but given that we tend to initialize
- // sfl::Mapper2d with the inscribed radius as "the" radius, that
- // does not hold.
- //
- // XXXX broken hack...another point to discuss. For the moment
- // treat it as if obstacle-1 means "between inscribed and
- // circumscribed" in the sfl::Mapper2d case.
int value;
if (rdt_->GetValue(index_x, index_y, value))
- return value >= cobstCostMinusOne_;
+ return value >= possibly_circumscribed_cost_;
return out_of_bounds_reply;
}
@@ -205,15 +192,16 @@
sfl::TraversabilityMap const * travmap_;
int const wobstCost_;
int const cobstCost_;
- int const cobstCostMinusOne_;
+ int const possibly_circumscribed_cost_;
- sflTravmapAccessor(sfl::TraversabilityMap const * travmap)
+ sflTravmapAccessor(sfl::TraversabilityMap const * travmap,
+ int possibly_circumscribed_cost)
: travmap_(travmap), wobstCost_(travmap->obstacle + 1), cobstCost_(travmap->obstacle),
- cobstCostMinusOne_(travmap->obstacle - 1) {}
+ possibly_circumscribed_cost_(possibly_circumscribed_cost) {}
virtual mpglue::cost_t getLethalCost() const { return wobstCost_; }
virtual mpglue::cost_t getInscribedCost() const { return cobstCost_; }
- virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return cobstCostMinusOne_; }
+ virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return possibly_circumscribed_cost_; }
virtual ssize_t getXBegin() const { return travmap_->grid.xbegin(); }
virtual ssize_t getXEnd() const { return travmap_->grid.xend(); }
@@ -239,11 +227,9 @@
virtual bool isPossiblyCircumcribed(ssize_t index_x, ssize_t index_y,
bool out_of_bounds_reply) const {
- // Problem (again): sfl::Mapper2d() does not distinguish between
- // inscribed and circumscribed radii...
int value;
if (travmap_->GetValue(index_x, index_y, value))
- return value >= cobstCostMinusOne_;
+ return value >= possibly_circumscribed_cost_;
return out_of_bounds_reply;
}
@@ -333,11 +319,13 @@
CostmapAccessor * createCostmapAccessor(costmap_2d::ObstacleMapAccessor const * cm)
{ return new cm2dCostmapAccessor(cm); }
- CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt)
- { return new sflRDTAccessor(rdt); }
+ CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt,
+ int possibly_circumscribed_cost)
+ { return new sflRDTAccessor(rdt, possibly_circumscribed_cost); }
- CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt)
- { return new sflTravmapAccessor(rdt); }
+ CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt,
+ int possibly_circumscribed_cost)
+ { return new sflTravmapAccessor(rdt, possibly_circumscribed_cost); }
IndexTransform * createIndexTransform(costmap_2d::ObstacleMapAccessor const * cm)
{ return new cm2dTransform(cm); }
Modified: pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -47,14 +47,17 @@
mutable shared_ptr<sfl::RDTravmap> rdt_;
mutable shared_ptr<mpglue::CostmapAccessor> cma_;
mutable shared_ptr<mpglue::IndexTransform> idxt_;
+ int const possibly_circumscribed_cost_;
- sflCostmapper(shared_ptr<sfl::Mapper2d> m2d): m2d_(m2d) {}
+ sflCostmapper(shared_ptr<sfl::Mapper2d> m2d,
+ int possibly_circumscribed_cost)
+ : m2d_(m2d), possibly_circumscribed_cost_(possibly_circumscribed_cost) {}
virtual boost::shared_ptr<mpglue::CostmapAccessor const> getAccessor() const
{
if ( ! cma_) {
rdt_ = m2d_->CreateRDTravmap();
- cma_.reset(mpglue::createCostmapAccessor(rdt_.get()));
+ cma_.reset(mpglue::createCostmapAccessor(rdt_.get(), possibly_circumscribed_cost_));
}
return cma_;
}
@@ -105,9 +108,10 @@
namespace mpglue {
- shared_ptr<Costmapper> createCostmapper(shared_ptr<sfl::Mapper2d> m2d)
+ shared_ptr<Costmapper> createCostmapper(shared_ptr<sfl::Mapper2d> m2d,
+ int possibly_circumscribed_cost)
{
- shared_ptr<Costmapper> foo(new sflCostmapper(m2d));
+ shared_ptr<Costmapper> foo(new sflCostmapper(m2d, possibly_circumscribed_cost));
return foo;
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -35,6 +35,7 @@
#include <mpglue/sbpl_environment.h>
#include <costmap_2d/costmap_2d.h>
#include <sbpl/headers.h>
+#include <sfl/util/strutil.hpp>
using namespace mpglue;
using namespace std;
@@ -395,6 +396,18 @@
std::ostream * dbgos) throw(std::exception)
{
EnvironmentNAVXYTHETALAT * env(new EnvironmentNAVXYTHETALAT());
+ if ( ! env->SetEnvParameter("cost_inscribed", cm->getInscribedCost())) {
+ delete env;
+ throw runtime_error("mpglue::SBPLEnvironment::createXYThetaLattice():"
+ " EnvironmentNAVXYTHETALAT::SetEnvParameter(\"cost_inscribed\", "
+ + sfl::to_string(cm->getInscribedCost()) + ") failed");
+ }
+ if ( ! env->SetEnvParameter("cost_possibly_circumscribed", cm->getPossiblyCircumcribedCost())) {
+ delete env;
+ throw runtime_error("mpglue::SBPLEnvironment::createXYThetaLattice():"
+ " EnvironmentNAVXYTHETALAT::SetEnvParameter(\"cost_possibly_circumscribed\", "
+ + sfl::to_string(cm->getPossiblyCircumcribedCost()) + ") failed");
+ }
int const obst_cost_thresh(cm->getLethalCost());
vector<sbpl_2Dpt_t> perimeterptsV;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
