21#include <navgraph/constraints/constraint_repo.h>
53 node_constraints_.push_back(constraint);
63 edge_constraints_.push_back(constraint);
73 edge_cost_constraints_.push_back(constraint);
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);
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())
148 NodeConstraintList::iterator it =
149 std::find_if(node_constraints_.begin(),
150 node_constraints_.end(),
152 if (it != node_constraints_.end()) {
166 EdgeConstraintList::iterator it =
167 std::find_if(edge_constraints_.begin(),
168 edge_constraints_.end(),
170 if (it != edge_constraints_.end()) {
184 EdgeCostConstraintList::iterator it =
185 std::find_if(edge_cost_constraints_.begin(),
186 edge_cost_constraints_.end(),
188 if (it != edge_cost_constraints_.end()) {
201 return node_constraints_;
210 return edge_constraints_;
219 return edge_cost_constraints_;
229 !(node_constraints_.empty() && edge_constraints_.empty() && edge_cost_constraints_.empty()));
264 if (c->blocks(node)) {
278std::map<std::string, std::string>
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) {
366std::map<std::pair<std::string, std::string>, std::string>
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();
395std::list<std::tuple<std::string, std::string, std::string, float>>
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());
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;
451 if (reset_modified) {
std::vector< fawkes::NavGraphEdgeCostConstraint * > EdgeCostConstraintList
List of navgraph edge cost constraints.
NavGraphConstraintRepo()
Constructor.
fawkes::NavGraphNodeConstraint * get_node_constraint(std::string &name)
Get a node constraint by name.
fawkes::NavGraphEdgeConstraint * get_edge_constraint(std::string &name)
Get an edge constraint by name.
const EdgeConstraintList & edge_constraints() const
Get a list of registered edge constraints.
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
~NavGraphConstraintRepo()
Destructor.
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
void register_constraint(NavGraphNodeConstraint *constraint)
Register a constraint.
bool has_constraints() const
Check if there are any constraints at all.
void unregister_constraint(std::string name)
Unregister a constraint by name.
NavGraphEdgeCostConstraint * increases_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Check if any constraint in the repo increases the cost of the edge.
fawkes::NavGraphEdgeCostConstraint * get_edge_cost_constraint(std::string &name)
Get an edge cost constraint by name.
bool modified(bool reset_modified=false)
Check if the constraint repo has been modified.
const NodeConstraintList & node_constraints() const
Get a list of registered node constraints.
float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get the highest increasing cost factor for an edge.
NavGraphNodeConstraint * blocks(const fawkes::NavGraphNode &node)
Check if any constraint in the repo blocks the node.
bool compute()
Call compute method on all registered constraints.
const EdgeCostConstraintList & edge_cost_constraints() const
Get a list of registered edge cost constraints.
bool has_constraint(std::string &name)
Check by name if a constraint has been registered.
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) noexcept=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.