Fawkes API Fawkes Development Version
rcsoft_map_graph.cpp
1
2/***************************************************************************
3 * rcsoft_map_graph.cpp - Access the annotated map.
4 *
5 * Created: Mon Mar 21 17:23:57 2011
6 * Copyright 2011 Daniel Beck
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 "rcsoft_map_graph.h"
23
24#include <core/exception.h>
25#include <navgraph/navgraph.h>
26#include <navgraph/navgraph_node.h>
27#include <navgraph/yaml_navgraph.h>
28
29#include <cstdio>
30#include <cstdlib>
31#include <cstring>
32#include <eclipseclass.h>
33#include <fstream>
34
35/** @class fawkes::EclExternalRCSoftMapGraph
36 * Wrapper class for using the RCSoftMapGraph in the implementation of
37 * the external predicates.
38 * @author Daniel Beck
39 */
40namespace fawkes {
42{
43public:
44 /** Cosntructor. */
45 EclExternalRCSoftMapGraph() : m_map_graph(0)
46 {
47 }
48 /** Destructor. */
50 {
51 }
52
53 /** Load map file.
54 * @param file the map file
55 */
56 void
57 load(const char *file)
58 {
59 std::ifstream inf(file);
60 std::string firstword;
61 inf >> firstword;
62 inf.close();
63
64 if (firstword == "%YAML") {
65 m_map_graph = load_yaml_navgraph(file);
66 } else {
67 throw Exception("Unknown graph format");
68 }
69 }
70
71 /** Query status.
72 * @return true if a map file is loaded; false otherwise
73 */
74 bool
76 {
77 return m_map_graph ? true : false;
78 }
79
80 /** Access the NavGraph instance.
81 * @return the NavGraph instance
82 */
83 NavGraph *
85 {
86 return m_map_graph;
87 }
88
89private:
90 NavGraph *m_map_graph;
91};
92
93} // namespace fawkes
94
95using namespace std;
96using namespace fawkes;
97
99
100int
101p_map_graph_load()
102{
103 if (g_map_graph.loaded()) {
104 printf("p_map_load(): map already loaded\n");
105 return EC_fail;
106 }
107
108 char *mapfile;
109 if (EC_succeed != EC_arg(1).is_string(&mapfile)) {
110 printf("p_map_load(): first argument is not a string\n");
111 return EC_fail;
112 }
113
114 try {
115 g_map_graph.load(mapfile);
116 } catch (Exception &e) {
117 e.print_trace();
118 return EC_fail;
119 }
120
121 return EC_succeed;
122}
123
124int
125p_is_map_graph_loaded()
126{
127 if (g_map_graph.loaded()) {
128 return EC_succeed;
129 } else {
130 return EC_fail;
131 }
132}
133
134int
135p_map_graph_get_node_coords3()
136{
137 if (!g_map_graph.loaded()) {
138 printf("p_map_get_node(): map file not loaded\n");
139 return EC_fail;
140 }
141
142 char *nodename;
143 if (EC_succeed != EC_arg(1).is_string(&nodename)) {
144 printf("p_map_get_node(): first argument is not a string\n");
145 return EC_fail;
146 }
147
148 NavGraphNode node = g_map_graph.map_graph()->node(string(nodename));
149
150 // x-coordinate
151 if (EC_succeed != EC_arg(2).unify(EC_word((double)node.x()))) {
152 printf("p_map_get_node(): could not bind return value\n");
153 return EC_fail;
154 }
155
156 // y-coordinate
157 if (EC_succeed != EC_arg(3).unify(EC_word((double)node.y()))) {
158 printf("p_map_get_node(): could not bind return value\n");
159 return EC_fail;
160 }
161
162 return EC_succeed;
163}
164
165int
166p_map_graph_get_node_coords4()
167{
168 if (EC_succeed != p_map_graph_get_node_coords3()) {
169 return EC_fail;
170 }
171
172 char *nodename;
173 if (EC_succeed != EC_arg(1).is_string(&nodename)) {
174 printf("p_map_get_node(): first argument is not a string\n");
175 return EC_fail;
176 }
177
178 NavGraphNode node = g_map_graph.map_graph()->node(string(nodename));
179
180 // check for orientation property
181 int result = EC_succeed;
182 if (node.has_property("orientation")) {
183 double ori = node.property_as_float("orientation");
184 result = EC_arg(4).unify(EC_word(ori));
185 } else {
186 result = EC_arg(4).unify(EC_atom((char *)"false"));
187 }
188
189 if (EC_succeed != result) {
190 return EC_fail;
191 }
192
193 return EC_succeed;
194}
195
196int
197p_map_graph_get_nodes()
198{
199 if (!g_map_graph.loaded()) {
200 printf("p_map_get_nodes(): map file not loaded\n");
201 return EC_fail;
202 }
203
204 vector<NavGraphNode> nodes = g_map_graph.map_graph()->nodes();
205 EC_word tail = nil();
206
207 for (vector<NavGraphNode>::iterator nit = nodes.begin(); nit != nodes.end(); ++nit) {
208 EC_word n =
209 ::list(nit->name().c_str(), ::list((double)nit->x(), ::list((double)nit->y(), nil())));
210 tail = ::list(n, tail);
211 }
212
213 if (EC_succeed != EC_arg(1).unify(tail)) {
214 printf("p_map_get_nodes(): could not bind return value\n");
215 return EC_fail;
216 }
217
218 return EC_succeed;
219}
220
221int
222p_map_graph_get_closest_node()
223{
224 if (!g_map_graph.loaded()) {
225 printf("p_map_search_nodes(): map file not loaded\n");
226 return EC_fail;
227 }
228
229 double x;
230 double y;
231 if (EC_succeed != EC_arg(1).is_double(&x)) {
232 printf("p_map_graph_get_closest_node(): no x-coordinate given\n");
233 return EC_fail;
234 }
235
236 if (EC_succeed != EC_arg(2).is_double(&y)) {
237 printf("p_map_graph_get_closest_node(): no y-coordinate given\n");
238 return EC_fail;
239 }
240
241 NavGraphNode node = g_map_graph.map_graph()->closest_node((float)x, (float)y, "");
242
243 if (EC_succeed != EC_arg(3).unify(EC_word(node.name().c_str()))) {
244 printf("p_map_graph_get_closest_node(): could not bind return value\n");
245 return EC_fail;
246 }
247
248 return EC_succeed;
249}
250
251int
252p_map_graph_search_nodes()
253{
254 if (!g_map_graph.loaded()) {
255 printf("p_map_search_nodes(): map file not loaded\n");
256 return EC_fail;
257 }
258
259 char *property;
260 if (EC_succeed != EC_arg(1).is_string(&property)) {
261 printf("p_map_search_nodes(): no property given\n");
262 return EC_fail;
263 }
264
265 vector<NavGraphNode> nodes = g_map_graph.map_graph()->search_nodes(string(property));
266 EC_word tail = nil();
267
268 for (vector<NavGraphNode>::iterator nit = nodes.begin(); nit != nodes.end(); ++nit) {
269 EC_word n =
270 ::list(nit->name().c_str(), ::list((double)nit->x(), ::list((double)nit->y(), nil())));
271 tail = ::list(n, tail);
272 }
273
274 if (EC_succeed != EC_arg(2).unify(tail)) {
275 printf("p_map_search_nodes(): could not bind return value\n");
276 return EC_fail;
277 }
278
279 return EC_succeed;
280}
281
282int
283p_map_graph_get_children()
284{
285 if (!g_map_graph.loaded()) {
286 printf("p_map_graph_get_children(): no map file loaded\n");
287 return EC_fail;
288 }
289
290 char *nodename;
291 if (EC_succeed != EC_arg(1).is_string(&nodename)) {
292 printf("p_map_graph_get_children(): no node name given\n");
293 return EC_fail;
294 }
295
296 NavGraphNode node = g_map_graph.map_graph()->node(nodename);
297 vector<string> children = node.reachable_nodes();
298 EC_word tail = nil();
299 for (vector<string>::iterator nit = children.begin(); nit != children.end(); ++nit) {
300 tail = ::list(EC_word((*nit).c_str()), tail);
301 }
302
303 if (EC_succeed != EC_arg(2).unify(tail)) {
304 printf("p_map_graph_get_children(): cannot bind return value\n");
305 return EC_fail;
306 }
307
308 return EC_succeed;
309}
Wrapper class for using the RCSoftMapGraph in the implementation of the external predicates.
void load(const char *file)
Load map file.
NavGraph * map_graph()
Access the NavGraph instance.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_trace() noexcept
Prints trace to stderr.
Definition: exception.cpp:601
Topological graph node.
Definition: navgraph_node.h:36
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
float property_as_float(const std::string &prop) const
Get property converted to float.
bool has_property(const std::string &property) const
Check if node has specified property.
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
Topological map graph.
Definition: navgraph.h:50
NavGraphNode closest_node(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
Definition: navgraph.cpp:186
std::vector< NavGraphNode > search_nodes(const std::string &property) const
Search nodes for given property.
Definition: navgraph.cpp:386
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:133
NavGraphNode node(const std::string &name) const
Get a specified node.
Definition: navgraph.cpp:163
Fawkes library namespace.
NavGraph * load_yaml_navgraph(std::string filename, bool allow_multi_graph)
Load topological map graph stored in RCSoft format.