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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines