Algorithm.cpp
Go to the documentation of this file.
1 #include "Mobility.h"
2 #include "PathPlanner.h"
3 #include "Algorithm.h"
4 #include "Roadmap.h"
5 #include "ConfigurationSpace.h"
6 
7 using namespace PathEngine;
8 
10  : start_(planner->getConfigurationSpace()->size()),
11  goal_ (planner->getConfigurationSpace()->size()),
12  isRunning_(false), planner_(planner), verbose_(false)
13 {
14  properties_["interpolation-distance"] = "0.1";
16 }
17 
19 {
20 }
21 
22 void Algorithm::setProperty(const std::string& key, const std::string& value)
23 {
24  properties_[key] = value;
25 }
26 
27 void Algorithm::setProperties(const std::map<std::string, std::string> &properties)
28 {
29  std::map<std::string, std::string>::const_iterator it;
30  it = properties.begin();
31  while (it != properties.end()) {
32  properties_[it->first] = it->second;
33  it++;
34  }
35 }
36 
37 void Algorithm::getProperties(std::vector<std::string> &names,
38  std::vector<std::string> &values) {
39  names.clear();
40  values.clear();
41 
42  std::map<std::string, std::string>::iterator it;
43  it = properties_.begin();
44  while (it != properties_.end()) {
45  names.push_back((*it).first);
46  values.push_back((*it).second);
47  it++;
48  }
49 }
50 
52 {
53  Mobility *mobility = planner_->getMobility();
54 
55  if (mobility->isReachable(start_, goal_)){
56  path_.push_back(start_);
57  path_.push_back(goal_);
58  return true;
59  }else{
60  return false;
61  }
62 }
63 
65 {
66  isRunning_ = true;
67 
68  Mobility::interpolationDistance(atof(properties_["interpolation-distance"].c_str()));
69 #if 1
70  std::map<std::string, std::string>::iterator it;
71  it = properties_.begin();
72  std::cout << "properties:" << std::endl;
73  while (it != properties_.end()) {
74  std::cout << " " << it->first << " = " << it->second << std::endl;
75  it++;
76  }
77  std::cout << std::endl;
78 #endif
79 
80  path_.clear();
81 
82  std::cout << "start:" << start_ << std::endl;
83  std::cout << "goal:" << goal_ << std::endl;
84 
85  // validity checks of start&goal configurations
87  if (!cspace->isValid(start_)){
88  std::cerr << "start configuration is invalid" << std::endl;
89  return false;
90  }
92  const std::pair<std::string, std::string> &pair
94  std::cerr << "start configuration is not collision-free("
95  << pair.first << "<->" << pair.second << ")" << std::endl;
96  return false;
97  }
98  if (!cspace->isValid(goal_)){
99  std::cerr << "goal configuration is invalid" << std::endl;
100  return false;
101  }
103  const std::pair<std::string, std::string> &pair
104  = planner_->collidingPair();
105  std::cerr << "goal configuration is not collision-free("
106  << pair.first << "<->" << pair.second << ")" << std::endl;
107  return false;
108  }
109 #if 0
110  Mobility *mobility = planner_->getMobility();
111  if (!mobility->isReachable(start_, goal_, false)){
112  std::cerr << "goal is not reachable even if collision is not checked"
113  << std::endl;
114  return false;
115  }
116 #endif
117 
118  return true;
119 }
bool preparePlanning()
経路計画の準備をし、初期位置と目標位置が有効なものであることをチェックする
Definition: Algorithm.cpp:64
void setProperties(const std::map< std::string, std::string > &properties)
アルゴリズムに対して各種情報を設定する
Definition: Algorithm.cpp:27
static double interpolationDistance()
補間時の隣接する2点間の最大距離を取得する
Definition: Mobility.h:90
RoadmapPtr roadmap_
ロードマップ
Definition: Algorithm.h:71
void getProperties(std::vector< std::string > &names, std::vector< std::string > &values)
プロパティ一覧を取得する
Definition: Algorithm.cpp:37
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
Configuration goal_
終了位置
Definition: Algorithm.h:37
virtual ~Algorithm()
デストラクタ
Definition: Algorithm.cpp:18
png_uint_32 size
Definition: png.h:1521
bool tryDirectConnection()
初期位置と終了位置を直接結べないか検査する
Definition: Algorithm.cpp:51
bool checkCollision()
現在の状態で干渉検出を行う
png_voidp int value
Definition: png.h:2113
bool isRunning_
計算中フラグ
Definition: Algorithm.h:51
移動アルゴリズム実装用抽象クラス
Definition: Mobility.h:20
const std::pair< std::string, std::string > & collidingPair()
std::map< std::string, std::string > properties_
プロパティ
Definition: Algorithm.h:44
Mobility * getMobility()
移動能力を取得する
ロードマップ
Definition: Roadmap.h:19
std::vector< Configuration > path_
計画された経路
Definition: Algorithm.h:58
bool ignoreCollisionAtStart_
Definition: Algorithm.h:81
void setProperty(const std::string &key, const std::string &value)
Definition: Algorithm.cpp:22
bool isReachable(Configuration &from, Configuration &to, bool checkCollision=true) const
fromからtoへ干渉なしに移動可能であるかどうか
Definition: Mobility.cpp:8
PathPlanner * planner_
計画経路エンジン
Definition: Algorithm.h:66
Algorithm(PathPlanner *planner)
コンストラクタ
Definition: Algorithm.cpp:9
bool isValid(const Configuration &cfg) const
全ての要素が有効な範囲内にあるかどうか検査する
boost::shared_ptr< Roadmap > RoadmapPtr
Definition: Roadmap.h:13
Configuration start_
開始位置
Definition: Algorithm.h:30


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:18