24 #include <navgraph/constraints/timed_reservation_list_edge_constraint.h> 44 NavGraphTimedReservationListEdgeConstraint::NavGraphTimedReservationListEdgeConstraint
58 NavGraphTimedReservationListEdgeConstraint::NavGraphTimedReservationListEdgeConstraint
60 std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> edge_time_list)
65 constraint_name_ = name;
66 edge_time_list_ = edge_time_list;
71 NavGraphTimedReservationListEdgeConstraint::~NavGraphTimedReservationListEdgeConstraint()
77 NavGraphTimedReservationListEdgeConstraint::compute(
void)
throw()
81 std::vector<std::pair<NavGraphEdge, fawkes::Time>> erase_list;
82 for (
const std::pair<NavGraphEdge, fawkes::Time> &ec : edge_time_list_) {
84 erase_list.push_back(ec);
87 for (
const std::pair<NavGraphEdge, fawkes::Time> &ec : erase_list) {
88 edge_time_list_.erase(std::remove(edge_time_list_.begin(), edge_time_list_.end(), ec),
89 edge_time_list_.end());
91 logger_->log_info(
"TimedEdgeConstraint",
92 "Deleted edge '%s_%s' from '%s' because it validity duration ran out",
93 ec.first.from().c_str(),ec.first.to().c_str(), name_.c_str() );
115 if (valid_time < now) {
116 logger_->log_warn(
"Timed Edge Constraint",
117 "Constraint '%s' received node with old reservation time='%f' - now='%f'",
120 if (! has_edge(edge)) {
122 edge_time_list_.push_back(std::make_pair(edge, valid_time));
123 std::string txt = edge.
from(); txt +=
"_"; txt+=edge.
to();
132 NavGraphTimedReservationListEdgeConstraint::add_edges(
133 const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &edges)
135 std::string txt =
"{";
136 for (
const std::pair<NavGraphEdge, fawkes::Time> &ec : edges) {
137 add_edge(ec.first, ec.second);
138 txt += ec.first.from(); txt +=
"_"; txt += ec.first.to(); txt +=
",";
140 txt.erase(txt.length()-1,1); txt +=
"}";
150 std::vector<std::pair<NavGraphEdge, fawkes::Time>>::iterator ec
151 = std::find_if(edge_time_list_.begin(), edge_time_list_.end(),
152 [&edge](
const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
153 return p.first == edge;
156 if (ec != edge_time_list_.end()) {
158 edge_time_list_.erase(ec);
170 return (std::find_if(edge_time_list_.begin(), edge_time_list_.end(),
171 [&edge](
const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
172 return p.first == edge;
174 != edge_time_list_.end());
181 for (
const std::pair<fawkes::NavGraphEdge, fawkes::Time> &te : edge_time_list_){
182 if( (( te.first.from() == from.name()) && (te.first.to() == to.name())) ||
183 ((te.first.to() == from.name()) && (te.first.from() == to.name())))
195 const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &
196 NavGraphTimedReservationListEdgeConstraint::edge_time_list()
const 198 return edge_time_list_;
204 NavGraphTimedReservationListEdgeConstraint::clear_edges()
206 if (! edge_time_list_.empty()) {
208 edge_time_list_.clear();
const std::string & from() const
Get edge originating node name.
double in_sec() const
Convet time to seconds.
Constraint that can be queried to check if an edge is blocked.
Fawkes library namespace.
This is supposed to be the central clock in Fawkes.
A class for handling time.
const std::string & to() const
Get edge target node name.