23 #include <navgraph/constraints/timed_reservation_list_node_constraint.h> 59 std::vector<std::pair<fawkes::NavGraphNode, fawkes::Time>> node_time_list)
64 constraint_name_ = name;
65 node_time_list_ = node_time_list;
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());
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'",
121 node_time_list_.push_back(std::make_pair(node, valid_time));
122 std::string txt = node.
name();
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) {
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>> &
194 return node_time_list_;
202 if (! node_time_list_.empty()) {
204 node_time_list_.clear();
double in_sec() const
Convet time to seconds.
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
void remove_node(const fawkes::NavGraphNode &node)
Remove a single node from the constraint list.
bool has_node(const fawkes::NavGraphNode &node)
Check if constraint has a specific node.
const std::vector< std::pair< fawkes::NavGraphNode, fawkes::Time > > & node_time_list() const
Get list of blocked nodes.
Fawkes library namespace.
This is supposed to be the central clock in Fawkes.
A class for handling time.
NavGraphTimedReservationListNodeConstraint(Logger *logger, std::string constraint_name, fawkes::Clock *clock)
Constructor.
const std::string & name() const
Get name of node.
virtual bool blocks(const fawkes::NavGraphNode &node)
Check if constraint blocks a node.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void add_node(const fawkes::NavGraphNode &node, const fawkes::Time valid_time)
Add a single node to constraint list.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
std::string name_
Name of constraint.
void clear_nodes()
Remove all nodes.
void add_nodes(const std::vector< std::pair< fawkes::NavGraphNode, fawkes::Time >> &timed_nodes)
Add multiple nodes to constraint list.
Constraint that can be queried to check if a node is blocked.
virtual ~NavGraphTimedReservationListNodeConstraint()
Virtual empty destructor.