22 #include "clips_navgraph_thread.h"
24 #include <navgraph/constraints/constraint_repo.h>
25 #include <navgraph/constraints/static_list_edge_constraint.h>
26 #include <navgraph/navgraph.h>
39 :
Thread(
"ClipsNavGraphThread",
Thread::OPMODE_WAITFORWAKEUP),
56 navgraph->constraint_repo()->register_constraint(edge_constraint_);
62 navgraph->constraint_repo()->unregister_constraint(edge_constraint_->
name());
63 delete edge_constraint_;
65 navgraph->remove_change_listener(
this);
73 envs_[env_name] = clips;
77 clips->batch_evaluate(SRCDIR
"/clips/navgraph.clp");
78 clips_navgraph_load(clips);
80 clips->add_function(
"navgraph-block-edge",
81 sigc::slot<void, std::string, std::string>(sigc::bind<0>(
82 sigc::mem_fun(*
this, &ClipsNavGraphThread::clips_navgraph_block_edge),
85 clips->add_function(
"navgraph-unblock-edge",
86 sigc::slot<void, std::string, std::string>(sigc::bind<0>(
87 sigc::mem_fun(*
this, &ClipsNavGraphThread::clips_navgraph_unblock_edge),
96 envs_.erase(env_name);
104 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
105 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
107 clips->assert_fact_f(
"(navgraph (name \"%s\"))",
navgraph->name().c_str());
110 std::string props_string;
111 const std::map<std::string, std::string> &properties = n.properties();
112 for (
auto p : properties) {
113 props_string +=
" \"" + p.first +
"\" \"" + p.second +
"\"";
115 clips->assert_fact_f(
"(navgraph-node (name \"%s\") (pos %f %f) (properties %s))",
119 props_string.c_str());
123 std::string props_string;
124 const std::map<std::string, std::string> &properties = e.properties();
125 for (
auto p : properties) {
126 props_string +=
" \"" + p.first +
"\" \"" + p.second +
"\"";
128 clips->assert_fact_f(
"(navgraph-edge (from \"%s\") (to \"%s\") (directed %s) "
132 e.is_directed() ?
"TRUE" :
"FALSE",
133 props_string.c_str());
139 clips->assert_fact_f(
"(navgraph-load-fail %s)", *(e.
begin()));
144 ClipsNavGraphThread::clips_navgraph_block_edge(std::string env_name,
148 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
151 if (edge.from() == from && edge.to() == to) {
158 "Environment %s tried to block edge %s--%s, "
159 "which does not exist in graph",
166 ClipsNavGraphThread::clips_navgraph_unblock_edge(std::string env_name,
170 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
173 if (edge.from() == from && edge.to() == to) {
180 "Environment %s tried to unblock edge %s--%s, "
181 "which does not exist in graph",
190 for (
auto e : envs_) {
191 logger->
log_debug(
name(),
"Graph changed, re-asserting in environment %s", e.first.c_str());
194 clips->evaluate(
"(navgraph-cleanup)");
195 clips_navgraph_load(clips);
virtual void init()
Initialize the thread.
ClipsNavGraphThread()
Constructor.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Initialize a CLIPS context to use the provided feature.
virtual void clips_context_destroyed(const std::string &env_name)
Notification that a CLIPS environment has been destroyed.
virtual ~ClipsNavGraphThread()
Destructor.
virtual void graph_changed()
Function called if the graph has been changed.
Thread aspect to provide a feature to CLIPS environments.
CLIPS feature maintainer.
Base class for exceptions in Fawkes.
iterator begin()
Get iterator for messages.
void lock() const
Lock access to the encapsulated object.
void unlock() const
Unlock object mutex.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
std::string name()
Get name of constraint.
Constraint that holds a list of edges to block.
void add_edge(const fawkes::NavGraphEdge &edge)
Add a single edge to constraint list.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.