Fawkes API  Fawkes Development Version
timed_reservation_list_edge_constraint.cpp
1 /***************************************************************************
2  * timed_reservation_list_edge_constraint.cpp - edge constraint that holds
3  * a static list of edges and
4  * a duration to block
5  *
6  * Created: Sat Jul 12 16:48:23 2014
7  * Copyright 2014 Sebastian Reuter
8  * 2014 Tim Niemueller
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include <navgraph/constraints/timed_reservation_list_edge_constraint.h>
25 
26 #include <algorithm>
27 
28 namespace fawkes{
29 #if 0 /* just to make Emacs auto-indent happy */
30 }
31 #endif
32 
33 /** @class NavGraphTimedReservationListEdgeConstraint <navgraph/constraints/timed_reservation_list_edge_constraint.h>
34  * Constraint that holds a list of edges to block with timeouts.
35  * @author Sebastian Reuter
36  * @author Tim Niemueller
37  */
38 
39 /** Constructor.
40  * @param logger logger used for debug logging
41  * @param name name of edge constraint
42  * @param clock time source to evaluate constraint timeouts
43  */
45  (Logger *logger, std::string name, fawkes::Clock *clock)
47 {
48  logger_ = logger;
49  clock_ = clock;
50 }
51 
52 /** Constructor.
53  * @param logger logger used for debug logging
54  * @param name name of edge constraint
55  * @param clock time source to evaluate constraint timeouts
56  * @param edge_time_list list of edges with valid_time
57  */
59  (Logger *logger, std::string name, fawkes::Clock *clock,
60  std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> edge_time_list)
62 {
63  logger_ = logger;
64  clock_ = clock;
65  constraint_name_ = name;
66  edge_time_list_ = edge_time_list;
67 }
68 
69 
70 /** Virtual empty destructor. */
72 {
73 }
74 
75 
76 bool
78 {
79 
80  fawkes::Time now(clock_);
81  std::vector<std::pair<NavGraphEdge, fawkes::Time>> erase_list;
82  for (const std::pair<NavGraphEdge, fawkes::Time> &ec : edge_time_list_) {
83  if(now > ec.second) {
84  erase_list.push_back(ec);
85  }
86  }
87  for (const std::pair<NavGraphEdge, fawkes::Time> &ec : erase_list) {
88  edge_time_list_.erase(std::remove(edge_time_list_.begin(), edge_time_list_.end(), ec),
89  edge_time_list_.end());
90  modified_ = true;
91  logger_->log_info("TimedEdgeConstraint",
92  "Deleted edge '%s_%s' from '%s' because it validity duration ran out",
93  ec.first.from().c_str(),ec.first.to().c_str(), name_.c_str() );
94  }
95 
96  if (modified_) {
97  modified_ = false;
98  return true;
99  } else {
100  return false;
101  }
102 }
103 
104 
105 /** Add a single edge to constraint list.
106  * @param edge edge to add to constraint list
107  * @param valid_time valid time for this edge
108  */
109 void
111  fawkes::Time valid_time)
112 {
113  fawkes::Time now(clock_);
114 
115  if (valid_time < now) {
116  logger_->log_warn("Timed Edge Constraint",
117  "Constraint '%s' received node with old reservation time='%f' - now='%f'",
118  name_.c_str(), valid_time.in_sec(), now.in_sec());
119  }
120  if (! has_edge(edge)) {
121  modified_ = true;
122  edge_time_list_.push_back(std::make_pair(edge, valid_time));
123  std::string txt = edge.from(); txt +="_"; txt+=edge.to();
124  }
125 }
126 
127 
128 /** Add multiple edges to constraint list.
129  * @param edges edges with timeout to add to constraint list
130  */
131 void
133  const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &edges)
134 {
135  std::string txt = "{";
136  for (const std::pair<NavGraphEdge, fawkes::Time> &ec : edges) {
137  add_edge(ec.first, ec.second);
138  txt += ec.first.from(); txt += "_"; txt += ec.first.to(); txt += ",";
139  }
140  txt.erase(txt.length()-1,1); txt += "}";
141 }
142 
143 
144 /** Remove a single edge from the constraint list.
145  * @param edge edge to remote
146  */
147 void
149 {
150  std::vector<std::pair<NavGraphEdge, fawkes::Time>>::iterator ec
151  = std::find_if(edge_time_list_.begin(), edge_time_list_.end(),
152  [&edge](const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
153  return p.first == edge;
154  });
155 
156  if (ec != edge_time_list_.end()) {
157  modified_ = true;
158  edge_time_list_.erase(ec);
159  }
160 }
161 
162 
163 /** Check if constraint has a specific edge.
164  * @param edge edge to check
165  * @return true if edge is in list, false otherwise
166  */
167 bool
169 {
170  return (std::find_if(edge_time_list_.begin(), edge_time_list_.end(),
171  [&edge](const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
172  return p.first == edge;
173  })
174  != edge_time_list_.end());
175 }
176 
177 bool
179  const fawkes::NavGraphNode &to) throw()
180 {
181  for (const std::pair<fawkes::NavGraphEdge, fawkes::Time> &te : edge_time_list_){
182  if( (( te.first.from() == from.name()) && (te.first.to() == to.name())) ||
183  ((te.first.to() == from.name()) && (te.first.from() == to.name())))
184  {
185  return true;
186  }
187  }
188  return false;
189 }
190 
191 
192 /** Get list of blocked edges.
193  * @return list of blocked edges
194  */
195 const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &
197 {
198  return edge_time_list_;
199 }
200 
201 
202 /** Remove all edges. */
203 void
205 {
206  if (! edge_time_list_.empty()) {
207  modified_ = true;
208  edge_time_list_.clear();
209  }
210 }
211 
212 
213 } // end of namespace fawkes
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
double in_sec() const
Convet time to seconds.
Definition: time.cpp:232
Constraint that can be queried to check if an edge is blocked.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
NavGraphTimedReservationListEdgeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
Fawkes library namespace.
This is supposed to be the central clock in Fawkes.
Definition: clock.h:34
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
A class for handling time.
Definition: time.h:91
std::string name_
Name of constraint.
virtual bool blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Check if constraint blocks an edge.
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:59
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time > > & edge_time_list() const
Get list of blocked edges.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
Topological graph edge.
Definition: navgraph_edge.h:39
Topological graph node.
Definition: navgraph_node.h:38
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time >> &edges)
Add multiple edges to constraint list.
void add_edge(const fawkes::NavGraphEdge &edge, const fawkes::Time valid_time)
Add a single edge to constraint list.
Interface for logging.
Definition: logger.h:34