ShapeSpacePlannerXYTheta.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef _SHAPE_SPACE_PLANNER_XYTHETA_H_
#define _SHAPE_SPACE_PLANNER_XYTHETA_H_

#include "Motion/Kinematics.h"

#include "Planners/RRT/ShapeSpacePlannerBase.h"

class ShapeSpacePlannerXYTheta;

//================ RRTNodeXYTheta ================

class RRTNodeXYTheta : public RRTNodeBase {
public:
  
  typedef std::pair<std::pair<float,float>,AngTwoPi> NodeValue_t;
  typedef GenericRRTBase::PlannerResult<2> PlannerResult;
  NodeValue_t q;
  
  //! Constructor
  RRTNodeXYTheta(const NodeValue_t &_q, unsigned int _parent) : RRTNodeBase(_parent), q(_q) {};
  
  class CollisionChecker : public ShapeSpaceCollisionCheckerBase<2> {
  public:
    CollisionChecker(DualCoding::ShapeSpace & shs,
                     const DualCoding::Shape<DualCoding::PolygonData> &_worldBounds,
                     float _inflation) :
    ShapeSpaceCollisionCheckerBase<2>(shs, _worldBounds, _inflation) {}
    
    virtual bool collides(const NodeValue_t &qnew, PlannerResult* result=NULL) const;
  };
  
  static const unsigned int maxInterpolations = 10; //!< Maximum number of interpolation steps in interpolate() when @a truncate is true
  static const AngSignPi turnLimit; //!< Maximum allowable angular difference between adjacent nodes
  
  virtual float distance(const NodeValue_t &target);
  static void generateSample(const NodeValue_t &lower, const NodeValue_t &upper, NodeValue_t &sample);
  static Interp_t interpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interp, 
                              bool truncate, CollisionChecker *cc, NodeValue_t &reached, bool fromOtherTree);
  static Interp_t smoothInterpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interp, CollisionChecker *cc);
  static bool goodAngle(const NodeValue_t &q1, const NodeValue_t &qnew);
  
  virtual std::string toString() const;
};

//================ ShapeSpacePlannerXYTheta ================

//! Plans a path in a 2D linear space with smooth angle changes, uses a rectangular bounding box for collision testing
class ShapeSpacePlannerXYTheta : public GenericRRT<RRTNodeXYTheta, 2> {
public:
  typedef RRTNodeXYTheta NodeType_t;
  typedef NodeType_t::NodeValue_t NodeValue_t;
  typedef GenericRRTBase::PlannerResult<2> PlannerResult;
  
  ShapeSpacePlannerXYTheta(DualCoding::ShapeSpace &shs,
                           const DualCoding::Shape<DualCoding::PolygonData> &worldBounds = Shape<PolygonData>(),
                           float inflation = 0);
  
  virtual ~ShapeSpacePlannerXYTheta() {}
  
  float targetHeading;
  
  virtual void initialize(const NodeValue_t &start, std::vector<NodeType_t> &treeStartResult,
                          const NodeValue_t &end, std::vector<NodeType_t> &treeEndResult);
  virtual void buildPath(const std::vector<NodeType_t> *treeStart, const std::vector<NodeType_t> *treeEnd, 
                         std::vector<NodeValue_t> &path);
  
  using GenericRRT<NodeType_t, 2>::planPath;
  PlannerResult
  planPath(const Point &start,
           const Point &end,
           const AngTwoPi &initialHeading,
           const AngTwoPi &targetHeading,
           unsigned int _maxIterations=4000,
           std::vector<NodeValue_t> *pathResult=NULL,
           std::vector<NodeType_t> *treeStartResult=NULL,
           std::vector<NodeType_t> *treeEndResult=NULL);
  
  static void plotPath(const std::vector<NodeValue_t> &path,
           Shape<GraphicsData> &graphics,
           rgb color = rgb(0,0,255));

  static void plotTree(const std::vector<NodeType_t> &tree,
           Shape<GraphicsData> &graphics,
           rgb color = rgb(0,0,255));
};

#endif