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
28namespace 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
75bool
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 */
108void
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 */
133void
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 */
152void
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 */
172bool
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
183bool
185 const fawkes::NavGraphNode &to) noexcept
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 */
199const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &
201{
202 return edge_time_list_;
203}
204
205/** Remove all edges. */
206void
208{
209 if (!edge_time_list_.empty()) {
210 modified_ = true;
211 edge_time_list_.clear();
212 }
213}
214
215} // end of namespace fawkes
This is supposed to be the central clock in Fawkes.
Definition: clock.h:35
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Constraint that can be queried to check if an edge is blocked.
std::string name_
Name of constraint.
std::string name()
Get name of constraint.
Topological graph edge.
Definition: navgraph_edge.h:38
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
Topological graph node.
Definition: navgraph_node.h:36
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time > > &edges)
Add multiple edges to constraint list.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
NavGraphTimedReservationListEdgeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
void add_edge(const fawkes::NavGraphEdge &edge, const fawkes::Time valid_time)
Add a single edge to constraint list.
virtual bool compute(void) noexcept
Perform compuations before graph search and to indicate re-planning.
const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time > > & edge_time_list() const
Get list of blocked edges.
virtual bool blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to) noexcept
Check if constraint blocks an edge.
A class for handling time.
Definition: time.h:93
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
Fawkes library namespace.