22 #include "clips_navgraph_thread.h" 24 #include <navgraph/navgraph.h> 25 #include <navgraph/constraints/static_list_edge_constraint.h> 26 #include <navgraph/constraints/constraint_repo.h> 39 :
Thread(
"ClipsNavGraphThread",
Thread::OPMODE_WAITFORWAKEUP),
57 navgraph->constraint_repo()->register_constraint(edge_constraint_);
64 navgraph->constraint_repo()->unregister_constraint(edge_constraint_->
name());
65 delete edge_constraint_;
67 navgraph->remove_change_listener(
this);
76 envs_[env_name] = clips;
80 clips->batch_evaluate(SRCDIR
"/clips/navgraph.clp");
81 clips_navgraph_load(clips);
83 clips->add_function(
"navgraph-block-edge",
84 sigc::slot<void, std::string, std::string>(
86 sigc::mem_fun(*
this, &ClipsNavGraphThread::clips_navgraph_block_edge),
91 clips->add_function(
"navgraph-unblock-edge",
92 sigc::slot<void, std::string, std::string>(
94 sigc::mem_fun(*
this, &ClipsNavGraphThread::clips_navgraph_unblock_edge),
105 envs_.erase(env_name);
113 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
114 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
116 clips->assert_fact_f(
"(navgraph (name \"%s\"))",
navgraph->name().c_str());
119 std::string props_string;
120 const std::map<std::string, std::string> &properties = n.properties();
121 for (
auto p : properties) {
122 props_string +=
" \"" + p.first +
"\" \"" + p.second +
"\"";
124 clips->assert_fact_f(
"(navgraph-node (name \"%s\") (pos %f %f) (properties %s))",
125 n.name().c_str(), n.x(), n.y(), props_string.c_str());
129 std::string props_string;
130 const std::map<std::string, std::string> &properties = e.properties();
131 for (
auto p : properties) {
132 props_string +=
" \"" + p.first +
"\" \"" + p.second +
"\"";
134 clips->assert_fact_f(
"(navgraph-edge (from \"%s\") (to \"%s\") (directed %s) " 136 e.from().c_str(), e.to().c_str(),
137 e.is_directed() ?
"TRUE" :
"FALSE", props_string.c_str());
143 clips->assert_fact_f(
"(navgraph-load-fail %s)", *(e.
begin()));
149 ClipsNavGraphThread::clips_navgraph_block_edge(std::string env_name,
150 std::string from, std::string to)
152 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
155 if (edge.from() == from && edge.to() == to) {
162 "which does not exist in graph", env_name.c_str(),
163 from.c_str(), to.c_str());
168 ClipsNavGraphThread::clips_navgraph_unblock_edge(std::string env_name,
169 std::string from, std::string to)
171 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
174 if (edge.from() == from && edge.to() == to) {
181 "which does not exist in graph", env_name.c_str(),
182 from.c_str(), to.c_str());
188 for (
auto e : envs_) {
189 logger->
log_debug(
name(),
"Graph changed, re-asserting in environment %s", e.first.c_str());
192 clips->evaluate(
"(navgraph-cleanup)");
193 clips_navgraph_load(clips);
Thread aspect to provide a feature to CLIPS environments.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
virtual void clips_context_destroyed(const std::string &env_name)
Notification that a CLIPS environment has been destroyed.
Fawkes library namespace.
void unlock() const
Unlock object mutex.
ClipsNavGraphThread()
Constructor.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
Base class for exceptions in Fawkes.
std::string name()
Get name of constraint.
iterator begin()
Get iterator for messages.
CLIPS feature maintainer.
virtual void finalize()
Finalize the thread.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual ~ClipsNavGraphThread()
Destructor.
virtual void init()
Initialize the thread.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void graph_changed()
Function called if the graph has been changed.
Constraint that holds a list of edges to block.
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Initialize a CLIPS context to use the provided feature.
void add_edge(const fawkes::NavGraphEdge &edge)
Add a single edge to constraint list.
virtual void loop()
Code to execute in the thread.
void lock() const
Lock access to the encapsulated object.