22 #include "rcsoft_map_graph.h"
24 #include <core/exception.h>
25 #include <navgraph/navgraph.h>
26 #include <navgraph/navgraph_node.h>
27 #include <navgraph/yaml_navgraph.h>
32 #include <eclipseclass.h>
59 std::ifstream inf(file);
60 std::string firstword;
64 if (firstword ==
"%YAML") {
77 return m_map_graph ? true :
false;
103 if (g_map_graph.
loaded()) {
104 printf(
"p_map_load(): map already loaded\n");
109 if (EC_succeed != EC_arg(1).is_string(&mapfile)) {
110 printf(
"p_map_load(): first argument is not a string\n");
115 g_map_graph.
load(mapfile);
125 p_is_map_graph_loaded()
127 if (g_map_graph.
loaded()) {
135 p_map_graph_get_node_coords3()
137 if (!g_map_graph.
loaded()) {
138 printf(
"p_map_get_node(): map file not loaded\n");
143 if (EC_succeed != EC_arg(1).is_string(&nodename)) {
144 printf(
"p_map_get_node(): first argument is not a string\n");
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");
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");
166 p_map_graph_get_node_coords4()
168 if (EC_succeed != p_map_graph_get_node_coords3()) {
173 if (EC_succeed != EC_arg(1).is_string(&nodename)) {
174 printf(
"p_map_get_node(): first argument is not a string\n");
181 int result = EC_succeed;
184 result = EC_arg(4).unify(EC_word(ori));
186 result = EC_arg(4).unify(EC_atom((
char *)
"false"));
189 if (EC_succeed != result) {
197 p_map_graph_get_nodes()
199 if (!g_map_graph.
loaded()) {
200 printf(
"p_map_get_nodes(): map file not loaded\n");
205 EC_word tail = nil();
207 for (vector<NavGraphNode>::iterator nit = nodes.begin(); nit != nodes.end(); ++nit) {
209 ::list(nit->name().c_str(), ::list((
double)nit->x(), ::list((
double)nit->y(), nil())));
210 tail = ::list(n, tail);
213 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()) {
225 printf(
"p_map_search_nodes(): map file not loaded\n");
231 if (EC_succeed != EC_arg(1).is_double(&x)) {
232 printf(
"p_map_graph_get_closest_node(): no x-coordinate given\n");
236 if (EC_succeed != EC_arg(2).is_double(&y)) {
237 printf(
"p_map_graph_get_closest_node(): no y-coordinate given\n");
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");
252 p_map_graph_search_nodes()
254 if (!g_map_graph.
loaded()) {
255 printf(
"p_map_search_nodes(): map file not loaded\n");
260 if (EC_succeed != EC_arg(1).is_string(&property)) {
261 printf(
"p_map_search_nodes(): no property given\n");
266 EC_word tail = nil();
268 for (vector<NavGraphNode>::iterator nit = nodes.begin(); nit != nodes.end(); ++nit) {
270 ::list(nit->name().c_str(), ::list((
double)nit->x(), ::list((
double)nit->y(), nil())));
271 tail = ::list(n, tail);
274 if (EC_succeed != EC_arg(2).unify(tail)) {
275 printf(
"p_map_search_nodes(): could not bind return value\n");
283 p_map_graph_get_children()
285 if (!g_map_graph.
loaded()) {
286 printf(
"p_map_graph_get_children(): no map file loaded\n");
291 if (EC_succeed != EC_arg(1).is_string(&nodename)) {
292 printf(
"p_map_graph_get_children(): no node name given\n");
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);
303 if (EC_succeed != EC_arg(2).unify(tail)) {
304 printf(
"p_map_graph_get_children(): cannot bind return value\n");