Program Listing for File PlannerData.h
↰ Return to documentation for file (src/ompl/base/PlannerData.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ryan Luna, Luis G. Torres */
#ifndef OMPL_BASE_PLANNER_DATA_
#define OMPL_BASE_PLANNER_DATA_
#include <iostream>
#include <vector>
#include <map>
#include <set>
#include "ompl/base/State.h"
#include "ompl/base/Cost.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/ClassForward.h"
#include <boost/serialization/access.hpp>
namespace ompl
{
namespace base
{
class PlannerDataVertex
{
public:
PlannerDataVertex(const State *st, int tag = 0) : state_(st), tag_(tag)
{
}
PlannerDataVertex(const PlannerDataVertex &rhs) = default;
virtual ~PlannerDataVertex() = default;
virtual int getTag() const
{
return tag_;
}
virtual void setTag(int tag)
{
tag_ = tag;
}
virtual const State *getState() const
{
return state_;
}
virtual PlannerDataVertex *clone() const
{
return new PlannerDataVertex(*this);
}
virtual bool operator==(const PlannerDataVertex &rhs) const
{
// States should be unique
return state_ == rhs.state_;
}
bool operator!=(const PlannerDataVertex &rhs) const
{
return !(*this == rhs);
}
protected:
PlannerDataVertex() = default;
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive &ar, const unsigned int /*version*/)
{
ar &tag_;
// Serialization of the state pointer is handled by PlannerDataStorage
}
const State *state_;
int tag_;
friend class PlannerData;
friend class PlannerDataStorage;
};
class PlannerDataEdge
{
public:
PlannerDataEdge() = default;
virtual ~PlannerDataEdge() = default;
virtual PlannerDataEdge *clone() const
{
return new PlannerDataEdge();
}
virtual bool operator==(const PlannerDataEdge &rhs) const
{
return this == &rhs;
}
bool operator!=(const PlannerDataEdge &rhs) const
{
return !(*this == rhs);
}
protected:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive & /*ar*/, const unsigned int /*version*/)
{
}
};
OMPL_CLASS_FORWARD(StateStorage);
OMPL_CLASS_FORWARD(PlannerData);
// Forward declaration for PlannerData::computeEdgeWeights
class OptimizationObjective;
class PlannerData
{
public:
class Graph;
static const PlannerDataEdge NO_EDGE;
static const PlannerDataVertex NO_VERTEX;
static const unsigned int INVALID_INDEX;
// non-copyable
PlannerData(const PlannerData &) = delete;
PlannerData &operator=(const PlannerData &) = delete;
PlannerData(SpaceInformationPtr si);
virtual ~PlannerData();
unsigned int addVertex(const PlannerDataVertex &st);
unsigned int addStartVertex(const PlannerDataVertex &v);
unsigned int addGoalVertex(const PlannerDataVertex &v);
bool markStartState(const State *st);
bool markGoalState(const State *st);
bool tagState(const State *st, int tag);
virtual bool removeVertex(const PlannerDataVertex &st);
virtual bool removeVertex(unsigned int vIndex);
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge = PlannerDataEdge(),
Cost weight = Cost(1.0));
virtual bool addEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2,
const PlannerDataEdge &edge = PlannerDataEdge(), Cost weight = Cost(1.0));
virtual bool removeEdge(unsigned int v1, unsigned int v2);
virtual bool removeEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2);
virtual void clear();
virtual void decoupleFromPlanner();
unsigned int numEdges() const;
unsigned int numVertices() const;
unsigned int numStartVertices() const;
unsigned int numGoalVertices() const;
bool vertexExists(const PlannerDataVertex &v) const;
const PlannerDataVertex &getVertex(unsigned int index) const;
PlannerDataVertex &getVertex(unsigned int index);
const PlannerDataVertex &getStartVertex(unsigned int i) const;
PlannerDataVertex &getStartVertex(unsigned int i);
const PlannerDataVertex &getGoalVertex(unsigned int i) const;
PlannerDataVertex &getGoalVertex(unsigned int i);
unsigned int getStartIndex(unsigned int i) const;
unsigned int getGoalIndex(unsigned int i) const;
bool isStartVertex(unsigned int index) const;
bool isGoalVertex(unsigned int index) const;
unsigned int vertexIndex(const PlannerDataVertex &v) const;
bool edgeExists(unsigned int v1, unsigned int v2) const;
const PlannerDataEdge &getEdge(unsigned int v1, unsigned int v2) const;
PlannerDataEdge &getEdge(unsigned int v1, unsigned int v2);
unsigned int getEdges(unsigned int v, std::vector<unsigned int> &edgeList) const;
unsigned int getEdges(unsigned int v, std::map<unsigned int, const PlannerDataEdge *> &edgeMap) const;
unsigned int getIncomingEdges(unsigned int v, std::vector<unsigned int> &edgeList) const;
unsigned int getIncomingEdges(unsigned int v,
std::map<unsigned int, const PlannerDataEdge *> &edgeMap) const;
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const;
bool setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight);
void computeEdgeWeights(const OptimizationObjective &opt);
void computeEdgeWeights();
void printGraphviz(std::ostream &out = std::cout) const;
void printGraphML(std::ostream &out = std::cout) const;
void printPLY(std::ostream &out, bool asIs = false) const;
void extractMinimumSpanningTree(unsigned int v, const OptimizationObjective &opt, PlannerData &mst) const;
void extractReachable(unsigned int v, PlannerData &data) const;
StateStoragePtr extractStateStorage() const;
Graph &toBoostGraph();
const Graph &toBoostGraph() const;
const SpaceInformationPtr &getSpaceInformation() const;
virtual bool hasControls() const;
std::map<std::string, std::string> properties;
protected:
std::map<const State *, unsigned int> stateIndexMap_;
std::vector<unsigned int> startVertexIndices_;
std::vector<unsigned int> goalVertexIndices_;
SpaceInformationPtr si_;
std::set<State *> decoupledStates_;
private:
void freeMemory();
// Abstract pointer that points to the Boost.Graph structure.
// Obscured to prevent unnecessary inclusion of BGL throughout the
// rest of the code.
void *graphRaw_;
};
}
}
#endif