$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 * 00029 */ 00030 00031 00040 #ifndef OCCUPANCY_GRID_UTILS_SHORTEST_PATH_H 00041 #define OCCUPANCY_GRID_UTILS_SHORTEST_PATH_H 00042 00043 #include <occupancy_grid_utils/coordinate_conversions.h> 00044 #include <occupancy_grid_utils/NavigationFunction.h> 00045 #include <boost/shared_ptr.hpp> 00046 #include <boost/optional.hpp> 00047 #include <set> 00048 00049 namespace occupancy_grid_utils 00050 { 00051 00052 /************************************************************ 00053 * Obstacle inflation 00054 ************************************************************/ 00055 00064 nav_msgs::OccupancyGrid::Ptr inflateObstacles (const nav_msgs::OccupancyGrid& g, 00065 double r, 00066 bool allow_unknown=false); 00067 00068 00069 /************************************************************ 00070 * Shortest paths 00071 ************************************************************/ 00072 00073 struct ShortestPathResult; 00074 typedef boost::shared_ptr<ShortestPathResult> ResultPtr; 00075 typedef std::set<Cell> Cells; 00076 typedef std::vector<Cell> Path; 00077 typedef std::pair<Path, double> AStarResult; 00078 00079 00086 struct TerminationCondition 00087 { 00088 TerminationCondition (); 00089 TerminationCondition (double max_distance, bool use_cells=true); 00090 TerminationCondition (const Cells& goals); 00091 TerminationCondition (const Cells& goals, const double max_distance, 00092 bool use_cells = true); 00093 00094 boost::optional<double> max_distance_; 00095 boost::optional<std::set<Cell> > goals_; 00096 bool use_cells_; 00097 }; 00098 00106 ResultPtr singleSourceShortestPaths (const nav_msgs::OccupancyGrid& g, const Cell& src, 00107 bool manhattan=false); 00108 00109 00119 ResultPtr singleSourceShortestPaths (const nav_msgs::OccupancyGrid& g, const Cell& src, 00120 const TerminationCondition& term, 00121 bool manhattan=false); 00122 00126 boost::optional<Path> extractPath (ResultPtr shortest_path_result, const Cell& dest); 00127 00130 boost::optional<double> distance(ResultPtr shortest_path_result, const Cell& dest); 00131 00134 boost::optional<double> distanceTo(ResultPtr shortest_path_result, const Cell& dest); 00135 00136 00138 boost::optional<double> distance(ResultPtr shortest_path_result, const Cell& dest); 00139 00141 ResultPtr shortestPathResultFromMessage (const NavigationFunction& msg); 00142 00144 NavigationFunction shortestPathResultToMessage (ResultPtr res); 00145 00146 00152 boost::optional<AStarResult> shortestPath(const nav_msgs::OccupancyGrid& g, 00153 const Cell& src, const Cell& dest); 00154 00160 boost::optional<AStarResult> shortestPathAStar(const nav_msgs::OccupancyGrid& g, 00161 const Cell& src, const Cell& dest); 00162 00163 00164 } // namespace 00165 00166 #endif // include guard 00167 00168 00169