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