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);
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.