demos/Point2DPlanning.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Rice University 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 Rice University 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 #include <ompl/base/spaces/RealVectorStateSpace.h> 00038 #include <ompl/geometric/SimpleSetup.h> 00039 #include <ompl/geometric/planners/rrt/RRTstar.h> 00040 #include <ompl/geometric/planners/rrt/RRTConnect.h> 00041 #include <ompl/util/PPM.h> 00042 00043 #include <ompl/config.h> 00044 #include <../tests/resources/config.h> 00045 00046 #include <boost/filesystem.hpp> 00047 #include <iostream> 00048 00049 namespace ob = ompl::base; 00050 namespace og = ompl::geometric; 00051 00052 class Plane2DEnvironment 00053 { 00054 public: 00055 00056 Plane2DEnvironment(const char *ppm_file) 00057 { 00058 bool ok = false; 00059 try 00060 { 00061 ppm_.loadFile(ppm_file); 00062 ok = true; 00063 } 00064 catch(ompl::Exception &ex) 00065 { 00066 OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what()); 00067 } 00068 if (ok) 00069 { 00070 ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace(); 00071 space->addDimension(0.0, ppm_.getWidth()); 00072 space->addDimension(0.0, ppm_.getHeight()); 00073 maxWidth_ = ppm_.getWidth() - 1; 00074 maxHeight_ = ppm_.getHeight() - 1; 00075 ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space))); 00076 00077 // set state validity checking for this space 00078 ss_->setStateValidityChecker(boost::bind(&Plane2DEnvironment::isStateValid, this, _1)); 00079 space->setup(); 00080 ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent()); 00081 // ss_->setPlanner(ob::PlannerPtr(new og::RRTConnect(ss_->getSpaceInformation()))); 00082 } 00083 } 00084 00085 bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col) 00086 { 00087 if (!ss_) 00088 return false; 00089 ob::ScopedState<> start(ss_->getStateSpace()); 00090 start[0] = start_row; 00091 start[1] = start_col; 00092 ob::ScopedState<> goal(ss_->getStateSpace()); 00093 goal[0] = goal_row; 00094 goal[1] = goal_col; 00095 ss_->setStartAndGoalStates(start, goal); 00096 // generate a few solutions; all will be added to the goal; 00097 for (int i = 0 ; i < 10 ; ++i) 00098 { 00099 if (ss_->getPlanner()) 00100 ss_->getPlanner()->clear(); 00101 ss_->solve(); 00102 } 00103 const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount(); 00104 OMPL_INFORM("Found %d solutions", (int)ns); 00105 if (ss_->haveSolutionPath()) 00106 { 00107 ss_->simplifySolution(); 00108 og::PathGeometric &p = ss_->getSolutionPath(); 00109 ss_->getPathSimplifier()->simplifyMax(p); 00110 ss_->getPathSimplifier()->smoothBSpline(p); 00111 return true; 00112 } 00113 else 00114 return false; 00115 } 00116 00117 void recordSolution() 00118 { 00119 if (!ss_ || !ss_->haveSolutionPath()) 00120 return; 00121 og::PathGeometric &p = ss_->getSolutionPath(); 00122 p.interpolate(); 00123 for (std::size_t i = 0 ; i < p.getStateCount() ; ++i) 00124 { 00125 const int w = std::min(maxWidth_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0]); 00126 const int h = std::min(maxHeight_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1]); 00127 ompl::PPM::Color &c = ppm_.getPixel(h, w); 00128 c.red = 255; 00129 c.green = 0; 00130 c.blue = 0; 00131 } 00132 } 00133 00134 void save(const char *filename) 00135 { 00136 if (!ss_) 00137 return; 00138 ppm_.saveFile(filename); 00139 } 00140 00141 private: 00142 00143 bool isStateValid(const ob::State *state) const 00144 { 00145 const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_); 00146 const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_); 00147 00148 const ompl::PPM::Color &c = ppm_.getPixel(h, w); 00149 return c.red > 127 && c.green > 127 && c.blue > 127; 00150 } 00151 00152 og::SimpleSetupPtr ss_; 00153 int maxWidth_; 00154 int maxHeight_; 00155 ompl::PPM ppm_; 00156 00157 }; 00158 00159 int main(int, char **) 00160 { 00161 std::cout << "OMPL version: " << OMPL_VERSION << std::endl; 00162 00163 boost::filesystem::path path(TEST_RESOURCES_DIR); 00164 Plane2DEnvironment env((path / "ppm/floor.ppm").string().c_str()); 00165 00166 if (env.plan(0, 0, 777, 1265)) 00167 { 00168 env.recordSolution(); 00169 env.save("result_demo.ppm"); 00170 } 00171 00172 return 0; 00173 }