ompl/geometric/PathHybridization.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
00038 #define OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
00039 
00040 #include "ompl/base/SpaceInformation.h"
00041 #include "ompl/geometric/PathGeometric.h"
00042 #include <boost/graph/graph_traits.hpp>
00043 #include <boost/graph/adjacency_list.hpp>
00044 #include <iostream>
00045 #include <set>
00046 
00047 namespace ompl
00048 {
00049     namespace geometric
00050     {
00051 
00053 
00054         OMPL_CLASS_FORWARD(PathHybridization);
00056 
00070         class PathHybridization
00071         {
00072         public:
00073 
00075             PathHybridization(const base::SpaceInformationPtr &si);
00076             ~PathHybridization();
00077 
00079             const base::PathPtr& getHybridPath() const;
00080 
00082             void computeHybridPath();
00083 
00086             unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps);
00087 
00089             std::size_t pathCount() const;
00090 
00096             void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost,
00097                             std::vector<int> &indexP, std::vector<int> &indexQ) const;
00098 
00100             void clear();
00101 
00103             void print(std::ostream &out = std::cout) const;
00104 
00106             const std::string& getName() const;
00107 
00108         private:
00109 
00111             struct vertex_state_t {
00112                 typedef boost::vertex_property_tag kind;
00113             };
00114 
00115             typedef boost::adjacency_list <
00116                 boost::vecS, boost::vecS, boost::undirectedS,
00117                 boost::property < vertex_state_t, base::State*,
00118                                   boost::property < boost::vertex_predecessor_t, unsigned long int,
00119                                                     boost::property < boost::vertex_rank_t, unsigned long int > > >,
00120                 boost::property < boost::edge_weight_t, double >
00121                 > HGraph;
00122 
00123             typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
00124             typedef boost::graph_traits<HGraph>::edge_descriptor   Edge;
00125 
00126             struct PathInfo
00127             {
00128                 PathInfo(const base::PathPtr &path) : path_(path), states_(static_cast<PathGeometric*>(path.get())->getStates()), length_(0.0)
00129                 {
00130                     vertices_.reserve(states_.size());
00131                 }
00132 
00133                 bool operator==(const PathInfo &other) const
00134                 {
00135                     return path_ == other.path_;
00136                 }
00137 
00138                 bool operator<(const PathInfo &other) const
00139                 {
00140                     return path_ < other.path_;
00141                 }
00142 
00143                 base::PathPtr                    path_;
00144                 const std::vector<base::State*> &states_;
00145                 double                           length_;
00146                 std::vector<Vertex>              vertices_;
00147             };
00149 
00150             void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
00151 
00152             base::SpaceInformationPtr                         si_;
00153             HGraph                                            g_;
00154             boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
00155             Vertex                                            root_;
00156             Vertex                                            goal_;
00157             std::set<PathInfo>                                paths_;
00158             base::PathPtr                                     hpath_;
00159 
00161             std::string                                       name_;
00162         };
00163     }
00164 }
00165 
00166 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines