23 #include <navgraph/constraints/timed_reservation_list_node_constraint.h> 43 NavGraphTimedReservationListNodeConstraint::NavGraphTimedReservationListNodeConstraint
57 NavGraphTimedReservationListNodeConstraint::NavGraphTimedReservationListNodeConstraint
59 std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> node_time_list)
64 constraint_name_ = name;
65 node_time_list_ = node_time_list;
70 NavGraphTimedReservationListNodeConstraint::~NavGraphTimedReservationListNodeConstraint()
76 NavGraphTimedReservationListNodeConstraint::compute(
void)
throw()
80 std::vector<std::pair<NavGraphNode, fawkes::Time>> erase_list;
81 for (
const std::pair<NavGraphNode, fawkes::Time> &ec : node_time_list_) {
83 erase_list.push_back(ec);
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());
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() );
114 if (valid_time < now) {
115 logger_->log_warn(
"TimedNodeConstraint",
116 "Constraint '%s' received node with old reservation time='%f' - now='%f'",
119 if (! has_node(node)) {
121 node_time_list_.push_back(std::make_pair(node, valid_time));
122 std::string txt = node.
name();
131 NavGraphTimedReservationListNodeConstraint::add_nodes
132 (
const std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> &timed_nodes)
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 +=
",";
139 txt.erase(txt.length()-1,1); txt +=
"}";
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;
155 if (ec != node_time_list_.end()) {
157 node_time_list_.erase(ec);
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;
173 != node_time_list_.end());
179 for (
const std::pair<fawkes::NavGraphNode, fawkes::Time> &te : node_time_list_) {
180 if(te.first.name() == node.name()) {
191 const std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> &
192 NavGraphTimedReservationListNodeConstraint::node_time_list()
const 194 return node_time_list_;
200 NavGraphTimedReservationListNodeConstraint::clear_nodes()
202 if (! node_time_list_.empty()) {
204 node_time_list_.clear();
double in_sec() const
Convet time to seconds.
Fawkes library namespace.
This is supposed to be the central clock in Fawkes.
A class for handling time.
const std::string & name() const
Get name of node.
Constraint that can be queried to check if a node is blocked.