23 #include "rcsoft_map_graph.h" 25 #include <utils/graph/rcsoft_map_graph.h> 26 #include <core/exception.h> 27 #include <eclipseclass.h> 50 void load(
const char* file )
60 return m_map_graph ?
true :
false;
86 if ( g_map_graph.
loaded() )
88 printf(
"p_map_load(): map already loaded\n" );
93 if ( EC_succeed != EC_arg( 1 ).is_string( &mapfile) )
95 printf(
"p_map_load(): first argument is not a string\n" );
101 g_map_graph.
load( mapfile );
113 p_map_graph_get_node_coords3()
115 if ( !g_map_graph.
loaded() )
117 printf(
"p_map_get_node(): map file not loaded\n" );
122 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
124 printf(
"p_map_get_node(): first argument is not a string\n" );
131 if ( EC_succeed != EC_arg( 2 ).unify( EC_word( (
double) node.
x() ) ) )
133 printf(
"p_map_get_node(): could not bind return value\n" );
138 if ( EC_succeed != EC_arg( 3 ).unify( EC_word( (
double) node.
y() ) ) )
140 printf(
"p_map_get_node(): could not bind return value\n" );
148 p_map_graph_get_node_coords4()
150 if ( EC_succeed != p_map_graph_get_node_coords3() )
154 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
156 printf(
"p_map_get_node(): first argument is not a string\n" );
163 int result = EC_succeed;
164 vector< string >::iterator pit;
169 if ( 0 == strncmp( (*pit).c_str(),
"Orientation", 11 ) )
171 double ori = atof( (*pit).substr( 11 ).c_str() );
172 result = EC_arg( 4 ).unify( EC_word( ori ) );
178 { result = EC_arg( 4 ).unify( EC_atom( (
char*)
"false" ) ); }
180 if ( EC_succeed != result)
188 p_map_graph_get_nodes()
190 if ( !g_map_graph.
loaded() )
192 printf(
"p_map_get_nodes(): map file not loaded\n" );
197 EC_word tail = nil();
199 for ( vector< RCSoftMapNode >::iterator nit = nodes.begin();
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 );
209 if ( EC_succeed != EC_arg( 1 ).unify( tail ) )
211 printf(
"p_map_get_nodes(): could not bind return value\n" );
219 p_map_graph_get_closest_node()
221 if ( !g_map_graph.
loaded() )
223 printf(
"p_map_search_nodes(): map file not loaded\n" );
229 if ( EC_succeed != EC_arg( 1 ).is_double( &x ) )
231 printf(
"p_map_graph_get_closest_node(): no x-coordinate given\n" );
235 if ( EC_succeed != EC_arg( 2 ).is_double( &y ) )
237 printf(
"p_map_graph_get_closest_node(): no y-coordinate given\n" );
245 if ( EC_succeed != EC_arg( 3 ).unify( EC_word( node.
name().c_str() ) ) )
247 printf(
"p_map_graph_get_closest_node(): could not bind return value\n" );
255 p_map_graph_search_nodes()
257 if ( !g_map_graph.
loaded() )
259 printf(
"p_map_search_nodes(): map file not loaded\n" );
264 if ( EC_succeed != EC_arg( 1 ).is_string( &property ) )
266 printf(
"p_map_search_nodes(): no property given\n" );
271 EC_word tail = nil();
273 for ( vector< RCSoftMapNode >::iterator nit = nodes.begin();
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 );
283 if ( EC_succeed != EC_arg( 1 ).unify( tail ) )
285 printf(
"p_map_search_nodes(): could not bind return value\n" );
293 p_map_graph_get_children()
295 if ( !g_map_graph.
loaded() )
297 printf(
"p_map_graph_get_children(): no map file loaded\n" );
302 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
304 printf(
"p_map_graph_get_children(): no node name given\n" );
309 vector< string > children = node.
children();
310 EC_word tail = nil();
311 for ( vector< string >::iterator nit = children.begin();
312 nit != children.end();
315 tail = list( EC_word( (*nit).c_str() ), tail );
318 if ( EC_succeed != EC_arg( 2 ).unify( tail ) )
320 printf(
"p_map_graph_get_children(): cannot bind return value\n" );
fawkes::RCSoftMapNode closest_node(float pos_x, float pos_y, std::string property)
Find node closest to a specified position.
std::vector< fawkes::RCSoftMapNode > nodes()
Get all parsed nodes.
EclExternalRCSoftMapGraph()
Cosntructor.
RCSoft map node representation.
Fawkes library namespace.
float x() const
Get node X coordinate.
bool loaded()
Query status.
fawkes::RCSoftMapNode node(std::string name_or_alias)
Get node with given name or alias.
Base class for exceptions in Fawkes.
std::vector< std::string > & properties()
Get properties of node.
const std::string & name() const
Get node name.
std::vector< fawkes::RCSoftMapNode > search_nodes(std::string property)
Search nodes for specific property.
void print_trace()
Prints trace to stderr.
~EclExternalRCSoftMapGraph()
Destructor.
std::vector< std::string > & children()
Get children of node.
RCSoftMapGraph * map_graph()
Access the RCSoftMapGraph instance.
Wrapper class for using the RCSoftMapGraph in the implementation of the external predicates.
void load(const char *file)
Load map file.
float y() const
Get node Y coordinate.