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  */
40 namespace fawkes {
42 {
43 public:
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 
89 private:
90  NavGraph *m_map_graph;
91 };
92 
93 } // namespace fawkes
94 
95 using namespace std;
96 using namespace fawkes;
97 
98 EclExternalRCSoftMapGraph g_map_graph;
99 
100 int
101 p_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 
124 int
125 p_is_map_graph_loaded()
126 {
127  if (g_map_graph.loaded()) {
128  return EC_succeed;
129  } else {
130  return EC_fail;
131  }
132 }
133 
134 int
135 p_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 
165 int
166 p_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 
196 int
197 p_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 
221 int
222 p_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 
251 int
252 p_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 
282 int
283 p_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()
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::vector< std::string > & reachable_nodes() const
Get reachable nodes.
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
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.