Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13624] pkg/trunk
|
From: <rph...@us...> - 2009-04-08 22:00:37
|
Revision: 13624
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13624&view=rev
Author: rphilippsen
Date: 2009-04-08 22:00:24 +0000 (Wed, 08 Apr 2009)
Log Message:
-----------
= in mpglue =
* Got rid of deprecated Pose2DFloat32, using robot_msgs/Pose now.
* Defined waypoint_s which includes a radial and an angular tolerance
for each waypoint (for planners who support that).
* SBPLEnvironment subclasses now inform the higher levels if they
support theta-planning.
* Restructed the plan conversion routines accordingly, adapted
existing planner wrappers.
= in mpbench =
* Adapted to the mpglue changes.
* Plot the robot orientation for path plans that use angle
tolerance < M_PI
* mkhtml.py now runs a comparison of non-sliding versus sliding
motion primitives for PR2
= in highlevel_controllers =
* Adapted to the mpglue changes.
* It still uses deprecated Pose2DFloat32 messages...
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml
pkg/trunk/motion_planning/mpbench/mkhtml.py
pkg/trunk/motion_planning/mpbench/src/gfx.cpp
pkg/trunk/motion_planning/mpbench/src/gfx.h
pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp
pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h
pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h
pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp
pkg/trunk/motion_planning/mpglue/src/plan.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -615,7 +615,14 @@
sentry<MoveBase> guard(this);
if (!isValid() || plan_.size() > newPlan.size()){
plan_.clear();
- std::copy(newPlan.begin(), newPlan.end(), std::back_inserter(plan_));
+ deprecated_msgs::Pose2DFloat32 pose;
+ for (mpglue::waypoint_plan_t::const_iterator ip(newPlan.begin());
+ ip != newPlan.end(); ++ip) {
+ pose.x = ip->x;
+ pose.y = ip->y;
+ pose.th = ip->theta;
+ plan_.push_back(pose);
+ }
publishPath(true, plan_);
}
}
Modified: pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml
===================================================================
--- pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/data/test-sideways-sliding.xml 2009-04-08 22:00:24 UTC (rev 13624)
@@ -40,6 +40,7 @@
<task>
<description>top / widest</description>
<goal><pose> 6.5 5.62 -1.55 </pose></goal>
+ <door> iejfrw wewoief </door>
<start><pose> 0.5 2.98 1.56 </pose></start>
</task>
</setup>
Modified: pkg/trunk/motion_planning/mpbench/mkhtml.py
===================================================================
--- pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-04-08 22:00:24 UTC (rev 13624)
@@ -11,17 +11,19 @@
worldspec = []
worldexpl = []
-worldspec.append("pgm:204:../data/willow-clip1-r25.pgm:../data/willow-clip1-r25-a.xml")
-worldexpl.append("hallway to office")
-worldspec.append("pgm:204:../data/willow-clip1-r25.pgm:../data/willow-clip1-r25-b.xml")
-worldexpl.append("office to hallway")
+worldspec.append("xml:../data/test-sideways-sliding.xml")
+worldexpl.append("test sideways sliding")
plannerspec = []
plannerexpl = []
plannerspec.append("ad:xythetalat:../data/pr2.mprim")
-plannerexpl.append("AD planner on lattice")
+plannerexpl.append("AD planner without sideways motion")
+plannerspec.append("ad:xythetalat:../data/pr2sides.mprim")
+plannerexpl.append("AD planner with sideways motion")
plannerspec.append("ara:xythetalat:../data/pr2.mprim")
-plannerexpl.append("ARA planner on lattice")
+plannerexpl.append("ARA planner without sideways motion")
+plannerspec.append("ara:xythetalat:../data/pr2sides.mprim")
+plannerexpl.append("ARA planner with sideways motion")
robotspec = []
robotexpl = []
@@ -33,8 +35,6 @@
costmapexpl = []
costmapspec.append("sfl:25:325:460:550")
costmapexpl.append("fully inflated")
-costmapspec.append("sfl:25:325:460:460")
-costmapexpl.append("inflated to circumscribed")
costmapspec.append("sfl:25:325:325:325")
costmapexpl.append("inflated to inscribed")
costmapspec.append("sfl:25:0:0:0")
Modified: pkg/trunk/motion_planning/mpbench/src/gfx.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -97,7 +97,6 @@
bool _websiteMode,
std::string const & _baseFilename,
ResultCollection const & _result,
- bool _ignorePlanTheta,
std::ostream & _logOs)
: setup(_setup),
world(*_setup.getWorld()),
@@ -109,7 +108,6 @@
baseFilename(_baseFilename),
footprint(_setup.getFootprint()),
result(_result),
- ignorePlanTheta(_ignorePlanTheta),
logOs(_logOs)
{
}
@@ -316,7 +314,8 @@
new npm::TraversabilityDrawing("costmap_dark", new CostmapWrapProxy(0),
npm::TraversabilityDrawing::MINIMAL_DARK);
- new PlanDrawing("planner", -1, -1, false);
+ static bool const detailed(true);
+ new PlanDrawing("planner", -1, -1, detailed);
view = new npm::View("planner", npm::Instance<npm::UniqueManager<npm::View> >());
// beware of weird npm::View::Configure() param order: x, y, width, height
view->Configure(v_width, 0, v_width, 1);
@@ -437,20 +436,27 @@
glColor3d(0.5, 1, 0);
glLineWidth(1);
if (plan) {
- for (wpi_t iw(plan->begin()); iw != plan->end(); iw += skip) {
+ for (wpi_t iw(plan->begin()); iw != plan->end(); /* this can segfault: iw += skip */) {
glPushMatrix();
glTranslated(iw->x, iw->y, 0);
- if (configptr->ignorePlanTheta) {
+ if (iw->ignoreTheta()) {
gluDisk(wrap_glu_quadric_instance(),
configptr->inscribedRadius,
configptr->inscribedRadius,
36, 1);
}
else {
- glRotated(180 * iw->th / M_PI, 0, 0, 1);
+ glRotated(180 * iw->theta / M_PI, 0, 0, 1);
drawFootprint();
}
glPopMatrix();
+ // quick hack to skip ahead while not missing the end of an
+ // STL container... should be done more properly.
+ for (size_t ii(0); ii < skip; ++ii) {
+ ++iw;
+ if (iw == plan->end())
+ break;
+ }
} // endfor(plan)
} // endif(plan)
} // endif(detailed)
Modified: pkg/trunk/motion_planning/mpbench/src/gfx.h
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/gfx.h 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/src/gfx.h 2009-04-08 22:00:24 UTC (rev 13624)
@@ -51,7 +51,6 @@
bool websiteMode,
std::string const & baseFilename,
ResultCollection const & result,
- bool ignorePlanTheta,
std::ostream & logOs);
Setup const & setup;
@@ -64,7 +63,6 @@
std::string const baseFilename;
mpglue::footprint_t const & footprint;
ResultCollection const & result;
- bool const ignorePlanTheta;
mutable std::ostream & logOs;
};
Modified: pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -126,7 +126,6 @@
websiteMode,
baseFilename(),
*result_collection,
- true, // XXXX to do: depends on 3DKIN
*logos),
"mpbench",
3, // hack: layoutID
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h 2009-04-08 22:00:24 UTC (rev 13624)
@@ -35,18 +35,42 @@
#ifndef MPGLUE_PLAN_HPP
#define MPGLUE_PLAN_HPP
+#include <robot_msgs/Pose.h>
#include <deprecated_msgs/Pose2DFloat32.h>
#include <vector>
namespace mpglue {
typedef std::vector<int> raw_sbpl_plan_t;
- typedef std::vector<deprecated_msgs::Pose2DFloat32> waypoint_plan_t;
+ namespace deprecated {
+ typedef std::vector<deprecated_msgs::Pose2DFloat32> waypoint_plan_t;
+ }
+
class SBPLEnvironment;
class IndexTransform;
+ struct waypoint_s {
+ static double const default_dr;
+ static double const default_dtheta;
+
+ waypoint_s(double x, double y, double theta, double dr, double dtheta);
+ waypoint_s(double x, double y, double theta);
+ waypoint_s(waypoint_s const & orig);
+ waypoint_s(robot_msgs::Pose const & pose, double dr, double dtheta);
+ explicit waypoint_s(robot_msgs::Pose const & pose);
+ waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose, double dr, double dtheta);
+ explicit waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose);
+
+ bool ignoreTheta() const;
+
+ double x, y, theta, dr, dtheta;
+ };
+
+ typedef std::vector<waypoint_s> waypoint_plan_t;
+
+
/**
Helper class for constructing waypoint_plan_t while updating plan
statistics.
@@ -55,9 +79,101 @@
{
public:
explicit PlanConverter(waypoint_plan_t * plan);
- void addWaypoint(deprecated_msgs::Pose2DFloat32 const & waypoint);
- void addWaypoint(double px, double py, double pth);
+ PlanConverter(waypoint_plan_t * plan, double default_dr, double default_dtheta);
+ void addWaypoint(double px, double py, double pth)
+ { addWaypoint(waypoint_s(px, py, pth, default_dr, default_dtheta)); }
+
+ void addWaypoint(double px, double py, double pth, double dr, double dtheta)
+ { addWaypoint(waypoint_s(px, py, pth, dr, dtheta)); }
+
+ void addWaypoint(waypoint_s const & wp);
+
+ /**
+ Convert a plan from a raw state ID sequence (as computed by
+ SBPLPlanner subclasses) to waypoints that are
+ understandable. Optionally provides some statistics on the
+ plan.
+ */
+ static void convertSBPL(/** in: how to translate state IDs to (x, y) or (x, y, theta) */
+ SBPLEnvironment const & environment,
+ /** in: the raw plan */
+ raw_sbpl_plan_t const & raw,
+ /** in: the radial tolerance of each waypoint */
+ double dr,
+ /** in: the angular tolerance of each
+ waypoint (use M_PI to ignore theta) */
+ double dtheta,
+ /** out: the converted plan (it is just appended
+ to, not cleared for you) */
+ waypoint_plan_t * plan,
+ /** optional out: the cumulated path length */
+ double * optPlanLengthM,
+ /** optional out: the cumulated change in the path
+ tangential direction (the angle between the
+ x-axis and the delta between two successive
+ waypoints) */
+ double * optTangentChangeRad,
+ /** optional out: the cumulated change in the
+ direction of the waypoints (the delta of
+ theta values).
+ \note This only makes sense for plans that are
+ aware of the robot's heading though, i.e. dtheta < M_PI. */
+ double * optDirectionChangeRad);
+
+ /**
+ Convert a plan from an float index-pair sequence (as computed by
+ NavFn) to waypoints that are understandable. Optionally provides
+ some statistics on the plan. Have a look at
+ convertXYInterpolate() though, it takes advantage of the
+ sub-pixel resolution available in NavFn.
+ */
+ static void convertXY(/** in: how to translate map (x, y) to global (x, y) */
+ IndexTransform const & itransform,
+ /** in: array of X-coordinates (continuous grid index). */
+ float const * path_x,
+ /** in: array of Y-coordinates (continuous grid index). */
+ float const * path_y,
+ /** in: the length of path_x[] and path_y[]. */
+ int path_len,
+ /** in: the radial tolerance of each waypoint */
+ double dr,
+ /** out: the converted plan (it is just appended
+ to, not cleared for you) */
+ waypoint_plan_t * plan,
+ /** optional out: the cumulated path length */
+ double * optPlanLengthM,
+ /** optional out: the cumulated change in the path
+ tangential direction (the angle between the
+ x-axis and the delta between two successive
+ waypoints) */
+ double * optTangentChangeRad);
+
+ /**
+ Uses interpolateIndexToGlobal() for sub-pixel resolution.
+ */
+ static void convertXYInterpolate(/** in: how to translate map (x, y) to global (x, y) */
+ IndexTransform const & itransform,
+ /** in: array of X-coordinates (continuous grid index). */
+ float const * path_x,
+ /** in: array of Y-coordinates (continuous grid index). */
+ float const * path_y,
+ /** in: the length of path_x[] and path_y[]. */
+ int path_len,
+ /** in: the radial tolerance of each waypoint */
+ double dr,
+ /** out: the converted plan (it is just appended
+ to, not cleared for you) */
+ waypoint_plan_t * plan,
+ /** optional out: the cumulated path length */
+ double * optPlanLengthM,
+ /** optional out: the cumulated change in the path
+ tangential direction (the angle between the
+ x-axis and the delta between two successive
+ waypoints) */
+ double * optTangentChangeRad);
+
+ double default_dr, default_dtheta;
double plan_length, tangent_change, direction_change;
private:
@@ -66,93 +182,6 @@
double prevx_, prevy_, prevtan_, prevdir_;
};
-
- /**
- Convert a plan from a raw state ID sequence (as computed by
- SBPLPlanner subclasses) to waypoints that are
- understandable. Optionally provides some statistics on the plan.
- */
- void convertPlan(/** in: how to translate state IDs to deprecated_msgs::Pose2DFloat32 */
- SBPLEnvironment const & environment,
- /** in: the raw plan */
- raw_sbpl_plan_t const & raw,
- /** out: the converted plan (it is just appended
- to, not cleared for you) */
- waypoint_plan_t * plan,
- /** optional out: the cumulated path length */
- double * optPlanLengthM,
- /** optional out: the cumulated change in the path
- tangential direction (the angle between the
- x-axis and the delta between two successive
- waypoints) */
- double * optTangentChangeRad,
- /** optional out: the cumulated change in the
- direction of the waypoints (the delta of
- deprecated_msgs::Pose2DFloat32::th values).
- \note This only makes sense for plans that are
- aware of the robot's heading though. */
- double * optDirectionChangeRad);
-
- /**
- Convert a plan from an float index-pair sequence (as computed by
- NavFn) to waypoints that are understandable. Optionally provides
- some statistics on the plan. Have a look at
- convertPlanInterpolate() though, it takes advantage of the
- sub-pixel resolution available in NavFn.
- */
- void convertPlan(/** in: how to translate state IDs to deprecated_msgs::Pose2DFloat32 */
- IndexTransform const & itransform,
- /** in: array of X-coordinates (continuous grid index). */
- float const * path_x,
- /** in: array of Y-coordinates (continuous grid index). */
- float const * path_y,
- /** in: the length of path_x[] and path_y[]. */
- int path_len,
- /** out: the converted plan (it is just appended
- to, not cleared for you) */
- waypoint_plan_t * plan,
- /** optional out: the cumulated path length */
- double * optPlanLengthM,
- /** optional out: the cumulated change in the path
- tangential direction (the angle between the
- x-axis and the delta between two successive
- waypoints) */
- double * optTangentChangeRad,
- /** optional out: the cumulated change in the
- direction of the waypoints (the delta of
- deprecated_msgs::Pose2DFloat32::th values).
- \note This only makes sense for plans that are
- aware of the robot's heading though. */
- double * optDirectionChangeRad);
-
- /**
- Uses interpolateIndexToGlobal() for sub-pixel resolution.
- */
- void convertPlanInterpolate(/** in: how to translate state IDs to deprecated_msgs::Pose2DFloat32 */
- IndexTransform const & itransform,
- /** in: array of X-coordinates (continuous grid index). */
- float const * path_x,
- /** in: array of Y-coordinates (continuous grid index). */
- float const * path_y,
- /** in: the length of path_x[] and path_y[]. */
- int path_len,
- /** out: the converted plan (it is just appended
- to, not cleared for you) */
- waypoint_plan_t * plan,
- /** optional out: the cumulated path length */
- double * optPlanLengthM,
- /** optional out: the cumulated change in the path
- tangential direction (the angle between the
- x-axis and the delta between two successive
- waypoints) */
- double * optTangentChangeRad,
- /** optional out: the cumulated change in the
- direction of the waypoints (the delta of
- deprecated_msgs::Pose2DFloat32::th values).
- \note This only makes sense for plans that are
- aware of the robot's heading though. */
- double * optDirectionChangeRad);
-
}
#endif // MPGLUE_PLAN_HPP
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h 2009-04-08 22:00:24 UTC (rev 13624)
@@ -169,6 +169,16 @@
boost::shared_ptr<CostmapAccessor const> getCostmap() const { return cm_; }
boost::shared_ptr<IndexTransform const> getIndexTransform() const { return it_; }
+ /**
+ \return the precision of the angular information contained in
+ the environment representation. For 2D planners, this is M_PI
+ (they do not take the theta angle into account). For planners
+ that do take theta into account, the returned value should be
+ half the angular resolution, such that +/- the tolerance covers
+ the full range.
+ */
+ virtual double GetAngularTolerance() const = 0;
+
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost) = 0;
Modified: pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/src/navfn_planner.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -103,13 +103,30 @@
if (planner_->calcNavFnAstar()) {
plan.reset(new waypoint_plan_t());
if (interpolate_plan_)
- convertPlanInterpolate(*itransform_, planner_->getPathX(), planner_->getPathY(),
- planner_->getPathLen(), plan.get(), &stats_.plan_length,
- &stats_.plan_angle_change, 0);
+ PlanConverter::convertXYInterpolate(*itransform_,
+ planner_->getPathX(),
+ planner_->getPathY(),
+ planner_->getPathLen(),
+ // using the cell size as
+ // waypoint tolerance
+ // seems like a good
+ // heuristic
+ itransform_->getResolution(),
+ plan.get(),
+ &stats_.plan_length,
+ &stats_.plan_angle_change);
else
- convertPlan(*itransform_, planner_->getPathX(), planner_->getPathY(),
- planner_->getPathLen(), plan.get(), &stats_.plan_length,
- &stats_.plan_angle_change, 0);
+ PlanConverter::convertXY(*itransform_,
+ planner_->getPathX(),
+ planner_->getPathY(),
+ planner_->getPathLen(),
+ // using the cell size as waypoint
+ // tolerance seems like a good
+ // heuristic
+ itransform_->getResolution(),
+ plan.get(),
+ &stats_.plan_length,
+ &stats_.plan_angle_change);
}
return plan;
}
Modified: pkg/trunk/motion_planning/mpglue/src/plan.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/plan.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/src/plan.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -38,10 +38,45 @@
namespace mpglue {
+ double const waypoint_s::default_dr(0.1);
+ double const waypoint_s::default_dtheta(M_PI);
+ waypoint_s::waypoint_s(double _x, double _y, double _theta, double _dr, double _dtheta)
+ : x(_x), y(_y), theta(_theta), dr(_dr), dtheta(_dtheta) {}
+
+ waypoint_s::waypoint_s(double _x, double _y, double _theta)
+ : x(_x), y(_y), theta(_theta), dr(default_dr), dtheta(default_dtheta) {}
+
+ waypoint_s::waypoint_s(waypoint_s const & orig)
+ : x(orig.x), y(orig.y), theta(orig.theta), dr(orig.dr), dtheta(orig.dtheta) {}
+
+ waypoint_s::waypoint_s(robot_msgs::Pose const & pose, double _dr, double _dtheta)
+ : x(pose.position.x), y(pose.position.y),
+ theta(atan2(pose.orientation.z, pose.orientation.w)),
+ dr(_dr), dtheta(_dtheta) {}
+
+ waypoint_s::waypoint_s(robot_msgs::Pose const & pose)
+ : x(pose.position.x), y(pose.position.y),
+ theta(atan2(pose.orientation.z, pose.orientation.w)),
+ dr(default_dr), dtheta(default_dtheta) {}
+
+ waypoint_s::waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose, double _dr, double _dtheta)
+ : x(pose.x), y(pose.y), theta(pose.th), dr(_dr), dtheta(_dtheta) {}
+
+ waypoint_s::waypoint_s(deprecated_msgs::Pose2DFloat32 const & pose)
+ : x(pose.x), y(pose.y), theta(pose.th), dr(default_dr), dtheta(default_dtheta) {}
+
+ bool waypoint_s::ignoreTheta() const
+ {
+ return dtheta >= M_PI;
+ }
+
+
PlanConverter::
PlanConverter(waypoint_plan_t * plan)
- : plan_length(0),
+ : default_dr(waypoint_s::default_dr),
+ default_dtheta(waypoint_s::default_dtheta),
+ plan_length(0),
tangent_change(0),
direction_change(0),
plan_(plan),
@@ -54,48 +89,57 @@
}
+ PlanConverter::
+ PlanConverter(waypoint_plan_t * plan, double _default_dr, double _default_dtheta)
+ : default_dr(_default_dr),
+ default_dtheta(_default_dtheta),
+ plan_length(0),
+ tangent_change(0),
+ direction_change(0),
+ plan_(plan),
+ count_(0),
+ prevx_(0),
+ prevy_(0),
+ prevtan_(0),
+ prevdir_(0)
+ {
+ }
+
+
void PlanConverter::
- addWaypoint(deprecated_msgs::Pose2DFloat32 const & waypoint)
+ addWaypoint(waypoint_s const & wp)
{
if (0 < count_) {
- double const dx(waypoint.x - prevx_);
- double const dy(waypoint.y - prevy_);
+ double const dx(wp.x - prevx_);
+ double const dy(wp.y - prevy_);
plan_length += sqrt(pow(dx, 2) + pow(dy, 2));
- direction_change += fabs(sfl::mod2pi(waypoint.th - prevdir_));
+ direction_change += fabs(sfl::mod2pi(wp.theta - prevdir_));
double const tangent(atan2(dy, dx));
if (1 < count_) // tangent change only available after 2nd point
tangent_change += fabs(sfl::mod2pi(tangent - prevtan_));
prevtan_ = tangent;
}
- plan_->push_back(waypoint);
+ plan_->push_back(wp);
++count_;
- prevx_ = waypoint.x;
- prevy_ = waypoint.y;
- prevdir_ = waypoint.th;
+ prevx_ = wp.x;
+ prevy_ = wp.y;
+ prevdir_ = wp.theta;
}
void PlanConverter::
- addWaypoint(double px, double py, double pth)
+ convertSBPL(SBPLEnvironment const & environment,
+ raw_sbpl_plan_t const & raw,
+ double dr,
+ double dtheta,
+ waypoint_plan_t * plan,
+ double * optPlanLengthM,
+ double * optTangentChangeRad,
+ double * optDirectionChangeRad)
{
- deprecated_msgs::Pose2DFloat32 pp;
- pp.x = px;
- pp.y = py;
- pp.th = pth;
- addWaypoint(pp);
- }
-
-
- void convertPlan(SBPLEnvironment const & environment,
- raw_sbpl_plan_t const & raw,
- waypoint_plan_t * plan,
- double * optPlanLengthM,
- double * optTangentChangeRad,
- double * optDirectionChangeRad)
- {
- PlanConverter pc(plan);
+ PlanConverter pc(plan, dr, dtheta);
for (raw_sbpl_plan_t::const_iterator it(raw.begin()); it != raw.end(); ++it)
- pc.addWaypoint(environment.GetPoseFromState(*it));
+ pc.addWaypoint(waypoint_s(environment.GetPoseFromState(*it), dr, dtheta));
if (optPlanLengthM)
*optPlanLengthM = pc.plan_length;
if (optTangentChangeRad)
@@ -105,16 +149,17 @@
}
- void convertPlan(IndexTransform const & itransform,
- float const * path_x,
- float const * path_y,
- int path_len,
- waypoint_plan_t * plan,
- double * optPlanLengthM,
- double * optTangentChangeRad,
- double * optDirectionChangeRad)
+ void PlanConverter::
+ convertXY(IndexTransform const & itransform,
+ float const * path_x,
+ float const * path_y,
+ int path_len,
+ double dr,
+ waypoint_plan_t * plan,
+ double * optPlanLengthM,
+ double * optTangentChangeRad)
{
- PlanConverter pc(plan);
+ PlanConverter pc(plan, dr, M_PI);
ssize_t prev_ix(0), prev_iy(0);
for (int ii(0); ii < path_len; ++ii, ++path_x, ++path_y) {
ssize_t const ix(static_cast<ssize_t>(rint(*path_x)));
@@ -131,21 +176,20 @@
*optPlanLengthM = pc.plan_length;
if (optTangentChangeRad)
*optTangentChangeRad = pc.tangent_change;
- if (optDirectionChangeRad)
- *optDirectionChangeRad = pc.direction_change;
}
- void convertPlanInterpolate(IndexTransform const & itransform,
- float const * path_x,
- float const * path_y,
- int path_len,
- waypoint_plan_t * plan,
- double * optPlanLengthM,
- double * optTangentChangeRad,
- double * optDirectionChangeRad)
+ void PlanConverter::
+ convertXYInterpolate(IndexTransform const & itransform,
+ float const * path_x,
+ float const * path_y,
+ int path_len,
+ double dr,
+ waypoint_plan_t * plan,
+ double * optPlanLengthM,
+ double * optTangentChangeRad)
{
- PlanConverter pc(plan);
+ PlanConverter pc(plan, dr, M_PI);
double const resolution(itransform.getResolution());
for (int ii(0); ii < path_len; ++ii, ++path_x, ++path_y) {
ssize_t ix(static_cast<ssize_t>(rint(*path_x)));
@@ -160,8 +204,6 @@
*optPlanLengthM = pc.plan_length;
if (optTangentChangeRad)
*optTangentChangeRad = pc.tangent_change;
- if (optDirectionChangeRad)
- *optDirectionChangeRad = pc.direction_change;
}
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -107,6 +107,8 @@
virtual deprecated_msgs::Pose2DFloat32 GetPoseFromState(int stateID) const throw(invalid_state);
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const;
+ virtual double GetAngularTolerance() const;
+
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost);
virtual StateChangeQuery const * createStateChangeQuery(std::vector<nav2dcell_t> const & changedcellsV) const;
@@ -131,10 +133,8 @@
SBPLEnvironmentDSI(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
DSIsubclass * env,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs)
- : SBPLEnvironment(cm, it), env_(env) {}
+ double angular_tolerance)
+ : SBPLEnvironment(cm, it), env_(env), angular_tolerance_(angular_tolerance) {}
virtual ~SBPLEnvironmentDSI()
{ delete env_; }
@@ -196,8 +196,12 @@
return env_->GetStateFromCoord(ix, iy, ith);
}
+ virtual double GetAngularTolerance() const
+ {
+ return angular_tolerance_;
+ }
+
protected:
-
/** \todo The check IsWithinMapCell() should be done inside
EnvironmentNAV3DKIN::UpdateCost() or whatever subclass we are
dealing with here. */
@@ -217,6 +221,8 @@
conceivable change the underlying DSIsubclass, which we
don't care about here. */
mutable DSIsubclass * env_;
+
+ double angular_tolerance_;
};
}
@@ -377,9 +383,8 @@
env->UpdateCost(ix, iy, cost);
}
- return new SBPLEnvironmentDSI<EnvironmentNAV3DKIN>(cm, it, env, footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs);
+ static double const dtheta(M_PI / NAV3DKIN_THETADIRS);
+ return new SBPLEnvironmentDSI<EnvironmentNAV3DKIN>(cm, it, env, dtheta);
}
@@ -465,9 +470,8 @@
env->UpdateCost(ix, iy, cost);
}
- return new SBPLEnvironmentDSI<EnvironmentNAVXYTHETALAT>(cm, it, env, footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs);
+ static double const dtheta(M_PI / NAVXYTHETALAT_THETADIRS);
+ return new SBPLEnvironmentDSI<EnvironmentNAVXYTHETALAT>(cm, it, env, dtheta);
}
@@ -488,13 +492,13 @@
EnvironmentNAVXYTHETADOOR * env(new EnvironmentNAVXYTHETADOOR());
if ( ! env->SetEnvParameter("cost_inscribed", cm->getInscribedCost())) {
delete env;
- throw runtime_error("mpglue::SBPLEnvironment::createXYThetaLattice():"
+ throw runtime_error("mpglue::SBPLEnvironment::createXYThetaDoor():"
" EnvironmentNAVXYTHETADOOR::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():"
+ throw runtime_error("mpglue::SBPLEnvironment::createXYThetaDoor():"
" EnvironmentNAVXYTHETADOOR::SetEnvParameter(\"cost_possibly_circumscribed\", "
+ sfl::to_string(cm->getPossiblyCircumcribedCost()) + ") failed");
}
@@ -514,7 +518,7 @@
static double const goaltol_theta(22.5 * M_PI / 180.0);
if (dbgos) {
- *dbgos << "mpglue::SBPLEnvironment:createXYThetaLattice():\n"
+ *dbgos << "mpglue::SBPLEnvironment:createXYThetaDoor():\n"
<< " motor_primitive_filename = " << motor_primitive_filename << "\n"
<< " perimeterptsV =\n";
for (vector<sbpl_2Dpt_t>::const_iterator ip(perimeterptsV.begin());
@@ -555,9 +559,9 @@
env->UpdateCost(ix, iy, cost);
}
- return new SBPLEnvironmentDSI<EnvironmentNAVXYTHETADOOR>(cm, it, env, footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs);
+ /**< \todo Experimental door planner, basically copy-pasted from NAVXYTHETALAT */
+ static double const dtheta(M_PI / NAVXYTHETALAT_THETADIRS);
+ return new SBPLEnvironmentDSI<EnvironmentNAVXYTHETADOOR>(cm, it, env, dtheta);
}
@@ -700,4 +704,11 @@
return env_->GetStateFromCoord(ix, iy);
}
+
+ double SBPLEnvironment2D::
+ GetAngularTolerance() const
+ {
+ return M_PI;
+ }
+
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp 2009-04-08 21:59:46 UTC (rev 13623)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp 2009-04-08 22:00:24 UTC (rev 13624)
@@ -194,11 +194,18 @@
if (1 == stats_.status) {
plan.reset(new waypoint_plan_t());
if (1 < solution.size())
- convertPlan(*environment_, solution, plan.get(),
- &stats_.plan_length,
- &stats_.plan_angle_change,
- 0 // XXXX to do: if 3DKIN we actually want something here
- );
+ PlanConverter::convertSBPL(*environment_,
+ solution,
+ // using the cell size as waypoint
+ // tolerance seems like a good
+ // heuristic
+ itransform_->getResolution(),
+ environment_->GetAngularTolerance(),
+ plan.get(),
+ &stats_.plan_length,
+ &stats_.plan_angle_change,
+ 0 // XXXX to do: if 3DKIN we actually want something here
+ );
}
return plan;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
