Fawkes API  Fawkes Development Version
clips_navgraph_thread.cpp
1 
2 /***************************************************************************
3  * clips_navgraph_thread.cpp - NavGraph feature for CLIPS
4  *
5  * Created: Wed Oct 09 19:27:41 2013
6  * Copyright 2006-2013 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "clips_navgraph_thread.h"
23 
24 #include <navgraph/constraints/constraint_repo.h>
25 #include <navgraph/constraints/static_list_edge_constraint.h>
26 #include <navgraph/navgraph.h>
27 
28 #include <clipsmm.h>
29 
30 using namespace fawkes;
31 
32 /** @class ClipsNavGraphThread "clips-protobuf-thread.h"
33  * Provide protobuf functionality to CLIPS environment.
34  * @author Tim Niemueller
35  */
36 
37 /** Constructor. */
39 : Thread("ClipsNavGraphThread", Thread::OPMODE_WAITFORWAKEUP),
40  CLIPSFeature("navgraph"),
41  CLIPSFeatureAspect(this)
42 {
43 }
44 
45 /** Destructor. */
47 {
48 }
49 
50 void
52 {
53  navgraph->add_change_listener(this);
54 
55  edge_constraint_ = new NavGraphStaticListEdgeConstraint("clips");
56  navgraph->constraint_repo()->register_constraint(edge_constraint_);
57 }
58 
59 void
61 {
62  navgraph->constraint_repo()->unregister_constraint(edge_constraint_->name());
63  delete edge_constraint_;
64 
65  navgraph->remove_change_listener(this);
66  envs_.clear();
67 }
68 
69 void
70 ClipsNavGraphThread::clips_context_init(const std::string & env_name,
72 {
73  envs_[env_name] = clips;
74  logger->log_info(name(), "Called to initialize environment %s", env_name.c_str());
75 
76  clips.lock();
77  clips->batch_evaluate(SRCDIR "/clips/navgraph.clp");
78  clips_navgraph_load(clips);
79 
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),
83  env_name)));
84 
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),
88  env_name)));
89 
90  clips.unlock();
91 }
92 
93 void
95 {
96  envs_.erase(env_name);
97  logger->log_info(name(), "Removing environment %s", env_name.c_str());
98 }
99 
100 void
101 ClipsNavGraphThread::clips_navgraph_load(LockPtr<CLIPS::Environment> &clips)
102 {
103  try {
104  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
105  const std::vector<NavGraphEdge> &edges = navgraph->edges();
106 
107  clips->assert_fact_f("(navgraph (name \"%s\"))", navgraph->name().c_str());
108 
109  for (const NavGraphNode &n : nodes) {
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 + "\"";
114  }
115  clips->assert_fact_f("(navgraph-node (name \"%s\") (pos %f %f) (properties %s))",
116  n.name().c_str(),
117  n.x(),
118  n.y(),
119  props_string.c_str());
120  }
121 
122  for (const NavGraphEdge &e : edges) {
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 + "\"";
127  }
128  clips->assert_fact_f("(navgraph-edge (from \"%s\") (to \"%s\") (directed %s) "
129  "(properties %s))",
130  e.from().c_str(),
131  e.to().c_str(),
132  e.is_directed() ? "TRUE" : "FALSE",
133  props_string.c_str());
134  }
135 
136  } catch (Exception &e) {
137  logger->log_warn(name(), "Failed to assert navgraph, exception follows");
138  logger->log_warn(name(), e);
139  clips->assert_fact_f("(navgraph-load-fail %s)", *(e.begin()));
140  }
141 }
142 
143 void
144 ClipsNavGraphThread::clips_navgraph_block_edge(std::string env_name,
145  std::string from,
146  std::string to)
147 {
148  const std::vector<NavGraphEdge> &graph_edges = navgraph->edges();
149 
150  for (const NavGraphEdge &edge : graph_edges) {
151  if (edge.from() == from && edge.to() == to) {
152  edge_constraint_->add_edge(edge);
153  return;
154  }
155  }
156 
157  logger->log_warn(name(),
158  "Environment %s tried to block edge %s--%s, "
159  "which does not exist in graph",
160  env_name.c_str(),
161  from.c_str(),
162  to.c_str());
163 }
164 
165 void
166 ClipsNavGraphThread::clips_navgraph_unblock_edge(std::string env_name,
167  std::string from,
168  std::string to)
169 {
170  const std::vector<NavGraphEdge> &graph_edges = navgraph->edges();
171 
172  for (const NavGraphEdge &edge : graph_edges) {
173  if (edge.from() == from && edge.to() == to) {
174  edge_constraint_->remove_edge(edge);
175  return;
176  }
177  }
178 
179  logger->log_warn(name(),
180  "Environment %s tried to unblock edge %s--%s, "
181  "which does not exist in graph",
182  env_name.c_str(),
183  from.c_str(),
184  to.c_str());
185 }
186 
187 void
189 {
190  for (auto e : envs_) {
191  logger->log_debug(name(), "Graph changed, re-asserting in environment %s", e.first.c_str());
192  fawkes::LockPtr<CLIPS::Environment> &clips = e.second;
193  clips.lock();
194  clips->evaluate("(navgraph-cleanup)");
195  clips_navgraph_load(clips);
196  clips.unlock();
197  }
198 }
199 
200 void
202 {
203 }
virtual void init()
Initialize the thread.
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.
Definition: clips_feature.h:58
CLIPS feature maintainer.
Definition: clips_feature.h:42
Base class for exceptions in Fawkes.
Definition: exception.h:36
iterator begin()
Get iterator for messages.
Definition: exception.cpp:676
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:257
void unlock() const
Unlock object mutex.
Definition: lockptr.h:273
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.
Definition: logging.h:41
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
std::string name()
Get name of constraint.
Topological graph edge.
Definition: navgraph_edge.h:38
Topological graph node.
Definition: navgraph_node.h:36
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.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.