Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "rcsoft_map_graph.h"
24 
25 #include <utils/graph/rcsoft_map_graph.h>
26 #include <core/exception.h>
27 #include <eclipseclass.h>
28 #include <cstdio>
29 #include <cstring>
30 #include <cstdlib>
31 
32 /** @class fawkes::EclExternalRCSoftMapGraph
33  * Wrapper class for using the RCSoftMapGraph in the implementation of
34  * the external predicates.
35  * @author Daniel Beck
36  */
37 namespace fawkes
38 {
40 {
41 public:
42  /** Cosntructor. */
43  EclExternalRCSoftMapGraph() : m_map_graph(0) {}
44  /** Destructor. */
45  ~EclExternalRCSoftMapGraph() { delete m_map_graph; }
46 
47  /** Load map file.
48  * @param file the map file
49  */
50  void load( const char* file )
51  {
52  m_map_graph = new RCSoftMapGraph( std::string(file) );
53  }
54 
55  /** Query status.
56  * @return true if a map file is loaded; false otherwise
57  */
58  bool loaded()
59  {
60  return m_map_graph ? true : false;
61  }
62 
63  /** Access the RCSoftMapGraph instance.
64  * @return the RCSoftMapGraph instance
65  */
67  {
68  return m_map_graph;
69  }
70 
71 private:
72  RCSoftMapGraph* m_map_graph;
73 
74 };
75 
76 }
77 
78 using namespace std;
79 using namespace fawkes;
80 
81 EclExternalRCSoftMapGraph g_map_graph;
82 
83 int
84 p_map_graph_load()
85 {
86  if ( g_map_graph.loaded() )
87  {
88  printf( "p_map_load(): map already loaded\n" );
89  return EC_fail;
90  }
91 
92  char* mapfile;
93  if ( EC_succeed != EC_arg( 1 ).is_string( &mapfile) )
94  {
95  printf( "p_map_load(): first argument is not a string\n" );
96  return EC_fail;
97  }
98 
99  try
100  {
101  g_map_graph.load( mapfile );
102  }
103  catch ( Exception& e )
104  {
105  e.print_trace();
106  return EC_fail;
107  }
108 
109  return EC_succeed;
110 }
111 
112 int
113 p_map_graph_get_node_coords3()
114 {
115  if ( !g_map_graph.loaded() )
116  {
117  printf( "p_map_get_node(): map file not loaded\n" );
118  return EC_fail;
119  }
120 
121  char* nodename;
122  if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
123  {
124  printf( "p_map_get_node(): first argument is not a string\n" );
125  return EC_fail;
126  }
127 
128  RCSoftMapNode node = g_map_graph.map_graph()->node( string(nodename) );
129 
130  // x-coordinate
131  if ( EC_succeed != EC_arg( 2 ).unify( EC_word( (double) node.x() ) ) )
132  {
133  printf( "p_map_get_node(): could not bind return value\n" );
134  return EC_fail;
135  }
136 
137  // y-coordinate
138  if ( EC_succeed != EC_arg( 3 ).unify( EC_word( (double) node.y() ) ) )
139  {
140  printf( "p_map_get_node(): could not bind return value\n" );
141  return EC_fail;
142  }
143 
144  return EC_succeed;
145 }
146 
147 int
148 p_map_graph_get_node_coords4()
149 {
150  if ( EC_succeed != p_map_graph_get_node_coords3() )
151  { return EC_fail; }
152 
153  char* nodename;
154  if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
155  {
156  printf( "p_map_get_node(): first argument is not a string\n" );
157  return EC_fail;
158  }
159 
160  RCSoftMapNode node = g_map_graph.map_graph()->node( string(nodename) );
161 
162  // check for orientation property
163  int result = EC_succeed;
164  vector< string >::iterator pit;
165  for ( pit = node.properties().begin();
166  pit != node.properties().end();
167  ++pit )
168  {
169  if ( 0 == strncmp( (*pit).c_str(), "Orientation", 11 ) )
170  {
171  double ori = atof( (*pit).substr( 11 ).c_str() );
172  result = EC_arg( 4 ).unify( EC_word( ori ) );
173  break;
174  }
175  }
176 
177  if ( node.properties().end() == pit )
178  { result = EC_arg( 4 ).unify( EC_atom( (char*) "false" ) ); }
179 
180  if ( EC_succeed != result)
181  { return EC_fail; }
182 
183  return EC_succeed;
184 }
185 
186 
187 int
188 p_map_graph_get_nodes()
189 {
190  if ( !g_map_graph.loaded() )
191  {
192  printf( "p_map_get_nodes(): map file not loaded\n" );
193  return EC_fail;
194  }
195 
196  vector< RCSoftMapNode > nodes = g_map_graph.map_graph()->nodes();
197  EC_word tail = nil();
198 
199  for ( vector< RCSoftMapNode >::iterator nit = nodes.begin();
200  nit != nodes.end();
201  ++nit )
202  {
203  EC_word n = list( nit->name().c_str(),
204  list( (double) nit->x(),
205  list( (double) nit->y(), nil() ) ) );
206  tail = list( n, tail );
207  }
208 
209  if ( EC_succeed != EC_arg( 1 ).unify( tail ) )
210  {
211  printf( "p_map_get_nodes(): could not bind return value\n" );
212  return EC_fail;
213  }
214 
215  return EC_succeed;
216 }
217 
218 int
219 p_map_graph_get_closest_node()
220 {
221  if ( !g_map_graph.loaded() )
222  {
223  printf( "p_map_search_nodes(): map file not loaded\n" );
224  return EC_fail;
225  }
226 
227  double x;
228  double y;
229  if ( EC_succeed != EC_arg( 1 ).is_double( &x ) )
230  {
231  printf( "p_map_graph_get_closest_node(): no x-coordinate given\n" );
232  return EC_fail;
233  }
234 
235  if ( EC_succeed != EC_arg( 2 ).is_double( &y ) )
236  {
237  printf( "p_map_graph_get_closest_node(): no y-coordinate given\n" );
238  return EC_fail;
239  }
240 
241  RCSoftMapNode node = g_map_graph.map_graph()->closest_node( (float) x,
242  (float) y,
243  "" );
244 
245  if ( EC_succeed != EC_arg( 3 ).unify( EC_word( node.name().c_str() ) ) )
246  {
247  printf( "p_map_graph_get_closest_node(): could not bind return value\n" );
248  return EC_fail;
249  }
250 
251  return EC_succeed;
252 }
253 
254 int
255 p_map_graph_search_nodes()
256 {
257  if ( !g_map_graph.loaded() )
258  {
259  printf( "p_map_search_nodes(): map file not loaded\n" );
260  return EC_fail;
261  }
262 
263  char* property;
264  if ( EC_succeed != EC_arg( 1 ).is_string( &property ) )
265  {
266  printf( "p_map_search_nodes(): no property given\n" );
267  return EC_fail;
268  }
269 
270  vector< RCSoftMapNode > nodes = g_map_graph.map_graph()->search_nodes( string(property) );
271  EC_word tail = nil();
272 
273  for ( vector< RCSoftMapNode >::iterator nit = nodes.begin();
274  nit != nodes.end();
275  ++nit )
276  {
277  EC_word n = list( nit->name().c_str(),
278  list( (double) nit->x(),
279  list( (double) nit->y(), nil() ) ) );
280  tail = list( n, tail );
281  }
282 
283  if ( EC_succeed != EC_arg( 1 ).unify( tail ) )
284  {
285  printf( "p_map_search_nodes(): could not bind return value\n" );
286  return EC_fail;
287  }
288 
289  return EC_succeed;
290 }
291 
292 int
293 p_map_graph_get_children()
294 {
295  if ( !g_map_graph.loaded() )
296  {
297  printf( "p_map_graph_get_children(): no map file loaded\n" );
298  return EC_fail;
299  }
300 
301  char* nodename;
302  if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
303  {
304  printf( "p_map_graph_get_children(): no node name given\n" );
305  return EC_fail;
306  }
307 
308  RCSoftMapNode node = g_map_graph.map_graph()->node( nodename );
309  vector< string > children = node.children();
310  EC_word tail = nil();
311  for ( vector< string >::iterator nit = children.begin();
312  nit != children.end();
313  ++nit )
314  {
315  tail = list( EC_word( (*nit).c_str() ), tail );
316  }
317 
318  if ( EC_succeed != EC_arg( 2 ).unify( tail ) )
319  {
320  printf( "p_map_graph_get_children(): cannot bind return value\n" );
321  return EC_fail;
322  }
323 
324  return EC_succeed;
325 }