24 #include <navgraph/constraints/timed_reservation_list_edge_constraint.h> 60 std::vector<std::pair<fawkes::NavGraphEdge, fawkes::Time>> edge_time_list)
65 constraint_name_ = name;
66 edge_time_list_ = edge_time_list;
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'",
122 edge_time_list_.push_back(std::make_pair(edge, valid_time));
123 std::string txt = edge.
from(); txt +=
"_"; txt+=edge.
to();
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) {
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>> &
198 return edge_time_list_;
206 if (! edge_time_list_.empty()) {
208 edge_time_list_.clear();
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
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.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
NavGraphTimedReservationListEdgeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
Fawkes library namespace.
This is supposed to be the central clock in Fawkes.
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
virtual ~NavGraphTimedReservationListEdgeConstraint()
Virtual empty destructor.
A class for handling time.
std::string name_
Name of constraint.
virtual bool blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Check if constraint blocks an edge.
void clear_edges()
Remove all edges.
const std::string & to() const
Get edge target node name.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time > > & edge_time_list() const
Get list of blocked edges.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, fawkes::Time >> &edges)
Add multiple edges to constraint list.
void add_edge(const fawkes::NavGraphEdge &edge, const fawkes::Time valid_time)
Add a single edge to constraint list.