diff --git a/nav2_route/README.md b/nav2_route/README.md index 14a9dd3dbbb..589ddcad72a 100644 --- a/nav2_route/README.md +++ b/nav2_route/README.md @@ -75,7 +75,9 @@ Otherwise, the Node, Edge, and Operations may contain other arbitrary applicatio These can be primitive types (float, int, string, etc), vector types (e.g. a polygon or other vector of information), or even contain information nested under namespaces - whereas a metadata object may exist as a key's value within `metadata`. While GeoJSON is not YAML-based, the following YAML file is provided as a more human-readable example for illustration of the conventions above. -Usable real graph file demos can be found in the `graphs/` directory. +Usable real graph file demos can be found in the `graphs/` directory. However, the +`sample_graph.geojson` in the `graph/` directory exists to show the different API for options +for operations, metadata, recursive metadata and vectors. ``` example_graph.yaml diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson index 680116d94c1..0e6f8bcc99a 100644 --- a/nav2_route/graphs/aws_graph.geojson +++ b/nav2_route/graphs/aws_graph.geojson @@ -1,6 +1,6 @@ { "type": "FeatureCollection", -"name": "nodes", +"name": "graph", "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, "date_generated": "Wed Feb 22 05:41:45 PM EST 2023", "features": [ diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson new file mode 100644 index 00000000000..0d1454effd1 --- /dev/null +++ b/nav2_route/graphs/sample_graph.geojson @@ -0,0 +1,121 @@ +{ + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Tue Feb 28 07:48:03 PM EST 2023", + "features": [ + { + "type": "Feature", + "properties": + { + "id": 0, + "frame": "map", + "metadata": + { + "region": + { + "x_values": [1.0, -1.0, -1.0, 1.0], + "y_values": [1.0, 1.0, -1.0, -1.0], + "properties": { + "class_type": "living_room", + "number_of_lights": 10 + } + } + }, + "operations": + { + "stop": + { + "type": "stop", + "trigger": "NODE", + "metadata": + { + "wait_for": 5.0 + } + } + } + }, + "geometry": + { + "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, + { + "type": "Feature", + "properties": + { + "id": 9, + "startid": 0, + "endid": 1, + "overridable": false, + "cost": 10.0, + "metadata": + { + "speed_limit": 0.85, + "aisle_number": 14 + }, + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_ENTER", + "metadata": + { + "door_id": 54, + "service_name": "open_door" + } + }, + "take_picture": + { + "type": "take_picture", + "trigger": "ON_EXIT", + "metadata": + { + "type": "jpg", + "resolution": [1080, 720] + } + } + } + }, + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + }, + { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } + + ] +} diff --git a/nav2_route/test/test_geojson_graph_file_loader.cpp b/nav2_route/test/test_geojson_graph_file_loader.cpp index 43f7dc7e94c..4a02f709564 100644 --- a/nav2_route/test/test_geojson_graph_file_loader.cpp +++ b/nav2_route/test/test_geojson_graph_file_loader.cpp @@ -16,7 +16,9 @@ #include #include #include +#include +#include "nav2_util/node_utils.hpp" #include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" class RclCppFixture @@ -326,3 +328,34 @@ TEST(GeoJsonGraphFileLoader, simple_graph) EXPECT_TRUE(graph[0].neighbors[0].edge_cost.overridable); EXPECT_EQ(graph[0].neighbors[0].edge_cost.cost, 0.0f); } + +TEST(GeoJsonGraphFileLoader, sample_graph) +{ + auto file_path = ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/sample_graph.geojson"; + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + + Metadata region; + region = graph[0].metadata.getValue("region", region); + EXPECT_EQ(region.data.size(), 3u); + + std::vector x_values; + x_values = region.getValue("x_values", x_values); + EXPECT_EQ(x_values.size(), 4u); + + EXPECT_EQ(graph[0].neighbors[0].edge_cost.cost, 10.0f); + EXPECT_EQ(graph[0].neighbors[0].edge_cost.overridable, false); + + auto & operations = graph[0].neighbors[0].operations; + + EXPECT_EQ(operations[0].type, "open_door"); + + std::string type; + type = operations[1].metadata.getValue("type", type); + EXPECT_EQ(type, "jpg"); +} diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp index b05ad8abcd7..3381c791d17 100644 --- a/nav2_route/test/test_graph_loader.cpp +++ b/nav2_route/test/test_graph_loader.cpp @@ -59,7 +59,7 @@ TEST(GraphLoader, test_api) nav2_util::declare_parameter_if_not_declared( node, "graph_filepath", rclcpp::ParameterValue( ament_index_cpp::get_package_share_directory("nav2_route") + - "/graphs/geojson/aws_graph.geojson")); + "/graphs/aws_graph.geojson")); GraphLoader graph_loader(node, tf, frame);