22 #include <core/exception.h> 23 #include <navgraph/constraints/static_list_edge_cost_constraint.h> 53 edge_cost_list_buffer_.lock();
54 edge_cost_list_ = edge_cost_list_buffer_;
55 edge_cost_list_buffer_.unlock();
83 const std::vector<std::pair<fawkes::NavGraphEdge, float>> &edges)
85 for (
const std::pair<NavGraphEdge, float> &ec : edges) {
96 std::vector<std::pair<NavGraphEdge, float>>::iterator ec =
97 std::find_if(edge_cost_list_buffer_.begin(),
98 edge_cost_list_buffer_.end(),
99 [&edge](
const std::pair<fawkes::NavGraphEdge, float> &p) {
100 return p.first == edge;
103 if (ec != edge_cost_list_buffer_.end()) {
116 return (std::find_if(edge_cost_list_buffer_.begin(),
117 edge_cost_list_buffer_.end(),
118 [&edge](
const std::pair<fawkes::NavGraphEdge, float> &p) {
119 return p.first == edge;
121 != edge_cost_list_buffer_.end());
130 const std::vector<std::pair<fawkes::NavGraphEdge, float>> &
133 return edge_cost_list_;
140 if (!edge_cost_list_buffer_.empty()) {
142 edge_cost_list_buffer_.clear();
150 for (std::pair<NavGraphEdge, float> &ec : edge_cost_list_) {
151 if ((ec.first.from() == from.name() && ec.first.to() == to.name())
152 || (ec.first.from() == to.name() && ec.first.to() == from.name())) {
void add_edge(const fawkes::NavGraphEdge &edge, const float cost_factor)
Add a single edge to constraint list.
void erase_locked(typename std::vector< Type >::iterator pos)
Erase given element with lock protection.
Fawkes library namespace.
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
NavGraphStaticListEdgeCostConstraint(std::string name)
Constructor.
Constraint that can be queried for an edge cost factor.
void push_back_locked(const Type &x)
Push element to vector at back with lock protection.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get cost factor for given edge.
virtual ~NavGraphStaticListEdgeCostConstraint()
Virtual empty destructor.
Base class for exceptions in Fawkes.
const std::vector< std::pair< fawkes::NavGraphEdge, float > > & edge_cost_list() const
Get list of blocked edges.
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, float >> &edge_costs)
Add multiple edges to constraint list.
void clear_edges()
Remove all edges.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.