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 
30 /** @class NavGraphTimedReservationListEdgeConstraint <navgraph/constraints/timed_reservation_list_edge_constraint.h>
31  * Constraint that holds a list of edges to block with timeouts.
32  * @author Sebastian Reuter
33  * @author Tim Niemueller
34  */
35 
36 /** Constructor.
37  * @param logger logger used for debug logging
38  * @param name name of edge constraint
39  * @param clock time source to evaluate constraint timeouts
40  */
42  Logger * logger,
43  std::string name,
44  fawkes::Clock *clock)
46 {
47  logger_ = logger;
48  clock_ = clock;
49 }
50 
51 /** Constructor.
52  * @param logger logger used for debug logging
53  * @param name name of edge constraint
54  * @param clock time source to evaluate constraint timeouts
55  * @param edge_time_list list of edges with valid_time
56  */
58  Logger * logger,
59  std::string name,
60  fawkes::Clock * clock,
61  std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> edge_time_list)
63 {
64  logger_ = logger;
65  clock_ = clock;
66  constraint_name_ = name;
67  edge_time_list_ = edge_time_list;
68 }
69 
70 /** Virtual empty destructor. */
72 {
73 }
74 
75 bool
77 {
78  fawkes::Time now(clock_);
79  std::vector<std::pair<NavGraphEdge, fawkes::Time>> erase_list;
80  for (const std::pair<NavGraphEdge, fawkes::Time> &ec : edge_time_list_) {
81  if (now > ec.second) {
82  erase_list.push_back(ec);
83  }
84  }
85  for (const std::pair<NavGraphEdge, fawkes::Time> &ec : erase_list) {
86  edge_time_list_.erase(std::remove(edge_time_list_.begin(), edge_time_list_.end(), ec),
87  edge_time_list_.end());
88  modified_ = true;
89  logger_->log_info("TimedEdgeConstraint",
90  "Deleted edge '%s_%s' from '%s' because it validity duration ran out",
91  ec.first.from().c_str(),
92  ec.first.to().c_str(),
93  name_.c_str());
94  }
95 
96  if (modified_) {
97  modified_ = false;
98  return true;
99  } else {
100  return false;
101  }
102 }
103 
104 /** Add a single edge to constraint list.
105  * @param edge edge to add to constraint list
106  * @param valid_time valid time for this edge
107  */
108 void
110  fawkes::Time valid_time)
111 {
112  fawkes::Time now(clock_);
113 
114  if (valid_time < now) {
115  logger_->log_warn("Timed Edge Constraint",
116  "Constraint '%s' received node with old reservation time='%f' - now='%f'",
117  name_.c_str(),
118  valid_time.in_sec(),
119  now.in_sec());
120  }
121  if (!has_edge(edge)) {
122  modified_ = true;
123  edge_time_list_.push_back(std::make_pair(edge, valid_time));
124  std::string txt = edge.from();
125  txt += "_";
126  txt += edge.to();
127  }
128 }
129 
130 /** Add multiple edges to constraint list.
131  * @param edges edges with timeout to add to constraint list
132  */
133 void
135  const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &edges)
136 {
137  std::string txt = "{";
138  for (const std::pair<NavGraphEdge, fawkes::Time> &ec : edges) {
139  add_edge(ec.first, ec.second);
140  txt += ec.first.from();
141  txt += "_";
142  txt += ec.first.to();
143  txt += ",";
144  }
145  txt.erase(txt.length() - 1, 1);
146  txt += "}";
147 }
148 
149 /** Remove a single edge from the constraint list.
150  * @param edge edge to remote
151  */
152 void
154 {
155  std::vector<std::pair<NavGraphEdge, fawkes::Time>>::iterator ec =
156  std::find_if(edge_time_list_.begin(),
157  edge_time_list_.end(),
158  [&edge](const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
159  return p.first == edge;
160  });
161 
162  if (ec != edge_time_list_.end()) {
163  modified_ = true;
164  edge_time_list_.erase(ec);
165  }
166 }
167 
168 /** Check if constraint has a specific edge.
169  * @param edge edge to check
170  * @return true if edge is in list, false otherwise
171  */
172 bool
174 {
175  return (std::find_if(edge_time_list_.begin(),
176  edge_time_list_.end(),
177  [&edge](const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
178  return p.first == edge;
179  })
180  != edge_time_list_.end());
181 }
182 
183 bool
185  const fawkes::NavGraphNode &to) throw()
186 {
187  for (const std::pair<fawkes::NavGraphEdge, fawkes::Time> &te : edge_time_list_) {
188  if (((te.first.from() == from.name()) && (te.first.to() == to.name()))
189  || ((te.first.to() == from.name()) && (te.first.from() == to.name()))) {
190  return true;
191  }
192  }
193  return false;
194 }
195 
196 /** Get list of blocked edges.
197  * @return list of blocked edges
198  */
199 const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &
201 {
202  return edge_time_list_;
203 }
204 
205 /** Remove all edges. */
206 void
208 {
209  if (!edge_time_list_.empty()) {
210  modified_ = true;
211  edge_time_list_.clear();
212  }
213 }
214 
215 } // end of namespace fawkes
fawkes::NavGraphNode
Topological graph node.
Definition: navgraph_node.h:36
fawkes::NavGraphTimedReservationListEdgeConstraint::edge_time_list
const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time > > & edge_time_list() const
Get list of blocked edges.
Definition: timed_reservation_list_edge_constraint.cpp:200
fawkes::NavGraphTimedReservationListEdgeConstraint::blocks
virtual bool blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Check if constraint blocks an edge.
Definition: timed_reservation_list_edge_constraint.cpp:184
fawkes::NavGraphTimedReservationListEdgeConstraint::clear_edges
void clear_edges()
Remove all edges.
Definition: timed_reservation_list_edge_constraint.cpp:207
fawkes::NavGraphEdge
Topological graph edge.
Definition: navgraph_edge.h:38
fawkes::NavGraphTimedReservationListEdgeConstraint::add_edge
void add_edge(const fawkes::NavGraphEdge &edge, const fawkes::Time valid_time)
Add a single edge to constraint list.
Definition: timed_reservation_list_edge_constraint.cpp:109
fawkes::NavGraphEdgeConstraint::name_
std::string name_
Name of constraint.
Definition: edge_constraint.h:49
fawkes::NavGraphTimedReservationListEdgeConstraint::add_edges
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time >> &edges)
Add multiple edges to constraint list.
Definition: timed_reservation_list_edge_constraint.cpp:134
fawkes::NavGraphTimedReservationListEdgeConstraint::remove_edge
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
Definition: timed_reservation_list_edge_constraint.cpp:153
fawkes::NavGraphEdgeConstraint
Constraint that can be queried to check if an edge is blocked.
Definition: edge_constraint.h:35
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes::NavGraphTimedReservationListEdgeConstraint::~NavGraphTimedReservationListEdgeConstraint
virtual ~NavGraphTimedReservationListEdgeConstraint()
Virtual empty destructor.
Definition: timed_reservation_list_edge_constraint.cpp:71
fawkes
Fawkes library namespace.
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::NavGraphEdge::to
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::NavGraphTimedReservationListEdgeConstraint::NavGraphTimedReservationListEdgeConstraint
NavGraphTimedReservationListEdgeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
Definition: timed_reservation_list_edge_constraint.cpp:41
fawkes::NavGraphTimedReservationListEdgeConstraint::compute
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
Definition: timed_reservation_list_edge_constraint.cpp:76
fawkes::NavGraphEdge::from
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
fawkes::Time::in_sec
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
fawkes::NavGraphTimedReservationListEdgeConstraint::has_edge
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
Definition: timed_reservation_list_edge_constraint.cpp:173
fawkes::Clock
This is supposed to be the central clock in Fawkes.
Definition: clock.h:35
fawkes::NavGraphEdgeConstraint::name
std::string name()
Get name of constraint.
Definition: edge_constraint.cpp:77