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);
125p_is_map_graph_loaded()
127 if (g_map_graph.
loaded()) {
135p_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");
166p_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) {
197p_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");
222p_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");
252p_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");
283p_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");
Wrapper class for using the RCSoftMapGraph in the implementation of the external predicates.
void load(const char *file)
Load map file.
~EclExternalRCSoftMapGraph()
Destructor.
EclExternalRCSoftMapGraph()
Cosntructor.
NavGraph * map_graph()
Access the NavGraph instance.
bool loaded()
Query status.
Base class for exceptions in Fawkes.
void print_trace() noexcept
Prints trace to stderr.
float x() const
Get X coordinate in global frame.
const std::string & name() const
Get name of node.
float property_as_float(const std::string &prop) const
Get property converted to float.
bool has_property(const std::string &property) const
Check if node has specified property.
float y() const
Get Y coordinate in global frame.
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
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.
std::vector< NavGraphNode > search_nodes(const std::string &property) const
Search nodes for given property.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
NavGraphNode node(const std::string &name) const
Get a specified node.
Fawkes library namespace.
NavGraph * load_yaml_navgraph(std::string filename, bool allow_multi_graph)
Load topological map graph stored in RCSoft format.