PRM.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <cstdio>
3 #include "Roadmap.h"
4 #include "RoadmapNode.h"
5 #include "ConfigurationSpace.h"
6 #include "PRM.h"
7 
8 using namespace PathEngine;
9 
11  : Algorithm(path)
12 {
13  // デフォルト値セット
14  properties_["max-dist"] = "1.0";
15  properties_["max-points"] = "100";
16 }
17 
19 }
20 
22 {
23  std::cerr << "new Roadmap is created" << std::endl;
24 
25  // 現在の点の数
26  unsigned long numPoints = 0, numTotalPoints = 0;
28 
29  while (numPoints < maxPoints_) {
30  if (!isRunning_) {
31  return false;
32  }
33 
34  // ランダムに与えた点
35  Configuration pos = cspace->random();
36  numTotalPoints++;
37 
38  // 干渉する位置でなければ追加
39  if (!planner_->checkCollision(pos)) {
40  RoadmapNodePtr node = RoadmapNodePtr(new RoadmapNode(pos));
41  roadmap_->addNode(node);
42  numPoints++;
43  }
44  printf("creating nodes, registered : %ld / tested : %ld\r", numPoints, numTotalPoints);
45  }
46  printf("\n");
47 
48  // エッジを作成
49  Mobility* mobility = planner_->getMobility();
50  RoadmapNodePtr from, to;
51  unsigned int n = roadmap_->nNodes();
52  for (unsigned long i=0; i<n; i++) {
53  if (!isRunning_) {
54  return false;
55  }
56  from = roadmap_->node(i);
57  for (unsigned long j=i+1; j<n; j++) {
58  to = roadmap_->node(j);
59  if (mobility->distance(from->position(), to->position()) < maxDist_) {
60  roadmap_->tryConnection(from, to);
61  }
62  }
63  }
64 
65  return true;
66 }
67 
69 {
70  std::cout << "PRM::calcPath()" << std::endl;
71  // Max Dists
72  maxDist_ = atof(properties_["max-dist"].c_str());
73 
74  // Max Points
75  maxPoints_ = atoi(properties_["max-points"].c_str());
76 
77  std::cerr << "maxDist:" << maxDist_ << std::endl;
78  std::cerr << "maxPoints:" << maxPoints_ << std::endl;
79 
80  if (roadmap_->nNodes() == 0) buildRoadmap();
81 
82  // スタートとゴールを追加
83  Mobility *mobility = planner_->getMobility();
86  roadmap_->addNode(startNode);
87  roadmap_->addNode(goalNode);
88 
89  RoadmapNodePtr node;
90  for (unsigned long i=0; i<roadmap_->nNodes(); i++) {
91  node = roadmap_->node(i);
92  const Configuration& pos = node->position();
93  if (mobility->distance(start_, pos) < maxDist_) {
94  roadmap_->tryConnection(startNode, node);
95  }
96  if (mobility->distance(goal_, pos) < maxDist_) {
97  roadmap_->tryConnection(goalNode, node);
98  }
99  }
100 
101  std::cout << "start node has " << startNode->nChildren()
102  << " childrens" << std::endl;
103  std::cout << "goal node has " << goalNode->nParents()
104  << " parents" << std::endl;
105 
106  std::vector<RoadmapNodePtr> nodePath;
107  nodePath = roadmap_->DFS(startNode, goalNode);
108  for (unsigned int i=0; i<nodePath.size(); i++){
109  path_.push_back(nodePath[i]->position());
110  }
111 
112  std::cout << "path_.size() = " << path_.size() << std::endl;
113 
114  return path_.size() != 0;
115 }
116 
virtual double distance(const Configuration &from, const Configuration &to) const =0
Configuration random()
generate random position
ロードマップのノード
Definition: RoadmapNode.h:17
RoadmapPtr roadmap_
ロードマップ
Definition: Algorithm.h:71
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
Configuration goal_
終了位置
Definition: Algorithm.h:37
bool checkCollision()
現在の状態で干渉検出を行う
bool isRunning_
計算中フラグ
Definition: Algorithm.h:51
png_uint_32 i
Definition: png.h:2735
移動アルゴリズム実装用抽象クラス
Definition: Mobility.h:20
unsigned long maxPoints_
Definition: PRM.h:25
std::map< std::string, std::string > properties_
プロパティ
Definition: Algorithm.h:44
Mobility * getMobility()
移動能力を取得する
boost::shared_ptr< RoadmapNode > RoadmapNodePtr
Definition: RoadmapNode.h:11
bool calcPath()
親クラスのドキュメントを参照
Definition: PRM.cpp:68
std::vector< Configuration > path_
計画された経路
Definition: Algorithm.h:58
bool buildRoadmap()
ロードマップを生成する
Definition: PRM.cpp:21
経路計画アルゴリズム基底クラス
Definition: Algorithm.h:23
PRM(PathPlanner *planner)
コンストラクタ
Definition: PRM.cpp:10
double maxDist_
Definition: PRM.h:28
PathPlanner * planner_
計画経路エンジン
Definition: Algorithm.h:66
Configuration start_
開始位置
Definition: Algorithm.h:30
virtual ~PRM()
デストラクタ
Definition: PRM.cpp:18


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05