Roadmap.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
2 #include "PathPlanner.h"
3 #include "Mobility.h"
4 #include "RoadmapNode.h"
5 #include "Roadmap.h"
6 
7 using namespace PathEngine;
8 
10 {
11  nodes_.clear();
12  m_nEdges = 0;
13 }
14 
16 {
17  clear();
18 }
19 
21 {
22  from->addChild(to);
23  to->addParent(from);
24  m_nEdges++;
25 }
26 
28 {
29  for (unsigned int i=0; i<nodes_.size(); i++){
30  rdmp->addNode(nodes_[i]);
31  }
32  nodes_.clear();
33 }
34 
35 RoadmapNodePtr Roadmap::node(unsigned int index)
36 {
37  if (index >= nodes_.size()) return RoadmapNodePtr();
38 
39  return nodes_[index];
40 }
41 
43  RoadmapNodePtr & node, double &distance)
44 {
45  if (nodes_.size() == 0){
46  node = RoadmapNodePtr();
47  return;
48  }
49 
50  node = nodes_[0];
51  Mobility *mobility = planner_->getMobility();
52  distance = mobility->distance(node->position(), pos);
53 
54  double d;
55  for (unsigned int i=1; i<nodes_.size(); i++) {
56  d = mobility->distance(nodes_[i]->position(), pos);
57  if (d < distance) {
58  distance = d;
59  node = nodes_[i];
60  }
61  }
62 }
63 
65 {
66  if (nodes_.size() == 0) return RoadmapNodePtr();
67  return nodes_[nodes_.size()-1];
68 }
69 
71 {
72  for (unsigned int i=0; i<nodes_.size(); i++){
73  if (nodes_[i] == node) return (int)i;
74  }
75  return -1;
76 }
77 
78 std::vector<RoadmapNodePtr > Roadmap::DFS(RoadmapNodePtr startNode, RoadmapNodePtr goalNode)
79 {
80  for (unsigned int i=0; i<nodes_.size(); i++) {
81  nodes_[i]->visited(false);
82  }
83 
84  std::vector<RoadmapNodePtr> path;
85  startNode->visited(true);
86  path.push_back(startNode);
87  while (path.size() > 0) {
88  RoadmapNodePtr node = path.back();
89 
90  if (node == goalNode) {
91  break;
92  } else {
94  for (unsigned int i=0; i<node->nChildren(); i++) {
95  if (!node->child(i)->visited()) {
96  child = node->child(i);
97  break;
98  }
99  }
100  if (child == NULL) {
101  path.pop_back();
102  } else {
103  child->visited(true);
104  path.push_back(child);
105  }
106  }
107  }
108 
109  return path;
110 }
111 
112 void Roadmap::tryConnection(RoadmapNodePtr from, RoadmapNodePtr to, bool tryReverse)
113 {
114  Mobility *mobility = planner_->getMobility();
115  if (mobility->isReachable(from->position(), to->position())){
116  addEdge(from, to);
117  if (tryReverse && mobility->isReversible()) addEdge(to, from);
118  }
119  if (tryReverse && !mobility->isReversible()){
120  if (mobility->isReachable(to->position(), from->position())){
121  addEdge(to, from);
122  }
123  }
124 }
125 
127 {
128  return from->removeChild(to) && to->removeParent(from);
129 }
virtual double distance(const Configuration &from, const Configuration &to) const =0
std::vector< RoadmapNodePtr > DFS(RoadmapNodePtr startNode, RoadmapNodePtr goalNode)
深さ優先探索
Definition: Roadmap.cpp:78
void addEdge(RoadmapNodePtr from, RoadmapNodePtr to)
有向エッジを追加する
Definition: Roadmap.cpp:20
~Roadmap()
デストラクタ
Definition: Roadmap.cpp:15
int indexOfNode(RoadmapNodePtr node)
指定されたノードのインデックスを取得する
Definition: Roadmap.cpp:70
png_uint_32 i
Definition: png.h:2735
移動アルゴリズム実装用抽象クラス
Definition: Mobility.h:20
unsigned int m_nEdges
Definition: Roadmap.h:124
RoadmapNodePtr node(unsigned int index)
ノードを取得する
Definition: Roadmap.cpp:35
Mobility * getMobility()
移動能力を取得する
PathPlanner * planner_
Definition: Roadmap.h:123
boost::shared_ptr< RoadmapNode > RoadmapNodePtr
Definition: RoadmapNode.h:11
void findNearestNode(const Configuration &cfg, RoadmapNodePtr &node, double &distance)
最も距離の小さいノードを返す
Definition: Roadmap.cpp:42
void tryConnection(RoadmapNodePtr from, RoadmapNodePtr to, bool tryReversed=true)
2つのノードの接続を試み、接続できた場合はエッジを追加する
Definition: Roadmap.cpp:112
bool removeEdge(RoadmapNodePtr from, RoadmapNodePtr to)
有向エッジを削除する
Definition: Roadmap.cpp:126
std::vector< RoadmapNodePtr > nodes_
ノードのリスト
Definition: Roadmap.h:122
path
virtual bool isReversible() const =0
この移動アルゴリズムでA→Bへ移動可能である時に、B→Aが同じ経路で移動可能であるかどうか ...
void integrate(RoadmapPtr rdmp)
このロードマップの内容を引数のロードマップに統合する
Definition: Roadmap.cpp:27
bool isReachable(Configuration &from, Configuration &to, bool checkCollision=true) const
fromからtoへ干渉なしに移動可能であるかどうか
Definition: Mobility.cpp:8
RoadmapNodePtr lastAddedNode()
最後に追加されたノードを取得する
Definition: Roadmap.cpp:64
boost::shared_ptr< Roadmap > RoadmapPtr
Definition: Roadmap.h:13
void clear()
ロードマップをクリアする
Definition: Roadmap.cpp:9


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