21 #include <navgraph/constraints/constraint_repo.h> 40 NavGraphConstraintRepo::NavGraphConstraintRepo()
46 NavGraphConstraintRepo::~NavGraphConstraintRepo()
58 node_constraints_.push_back(constraint);
68 edge_constraints_.push_back(constraint);
78 edge_cost_constraints_.push_back(constraint);
86 NavGraphConstraintRepo::unregister_constraint(std::string name)
90 NodeConstraintList::iterator nc =
91 std::find_if(node_constraints_.begin(), node_constraints_.end(),
95 if (nc != node_constraints_.end()) {
96 node_constraints_.erase(nc);
99 EdgeConstraintList::iterator ec =
100 std::find_if(edge_constraints_.begin(), edge_constraints_.end(),
104 if (ec != edge_constraints_.end()) {
105 edge_constraints_.erase(ec);
108 EdgeCostConstraintList::iterator ecc =
109 std::find_if(edge_cost_constraints_.begin(), edge_cost_constraints_.end(),
113 if (ecc != edge_cost_constraints_.end()) {
114 edge_cost_constraints_.erase(ecc);
125 NavGraphConstraintRepo::has_constraint(std::string &name)
127 NodeConstraintList::iterator nc =
128 std::find_if(node_constraints_.begin(), node_constraints_.end(),
132 if (nc != node_constraints_.end())
return true;
134 EdgeConstraintList::iterator ec =
135 std::find_if(edge_constraints_.begin(), edge_constraints_.end(),
139 if (ec != edge_constraints_.end())
return true;
141 EdgeCostConstraintList::iterator ecc =
142 std::find_if(edge_cost_constraints_.begin(), edge_cost_constraints_.end(),
146 if (ecc != edge_cost_constraints_.end())
return true;
157 NavGraphConstraintRepo::get_node_constraint(std::string &name)
159 NodeConstraintList::iterator it =
160 std::find_if(node_constraints_.begin(), node_constraints_.end(),
164 if (it != node_constraints_.end()) {
176 NavGraphConstraintRepo::get_edge_constraint(std::string &name)
178 EdgeConstraintList::iterator it =
179 std::find_if(edge_constraints_.begin(), edge_constraints_.end(),
183 if (it != edge_constraints_.end()) {
196 NavGraphConstraintRepo::get_edge_cost_constraint(std::string &name)
198 EdgeCostConstraintList::iterator it =
199 std::find_if(edge_cost_constraints_.begin(), edge_cost_constraints_.end(),
203 if (it != edge_cost_constraints_.end()) {
215 NavGraphConstraintRepo::node_constraints()
const 217 return node_constraints_;
225 NavGraphConstraintRepo::edge_constraints()
const 227 return edge_constraints_;
234 NavGraphConstraintRepo::edge_cost_constraints()
const 236 return edge_cost_constraints_;
244 NavGraphConstraintRepo::has_constraints()
const 246 return (! (node_constraints_.empty() &&
247 edge_constraints_.empty() &&
248 edge_cost_constraints_.empty()));
256 NavGraphConstraintRepo::compute()
258 bool modified =
false;
260 if (c->compute()) modified =
true;
263 if (c->compute()) modified =
true;
266 if (c->compute()) modified =
true;
282 if (c->blocks(node)) {
297 std::map<std::string, std::string>
298 NavGraphConstraintRepo::blocks(
const std::vector<fawkes::NavGraphNode> &nodes)
300 std::map<std::string, std::string> rv;
304 rv[n.name()] = c->name();
324 if (c->blocks(from, to)) {
343 if (c->cost_factor(from, to) >= 1.00001) {
365 float max_cost = 1.0;
369 if (cost_factor > max_cost) {
370 max_cost = cost_factor;
374 if (max_cost >= 1.00001) {
375 cost_factor = max_cost;
389 std::map<std::pair<std::string, std::string>, std::string>
390 NavGraphConstraintRepo::blocks(
const std::vector<fawkes::NavGraphEdge> &edges)
392 std::map<std::pair<std::string, std::string>, std::string> rv;
395 if (c->blocks(e.from_node(), e.to_node()) ){
396 rv[std::make_pair(e.from(), e.to())] = c->name();
420 std::list<std::tuple<std::string, std::string, std::string, float>>
421 NavGraphConstraintRepo::cost_factor(
const std::vector<fawkes::NavGraphEdge> &edges)
423 std::list<std::tuple<std::string, std::string, std::string, float>> rv;
425 float max_cost = 1.0;
428 float cost_factor = c->
cost_factor(e.from_node(), e.to_node());
429 if (cost_factor > max_cost) {
430 max_cost = cost_factor;
434 if (max_c && max_cost >= 1.00001) {
435 rv.push_back(std::make_tuple(e.from(), e.to(), max_c->
name(), max_cost));
459 float max_cost = 1.0;
461 float cost_factor = c->cost_factor(from, to);
462 if (cost_factor > max_cost) {
463 max_cost = cost_factor;
476 NavGraphConstraintRepo::modified(
bool reset_modified)
478 if (reset_modified) {
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
Constraint that can be queried to check if an edge is blocked.
Fawkes library namespace.
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
Constraint that can be queried for an edge cost factor.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)=0
Get cost factor for given edge.
Constraint that can be queried to check if a node is blocked.
std::string name()
Get name of constraint.
std::vector< fawkes::NavGraphEdgeCostConstraint * > EdgeCostConstraintList
List of navgraph edge cost constraints.