Go to the documentation of this file.00001 #include "Mobility.h"
00002 #include "PathPlanner.h"
00003 #include "Algorithm.h"
00004 #include "Roadmap.h"
00005 #include "ConfigurationSpace.h"
00006
00007 using namespace PathEngine;
00008
00009 Algorithm::Algorithm(PathPlanner* planner)
00010 : start_(planner->getConfigurationSpace()->size()),
00011 goal_ (planner->getConfigurationSpace()->size()),
00012 isRunning_(false), planner_(planner), verbose_(false)
00013 {
00014 properties_["interpolation-distance"] = "0.1";
00015 roadmap_ = RoadmapPtr(new Roadmap(planner_));
00016 }
00017
00018 Algorithm::~Algorithm()
00019 {
00020 }
00021
00022 void Algorithm::setProperty(const std::string& key, const std::string& value)
00023 {
00024 properties_[key] = value;
00025 }
00026
00027 void Algorithm::setProperties(const std::map<std::string, std::string> &properties)
00028 {
00029 std::map<std::string, std::string>::const_iterator it;
00030 it = properties.begin();
00031 while (it != properties.end()) {
00032 properties_[it->first] = it->second;
00033 it++;
00034 }
00035 }
00036
00037 void Algorithm::getProperties(std::vector<std::string> &names,
00038 std::vector<std::string> &values) {
00039 names.clear();
00040 values.clear();
00041
00042 std::map<std::string, std::string>::iterator it;
00043 it = properties_.begin();
00044 while (it != properties_.end()) {
00045 names.push_back((*it).first);
00046 values.push_back((*it).second);
00047 it++;
00048 }
00049 }
00050
00051 bool Algorithm::tryDirectConnection()
00052 {
00053 Mobility *mobility = planner_->getMobility();
00054
00055 if (mobility->isReachable(start_, goal_)){
00056 path_.push_back(start_);
00057 path_.push_back(goal_);
00058 return true;
00059 }else{
00060 return false;
00061 }
00062 }
00063
00064 bool Algorithm::preparePlanning()
00065 {
00066 isRunning_ = true;
00067
00068 Mobility::interpolationDistance(atof(properties_["interpolation-distance"].c_str()));
00069 #if 1
00070 std::map<std::string, std::string>::iterator it;
00071 it = properties_.begin();
00072 std::cout << "properties:" << std::endl;
00073 while (it != properties_.end()) {
00074 std::cout << " " << it->first << " = " << it->second << std::endl;
00075 it++;
00076 }
00077 std::cout << std::endl;
00078 #endif
00079
00080 path_.clear();
00081
00082 std::cout << "start:" << start_ << std::endl;
00083 std::cout << "goal:" << goal_ << std::endl;
00084
00085
00086 ConfigurationSpace *cspace = planner_->getConfigurationSpace();
00087 if (!cspace->isValid(start_)){
00088 std::cerr << "start configuration is invalid" << std::endl;
00089 return false;
00090 }
00091 if (!ignoreCollisionAtStart_ && planner_->checkCollision(start_)){
00092 const std::pair<std::string, std::string> &pair
00093 = planner_->collidingPair();
00094 std::cerr << "start configuration is not collision-free("
00095 << pair.first << "<->" << pair.second << ")" << std::endl;
00096 return false;
00097 }
00098 if (!cspace->isValid(goal_)){
00099 std::cerr << "goal configuration is invalid" << std::endl;
00100 return false;
00101 }
00102 if (!ignoreCollisionAtGoal_ && planner_->checkCollision(goal_)){
00103 const std::pair<std::string, std::string> &pair
00104 = planner_->collidingPair();
00105 std::cerr << "goal configuration is not collision-free("
00106 << pair.first << "<->" << pair.second << ")" << std::endl;
00107 return false;
00108 }
00109 #if 0
00110 Mobility *mobility = planner_->getMobility();
00111 if (!mobility->isReachable(start_, goal_, false)){
00112 std::cerr << "goal is not reachable even if collision is not checked"
00113 << std::endl;
00114 return false;
00115 }
00116 #endif
00117
00118 return true;
00119 }