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 
27 namespace fawkes{
28 #if 0 /* just to make Emacs auto-indent happy */
29 }
30 #endif
31 
32 /** @class NavGraphTimedReservationListNodeConstraint <navgraph/constraints/timed_reservation_list_node_constraint.h>
33  * Constraint that holds a list of nodes to block with timeouts.
34  * @author Sebastian Reuter
35  * @author Tim Niemueller
36  */
37 
38 /** Constructor.
39  * @param logger logger used for debug logging
40  * @param name name of node constraint
41  * @param clock time source to evaluate constraint timeouts
42  */
44  (Logger *logger, std::string name, 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 node constraint
54  * @param clock time source to evaluate constraint timeouts
55  * @param node_time_list list of nodes with valid_time
56  */
58  (Logger *logger, std::string name, fawkes::Clock *clock,
59  std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> node_time_list)
61 {
62  logger_ = logger;
63  clock_ = clock;
64  constraint_name_ = name;
65  node_time_list_ = node_time_list;
66 }
67 
68 
69 /** Virtual empty destructor. */
71 {
72 }
73 
74 
75 bool
77 {
78 
79  fawkes::Time now(clock_);
80  std::vector<std::pair<NavGraphNode, fawkes::Time>> erase_list;
81  for (const std::pair<NavGraphNode, fawkes::Time> &ec : node_time_list_) {
82  if(now > ec.second){
83  erase_list.push_back(ec);
84  }
85  }
86  for (const std::pair<NavGraphNode, fawkes::Time> &ec : erase_list) {
87  node_time_list_.erase(std::remove(node_time_list_.begin(), node_time_list_.end(), ec),
88  node_time_list_.end());
89  modified_ = true;
90  logger_->log_debug("TimedNodeConstraint",
91  "Deleted node '%s' from '%s' because its validity duration ran out",
92  ec.first.name().c_str(), name_.c_str() );
93  }
94 
95  if (modified_) {
96  modified_ = false;
97  return true;
98  } else {
99  return false;
100  }
101 }
102 
103 
104 /** Add a single node to constraint list.
105  * @param node node to add to constraint list
106  * @param valid_time valid time for this node
107  */
108 void
110  fawkes::Time valid_time)
111 {
112  fawkes::Time now(clock_);
113 
114  if (valid_time < now) {
115  logger_->log_warn("TimedNodeConstraint",
116  "Constraint '%s' received node with old reservation time='%f' - now='%f'",
117  name_.c_str(), valid_time.in_sec(), 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 
127 /** Add multiple nodes to constraint list.
128  * @param timed_nodes nodes with timeout to add to constraint list
129  */
130 void
132  (const std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> &timed_nodes)
133 {
134  std::string txt = "{";
135  for (const std::pair<NavGraphNode, fawkes::Time> &ec : timed_nodes) {
136  add_node(ec.first, ec.second);
137  txt += ec.first.name(); txt += ",";
138  }
139  txt.erase(txt.length()-1,1); txt += "}";
140 }
141 
142 
143 /** Remove a single node from the constraint list.
144  * @param node node to remote
145  */
146 void
148 {
149  std::vector<std::pair<NavGraphNode, fawkes::Time>>::iterator ec
150  = std::find_if(node_time_list_.begin(), node_time_list_.end(),
151  [&node](const std::pair<fawkes::NavGraphNode, fawkes::Time> &p) {
152  return p.first == node;
153  });
154 
155  if (ec != node_time_list_.end()) {
156  modified_ = true;
157  node_time_list_.erase(ec);
158  }
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  */
166 bool
168 {
169  return (std::find_if(node_time_list_.begin(), node_time_list_.end(),
170  [&node](const std::pair<fawkes::NavGraphNode, fawkes::Time> &p) {
171  return p.first == node;
172  })
173  != node_time_list_.end());
174 }
175 
176 bool
178 {
179  for (const std::pair<fawkes::NavGraphNode, fawkes::Time> &te : node_time_list_) {
180  if(te.first.name() == node.name()) {
181  return true;
182  }
183  }
184  return false;
185 }
186 
187 
188 /** Get list of blocked nodes.
189  * @return list of blocked nodes
190  */
191 const std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> &
193 {
194  return node_time_list_;
195 }
196 
197 
198 /** Remove all nodes. */
199 void
201 {
202  if (! node_time_list_.empty()) {
203  modified_ = true;
204  node_time_list_.clear();
205  }
206 }
207 
208 
209 } // end of namespace fawkes
double in_sec() const
Convet time to seconds.
Definition: time.cpp:232
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
void remove_node(const fawkes::NavGraphNode &node)
Remove a single node from the constraint list.
bool has_node(const fawkes::NavGraphNode &node)
Check if constraint has a specific node.
const std::vector< std::pair< fawkes::NavGraphNode, fawkes::Time > > & node_time_list() const
Get list of blocked nodes.
Fawkes library namespace.
This is supposed to be the central clock in Fawkes.
Definition: clock.h:34
A class for handling time.
Definition: time.h:91
NavGraphTimedReservationListNodeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:49
virtual bool blocks(const fawkes::NavGraphNode &node)
Check if constraint blocks a node.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void add_node(const fawkes::NavGraphNode &node, const fawkes::Time valid_time)
Add a single node to constraint list.
Topological graph node.
Definition: navgraph_node.h:38
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
std::string name_
Name of constraint.
void add_nodes(const std::vector< std::pair< fawkes::NavGraphNode, fawkes::Time >> &timed_nodes)
Add multiple nodes to constraint list.
Constraint that can be queried to check if a node is blocked.
Interface for logging.
Definition: logger.h:34