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