All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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         ClassForward(PathHybridization);
00056 
00070         class PathHybridization
00071         {
00072         public:
00073 
00075             PathHybridization(const base::SpaceInformationPtr &si);
00076             ~PathHybridization(void);
00077 
00079             const base::PathPtr& getHybridPath(void) const;
00080 
00082             void computeHybridPath(void);
00083 
00086             unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps);
00087 
00089             std::size_t pathCount(void) 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(void);
00101 
00103             void print(std::ostream &out = std::cout) const;
00104 
00105         private:
00106 
00108             struct vertex_state_t {
00109                 typedef boost::vertex_property_tag kind;
00110             };
00111 
00112             typedef boost::adjacency_list <
00113                 boost::vecS, boost::vecS, boost::undirectedS,
00114                 boost::property < vertex_state_t, base::State*,
00115                                   boost::property < boost::vertex_predecessor_t, unsigned long int,
00116                                                     boost::property < boost::vertex_rank_t, unsigned long int > > >,
00117                 boost::property < boost::edge_weight_t, double >
00118                 > HGraph;
00119 
00120             typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
00121             typedef boost::graph_traits<HGraph>::edge_descriptor   Edge;
00122 
00123             struct PathInfo
00124             {
00125                 PathInfo(const base::PathPtr &path) : path_(path), states_(static_cast<PathGeometric*>(path.get())->getStates()), length_(0.0)
00126                 {
00127                     vertices_.reserve(states_.size());
00128                 }
00129 
00130                 bool operator==(const PathInfo &other) const
00131                 {
00132                     return path_ == other.path_;
00133                 }
00134 
00135                 bool operator<(const PathInfo &other) const
00136                 {
00137                     return path_ < other.path_;
00138                 }
00139 
00140                 base::PathPtr                    path_;
00141                 const std::vector<base::State*> &states_;
00142                 double                           length_;
00143                 std::vector<Vertex>              vertices_;
00144             };
00146 
00147             void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
00148 
00149             base::SpaceInformationPtr                         si_;
00150             HGraph                                            g_;
00151             boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
00152             Vertex                                            root_;
00153             Vertex                                            goal_;
00154             std::set<PathInfo>                                paths_;
00155             base::PathPtr                                     hpath_;
00156 
00157             msg::Interface                                    msg_;
00158         };
00159     }
00160 }
00161 
00162 #endif
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends