24 #include <navgraph/constraints/timed_reservation_list_edge_constraint.h>
61 std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> edge_time_list)
66 constraint_name_ =
name;
79 std::vector<std::pair<NavGraphEdge, fawkes::Time>> erase_list;
80 for (
const std::pair<NavGraphEdge, fawkes::Time> &ec : edge_time_list_) {
81 if (now > ec.second) {
82 erase_list.push_back(ec);
85 for (
const std::pair<NavGraphEdge, fawkes::Time> &ec : erase_list) {
86 edge_time_list_.erase(std::remove(edge_time_list_.begin(), edge_time_list_.end(), ec),
87 edge_time_list_.end());
89 logger_->log_info(
"TimedEdgeConstraint",
90 "Deleted edge '%s_%s' from '%s' because it validity duration ran out",
91 ec.first.from().c_str(),
92 ec.first.to().c_str(),
114 if (valid_time < now) {
115 logger_->
log_warn(
"Timed Edge Constraint",
116 "Constraint '%s' received node with old reservation time='%f' - now='%f'",
123 edge_time_list_.push_back(std::make_pair(edge, valid_time));
124 std::string txt = edge.
from();
135 const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &edges)
137 std::string txt =
"{";
138 for (
const std::pair<NavGraphEdge, fawkes::Time> &ec : edges) {
140 txt += ec.first.from();
142 txt += ec.first.to();
145 txt.erase(txt.length() - 1, 1);
155 std::vector<std::pair<NavGraphEdge, fawkes::Time>>::iterator ec =
156 std::find_if(edge_time_list_.begin(),
157 edge_time_list_.end(),
158 [&edge](
const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
159 return p.first == edge;
162 if (ec != edge_time_list_.end()) {
164 edge_time_list_.erase(ec);
175 return (std::find_if(edge_time_list_.begin(),
176 edge_time_list_.end(),
177 [&edge](
const std::pair<fawkes::NavGraphEdge, fawkes::Time> &p) {
178 return p.first == edge;
180 != edge_time_list_.end());
187 for (
const std::pair<fawkes::NavGraphEdge, fawkes::Time> &te : edge_time_list_) {
188 if (((te.first.from() == from.name()) && (te.first.to() == to.name()))
189 || ((te.first.to() == from.name()) && (te.first.from() == to.name()))) {
199 const std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> &
202 return edge_time_list_;
209 if (!edge_time_list_.empty()) {
211 edge_time_list_.clear();
This is supposed to be the central clock in Fawkes.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Constraint that can be queried to check if an edge is blocked.
std::string name_
Name of constraint.
std::string name()
Get name of constraint.
const std::string & to() const
Get edge target node name.
const std::string & from() const
Get edge originating node name.
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
NavGraphTimedReservationListEdgeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
void add_edge(const fawkes::NavGraphEdge &edge, const fawkes::Time valid_time)
Add a single edge to constraint list.
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time >> &edges)
Add multiple edges to constraint list.
const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time > > & edge_time_list() const
Get list of blocked edges.
virtual ~NavGraphTimedReservationListEdgeConstraint()
Virtual empty destructor.
virtual bool blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Check if constraint blocks an edge.
void clear_edges()
Remove all edges.
A class for handling time.
double in_sec() const
Convet time to seconds.
Fawkes library namespace.