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/navgraph.h>
25 #include <navgraph/constraints/static_list_edge_constraint.h>
26 #include <navgraph/constraints/constraint_repo.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"), CLIPSFeatureAspect(this)
41 {
42 }
43 
44 
45 /** Destructor. */
47 {
48 }
49 
50 
51 void
53 {
54  navgraph->add_change_listener(this);
55 
56  edge_constraint_ = new NavGraphStaticListEdgeConstraint("clips");
57  navgraph->constraint_repo()->register_constraint(edge_constraint_);
58 }
59 
60 
61 void
63 {
64  navgraph->constraint_repo()->unregister_constraint(edge_constraint_->name());
65  delete edge_constraint_;
66 
67  navgraph->remove_change_listener(this);
68  envs_.clear();
69 }
70 
71 
72 void
73 ClipsNavGraphThread::clips_context_init(const std::string &env_name,
75 {
76  envs_[env_name] = clips;
77  logger->log_info(name(), "Called to initialize environment %s", env_name.c_str());
78 
79  clips.lock();
80  clips->batch_evaluate(SRCDIR"/clips/navgraph.clp");
81  clips_navgraph_load(clips);
82 
83  clips->add_function("navgraph-block-edge",
84  sigc::slot<void, std::string, std::string>(
85  sigc::bind<0>(
86  sigc::mem_fun(*this, &ClipsNavGraphThread::clips_navgraph_block_edge),
87  env_name)
88  )
89  );
90 
91  clips->add_function("navgraph-unblock-edge",
92  sigc::slot<void, std::string, std::string>(
93  sigc::bind<0>(
94  sigc::mem_fun(*this, &ClipsNavGraphThread::clips_navgraph_unblock_edge),
95  env_name)
96  )
97  );
98 
99  clips.unlock();
100 }
101 
102 void
104 {
105  envs_.erase(env_name);
106  logger->log_info(name(), "Removing environment %s", env_name.c_str());
107 }
108 
109 void
110 ClipsNavGraphThread::clips_navgraph_load(LockPtr<CLIPS::Environment> &clips)
111 {
112  try {
113  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
114  const std::vector<NavGraphEdge> &edges = navgraph->edges();
115 
116  clips->assert_fact_f("(navgraph (name \"%s\"))", navgraph->name().c_str());
117 
118  for (const NavGraphNode &n : nodes) {
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 + "\"";
123  }
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());
126  }
127 
128  for (const NavGraphEdge &e : edges) {
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 + "\"";
133  }
134  clips->assert_fact_f("(navgraph-edge (from \"%s\") (to \"%s\") (directed %s) "
135  "(properties %s))",
136  e.from().c_str(), e.to().c_str(),
137  e.is_directed() ? "TRUE" : "FALSE", props_string.c_str());
138  }
139 
140  } catch (Exception &e) {
141  logger->log_warn(name(), "Failed to assert navgraph, exception follows");
142  logger->log_warn(name(), e);
143  clips->assert_fact_f("(navgraph-load-fail %s)", *(e.begin()));
144  }
145 }
146 
147 
148 void
149 ClipsNavGraphThread::clips_navgraph_block_edge(std::string env_name,
150  std::string from, std::string to)
151 {
152  const std::vector<NavGraphEdge> &graph_edges = navgraph->edges();
153 
154  for (const NavGraphEdge &edge : graph_edges) {
155  if (edge.from() == from && edge.to() == to) {
156  edge_constraint_->add_edge(edge);
157  return;
158  }
159  }
160 
161  logger->log_warn(name(), "Environment %s tried to block edge %s--%s, "
162  "which does not exist in graph", env_name.c_str(),
163  from.c_str(), to.c_str());
164 }
165 
166 
167 void
168 ClipsNavGraphThread::clips_navgraph_unblock_edge(std::string env_name,
169  std::string from, std::string to)
170 {
171  const std::vector<NavGraphEdge> &graph_edges = navgraph->edges();
172 
173  for (const NavGraphEdge &edge : graph_edges) {
174  if (edge.from() == from && edge.to() == to) {
175  edge_constraint_->remove_edge(edge);
176  return;
177  }
178  }
179 
180  logger->log_warn(name(), "Environment %s tried to unblock edge %s--%s, "
181  "which does not exist in graph", env_name.c_str(),
182  from.c_str(), to.c_str());
183 }
184 
185 void
187 {
188  for (auto e : envs_) {
189  logger->log_debug(name(), "Graph changed, re-asserting in environment %s", e.first.c_str());
190  fawkes::LockPtr<CLIPS::Environment> &clips = e.second;
191  clips.lock();
192  clips->evaluate("(navgraph-cleanup)");
193  clips_navgraph_load(clips);
194  clips.unlock();
195  }
196 }
197 
198 
199 void
201 {
202 }
Thread aspect to provide a feature to CLIPS environments.
Definition: clips_feature.h:57
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:46
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.
Definition: lockptr.h:255
ClipsNavGraphThread()
Constructor.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Base class for exceptions in Fawkes.
Definition: exception.h:36
std::string name()
Get name of constraint.
iterator begin()
Get iterator for messages.
Definition: exception.cpp:700
CLIPS feature maintainer.
Definition: clips_feature.h:41
virtual void finalize()
Finalize the thread.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual ~ClipsNavGraphThread()
Destructor.
Topological graph edge.
Definition: navgraph_edge.h:39
virtual void init()
Initialize the thread.
Topological graph node.
Definition: navgraph_node.h:38
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.
Definition: lockptr.h:247