21 #include <navgraph/constraints/constraint_repo.h>
36 NavGraphConstraintRepo::NavGraphConstraintRepo()
42 NavGraphConstraintRepo::~NavGraphConstraintRepo()
53 node_constraints_.push_back(constraint);
63 edge_constraints_.push_back(constraint);
73 edge_cost_constraints_.push_back(constraint);
80 NavGraphConstraintRepo::unregister_constraint(std::string name)
84 NodeConstraintList::iterator nc =
85 std::find_if(node_constraints_.begin(),
86 node_constraints_.end(),
88 if (nc != node_constraints_.end()) {
89 node_constraints_.erase(nc);
92 EdgeConstraintList::iterator ec =
93 std::find_if(edge_constraints_.begin(),
94 edge_constraints_.end(),
96 if (ec != edge_constraints_.end()) {
97 edge_constraints_.erase(ec);
100 EdgeCostConstraintList::iterator ecc =
101 std::find_if(edge_cost_constraints_.begin(),
102 edge_cost_constraints_.end(),
104 if (ecc != edge_cost_constraints_.end()) {
105 edge_cost_constraints_.erase(ecc);
115 NavGraphConstraintRepo::has_constraint(std::string &name)
117 NodeConstraintList::iterator nc =
118 std::find_if(node_constraints_.begin(),
119 node_constraints_.end(),
121 if (nc != node_constraints_.end())
124 EdgeConstraintList::iterator ec =
125 std::find_if(edge_constraints_.begin(),
126 edge_constraints_.end(),
128 if (ec != edge_constraints_.end())
131 EdgeCostConstraintList::iterator ecc =
132 std::find_if(edge_cost_constraints_.begin(),
133 edge_cost_constraints_.end(),
135 if (ecc != edge_cost_constraints_.end())
146 NavGraphConstraintRepo::get_node_constraint(std::string &name)
148 NodeConstraintList::iterator it =
149 std::find_if(node_constraints_.begin(),
150 node_constraints_.end(),
152 if (it != node_constraints_.end()) {
164 NavGraphConstraintRepo::get_edge_constraint(std::string &name)
166 EdgeConstraintList::iterator it =
167 std::find_if(edge_constraints_.begin(),
168 edge_constraints_.end(),
170 if (it != edge_constraints_.end()) {
182 NavGraphConstraintRepo::get_edge_cost_constraint(std::string &name)
184 EdgeCostConstraintList::iterator it =
185 std::find_if(edge_cost_constraints_.begin(),
186 edge_cost_constraints_.end(),
188 if (it != edge_cost_constraints_.end()) {
199 NavGraphConstraintRepo::node_constraints()
const
201 return node_constraints_;
208 NavGraphConstraintRepo::edge_constraints()
const
210 return edge_constraints_;
217 NavGraphConstraintRepo::edge_cost_constraints()
const
219 return edge_cost_constraints_;
226 NavGraphConstraintRepo::has_constraints()
const
229 !(node_constraints_.empty() && edge_constraints_.empty() && edge_cost_constraints_.empty()));
236 NavGraphConstraintRepo::compute()
238 bool modified =
false;
264 if (c->blocks(node)) {
278 std::map<std::string, std::string>
279 NavGraphConstraintRepo::blocks(
const std::vector<fawkes::NavGraphNode> &nodes)
281 std::map<std::string, std::string> rv;
285 rv[n.name()] = c->
name();
303 if (c->blocks(from, to)) {
322 if (c->cost_factor(from, to) >= 1.00001) {
343 float max_cost = 1.0;
347 if (edge_cost_factor > max_cost) {
348 max_cost = edge_cost_factor;
352 if (max_cost >= 1.00001) {
353 cost_factor = max_cost;
366 std::map<std::pair<std::string, std::string>, std::string>
367 NavGraphConstraintRepo::blocks(
const std::vector<fawkes::NavGraphEdge> &edges)
369 std::map<std::pair<std::string, std::string>, std::string> rv;
372 if (c->blocks(e.from_node(), e.to_node())) {
373 rv[std::make_pair(e.from(), e.to())] = c->
name();
395 std::list<std::tuple<std::string, std::string, std::string, float>>
396 NavGraphConstraintRepo::cost_factor(
const std::vector<fawkes::NavGraphEdge> &edges)
398 std::list<std::tuple<std::string, std::string, std::string, float>> rv;
400 float max_cost = 1.0;
403 float cost_factor = c->
cost_factor(e.from_node(), e.to_node());
404 if (cost_factor > max_cost) {
405 max_cost = cost_factor;
409 if (max_c && max_cost >= 1.00001) {
410 rv.push_back(std::make_tuple(e.from(), e.to(), max_c->
name(), max_cost));
433 float max_cost = 1.0;
436 if (cost_factor > max_cost) {
437 max_cost = cost_factor;
449 NavGraphConstraintRepo::modified(
bool reset_modified)
451 if (reset_modified) {
std::vector< fawkes::NavGraphEdgeCostConstraint * > EdgeCostConstraintList
List of navgraph edge cost constraints.
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
Constraint that can be queried to check if an edge is blocked.
Constraint that can be queried for an edge cost factor.
std::string name()
Get name of constraint.
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.
Fawkes library namespace.