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