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
30using 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"),
42{
43}
44
45/** Destructor. */
47{
48}
49
50void
52{
53 navgraph->add_change_listener(this);
54
55 edge_constraint_ = new NavGraphStaticListEdgeConstraint("clips");
56 navgraph->constraint_repo()->register_constraint(edge_constraint_);
57}
58
59void
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
69void
70ClipsNavGraphThread::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
93void
95{
96 envs_.erase(env_name);
97 logger->log_info(name(), "Removing environment %s", env_name.c_str());
98}
99
100void
101ClipsNavGraphThread::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
143void
144ClipsNavGraphThread::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
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
165void
166ClipsNavGraphThread::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
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
187void
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
200void
202{
203}
virtual void init()
Initialize the thread.
virtual void graph_changed() noexcept
Function called if the graph has been changed.
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.
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() noexcept
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.