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 
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)
44 : NavGraphNodeConstraint(name)
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 
74 bool
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  */
106 void
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  */
129 void
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  */
146 void
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  */
166 bool
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 
177 bool
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  */
191 const std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> &
193 {
194  return node_time_list_;
195 }
196 
197 /** Remove all nodes. */
198 void
200 {
201  if (!node_time_list_.empty()) {
202  modified_ = true;
203  node_time_list_.clear();
204  }
205 }
206 
207 } // end of namespace fawkes
fawkes::NavGraphNode
Definition: navgraph_node.h:40
fawkes::NavGraphTimedReservationListNodeConstraint::add_node
void add_node(const fawkes::NavGraphNode &node, const fawkes::Time valid_time)
Add a single node to constraint list.
Definition: timed_reservation_list_node_constraint.cpp:114
fawkes::NavGraphTimedReservationListNodeConstraint::NavGraphTimedReservationListNodeConstraint
NavGraphTimedReservationListNodeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
Definition: timed_reservation_list_node_constraint.cpp:47
fawkes::NavGraphTimedReservationListNodeConstraint::blocks
virtual bool blocks(const fawkes::NavGraphNode &node)
Definition: timed_reservation_list_node_constraint.cpp:185
fawkes::NavGraphTimedReservationListNodeConstraint::add_nodes
void add_nodes(const std::vector< std::pair< fawkes::NavGraphNode, fawkes::Time >> &timed_nodes)
Add multiple nodes to constraint list.
Definition: timed_reservation_list_node_constraint.cpp:137
fawkes::NavGraphNode::name
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:60
fawkes::NavGraphTimedReservationListNodeConstraint::has_node
bool has_node(const fawkes::NavGraphNode &node)
Check if constraint has a specific node.
Definition: timed_reservation_list_node_constraint.cpp:174
fawkes::NavGraphTimedReservationListNodeConstraint::compute
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
Definition: timed_reservation_list_node_constraint.cpp:82
fawkes::NavGraphTimedReservationListNodeConstraint::remove_node
void remove_node(const fawkes::NavGraphNode &node)
Remove a single node from the constraint list.
Definition: timed_reservation_list_node_constraint.cpp:154
fawkes::NavGraphNodeConstraint
Definition: node_constraint.h:40
fawkes::NavGraphNodeConstraint::name
std::string name()
Get name of constraint.
Definition: node_constraint.cpp:75
fawkes::Logger
Definition: logger.h:41
fawkes
fawkes::NavGraphNodeConstraint::name_
std::string name_
Definition: node_constraint.h:55
fawkes::NavGraphTimedReservationListNodeConstraint::~NavGraphTimedReservationListNodeConstraint
virtual ~NavGraphTimedReservationListNodeConstraint()
Virtual empty destructor.
Definition: timed_reservation_list_node_constraint.cpp:77
fawkes::NavGraphTimedReservationListNodeConstraint::clear_nodes
void clear_nodes()
Remove all nodes.
Definition: timed_reservation_list_node_constraint.cpp:206
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::Time
Definition: time.h:98
fawkes::NavGraphTimedReservationListNodeConstraint::node_time_list
const std::vector< std::pair< fawkes::NavGraphNode, fawkes::Time > > & node_time_list() const
Get list of blocked nodes.
Definition: timed_reservation_list_node_constraint.cpp:199
fawkes::Time::in_sec
double in_sec() const
Convet time to seconds.
Definition: time.cpp:226
fawkes::Clock
Definition: clock.h:41