22 #include "rcsoft_map_graph.h" 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> 53 void load(
const char* file )
55 std::ifstream inf(file);
56 std::string firstword;
60 if (firstword ==
"%YAML") {
72 return m_map_graph ? true :
false;
98 if ( g_map_graph.
loaded() )
100 printf(
"p_map_load(): map already loaded\n" );
105 if ( EC_succeed != EC_arg( 1 ).is_string( &mapfile) )
107 printf(
"p_map_load(): first argument is not a string\n" );
113 g_map_graph.
load( mapfile );
125 p_map_graph_get_node_coords3()
127 if ( !g_map_graph.
loaded() )
129 printf(
"p_map_get_node(): map file not loaded\n" );
134 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
136 printf(
"p_map_get_node(): first argument is not a string\n" );
143 if ( EC_succeed != EC_arg( 2 ).unify( EC_word( (
double) node.
x() ) ) )
145 printf(
"p_map_get_node(): could not bind return value\n" );
150 if ( EC_succeed != EC_arg( 3 ).unify( EC_word( (
double) node.
y() ) ) )
152 printf(
"p_map_get_node(): could not bind return value\n" );
160 p_map_graph_get_node_coords4()
162 if ( EC_succeed != p_map_graph_get_node_coords3() )
166 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
168 printf(
"p_map_get_node(): first argument is not a string\n" );
175 int result = EC_succeed;
178 result = EC_arg( 4 ).unify( EC_word( ori ) );
180 result = EC_arg( 4 ).unify( EC_atom( (
char*)
"false" ) );
183 if ( EC_succeed != result)
191 p_map_graph_get_nodes()
193 if ( !g_map_graph.
loaded() )
195 printf(
"p_map_get_nodes(): map file not loaded\n" );
200 EC_word tail = nil();
202 for ( vector< NavGraphNode >::iterator nit = nodes.begin();
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 );
212 if ( EC_succeed != EC_arg( 1 ).unify( tail ) )
214 printf(
"p_map_get_nodes(): could not bind return value\n" );
222 p_map_graph_get_closest_node()
224 if ( !g_map_graph.
loaded() )
226 printf(
"p_map_search_nodes(): map file not loaded\n" );
232 if ( EC_succeed != EC_arg( 1 ).is_double( &x ) )
234 printf(
"p_map_graph_get_closest_node(): no x-coordinate given\n" );
238 if ( EC_succeed != EC_arg( 2 ).is_double( &y ) )
240 printf(
"p_map_graph_get_closest_node(): no y-coordinate given\n" );
248 if ( EC_succeed != EC_arg( 3 ).unify( EC_word( node.
name().c_str() ) ) )
250 printf(
"p_map_graph_get_closest_node(): could not bind return value\n" );
258 p_map_graph_search_nodes()
260 if ( !g_map_graph.
loaded() )
262 printf(
"p_map_search_nodes(): map file not loaded\n" );
267 if ( EC_succeed != EC_arg( 1 ).is_string( &property ) )
269 printf(
"p_map_search_nodes(): no property given\n" );
274 EC_word tail = nil();
276 for ( vector< NavGraphNode >::iterator nit = nodes.begin();
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 );
286 if ( EC_succeed != EC_arg( 2 ).unify( tail ) )
288 printf(
"p_map_search_nodes(): could not bind return value\n" );
296 p_map_graph_get_children()
298 if ( !g_map_graph.
loaded() )
300 printf(
"p_map_graph_get_children(): no map file loaded\n" );
305 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) )
307 printf(
"p_map_graph_get_children(): no node name given\n" );
313 EC_word tail = nil();
314 for ( vector< string >::iterator nit = children.begin();
315 nit != children.end();
318 tail = ::list( EC_word( (*nit).c_str() ), tail );
321 if ( EC_succeed != EC_arg( 2 ).unify( tail ) )
323 printf(
"p_map_graph_get_children(): cannot bind return value\n" );
EclExternalRCSoftMapGraph()
Cosntructor.
Fawkes library namespace.
bool has_property(const std::string &property) const
Check if node has specified property.
std::vector< NavGraphNode > search_nodes(const std::string &property) const
Search nodes for given property.
bool loaded()
Query status.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
NavGraph * map_graph()
Access the NavGraph instance.
Base class for exceptions in Fawkes.
NavGraphNode node(const std::string &name) const
Get a specified node.
NavGraph * load_yaml_navgraph(std::string filename)
Load topological map graph stored in RCSoft format.
const std::string & name() const
Get name of node.
float y() const
Get Y coordinate in global frame.
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.
void print_trace()
Prints trace to stderr.
~EclExternalRCSoftMapGraph()
Destructor.
float x() const
Get X coordinate in global frame.
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.