From 89e90d2035fd251f2bc8818fef981c944764a7fd Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 10 Sep 2021 19:27:09 +0800 Subject: [PATCH 01/27] Add functions to convert Graph between rmf_building_maps and rmf_traffic Signed-off-by: Luca Della Vedova --- rmf_traffic_ros2/CMakeLists.txt | 4 ++ .../include/rmf_traffic_ros2/agv/Graph.hpp | 28 ++++++++ rmf_traffic_ros2/package.xml | 1 + .../src/rmf_traffic_ros2/convert_Graph.cpp | 72 +++++++++++++++++++ 4 files changed, 105 insertions(+) create mode 100644 rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp create mode 100644 rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 31ace0718..7f6d0f142 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -16,6 +16,7 @@ include(GNUInstallDirs) find_package(ament_cmake REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_traffic_msgs REQUIRED) +find_package(rmf_building_map_msgs REQUIRED) find_package(rmf_fleet_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) @@ -120,6 +121,7 @@ target_link_libraries(rmf_traffic_ros2 PUBLIC rmf_traffic::rmf_traffic ${rmf_traffic_msgs_LIBRARIES} + ${rmf_building_map_msgs_LIBRARIES} ${rclcpp_LIBRARIES} yaml-cpp ) @@ -129,6 +131,7 @@ target_include_directories(rmf_traffic_ros2 $ $ ${rmf_traffic_msgs_INCLUDE_DIRS} + ${rmf_building_map_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) @@ -138,6 +141,7 @@ ament_export_dependencies( rmf_traffic rmf_traffic_msgs rmf_fleet_msgs + rmf_building_map_msgs Eigen3 rclcpp yaml-cpp diff --git a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp new file mode 100644 index 000000000..d9abacf72 --- /dev/null +++ b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +namespace rmf_traffic_ros2 { + +//============================================================================== +rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, + int waypoint_offset = 0); + +} // namespace rmf_traffic_ros2 diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index ffba68645..cd750ee9b 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -13,6 +13,7 @@ rmf_utils rmf_traffic rmf_traffic_msgs + rmf_building_map_msgs rmf_fleet_msgs rclcpp yaml-cpp diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp new file mode 100644 index 000000000..9243958ff --- /dev/null +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +namespace rmf_traffic_ros2 { + +//============================================================================== +rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, + int waypoint_offset) +{ + rmf_traffic::agv::Graph graph; + // Iterate over vertices / waypoints + // Graph params are not used for now + for (const auto& vertex : from.vertices) + { + const Eigen::Vector2d location{ + vertex.x, vertex.y}; + auto wp = graph.add_waypoint(from.name, location); + // Add waypoint name if in the message + if (vertex.name.size() > 0 && !graph.add_key(vertex.name, wp.index())) + { + throw std::runtime_error( + "Duplicated waypoint name [" + vertex.name + "]"); + } + for (const auto& param : vertex.params) + { + if (param.name == "is_parking_spot") + wp.set_parking_spot(param.value_bool); + else if (param.name == "is_holding_point") + wp.set_holding_point(param.value_bool); + else if (param.name == "is_passthrough_point") + wp.set_passthrough_point(param.value_bool); + else if (param.name == "is_charger") + wp.set_charger(param.value_bool); + } + } + // Iterate over edges / lanes + for (const auto& edge : from.edges) + { + using Lane = rmf_traffic::agv::Graph::Lane; + using Event = Lane::Event; + // TODO(LDV) Add remaining functionality, lifts, doors, docking points + // events, orientation constraints + rmf_utils::clone_ptr entry_event; + rmf_utils::clone_ptr exit_event; + // Waypoint offset is applied to ensure unique IDs when multiple levels + // are present + const int start_wp = edge.v1_idx + waypoint_offset; + const int end_wp = edge.v2_idx + waypoint_offset; + graph.add_lane({start_wp, entry_event}, {end_wp, exit_event}); + if (edge.edge_type == edge.EDGE_TYPE_BIDIRECTIONAL) + graph.add_lane({end_wp, entry_event}, {start_wp, exit_event}); + } + return graph; +} + +} // namespace rmf_traffic_ros2 From 5acad92c9cacace6f5b4ce9b90238a9d1bc048ff Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 14 Sep 2021 13:13:41 +0800 Subject: [PATCH 02/27] Add unit tests, fix reference Signed-off-by: Luca Della Vedova --- .../include/rmf_traffic_ros2/agv/Graph.hpp | 2 +- .../src/rmf_traffic_ros2/convert_Graph.cpp | 12 +- .../test/unit/test_convert_Graph.cpp | 122 ++++++++++++++++++ 3 files changed, 129 insertions(+), 7 deletions(-) create mode 100644 rmf_traffic_ros2/test/unit/test_convert_Graph.cpp diff --git a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp index d9abacf72..5a67284e2 100644 --- a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp +++ b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp @@ -23,6 +23,6 @@ namespace rmf_traffic_ros2 { //============================================================================== rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, - int waypoint_offset = 0); + int waypoint_offset = 0); } // namespace rmf_traffic_ros2 diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 9243958ff..ded9564a7 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -21,7 +21,7 @@ namespace rmf_traffic_ros2 { //============================================================================== rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, - int waypoint_offset) + int waypoint_offset) { rmf_traffic::agv::Graph graph; // Iterate over vertices / waypoints @@ -30,12 +30,12 @@ rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, { const Eigen::Vector2d location{ vertex.x, vertex.y}; - auto wp = graph.add_waypoint(from.name, location); + auto& wp = graph.add_waypoint(from.name, location); // Add waypoint name if in the message if (vertex.name.size() > 0 && !graph.add_key(vertex.name, wp.index())) { throw std::runtime_error( - "Duplicated waypoint name [" + vertex.name + "]"); + "Duplicated waypoint name [" + vertex.name + "]"); } for (const auto& param : vertex.params) { @@ -54,14 +54,14 @@ rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, { using Lane = rmf_traffic::agv::Graph::Lane; using Event = Lane::Event; - // TODO(LDV) Add remaining functionality, lifts, doors, docking points + // TODO(luca) Add remaining functionality, lifts, doors, docking points // events, orientation constraints rmf_utils::clone_ptr entry_event; rmf_utils::clone_ptr exit_event; // Waypoint offset is applied to ensure unique IDs when multiple levels // are present - const int start_wp = edge.v1_idx + waypoint_offset; - const int end_wp = edge.v2_idx + waypoint_offset; + const std::size_t start_wp = edge.v1_idx + waypoint_offset; + const std::size_t end_wp = edge.v2_idx + waypoint_offset; graph.add_lane({start_wp, entry_event}, {end_wp, exit_event}); if (edge.edge_type == edge.EDGE_TYPE_BIDIRECTIONAL) graph.add_lane({end_wp, entry_event}, {start_wp, exit_event}); diff --git a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp new file mode 100644 index 000000000..23e5d36d3 --- /dev/null +++ b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include + +static auto make_graph_node(const double x, const double y, + const std::string& name, + const std::vector& params) +{ + rmf_building_map_msgs::msg::GraphNode node; + node.x = x; + node.y = y; + node.name = name; + for (const auto& param : params) + node.params.push_back(param); + return node; +} + +static auto make_bool_param(const std::string& name, const bool val) +{ + rmf_building_map_msgs::msg::Param param; + param.name = name; + param.type = param.TYPE_BOOL; + param.value_bool = val; + return param; +} + +SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") +{ + GIVEN("a graph with two nodes") + { + rmf_building_map_msgs::msg::Graph input_msg; + // Populate input graph + input_msg.name = "test_graph"; + rmf_building_map_msgs::msg::GraphNode node; + input_msg.vertices.push_back( + make_graph_node(10, 20, "wp_0", { + make_bool_param("is_parking_spot", true), + make_bool_param("is_holding_point", true) + })); + input_msg.vertices.push_back( + make_graph_node(30, 40, "wp_1", { + make_bool_param("is_passthrough_point", true), + make_bool_param("is_charger", true) + })); + // A node without a key + input_msg.vertices.push_back( + make_graph_node(50, 60, "", {})); + rmf_building_map_msgs::msg::GraphEdge edge; + edge.v1_idx = 0; + edge.v2_idx = 1; + WHEN("the lane is not bidirectional") + { + THEN("graph has only one lane") + { + edge.edge_type = edge.EDGE_TYPE_UNIDIRECTIONAL; + input_msg.edges = {edge}; + auto res = rmf_traffic_ros2::convert(input_msg); + REQUIRE(res.num_lanes() == 1); + auto lane = res.get_lane(0); + CHECK(lane.entry().waypoint_index() == 0); + CHECK(lane.exit().waypoint_index() == 1); + } + } + WHEN("the lane is bidirectional") + { + + THEN("graph has two lanes") + { + edge.edge_type = edge.EDGE_TYPE_BIDIRECTIONAL; + input_msg.edges = {edge}; + auto res = rmf_traffic_ros2::convert(input_msg); + REQUIRE(res.num_lanes() == 2); + auto lane = res.get_lane(0); + CHECK(lane.entry().waypoint_index() == 0); + CHECK(lane.exit().waypoint_index() == 1); + lane = res.get_lane(1); + CHECK(lane.entry().waypoint_index() == 1); + CHECK(lane.exit().waypoint_index() == 0); + } + } + THEN("output graph has two nodes with right properties") + { + auto res = rmf_traffic_ros2::convert(input_msg); + auto keys = res.keys(); + // Only two nodes had a key but three nodes + CHECK(keys.size() == 2); + CHECK(res.num_waypoints() == 3); + // Check the nodes properties + REQUIRE(res.find_waypoint("wp_0") != nullptr); + REQUIRE(res.find_waypoint("wp_1") != nullptr); + REQUIRE(res.find_waypoint("wp_2") == nullptr); + auto wp = res.find_waypoint("wp_0"); + CHECK(wp->is_parking_spot() == true); + CHECK(wp->is_holding_point() == true); + CHECK(wp->is_passthrough_point() == false); + CHECK(wp->is_charger() == false); + wp = res.find_waypoint("wp_1"); + CHECK(wp->is_parking_spot() == false); + CHECK(wp->is_holding_point() == false); + CHECK(wp->is_passthrough_point() == true); + CHECK(wp->is_charger() == true); + } + } +} From 0fd8f1f15aac86b99b1a8c6c60f9db7dcd80fc0b Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 14 Sep 2021 15:21:29 +0800 Subject: [PATCH 03/27] Add dock event Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 16 ++++- .../test/unit/test_convert_Graph.cpp | 68 ++++++++++++------- 2 files changed, 55 insertions(+), 29 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index ded9564a7..70c4fbdfe 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -54,17 +54,27 @@ rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, { using Lane = rmf_traffic::agv::Graph::Lane; using Event = Lane::Event; - // TODO(luca) Add remaining functionality, lifts, doors, docking points - // events, orientation constraints + // TODO(luca) Add lifts, doors, orientation constraints rmf_utils::clone_ptr entry_event; rmf_utils::clone_ptr exit_event; // Waypoint offset is applied to ensure unique IDs when multiple levels // are present const std::size_t start_wp = edge.v1_idx + waypoint_offset; const std::size_t end_wp = edge.v2_idx + waypoint_offset; - graph.add_lane({start_wp, entry_event}, {end_wp, exit_event}); + std::string dock_name; + for (const auto& param : edge.params) + { + if (param.name == "dock_name") + dock_name = param.value_string; + } + // dock_name is only applied to the lane going to the waypoint, not exiting if (edge.edge_type == edge.EDGE_TYPE_BIDIRECTIONAL) graph.add_lane({end_wp, entry_event}, {start_wp, exit_event}); + + const rmf_traffic::Duration duration = std::chrono::seconds(5); + entry_event = Event::make(Lane::Dock(dock_name, duration)); + graph.add_lane({start_wp, entry_event}, + {end_wp, exit_event}); } return graph; } diff --git a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp index 23e5d36d3..717b262fa 100644 --- a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp +++ b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp @@ -42,6 +42,15 @@ static auto make_bool_param(const std::string& name, const bool val) return param; } +static auto make_string_param(const std::string& name, const std::string& val) +{ + rmf_building_map_msgs::msg::Param param; + param.name = name; + param.type = param.TYPE_STRING; + param.value_string = val; + return param; +} + SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") { GIVEN("a graph with two nodes") @@ -53,7 +62,8 @@ SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") input_msg.vertices.push_back( make_graph_node(10, 20, "wp_0", { make_bool_param("is_parking_spot", true), - make_bool_param("is_holding_point", true) + make_bool_param("is_holding_point", true), + make_string_param("dock_name", "dock_1") })); input_msg.vertices.push_back( make_graph_node(30, 40, "wp_1", { @@ -66,36 +76,42 @@ SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") rmf_building_map_msgs::msg::GraphEdge edge; edge.v1_idx = 0; edge.v2_idx = 1; - WHEN("the lane is not bidirectional") + WHEN("the lane is not bidirectional") + { + THEN("graph has only one lane") { - THEN("graph has only one lane") - { - edge.edge_type = edge.EDGE_TYPE_UNIDIRECTIONAL; - input_msg.edges = {edge}; - auto res = rmf_traffic_ros2::convert(input_msg); - REQUIRE(res.num_lanes() == 1); - auto lane = res.get_lane(0); - CHECK(lane.entry().waypoint_index() == 0); - CHECK(lane.exit().waypoint_index() == 1); - } + edge.edge_type = edge.EDGE_TYPE_UNIDIRECTIONAL; + input_msg.edges = {edge}; + auto res = rmf_traffic_ros2::convert(input_msg); + REQUIRE(res.num_lanes() == 1); + auto lane = res.get_lane(0); + CHECK(lane.entry().waypoint_index() == 0); + CHECK(lane.exit().waypoint_index() == 1); } - WHEN("the lane is bidirectional") + } + WHEN("the lane is bidirectional") + { + THEN("graph has two lanes") { + edge.edge_type = edge.EDGE_TYPE_BIDIRECTIONAL; + input_msg.edges = {edge}; + auto res = rmf_traffic_ros2::convert(input_msg); + REQUIRE(res.num_lanes() == 2); + auto lane = res.get_lane(0); + // Expect one to be 1 and one to be 0, hence the XOR + CHECK( + (lane.entry().waypoint_index() ^ lane.exit().waypoint_index()) == 1); + lane = res.get_lane(1); + CHECK( + (lane.entry().waypoint_index() ^ lane.exit().waypoint_index()) == 1); + CHECK(lane.entry().waypoint_index() != + res.get_lane(0).entry().waypoint_index()); - THEN("graph has two lanes") - { - edge.edge_type = edge.EDGE_TYPE_BIDIRECTIONAL; - input_msg.edges = {edge}; - auto res = rmf_traffic_ros2::convert(input_msg); - REQUIRE(res.num_lanes() == 2); - auto lane = res.get_lane(0); - CHECK(lane.entry().waypoint_index() == 0); - CHECK(lane.exit().waypoint_index() == 1); - lane = res.get_lane(1); - CHECK(lane.entry().waypoint_index() == 1); - CHECK(lane.exit().waypoint_index() == 0); - } + // Check for the dock event in the first lane + auto entry_event = lane.entry().event(); + CHECK(entry_event != nullptr); } + } THEN("output graph has two nodes with right properties") { auto res = rmf_traffic_ros2::convert(input_msg); From 1fbfe5deed52bbd500c14d3c9fc5f6ee43855116 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 20 Sep 2021 17:39:55 +0800 Subject: [PATCH 04/27] Fix docking logic Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 3 ++- rmf_traffic_ros2/test/unit/test_convert_Graph.cpp | 12 ++++++++---- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 70c4fbdfe..b654cfea3 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -72,7 +72,8 @@ rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, graph.add_lane({end_wp, entry_event}, {start_wp, exit_event}); const rmf_traffic::Duration duration = std::chrono::seconds(5); - entry_event = Event::make(Lane::Dock(dock_name, duration)); + if (dock_name.size() > 0) + entry_event = Event::make(Lane::Dock(dock_name, duration)); graph.add_lane({start_wp, entry_event}, {end_wp, exit_event}); } diff --git a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp index 717b262fa..2a6c8cba7 100644 --- a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp +++ b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp @@ -62,8 +62,7 @@ SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") input_msg.vertices.push_back( make_graph_node(10, 20, "wp_0", { make_bool_param("is_parking_spot", true), - make_bool_param("is_holding_point", true), - make_string_param("dock_name", "dock_1") + make_bool_param("is_holding_point", true) })); input_msg.vertices.push_back( make_graph_node(30, 40, "wp_1", { @@ -76,6 +75,7 @@ SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") rmf_building_map_msgs::msg::GraphEdge edge; edge.v1_idx = 0; edge.v2_idx = 1; + edge.params.push_back(make_string_param("dock_name", "dock_1")); WHEN("the lane is not bidirectional") { THEN("graph has only one lane") @@ -98,18 +98,22 @@ SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") auto res = rmf_traffic_ros2::convert(input_msg); REQUIRE(res.num_lanes() == 2); auto lane = res.get_lane(0); + int num_events = 0; + if (lane.entry().event() != nullptr) + ++num_events; // Expect one to be 1 and one to be 0, hence the XOR CHECK( (lane.entry().waypoint_index() ^ lane.exit().waypoint_index()) == 1); lane = res.get_lane(1); + if (lane.entry().event() != nullptr) + ++num_events; CHECK( (lane.entry().waypoint_index() ^ lane.exit().waypoint_index()) == 1); CHECK(lane.entry().waypoint_index() != res.get_lane(0).entry().waypoint_index()); // Check for the dock event in the first lane - auto entry_event = lane.entry().event(); - CHECK(entry_event != nullptr); + CHECK(num_events == 1); } } THEN("output graph has two nodes with right properties") From 84a76dd4806c07cd7e564f19aa307e88958968b0 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 21 Sep 2021 08:58:35 +0800 Subject: [PATCH 05/27] Add lane speed limit Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index b654cfea3..ca156155d 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -62,20 +62,28 @@ rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, const std::size_t start_wp = edge.v1_idx + waypoint_offset; const std::size_t end_wp = edge.v2_idx + waypoint_offset; std::string dock_name; + std::optional speed_limit; for (const auto& param : edge.params) { if (param.name == "dock_name") dock_name = param.value_string; + if (param.name == "speed_limit") + speed_limit = param.value_float; } // dock_name is only applied to the lane going to the waypoint, not exiting if (edge.edge_type == edge.EDGE_TYPE_BIDIRECTIONAL) - graph.add_lane({end_wp, entry_event}, {start_wp, exit_event}); + { + auto& lane = graph.add_lane({end_wp, entry_event}, + {start_wp, exit_event}); + lane.properties().speed_limit(speed_limit); + } const rmf_traffic::Duration duration = std::chrono::seconds(5); if (dock_name.size() > 0) entry_event = Event::make(Lane::Dock(dock_name, duration)); - graph.add_lane({start_wp, entry_event}, + auto& lane = graph.add_lane({start_wp, entry_event}, {end_wp, exit_event}); + lane.properties().speed_limit(speed_limit); } return graph; } From 63e21ae49a577647e30401032b227c98cac2fb2f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 6 Oct 2021 17:45:38 +0800 Subject: [PATCH 06/27] WIP towards geopackage graph parsing Signed-off-by: Luca Della Vedova --- rmf_traffic_ros2/CMakeLists.txt | 13 +- .../include/rmf_traffic_ros2/agv/Graph.hpp | 8 +- rmf_traffic_ros2/package.xml | 4 +- .../src/rmf_traffic_ros2/convert_Graph.cpp | 215 ++++++++++++++---- .../test/unit/test_convert_Graph.cpp | 2 + 5 files changed, 188 insertions(+), 54 deletions(-) diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 7f6d0f142..e7907c8a1 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.14) project(rmf_traffic_ros2) @@ -16,9 +16,10 @@ include(GNUInstallDirs) find_package(ament_cmake REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_traffic_msgs REQUIRED) -find_package(rmf_building_map_msgs REQUIRED) +find_package(rmf_site_map_msgs REQUIRED) find_package(rmf_fleet_msgs REQUIRED) find_package(Eigen3 REQUIRED) +find_package(GDAL) find_package(rclcpp REQUIRED) find_package(yaml-cpp REQUIRED) @@ -121,7 +122,8 @@ target_link_libraries(rmf_traffic_ros2 PUBLIC rmf_traffic::rmf_traffic ${rmf_traffic_msgs_LIBRARIES} - ${rmf_building_map_msgs_LIBRARIES} + ${rmf_site_map_msgs_LIBRARIES} + ${GDAL_LIBRARIES} ${rclcpp_LIBRARIES} yaml-cpp ) @@ -131,7 +133,8 @@ target_include_directories(rmf_traffic_ros2 $ $ ${rmf_traffic_msgs_INCLUDE_DIRS} - ${rmf_building_map_msgs_INCLUDE_DIRS} + ${rmf_site_map_msgs_INCLUDE_DIRS} + ${GDAL_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) @@ -141,7 +144,7 @@ ament_export_dependencies( rmf_traffic rmf_traffic_msgs rmf_fleet_msgs - rmf_building_map_msgs + rmf_site_map_msgs Eigen3 rclcpp yaml-cpp diff --git a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp index 5a67284e2..61f7356a0 100644 --- a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp +++ b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp @@ -17,12 +17,18 @@ #include -#include +#include namespace rmf_traffic_ros2 { //============================================================================== +/* rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, int waypoint_offset = 0); +*/ + +//============================================================================== +rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, + int graph_idx = 0); } // namespace rmf_traffic_ros2 diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index cd750ee9b..d7cb95308 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -13,11 +13,13 @@ rmf_utils rmf_traffic rmf_traffic_msgs - rmf_building_map_msgs rmf_fleet_msgs + rmf_site_map_msgs rclcpp yaml-cpp + libgdal-dev + eigen ament_cmake_catch2 diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index ca156155d..65859e204 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -15,77 +15,198 @@ * */ +#include +#include +#include + +#include +#include + #include namespace rmf_traffic_ros2 { +// Usage map[level_idx][truncated_x][truncated_y] = id; +// Truncation is to 1e-3 meters +using CoordsIdxHashMap = std::unordered_map>>; + //============================================================================== -rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, - int waypoint_offset) +rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, + int graph_idx) { + CoordsIdxHashMap idx_map; rmf_traffic::agv::Graph graph; - // Iterate over vertices / waypoints - // Graph params are not used for now - for (const auto& vertex : from.vertices) + // Sqlite3 needs to work on a physical file, write the package to a tmp file + auto filename = std::tmpnam(nullptr); + FILE* fd = fopen(filename, "wb"); + GDALAllRegister(); + // Not supported + if (from.encoding != from.MAP_DATA_GPKG) + return graph; + fwrite(&from.data[0], sizeof(char), from.data.size(), fd); + // Get the filename + printf("Filename is %s\n", filename); + GDALDatasetUniquePtr poDS(GDALDataset::Open(filename, GDAL_OF_VECTOR)); + if( poDS == nullptr ) { - const Eigen::Vector2d location{ - vertex.x, vertex.y}; - auto& wp = graph.add_waypoint(from.name, location); - // Add waypoint name if in the message - if (vertex.name.size() > 0 && !graph.add_key(vertex.name, wp.index())) + printf( "Open failed.\n" ); + exit( 1 ); + } + // Iterate over vertices + auto vertices_layer = poDS->GetLayerByName("vertices"); + while (const auto& feature = vertices_layer->GetNextFeature()) + { + int level_idx = 0; + for (const auto& field : feature) { - throw std::runtime_error( - "Duplicated waypoint name [" + vertex.name + "]"); + std::cout << "Field name is " << field.GetName() << std::endl; + if (strcmp(field.GetName(), "level_idx") == 0) + level_idx = field.GetAsInteger(); + else if (strcmp(field.GetName(), "parameters") == 0) + { + // TODO parse parameters here + std::cout << field.GetIndex() << "," << field.GetName()<< ": " << field.GetAsString() << std::endl; + } } - for (const auto& param : vertex.params) + const auto& name_feat = (*feature)["name"]; + const std::string name(name_feat.GetAsString()); + const auto& point = feature->GetGeometryRef()->toPoint(); + // Flatten geometry to extract + printf("Got feature with name %s\n", name.c_str()); + printf("Coordinates are %.2f %.2f\n", point->getX(), point->getY()); + const Eigen::Vector2d location{ + point->getX(), point->getY()}; + // TODO map name + std::string map_name("test"); + auto& wp = graph.add_waypoint(map_name, location); + if (name.size() > 0 && !graph.add_key(name, wp.index())) { - if (param.name == "is_parking_spot") - wp.set_parking_spot(param.value_bool); - else if (param.name == "is_holding_point") - wp.set_holding_point(param.value_bool); - else if (param.name == "is_passthrough_point") - wp.set_passthrough_point(param.value_bool); - else if (param.name == "is_charger") - wp.set_charger(param.value_bool); + throw std::runtime_error( + "Duplicated waypoint name [" + name + "]"); } + // Round x and y to 1e-3 meters + double rounded_x = std::round(point->getX() * 1000.0) / 1000.0; + double rounded_y = std::round(point->getY() * 1000.0) / 1000.0; + idx_map[level_idx][rounded_x][rounded_y] = wp.index(); } - // Iterate over edges / lanes - for (const auto& edge : from.edges) + // Iterate over edges + auto edges_layer = poDS->GetLayerByName("edges"); + while (const auto& feature = edges_layer->GetNextFeature()) { + int level_idx = 0; + std::optional speed_limit; + std::optional dock_name; + for (const auto& field : feature) + { + std::cout << "Field name is " << field.GetName() << std::endl; + if (strcmp(field.GetName(), "level_idx") == 0) + level_idx = field.GetAsInteger(); + else if (strcmp(field.GetName(), "parameters") == 0) + { + // TODO parse parameters here, graph_idx and speed_limit + std::cout << field.GetIndex() << "," << field.GetName()<< ": " << field.GetAsString() << std::endl; + // Skip if graph_idx is not the equal to the argument + } + } + const auto& lane_feat = feature->GetGeometryRef()->toLineString(); + // Get the points + double x0 = lane_feat->getX(0); + double x1 = lane_feat->getX(1); + double y0 = lane_feat->getY(0); + double y1 = lane_feat->getY(1); + double rounded_x0 = std::round(x0 * 1000.0) / 1000.0; + double rounded_y0 = std::round(y0 * 1000.0) / 1000.0; + double rounded_x1 = std::round(x1 * 1000.0) / 1000.0; + double rounded_y1 = std::round(y1 * 1000.0) / 1000.0; + auto m0_iter = idx_map[level_idx][rounded_x0].find(rounded_y0); + if (m0_iter == idx_map[level_idx][rounded_x0].end()) + continue; + auto m1_iter = idx_map[level_idx][rounded_x1].find(rounded_y1); + if (m1_iter == idx_map[level_idx][rounded_x1].end()) + continue; + // TODO waypoint offset + // Waypoint offset is applied to ensure unique IDs when multiple levels + // are present + const std::size_t start_wp = m0_iter->second; + const std::size_t end_wp = m1_iter->second; + using Lane = rmf_traffic::agv::Graph::Lane; using Event = Lane::Event; // TODO(luca) Add lifts, doors, orientation constraints rmf_utils::clone_ptr entry_event; rmf_utils::clone_ptr exit_event; - // Waypoint offset is applied to ensure unique IDs when multiple levels - // are present - const std::size_t start_wp = edge.v1_idx + waypoint_offset; - const std::size_t end_wp = edge.v2_idx + waypoint_offset; - std::string dock_name; - std::optional speed_limit; - for (const auto& param : edge.params) - { - if (param.name == "dock_name") - dock_name = param.value_string; - if (param.name == "speed_limit") - speed_limit = param.value_float; - } // dock_name is only applied to the lane going to the waypoint, not exiting - if (edge.edge_type == edge.EDGE_TYPE_BIDIRECTIONAL) - { - auto& lane = graph.add_lane({end_wp, entry_event}, - {start_wp, exit_event}); - lane.properties().speed_limit(speed_limit); - } - - const rmf_traffic::Duration duration = std::chrono::seconds(5); - if (dock_name.size() > 0) - entry_event = Event::make(Lane::Dock(dock_name, duration)); + // TODO bidirectional edges + // TODO docking auto& lane = graph.add_lane({start_wp, entry_event}, {end_wp, exit_event}); lane.properties().speed_limit(speed_limit); } - return graph; +/* +// Iterate over vertices / waypoints +// Graph params are not used for now +for (const auto& vertex : from.vertices) +{ + const Eigen::Vector2d location{ + vertex.x, vertex.y}; + auto& wp = graph.add_waypoint(from.name, location); + // Add waypoint name if in the message + if (vertex.name.size() > 0 && !graph.add_key(vertex.name, wp.index())) + { + throw std::runtime_error( + "Duplicated waypoint name [" + vertex.name + "]"); + } + for (const auto& param : vertex.params) + { + if (param.name == "is_parking_spot") + wp.set_parking_spot(param.value_bool); + else if (param.name == "is_holding_point") + wp.set_holding_point(param.value_bool); + else if (param.name == "is_passthrough_point") + wp.set_passthrough_point(param.value_bool); + else if (param.name == "is_charger") + wp.set_charger(param.value_bool); + } +} +// Iterate over edges / lanes +for (const auto& edge : from.edges) +{ + using Lane = rmf_traffic::agv::Graph::Lane; + using Event = Lane::Event; + // TODO(luca) Add lifts, doors, orientation constraints + rmf_utils::clone_ptr entry_event; + rmf_utils::clone_ptr exit_event; + // Waypoint offset is applied to ensure unique IDs when multiple levels + // are present + const std::size_t start_wp = edge.v1_idx + waypoint_offset; + const std::size_t end_wp = edge.v2_idx + waypoint_offset; + std::string dock_name; + std::optional speed_limit; + for (const auto& param : edge.params) + { + if (param.name == "dock_name") + dock_name = param.value_string; + if (param.name == "speed_limit") + speed_limit = param.value_float; + } + // dock_name is only applied to the lane going to the waypoint, not exiting + if (edge.edge_type == edge.EDGE_TYPE_BIDIRECTIONAL) + { + auto& lane = graph.add_lane({end_wp, entry_event}, + {start_wp, exit_event}); + lane.properties().speed_limit(speed_limit); + } + + const rmf_traffic::Duration duration = std::chrono::seconds(5); + if (dock_name.size() > 0) + entry_event = Event::make(Lane::Dock(dock_name, duration)); + auto& lane = graph.add_lane({start_wp, entry_event}, + {end_wp, exit_event}); + lane.properties().speed_limit(speed_limit); +} +*/ +return graph; } } // namespace rmf_traffic_ros2 diff --git a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp index 2a6c8cba7..d70ca7235 100644 --- a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp +++ b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp @@ -20,6 +20,7 @@ #include #include +/* static auto make_graph_node(const double x, const double y, const std::string& name, const std::vector& params) @@ -140,3 +141,4 @@ SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") } } } +*/ From 484c8a6c92f5f14e3cd63a98a5764d14516e1fd5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 6 Oct 2021 18:03:49 +0800 Subject: [PATCH 07/27] Remove debug printouts Signed-off-by: Luca Della Vedova --- rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 65859e204..e8d308421 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -60,21 +60,17 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, int level_idx = 0; for (const auto& field : feature) { - std::cout << "Field name is " << field.GetName() << std::endl; if (strcmp(field.GetName(), "level_idx") == 0) level_idx = field.GetAsInteger(); else if (strcmp(field.GetName(), "parameters") == 0) { // TODO parse parameters here - std::cout << field.GetIndex() << "," << field.GetName()<< ": " << field.GetAsString() << std::endl; } } const auto& name_feat = (*feature)["name"]; const std::string name(name_feat.GetAsString()); const auto& point = feature->GetGeometryRef()->toPoint(); // Flatten geometry to extract - printf("Got feature with name %s\n", name.c_str()); - printf("Coordinates are %.2f %.2f\n", point->getX(), point->getY()); const Eigen::Vector2d location{ point->getX(), point->getY()}; // TODO map name @@ -99,13 +95,11 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, std::optional dock_name; for (const auto& field : feature) { - std::cout << "Field name is " << field.GetName() << std::endl; if (strcmp(field.GetName(), "level_idx") == 0) level_idx = field.GetAsInteger(); else if (strcmp(field.GetName(), "parameters") == 0) { // TODO parse parameters here, graph_idx and speed_limit - std::cout << field.GetIndex() << "," << field.GetName()<< ": " << field.GetAsString() << std::endl; // Skip if graph_idx is not the equal to the argument } } From 745e307fcc7689a42fbaae4cd2e4373ccfb03ed2 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 11 Oct 2021 09:49:15 +0800 Subject: [PATCH 08/27] Add json dependency Signed-off-by: Luca Della Vedova --- rmf_traffic_ros2/CMakeLists.txt | 3 +++ .../include/rmf_traffic_ros2/agv/Graph.hpp | 2 +- rmf_traffic_ros2/package.xml | 1 + .../src/rmf_traffic_ros2/convert_Graph.cpp | 16 ++++++++-------- 4 files changed, 13 insertions(+), 9 deletions(-) diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index e7907c8a1..9d5433ddf 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(Eigen3 REQUIRED) find_package(GDAL) find_package(rclcpp REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(nlohmann_json REQUIRED) if (rmf_traffic_FOUND) message(STATUS "found rmf_traffic") @@ -121,6 +122,7 @@ add_library(rmf_traffic_ros2 SHARED ${core_lib_srcs}) target_link_libraries(rmf_traffic_ros2 PUBLIC rmf_traffic::rmf_traffic + nlohmann_json::nlohmann_json ${rmf_traffic_msgs_LIBRARIES} ${rmf_site_map_msgs_LIBRARIES} ${GDAL_LIBRARIES} @@ -148,6 +150,7 @@ ament_export_dependencies( Eigen3 rclcpp yaml-cpp + nlohmann_json ) # TODO(MXG): Change these executables into shared libraries that can act as diff --git a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp index 61f7356a0..15856a596 100644 --- a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp +++ b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp @@ -29,6 +29,6 @@ rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, //============================================================================== rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, - int graph_idx = 0); + int graph_idx = 0, double wp_tolerance = 1e-3); } // namespace rmf_traffic_ros2 diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index d7cb95308..07a8fdb32 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -17,6 +17,7 @@ rmf_site_map_msgs rclcpp yaml-cpp + nlohmann-json-dev libgdal-dev diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index e8d308421..a3706b720 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -33,7 +33,7 @@ using CoordsIdxHashMap = std::unordered_mapgetX() * 1000.0) / 1000.0; - double rounded_y = std::round(point->getY() * 1000.0) / 1000.0; + // Round x and y to wp_tolerance + double rounded_x = std::round(point->getX() / wp_tolerance) * wp_tolerance; + double rounded_y = std::round(point->getY() / wp_tolerance) * wp_tolerance; idx_map[level_idx][rounded_x][rounded_y] = wp.index(); } // Iterate over edges @@ -109,10 +109,10 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, double x1 = lane_feat->getX(1); double y0 = lane_feat->getY(0); double y1 = lane_feat->getY(1); - double rounded_x0 = std::round(x0 * 1000.0) / 1000.0; - double rounded_y0 = std::round(y0 * 1000.0) / 1000.0; - double rounded_x1 = std::round(x1 * 1000.0) / 1000.0; - double rounded_y1 = std::round(y1 * 1000.0) / 1000.0; + double rounded_x0 = std::round(x0 / wp_tolerance) * wp_tolerance; + double rounded_y0 = std::round(y0 / wp_tolerance) * wp_tolerance; + double rounded_x1 = std::round(x1 / wp_tolerance) * wp_tolerance; + double rounded_y1 = std::round(y1 / wp_tolerance) * wp_tolerance; auto m0_iter = idx_map[level_idx][rounded_x0].find(rounded_y0); if (m0_iter == idx_map[level_idx][rounded_x0].end()) continue; From aa7d1075944aa7a0934c1574d5bf96e15175579f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 11 Oct 2021 10:38:35 +0800 Subject: [PATCH 09/27] Parse graph_idx and speed_limit Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index a3706b720..151f78274 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -22,6 +22,8 @@ #include #include +#include + #include namespace rmf_traffic_ros2 { @@ -38,6 +40,7 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, CoordsIdxHashMap idx_map; rmf_traffic::agv::Graph graph; // Sqlite3 needs to work on a physical file, write the package to a tmp file + // TODO delete file once done auto filename = std::tmpnam(nullptr); FILE* fd = fopen(filename, "wb"); GDALAllRegister(); @@ -99,8 +102,17 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, level_idx = field.GetAsInteger(); else if (strcmp(field.GetName(), "parameters") == 0) { - // TODO parse parameters here, graph_idx and speed_limit + const auto& params_str = field.GetAsString(); + nlohmann::json j = nlohmann::json::parse(params_str); + auto graph_idx_it = j.find("graph_idx"); // Skip if graph_idx is not the equal to the argument + if (graph_idx_it != j.end() && + graph_idx_it->get() != graph_idx) + continue; + // Parse speed limit + auto speed_limit_it = j.find("speed_limit"); + if (speed_limit_it != j.end()) + speed_limit = speed_limit_it->get(); } } const auto& lane_feat = feature->GetGeometryRef()->toLineString(); From 05548905e0d979f51ffb85af9ec1b94e7bcab0de Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 12 Oct 2021 11:25:34 +0800 Subject: [PATCH 10/27] Remove debug exit() Signed-off-by: Luca Della Vedova --- rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 151f78274..7fd62dbff 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -51,11 +51,6 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, // Get the filename printf("Filename is %s\n", filename); GDALDatasetUniquePtr poDS(GDALDataset::Open(filename, GDAL_OF_VECTOR)); - if( poDS == nullptr ) - { - printf( "Open failed.\n" ); - exit( 1 ); - } // Iterate over vertices auto vertices_layer = poDS->GetLayerByName("vertices"); while (const auto& feature = vertices_layer->GetNextFeature()) From 5fc0808470d4759cb652f0628a092f738ae55ca9 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 8 Nov 2021 15:35:34 +0800 Subject: [PATCH 11/27] Fix graph_idx skipping logic Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 7fd62dbff..55fe290f2 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -91,6 +91,7 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, int level_idx = 0; std::optional speed_limit; std::optional dock_name; + bool is_correct_graph = false; for (const auto& field : feature) { if (strcmp(field.GetName(), "level_idx") == 0) @@ -100,16 +101,17 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, const auto& params_str = field.GetAsString(); nlohmann::json j = nlohmann::json::parse(params_str); auto graph_idx_it = j.find("graph_idx"); - // Skip if graph_idx is not the equal to the argument - if (graph_idx_it != j.end() && - graph_idx_it->get() != graph_idx) - continue; + if (graph_idx_it != j.end()) + is_correct_graph = (graph_idx_it->get() == graph_idx); // Parse speed limit auto speed_limit_it = j.find("speed_limit"); if (speed_limit_it != j.end()) speed_limit = speed_limit_it->get(); } } + // Skip if graph_idx is not the equal to the argument + if (!is_correct_graph) + continue; const auto& lane_feat = feature->GetGeometryRef()->toLineString(); // Get the points double x0 = lane_feat->getX(0); From c2f1d684962dc6a0728c9912cea0de6816cf2455 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 10 Nov 2021 11:30:02 +0800 Subject: [PATCH 12/27] Delete tmp file on conversion done Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 55fe290f2..3bc89102b 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -33,6 +33,33 @@ namespace rmf_traffic_ros2 { using CoordsIdxHashMap = std::unordered_map>>; +// Class to wrap temporary files so they are deleted on destruction +class GeopackageTmpFile +{ +private: + FILE *fd; + +public: + std::string filename; + + GeopackageTmpFile(const std::vector& data) + { + // TODO use return value for errors + char tmpnam[] = "/tmp/geopkgXXXXXX"; + int res = mkstemp(tmpnam); + filename = tmpnam; + fd = fopen(filename.c_str(), "wb"); + fwrite(&data[0], sizeof(char), data.size(), fd); + } + + ~GeopackageTmpFile() + { + // Delete the tmp file + fclose(fd); + remove(filename.c_str()); + } +}; + //============================================================================== rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, int graph_idx, double wp_tolerance) @@ -41,16 +68,12 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, rmf_traffic::agv::Graph graph; // Sqlite3 needs to work on a physical file, write the package to a tmp file // TODO delete file once done - auto filename = std::tmpnam(nullptr); - FILE* fd = fopen(filename, "wb"); GDALAllRegister(); // Not supported if (from.encoding != from.MAP_DATA_GPKG) return graph; - fwrite(&from.data[0], sizeof(char), from.data.size(), fd); - // Get the filename - printf("Filename is %s\n", filename); - GDALDatasetUniquePtr poDS(GDALDataset::Open(filename, GDAL_OF_VECTOR)); + GeopackageTmpFile gpkg_file(from.data); + GDALDatasetUniquePtr poDS(GDALDataset::Open(gpkg_file.filename.c_str(), GDAL_OF_VECTOR)); // Iterate over vertices auto vertices_layer = poDS->GetLayerByName("vertices"); while (const auto& feature = vertices_layer->GetNextFeature()) From 1956e8547654664d182fd9b322b62405a01f45ad Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 10 Nov 2021 12:10:27 +0800 Subject: [PATCH 13/27] Add bidirectional edges and docking Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 3bc89102b..630d6beb9 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -114,6 +114,7 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, int level_idx = 0; std::optional speed_limit; std::optional dock_name; + bool is_bidirectional = false; bool is_correct_graph = false; for (const auto& field : feature) { @@ -130,6 +131,13 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, auto speed_limit_it = j.find("speed_limit"); if (speed_limit_it != j.end()) speed_limit = speed_limit_it->get(); + auto dock_name_it = j.find("dock_name"); + if (dock_name_it != j.end()) + dock_name = dock_name_it->get(); + auto bidirectional_it = j.find("bidirectional"); + if (bidirectional_it != j.end()) + is_bidirectional = bidirectional_it->get(); + } } // Skip if graph_idx is not the equal to the argument @@ -162,9 +170,18 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, // TODO(luca) Add lifts, doors, orientation constraints rmf_utils::clone_ptr entry_event; rmf_utils::clone_ptr exit_event; + if (is_bidirectional) + { + // Lane in the opposite direction + auto& lane = graph.add_lane({end_wp, entry_event}, + {start_wp, exit_event}); + lane.properties().speed_limit(speed_limit); + } + // dock_name is only applied to the lane going to the waypoint, not exiting - // TODO bidirectional edges - // TODO docking + const rmf_traffic::Duration duration = std::chrono::seconds(5); + if (dock_name.has_value()) + entry_event = Event::make(Lane::Dock(dock_name.value(), duration)); auto& lane = graph.add_lane({start_wp, entry_event}, {end_wp, exit_event}); lane.properties().speed_limit(speed_limit); From deb55b63a723539e60b10f965fa993e7b4f82e5f Mon Sep 17 00:00:00 2001 From: Morgan Quigley Date: Thu, 18 Nov 2021 19:40:13 +0800 Subject: [PATCH 14/27] WIP geojson Signed-off-by: Morgan Quigley --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 630d6beb9..4974f4c45 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -18,6 +18,8 @@ #include #include #include +#include + #include #include @@ -70,8 +72,19 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, // TODO delete file once done GDALAllRegister(); // Not supported - if (from.encoding != from.MAP_DATA_GPKG) - return graph; + if (from.encoding == from.MAP_DATA_GEOJSON) + { + printf("converting GeoJSON map\n"); + } + else if (from.encoding == from.MAP_DATA_GEOJSON_GZ) + { + printf("converting compressed GeoJSON map\n"); + } + else + return graph; // unexpected encoding + + return graph; + GeopackageTmpFile gpkg_file(from.data); GDALDatasetUniquePtr poDS(GDALDataset::Open(gpkg_file.filename.c_str(), GDAL_OF_VECTOR)); // Iterate over vertices From 314446a30c549baf4c25530fbfb9bfc575ac325f Mon Sep 17 00:00:00 2001 From: Morgan Quigley Date: Fri, 19 Nov 2021 20:19:10 +0800 Subject: [PATCH 15/27] WIP geojson and geojson.gz Signed-off-by: Morgan Quigley --- rmf_traffic_ros2/CMakeLists.txt | 6 +- rmf_traffic_ros2/package.xml | 4 +- .../src/rmf_traffic_ros2/convert_Graph.cpp | 221 +++++++++++++----- 3 files changed, 172 insertions(+), 59 deletions(-) diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 9d5433ddf..5a792d7f5 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -19,10 +19,10 @@ find_package(rmf_traffic_msgs REQUIRED) find_package(rmf_site_map_msgs REQUIRED) find_package(rmf_fleet_msgs REQUIRED) find_package(Eigen3 REQUIRED) -find_package(GDAL) find_package(rclcpp REQUIRED) find_package(yaml-cpp REQUIRED) find_package(nlohmann_json REQUIRED) +find_package(ZLIB REQUIRED) if (rmf_traffic_FOUND) message(STATUS "found rmf_traffic") @@ -125,9 +125,10 @@ target_link_libraries(rmf_traffic_ros2 nlohmann_json::nlohmann_json ${rmf_traffic_msgs_LIBRARIES} ${rmf_site_map_msgs_LIBRARIES} - ${GDAL_LIBRARIES} ${rclcpp_LIBRARIES} + proj yaml-cpp + z ) target_include_directories(rmf_traffic_ros2 @@ -136,7 +137,6 @@ target_include_directories(rmf_traffic_ros2 $ ${rmf_traffic_msgs_INCLUDE_DIRS} ${rmf_site_map_msgs_INCLUDE_DIRS} - ${GDAL_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index 07a8fdb32..3b3a7c123 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -18,8 +18,8 @@ rclcpp yaml-cpp nlohmann-json-dev - - libgdal-dev + proj + zlib eigen diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 4974f4c45..a2b37bc3b 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -20,10 +20,7 @@ #include #include - -#include -#include - +#include #include #include @@ -35,91 +32,206 @@ namespace rmf_traffic_ros2 { using CoordsIdxHashMap = std::unordered_map>>; -// Class to wrap temporary files so they are deleted on destruction -class GeopackageTmpFile -{ -private: - FILE *fd; - -public: - std::string filename; +// local helper function to factor json parsing code for both +// the compressed and uncompressed case +static rmf_traffic::agv::Graph json_to_graph( + const std::vector& json_doc, + const int graph_idx, + const double wp_tolerance); - GeopackageTmpFile(const std::vector& data) +bool decompress_gzip(const std::vector& in, std::vector& out) +{ + z_stream strm; + memset(&strm, 0, sizeof(strm)); + strm.zalloc = Z_NULL; + strm.zfree = Z_NULL; + strm.opaque = Z_NULL; + strm.avail_in = 0; + strm.next_in = Z_NULL; + const int inflate_init_ret = inflateInit2(&strm, 15 + 32); + if (inflate_init_ret != Z_OK) { - // TODO use return value for errors - char tmpnam[] = "/tmp/geopkgXXXXXX"; - int res = mkstemp(tmpnam); - filename = tmpnam; - fd = fopen(filename.c_str(), "wb"); - fwrite(&data[0], sizeof(char), data.size(), fd); + printf("error in inflateInit2()\n"); + return false; } - ~GeopackageTmpFile() + const size_t READ_CHUNK_SIZE = 128 * 1024; + size_t read_pos = 0; + + const size_t OUT_CHUNK_SIZE = 128 * 1024; + std::vector inflate_buf(OUT_CHUNK_SIZE); + + do { - // Delete the tmp file - fclose(fd); - remove(filename.c_str()); - } -}; + if (read_pos + READ_CHUNK_SIZE < in.size()) + strm.avail_in = READ_CHUNK_SIZE; + else + strm.avail_in = in.size() - read_pos; + // printf("read %d\n", (int)strm.avail_in); + strm.next_in = (unsigned char *)&in[read_pos]; + read_pos += strm.avail_in; + + int inflate_ret = 0; + do + { + strm.avail_out = inflate_buf.size(); + strm.next_out = &inflate_buf[0]; + inflate_ret = inflate(&strm, Z_NO_FLUSH); + if (inflate_ret == Z_NEED_DICT || + inflate_ret == Z_DATA_ERROR || + inflate_ret == Z_MEM_ERROR) + { + printf("unrecoverable zlib inflate error\n"); + inflateEnd(&strm); + return false; + } + const int n_have = inflate_buf.size() - strm.avail_out; + out.insert( + out.end(), + inflate_buf.begin(), + inflate_buf.begin() + n_have); + /* + printf("write %d, output size: %d\n", + (int)n_have, + (int)out.size()); + */ + } while (strm.avail_out == 0); + + if (inflate_ret == Z_STREAM_END) + break; + } while (read_pos < in.size()); + + printf("inflated: %d -> %d\n", (int)in.size(), (int)out.size()); + + return true; +} //============================================================================== rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, int graph_idx, double wp_tolerance) { - CoordsIdxHashMap idx_map; rmf_traffic::agv::Graph graph; - // Sqlite3 needs to work on a physical file, write the package to a tmp file - // TODO delete file once done - GDALAllRegister(); - // Not supported if (from.encoding == from.MAP_DATA_GEOJSON) { printf("converting GeoJSON map\n"); + return json_to_graph(from.data, graph_idx, wp_tolerance); } else if (from.encoding == from.MAP_DATA_GEOJSON_GZ) { printf("converting compressed GeoJSON map\n"); + std::vector uncompressed; + if (!decompress_gzip(from.data, uncompressed)) + return graph; // failed to decompress gzip, cannot proceed + return json_to_graph(uncompressed, graph_idx, wp_tolerance); } else return graph; // unexpected encoding +} - return graph; +rmf_traffic::agv::Graph json_to_graph( + const std::vector& json_doc, + const int graph_idx, + const double wp_tolerance) +{ + rmf_traffic::agv::Graph graph; + printf("json_to_graph with doc length %d\n", (int)json_doc.size()); + nlohmann::json j = nlohmann::json::parse(json_doc); + printf("parsed %d entries in json\n", (int)j.size()); + //auto graph_idx_it = j.find("graph_idx"); + + if (!j.contains("preferred_crs") || !j["preferred_crs"].is_string()) { + printf("GeoJSON does not contain top-level preferred_crs key!\n"); + return graph; + } + const std::string preferred_crs = j["preferred_crs"]; + printf("preferred_crs: [%s]\n", preferred_crs.c_str()); + + if (!j.contains("features") || !j["features"].is_array()) { + printf("GeoJSON does not contain top-level features array!\n"); + return graph; + } + + CoordsIdxHashMap idx_map; + + // spin through features and find all vertices + PJ_CONTEXT *proj_context = proj_context_create(); + PJ *projector = proj_create_crs_to_crs( + proj_context, + "EPSG:4326", // aka WGS84, always used in GeoJSON + preferred_crs.c_str(), + NULL); + if (!projector) + { + printf("unable to create coordinate projector!\n"); + return graph; + } - GeopackageTmpFile gpkg_file(from.data); - GDALDatasetUniquePtr poDS(GDALDataset::Open(gpkg_file.filename.c_str(), GDAL_OF_VECTOR)); - // Iterate over vertices - auto vertices_layer = poDS->GetLayerByName("vertices"); - while (const auto& feature = vertices_layer->GetNextFeature()) + for (const auto& feature : j["features"]) { + const std::string feature_type = feature["feature_type"]; + if (feature_type != "nav_vertex") + continue; + + // sanity check the object structure + if (!feature.contains("properties") || !feature["properties"].is_object()) + continue; + if (!feature.contains("geometry") || !feature["geometry"].is_object()) + continue; + const auto& geom = feature["geometry"]; + if (!geom.contains("type") || !geom["type"].is_string()) + continue; + if (geom["type"] != "Point") + continue; + if (!geom.contains("coordinates") || !geom["coordinates"].is_array()) + continue; + if (geom["coordinates"].size() < 2) + continue; + + // GeoJSON always encodes coordinates as (lon, lat) + const double lon = geom["coordinates"][0]; + const double lat = geom["coordinates"][1]; + + std::string name; + if (feature["properties"].contains("name")) + name = feature["properties"]["name"]; + int level_idx = 0; - for (const auto& field : feature) - { - if (strcmp(field.GetName(), "level_idx") == 0) - level_idx = field.GetAsInteger(); - else if (strcmp(field.GetName(), "parameters") == 0) - { - // TODO parse parameters here - } - } - const auto& name_feat = (*feature)["name"]; - const std::string name(name_feat.GetAsString()); - const auto& point = feature->GetGeometryRef()->toPoint(); - // Flatten geometry to extract - const Eigen::Vector2d location{ - point->getX(), point->getY()}; + if (feature["properties"].contains("level_idx")) + level_idx = feature["properties"]["level_idx"]; + + // todo: parse other parameters here + + const PJ_COORD wgs84_coord = proj_coord(lat, lon, 0, 0); + const PJ_COORD p = proj_trans(projector, PJ_FWD, wgs84_coord); + + // not sure why the coordinate-flip is required, but... it is. + const double easting = p.enu.n; + const double northing = p.enu.e; + + const Eigen::Vector2d location{easting, northing}; + // TODO map name std::string map_name("test"); auto& wp = graph.add_waypoint(map_name, location); + if (name.size() > 0 && !graph.add_key(name, wp.index())) { throw std::runtime_error( "Duplicated waypoint name [" + name + "]"); } - // Round x and y to wp_tolerance - double rounded_x = std::round(point->getX() / wp_tolerance) * wp_tolerance; - double rounded_y = std::round(point->getY() / wp_tolerance) * wp_tolerance; + + double rounded_x = std::round(easting / wp_tolerance) * wp_tolerance; + double rounded_y = std::round(northing / wp_tolerance) * wp_tolerance; idx_map[level_idx][rounded_x][rounded_y] = wp.index(); + + //printf("vertex name: [%s] coords: (%.6f, %.6f) -> (%.2f, %.2f)\n", + // name.c_str(), lon, lat, easting, northing); } + proj_context_destroy(proj_context); + return graph; +} + +#if 0 // Iterate over edges auto edges_layer = poDS->GetLayerByName("edges"); while (const auto& feature = edges_layer->GetNextFeature()) @@ -264,5 +376,6 @@ for (const auto& edge : from.edges) */ return graph; } +#endif } // namespace rmf_traffic_ros2 From ee515cceb98e8f8a523805b6e5ec28689a68ec31 Mon Sep 17 00:00:00 2001 From: Morgan Quigley Date: Fri, 19 Nov 2021 22:22:16 +0800 Subject: [PATCH 16/27] WIP parsing lanes from GeoJSON Signed-off-by: Morgan Quigley --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 120 ++++++++++-------- 1 file changed, 70 insertions(+), 50 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index a2b37bc3b..36ecd9bd0 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -67,7 +67,7 @@ bool decompress_gzip(const std::vector& in, std::vector& out) strm.avail_in = READ_CHUNK_SIZE; else strm.avail_in = in.size() - read_pos; - // printf("read %d\n", (int)strm.avail_in); + strm.next_in = (unsigned char *)&in[read_pos]; read_pos += strm.avail_in; @@ -90,11 +90,6 @@ bool decompress_gzip(const std::vector& in, std::vector& out) out.end(), inflate_buf.begin(), inflate_buf.begin() + n_have); - /* - printf("write %d, output size: %d\n", - (int)n_have, - (int)out.size()); - */ } while (strm.avail_out == 0); if (inflate_ret == Z_STREAM_END) @@ -125,7 +120,10 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, return json_to_graph(uncompressed, graph_idx, wp_tolerance); } else + { + printf("unexpected encoding value: %d\n", from.encoding); return graph; // unexpected encoding + } } rmf_traffic::agv::Graph json_to_graph( @@ -227,57 +225,71 @@ rmf_traffic::agv::Graph json_to_graph( //printf("vertex name: [%s] coords: (%.6f, %.6f) -> (%.2f, %.2f)\n", // name.c_str(), lon, lat, easting, northing); } - proj_context_destroy(proj_context); - return graph; -} -#if 0 - // Iterate over edges - auto edges_layer = poDS->GetLayerByName("edges"); - while (const auto& feature = edges_layer->GetNextFeature()) + // now spin through the features again, looking for lanes + for (const auto& feature : j["features"]) { + const std::string feature_type = feature["feature_type"]; + if (feature_type != "nav_lane") + continue; + + if (!feature.contains("geometry") || !feature["geometry"].is_object()) + continue; + const auto& geom = feature["geometry"]; + if (!geom.contains("type") || !geom["type"].is_string()) + continue; + if (geom["type"] != "LineString") + continue; + if (!geom.contains("coordinates") || !geom["coordinates"].is_array()) + continue; + if (geom["coordinates"].size() < 2) + continue; + + if (feature["properties"].contains("graph_idx")) + { + const int lane_graph_idx = feature["properties"]["graph_idx"]; + if (lane_graph_idx != graph_idx) + continue; // wrong lane. forget it. + } + int level_idx = 0; + if (feature["properties"].contains("level_idx")) + level_idx = feature["properties"]["level_idx"]; + std::optional speed_limit; + if (feature["properties"].contains("speed_limit")) + speed_limit = feature["properties"]["speed_limit"]; + std::optional dock_name; + if (feature["properties"].contains("dock_name")) + dock_name = feature["properties"]["dock_name"]; + bool is_bidirectional = false; - bool is_correct_graph = false; - for (const auto& field : feature) - { - if (strcmp(field.GetName(), "level_idx") == 0) - level_idx = field.GetAsInteger(); - else if (strcmp(field.GetName(), "parameters") == 0) - { - const auto& params_str = field.GetAsString(); - nlohmann::json j = nlohmann::json::parse(params_str); - auto graph_idx_it = j.find("graph_idx"); - if (graph_idx_it != j.end()) - is_correct_graph = (graph_idx_it->get() == graph_idx); - // Parse speed limit - auto speed_limit_it = j.find("speed_limit"); - if (speed_limit_it != j.end()) - speed_limit = speed_limit_it->get(); - auto dock_name_it = j.find("dock_name"); - if (dock_name_it != j.end()) - dock_name = dock_name_it->get(); - auto bidirectional_it = j.find("bidirectional"); - if (bidirectional_it != j.end()) - is_bidirectional = bidirectional_it->get(); + if (feature["properties"].contains("bidirectional")) + is_bidirectional = feature["properties"]["bidirectional"]; + + const double lon_0 = geom["coordinates"][0][0]; + const double lat_0 = geom["coordinates"][0][1]; + const double lon_1 = geom["coordinates"][1][0]; + const double lat_1 = geom["coordinates"][1][1]; + + const PJ_COORD wgs84_coord_0 = proj_coord(lat_0, lon_0, 0, 0); + const PJ_COORD wgs84_coord_1 = proj_coord(lat_1, lon_1, 0, 0); + const PJ_COORD p0 = proj_trans(projector, PJ_FWD, wgs84_coord_0); + const PJ_COORD p1 = proj_trans(projector, PJ_FWD, wgs84_coord_1); + + // not sure why the coordinate-flip is required, but... it is. + // maybe can use proj_normalize_for_visualization someday? + const double x0 = p0.enu.n; + const double y0 = p0.enu.e; + const double x1 = p1.enu.n; + const double y1 = p1.enu.e; + + const double rounded_x0 = std::round(x0 / wp_tolerance) * wp_tolerance; + const double rounded_y0 = std::round(y0 / wp_tolerance) * wp_tolerance; + const double rounded_x1 = std::round(x1 / wp_tolerance) * wp_tolerance; + const double rounded_y1 = std::round(y1 / wp_tolerance) * wp_tolerance; - } - } - // Skip if graph_idx is not the equal to the argument - if (!is_correct_graph) - continue; - const auto& lane_feat = feature->GetGeometryRef()->toLineString(); - // Get the points - double x0 = lane_feat->getX(0); - double x1 = lane_feat->getX(1); - double y0 = lane_feat->getY(0); - double y1 = lane_feat->getY(1); - double rounded_x0 = std::round(x0 / wp_tolerance) * wp_tolerance; - double rounded_y0 = std::round(y0 / wp_tolerance) * wp_tolerance; - double rounded_x1 = std::round(x1 / wp_tolerance) * wp_tolerance; - double rounded_y1 = std::round(y1 / wp_tolerance) * wp_tolerance; auto m0_iter = idx_map[level_idx][rounded_x0].find(rounded_y0); if (m0_iter == idx_map[level_idx][rounded_x0].end()) continue; @@ -311,6 +323,14 @@ rmf_traffic::agv::Graph json_to_graph( {end_wp, exit_event}); lane.properties().speed_limit(speed_limit); } + + proj_destroy(projector); + proj_context_destroy(proj_context); + return graph; +} + +#if 0 + } /* // Iterate over vertices / waypoints // Graph params are not used for now From d792d5d6e00efd53e8532e117f21b722b9c9f3ae Mon Sep 17 00:00:00 2001 From: Morgan Quigley Date: Sun, 21 Nov 2021 22:44:23 +0800 Subject: [PATCH 17/27] uncrustify Signed-off-by: Morgan Quigley --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 30 +++++++++---------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 36ecd9bd0..6a2d66f24 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -30,7 +30,7 @@ namespace rmf_traffic_ros2 { // Usage map[level_idx][truncated_x][truncated_y] = id; // Truncation is to 1e-3 meters using CoordsIdxHashMap = std::unordered_map>>; + double, std::unordered_map>>; // local helper function to factor json parsing code for both // the compressed and uncompressed case @@ -68,7 +68,7 @@ bool decompress_gzip(const std::vector& in, std::vector& out) else strm.avail_in = in.size() - read_pos; - strm.next_in = (unsigned char *)&in[read_pos]; + strm.next_in = (unsigned char*)&in[read_pos]; read_pos += strm.avail_in; int inflate_ret = 0; @@ -78,8 +78,8 @@ bool decompress_gzip(const std::vector& in, std::vector& out) strm.next_out = &inflate_buf[0]; inflate_ret = inflate(&strm, Z_NO_FLUSH); if (inflate_ret == Z_NEED_DICT || - inflate_ret == Z_DATA_ERROR || - inflate_ret == Z_MEM_ERROR) + inflate_ret == Z_DATA_ERROR || + inflate_ret == Z_MEM_ERROR) { printf("unrecoverable zlib inflate error\n"); inflateEnd(&strm); @@ -91,7 +91,7 @@ bool decompress_gzip(const std::vector& in, std::vector& out) inflate_buf.begin(), inflate_buf.begin() + n_have); } while (strm.avail_out == 0); - + if (inflate_ret == Z_STREAM_END) break; } while (read_pos < in.size()); @@ -116,7 +116,7 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, printf("converting compressed GeoJSON map\n"); std::vector uncompressed; if (!decompress_gzip(from.data, uncompressed)) - return graph; // failed to decompress gzip, cannot proceed + return graph; return json_to_graph(uncompressed, graph_idx, wp_tolerance); } else @@ -137,14 +137,16 @@ rmf_traffic::agv::Graph json_to_graph( printf("parsed %d entries in json\n", (int)j.size()); //auto graph_idx_it = j.find("graph_idx"); - if (!j.contains("preferred_crs") || !j["preferred_crs"].is_string()) { + if (!j.contains("preferred_crs") || !j["preferred_crs"].is_string()) + { printf("GeoJSON does not contain top-level preferred_crs key!\n"); return graph; } const std::string preferred_crs = j["preferred_crs"]; printf("preferred_crs: [%s]\n", preferred_crs.c_str()); - if (!j.contains("features") || !j["features"].is_array()) { + if (!j.contains("features") || !j["features"].is_array()) + { printf("GeoJSON does not contain top-level features array!\n"); return graph; } @@ -152,8 +154,8 @@ rmf_traffic::agv::Graph json_to_graph( CoordsIdxHashMap idx_map; // spin through features and find all vertices - PJ_CONTEXT *proj_context = proj_context_create(); - PJ *projector = proj_create_crs_to_crs( + PJ_CONTEXT* proj_context = proj_context_create(); + PJ* projector = proj_create_crs_to_crs( proj_context, "EPSG:4326", // aka WGS84, always used in GeoJSON preferred_crs.c_str(), @@ -188,7 +190,7 @@ rmf_traffic::agv::Graph json_to_graph( // GeoJSON always encodes coordinates as (lon, lat) const double lon = geom["coordinates"][0]; const double lat = geom["coordinates"][1]; - + std::string name; if (feature["properties"].contains("name")) name = feature["properties"]["name"]; @@ -221,9 +223,6 @@ rmf_traffic::agv::Graph json_to_graph( double rounded_x = std::round(easting / wp_tolerance) * wp_tolerance; double rounded_y = std::round(northing / wp_tolerance) * wp_tolerance; idx_map[level_idx][rounded_x][rounded_y] = wp.index(); - - //printf("vertex name: [%s] coords: (%.6f, %.6f) -> (%.2f, %.2f)\n", - // name.c_str(), lon, lat, easting, northing); } // now spin through the features again, looking for lanes @@ -249,7 +248,7 @@ rmf_traffic::agv::Graph json_to_graph( { const int lane_graph_idx = feature["properties"]["graph_idx"]; if (lane_graph_idx != graph_idx) - continue; // wrong lane. forget it. + continue; } int level_idx = 0; @@ -330,7 +329,6 @@ rmf_traffic::agv::Graph json_to_graph( } #if 0 - } /* // Iterate over vertices / waypoints // Graph params are not used for now From 5070919c4390e9702c965c4f738eff448da441e1 Mon Sep 17 00:00:00 2001 From: Morgan Quigley Date: Mon, 22 Nov 2021 11:12:05 +0800 Subject: [PATCH 18/27] uncrustify Signed-off-by: Morgan Quigley --- rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 6a2d66f24..953ece99b 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -319,7 +319,7 @@ rmf_traffic::agv::Graph json_to_graph( if (dock_name.has_value()) entry_event = Event::make(Lane::Dock(dock_name.value(), duration)); auto& lane = graph.add_lane({start_wp, entry_event}, - {end_wp, exit_event}); + {end_wp, exit_event}); lane.properties().speed_limit(speed_limit); } From efd773d7e7d8e624b480248a644985be19f3f0e8 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 23 Nov 2021 15:31:25 +0800 Subject: [PATCH 19/27] Minor cleanups and refactors Signed-off-by: Luca Della Vedova --- .../include/rmf_traffic_ros2/agv/Graph.hpp | 6 - .../src/rmf_traffic_ros2/convert_Graph.cpp | 152 +++++------------- 2 files changed, 41 insertions(+), 117 deletions(-) diff --git a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp index 15856a596..580acafa8 100644 --- a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp +++ b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp @@ -21,12 +21,6 @@ namespace rmf_traffic_ros2 { -//============================================================================== -/* -rmf_traffic::agv::Graph convert(const rmf_building_map_msgs::msg::Graph& from, - int waypoint_offset = 0); -*/ - //============================================================================== rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, int graph_idx = 0, double wp_tolerance = 1e-3); diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 953ece99b..89e42c3c5 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -28,7 +28,7 @@ namespace rmf_traffic_ros2 { // Usage map[level_idx][truncated_x][truncated_y] = id; -// Truncation is to 1e-3 meters +// Truncation is to 1e-3 meters by default using CoordsIdxHashMap = std::unordered_map>>; @@ -39,8 +39,10 @@ static rmf_traffic::agv::Graph json_to_graph( const int graph_idx, const double wp_tolerance); -bool decompress_gzip(const std::vector& in, std::vector& out) +std::optional> decompress_gzip( + const std::vector& in) { + std::vector out; z_stream strm; memset(&strm, 0, sizeof(strm)); strm.zalloc = Z_NULL; @@ -51,15 +53,14 @@ bool decompress_gzip(const std::vector& in, std::vector& out) const int inflate_init_ret = inflateInit2(&strm, 15 + 32); if (inflate_init_ret != Z_OK) { - printf("error in inflateInit2()\n"); - return false; + std::cout << "error in inflateInit2()" << std::endl; + return std::nullopt; } - const size_t READ_CHUNK_SIZE = 128 * 1024; - size_t read_pos = 0; + const std::size_t READ_CHUNK_SIZE = 128 * 1024; + std::size_t read_pos = 0; - const size_t OUT_CHUNK_SIZE = 128 * 1024; - std::vector inflate_buf(OUT_CHUNK_SIZE); + std::vector inflate_buf(READ_CHUNK_SIZE); do { @@ -81,9 +82,9 @@ bool decompress_gzip(const std::vector& in, std::vector& out) inflate_ret == Z_DATA_ERROR || inflate_ret == Z_MEM_ERROR) { - printf("unrecoverable zlib inflate error\n"); + std::cout << "unrecoverable zlib inflate error" << std::endl; inflateEnd(&strm); - return false; + return std::nullopt; } const int n_have = inflate_buf.size() - strm.avail_out; out.insert( @@ -96,9 +97,9 @@ bool decompress_gzip(const std::vector& in, std::vector& out) break; } while (read_pos < in.size()); - printf("inflated: %d -> %d\n", (int)in.size(), (int)out.size()); + std::cout << "inflated: " << in.size() << " -> " << out.size() << std::endl; - return true; + return {out}; } //============================================================================== @@ -108,20 +109,20 @@ rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, rmf_traffic::agv::Graph graph; if (from.encoding == from.MAP_DATA_GEOJSON) { - printf("converting GeoJSON map\n"); + std::cout << "converting GeoJSON map" << std::endl; return json_to_graph(from.data, graph_idx, wp_tolerance); } else if (from.encoding == from.MAP_DATA_GEOJSON_GZ) { - printf("converting compressed GeoJSON map\n"); - std::vector uncompressed; - if (!decompress_gzip(from.data, uncompressed)) + std::cout << "converting compressed GeoJSON map" << std::endl; + const auto uncompressed = decompress_gzip(from.data); + if (!uncompressed.has_value()) return graph; - return json_to_graph(uncompressed, graph_idx, wp_tolerance); + return json_to_graph(uncompressed.value(), graph_idx, wp_tolerance); } else { - printf("unexpected encoding value: %d\n", from.encoding); + std::cout << "unexpected encoding value: " << from.encoding << std::endl; return graph; // unexpected encoding } } @@ -132,25 +133,33 @@ rmf_traffic::agv::Graph json_to_graph( const double wp_tolerance) { rmf_traffic::agv::Graph graph; - printf("json_to_graph with doc length %d\n", (int)json_doc.size()); + std::cout << "json_to_graph with doc length " << json_doc.size() << std::endl; nlohmann::json j = nlohmann::json::parse(json_doc); - printf("parsed %d entries in json\n", (int)j.size()); - //auto graph_idx_it = j.find("graph_idx"); + std::cout << "parsed " << j.size() << "entries in json" << std::endl;; - if (!j.contains("preferred_crs") || !j["preferred_crs"].is_string()) + const auto preferred_crs_it = j.find("preferred_crs"); + if (preferred_crs_it == j.end() || !preferred_crs_it->is_string()) { - printf("GeoJSON does not contain top-level preferred_crs key!\n"); + std::cout << "GeoJSON does not contain top-level preferred_crs key!" << std::endl; return graph; } - const std::string preferred_crs = j["preferred_crs"]; - printf("preferred_crs: [%s]\n", preferred_crs.c_str()); + const std::string preferred_crs = *preferred_crs_it; + std::cout << "preferred_crs: " << preferred_crs << std::endl; if (!j.contains("features") || !j["features"].is_array()) { - printf("GeoJSON does not contain top-level features array!\n"); + std::cout << "GeoJSON does not contain top-level features array!" << std::endl; return graph; } + const auto site_name_it = j.find("site_name"); + if (site_name_it == j.end() || !site_name_it->is_string()) + { + std::cout << "Site name not found in map" << std::endl; + return graph; + } + const std::string site_name = *site_name_it; + CoordsIdxHashMap idx_map; // spin through features and find all vertices @@ -162,7 +171,7 @@ rmf_traffic::agv::Graph json_to_graph( NULL); if (!projector) { - printf("unable to create coordinate projector!\n"); + std::cout << "unable to create coordinate projector!" << std::endl; return graph; } @@ -191,13 +200,9 @@ rmf_traffic::agv::Graph json_to_graph( const double lon = geom["coordinates"][0]; const double lat = geom["coordinates"][1]; - std::string name; - if (feature["properties"].contains("name")) - name = feature["properties"]["name"]; + std::string name = feature["properties"].value("name", ""); - int level_idx = 0; - if (feature["properties"].contains("level_idx")) - level_idx = feature["properties"]["level_idx"]; + int level_idx = feature["properties"].value("level_idx", 0); // todo: parse other parameters here @@ -210,9 +215,7 @@ rmf_traffic::agv::Graph json_to_graph( const Eigen::Vector2d location{easting, northing}; - // TODO map name - std::string map_name("test"); - auto& wp = graph.add_waypoint(map_name, location); + auto& wp = graph.add_waypoint(site_name, location); if (name.size() > 0 && !graph.add_key(name, wp.index())) { @@ -251,9 +254,8 @@ rmf_traffic::agv::Graph json_to_graph( continue; } - int level_idx = 0; - if (feature["properties"].contains("level_idx")) - level_idx = feature["properties"]["level_idx"]; + int level_idx = feature["properties"].value("level_idx", 0); + bool is_bidirectional = feature["properties"].value("bidirectional", false); std::optional speed_limit; if (feature["properties"].contains("speed_limit")) @@ -263,10 +265,6 @@ rmf_traffic::agv::Graph json_to_graph( if (feature["properties"].contains("dock_name")) dock_name = feature["properties"]["dock_name"]; - bool is_bidirectional = false; - if (feature["properties"].contains("bidirectional")) - is_bidirectional = feature["properties"]["bidirectional"]; - const double lon_0 = geom["coordinates"][0][0]; const double lat_0 = geom["coordinates"][0][1]; const double lon_1 = geom["coordinates"][1][0]; @@ -328,72 +326,4 @@ rmf_traffic::agv::Graph json_to_graph( return graph; } -#if 0 -/* -// Iterate over vertices / waypoints -// Graph params are not used for now -for (const auto& vertex : from.vertices) -{ - const Eigen::Vector2d location{ - vertex.x, vertex.y}; - auto& wp = graph.add_waypoint(from.name, location); - // Add waypoint name if in the message - if (vertex.name.size() > 0 && !graph.add_key(vertex.name, wp.index())) - { - throw std::runtime_error( - "Duplicated waypoint name [" + vertex.name + "]"); - } - for (const auto& param : vertex.params) - { - if (param.name == "is_parking_spot") - wp.set_parking_spot(param.value_bool); - else if (param.name == "is_holding_point") - wp.set_holding_point(param.value_bool); - else if (param.name == "is_passthrough_point") - wp.set_passthrough_point(param.value_bool); - else if (param.name == "is_charger") - wp.set_charger(param.value_bool); - } -} -// Iterate over edges / lanes -for (const auto& edge : from.edges) -{ - using Lane = rmf_traffic::agv::Graph::Lane; - using Event = Lane::Event; - // TODO(luca) Add lifts, doors, orientation constraints - rmf_utils::clone_ptr entry_event; - rmf_utils::clone_ptr exit_event; - // Waypoint offset is applied to ensure unique IDs when multiple levels - // are present - const std::size_t start_wp = edge.v1_idx + waypoint_offset; - const std::size_t end_wp = edge.v2_idx + waypoint_offset; - std::string dock_name; - std::optional speed_limit; - for (const auto& param : edge.params) - { - if (param.name == "dock_name") - dock_name = param.value_string; - if (param.name == "speed_limit") - speed_limit = param.value_float; - } - // dock_name is only applied to the lane going to the waypoint, not exiting - if (edge.edge_type == edge.EDGE_TYPE_BIDIRECTIONAL) - { - auto& lane = graph.add_lane({end_wp, entry_event}, - {start_wp, exit_event}); - lane.properties().speed_limit(speed_limit); - } - - const rmf_traffic::Duration duration = std::chrono::seconds(5); - if (dock_name.size() > 0) - entry_event = Event::make(Lane::Dock(dock_name, duration)); - auto& lane = graph.add_lane({start_wp, entry_event}, - {end_wp, exit_event}); - lane.properties().speed_limit(speed_limit); -} -*/ -return graph; -} -#endif - } // namespace rmf_traffic_ros2 From ba5aac9614c558aab2c0967beae7b2a3eae96bcf Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 23 Nov 2021 16:53:42 +0800 Subject: [PATCH 20/27] Uncrustify Signed-off-by: Luca Della Vedova --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 89e42c3c5..eecd00ab1 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -40,7 +40,7 @@ static rmf_traffic::agv::Graph json_to_graph( const double wp_tolerance); std::optional> decompress_gzip( - const std::vector& in) + const std::vector& in) { std::vector out; z_stream strm; @@ -135,12 +135,13 @@ rmf_traffic::agv::Graph json_to_graph( rmf_traffic::agv::Graph graph; std::cout << "json_to_graph with doc length " << json_doc.size() << std::endl; nlohmann::json j = nlohmann::json::parse(json_doc); - std::cout << "parsed " << j.size() << "entries in json" << std::endl;; + std::cout << "parsed " << j.size() << "entries in json" << std::endl; const auto preferred_crs_it = j.find("preferred_crs"); if (preferred_crs_it == j.end() || !preferred_crs_it->is_string()) { - std::cout << "GeoJSON does not contain top-level preferred_crs key!" << std::endl; + std::cout << "GeoJSON does not contain top-level preferred_crs key!" << + std::endl; return graph; } const std::string preferred_crs = *preferred_crs_it; @@ -148,7 +149,8 @@ rmf_traffic::agv::Graph json_to_graph( if (!j.contains("features") || !j["features"].is_array()) { - std::cout << "GeoJSON does not contain top-level features array!" << std::endl; + std::cout << "GeoJSON does not contain top-level features array!" << + std::endl; return graph; } From 245baeacd216c840358b85de256f652d3d744be8 Mon Sep 17 00:00:00 2001 From: Morgan Quigley Date: Wed, 1 Dec 2021 18:37:42 +0800 Subject: [PATCH 21/27] use rmf_ prefix for RMF feature types Signed-off-by: Morgan Quigley --- rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 953ece99b..272a0bb92 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -169,7 +169,7 @@ rmf_traffic::agv::Graph json_to_graph( for (const auto& feature : j["features"]) { const std::string feature_type = feature["feature_type"]; - if (feature_type != "nav_vertex") + if (feature_type != "rmf_vertex") continue; // sanity check the object structure @@ -229,7 +229,7 @@ rmf_traffic::agv::Graph json_to_graph( for (const auto& feature : j["features"]) { const std::string feature_type = feature["feature_type"]; - if (feature_type != "nav_lane") + if (feature_type != "rmf_lane") continue; if (!feature.contains("geometry") || !feature["geometry"].is_object()) From 1ad0c4b4abc5c495d4faa3b3b8b7523ca9619f05 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 11 Feb 2022 23:44:33 +0800 Subject: [PATCH 22/27] Fix cmake config script Signed-off-by: Michael X. Grey --- rmf_traffic_ros2/CMakeLists.txt | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index ac25cf227..bdf7831fa 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -24,6 +24,12 @@ find_package(yaml-cpp REQUIRED) find_package(nlohmann_json REQUIRED) find_package(ZLIB REQUIRED) +# NOTE(MXG): libproj-dev does not currently distribute its cmake config-files +# in Debian, so we can't use find_package and need to rely on pkg-config. +# find_package(PROJ REQUIRED) +find_package(PkgConfig REQUIRED) +pkg_check_modules(PROJ REQUIRED IMPORTED_TARGET proj) + if (rmf_traffic_FOUND) message(STATUS "found rmf_traffic") message(STATUS "rmf_traffic_LIBRARIES: ${rmf_traffic_LIBRARIES}") @@ -161,9 +167,9 @@ target_link_libraries(rmf_traffic_ros2 ${rmf_traffic_msgs_LIBRARIES} ${rmf_site_map_msgs_LIBRARIES} ${rclcpp_LIBRARIES} - proj yaml-cpp - z + ZLIB::ZLIB + PkgConfig::PROJ ) target_include_directories(rmf_traffic_ros2 From 75124bfd0f6c60fefeb4e80837624184cb51cfe1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 11 Feb 2022 23:48:24 +0800 Subject: [PATCH 23/27] Leave a note about bringing tests back Signed-off-by: Michael X. Grey --- rmf_traffic_ros2/test/unit/test_convert_Graph.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp index d70ca7235..bf870418d 100644 --- a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp +++ b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp @@ -20,6 +20,7 @@ #include #include +// TODO: Resuscitate these tests when time permits /* static auto make_graph_node(const double x, const double y, const std::string& name, From 0bb350b81250a696a9063584fdcce094011642b8 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Feb 2022 00:13:47 +0800 Subject: [PATCH 24/27] Export all dependencies Signed-off-by: Michael X. Grey --- rmf_traffic_ros2/CMakeLists.txt | 6 +++++- rmf_traffic_ros2/cmake/proj_dependency.cmake | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) create mode 100644 rmf_traffic_ros2/cmake/proj_dependency.cmake diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index bdf7831fa..360258b90 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -192,6 +192,7 @@ ament_export_dependencies( rclcpp yaml-cpp nlohmann_json + ZLIB ) # TODO(MXG): Change these executables into shared libraries that can act as @@ -253,4 +254,7 @@ install( ARCHIVE DESTINATION lib ) -ament_package() +ament_package( + CONFIG_EXTRAS + cmake/proj_dependency.cmake +) diff --git a/rmf_traffic_ros2/cmake/proj_dependency.cmake b/rmf_traffic_ros2/cmake/proj_dependency.cmake new file mode 100644 index 000000000..8a5140cf7 --- /dev/null +++ b/rmf_traffic_ros2/cmake/proj_dependency.cmake @@ -0,0 +1,2 @@ +find_package(PkgConfig REQUIRED) +pkg_check_modules(PROJ REQUIRED IMPORTED_TARGET proj) From 5b85a9c2933fb32e65a477e615e128078f06ee60 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 12 Feb 2022 23:49:53 +0800 Subject: [PATCH 25/27] Skip Python tests on CI until the configuration can be fixed Signed-off-by: Michael X. Grey --- .github/workflows/build.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 833399471..a655691e1 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -23,7 +23,7 @@ jobs: rmf_traffic_ros2 rmf_task_ros2 rmf_fleet_adapter - rmf_fleet_adapter_python + # rmf_fleet_adapter_python # TODO(MXG): These tests are failing in the CI, and it seems to be related to https://github.com/ros2/launch/pull/592. Figure out how to fix this. vcs-repo-file-url: | https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos colcon-defaults: | From 6b5883ff0f977150bc433493f72d469506841c77 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Feb 2022 02:01:19 +0800 Subject: [PATCH 26/27] Increase test time threshold Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/test/phases/test_DoorClose.cpp | 12 ++++++------ rmf_fleet_adapter/test/phases/test_DoorOpen.cpp | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rmf_fleet_adapter/test/phases/test_DoorClose.cpp b/rmf_fleet_adapter/test/phases/test_DoorClose.cpp index 053c74007..8d090bd1f 100644 --- a/rmf_fleet_adapter/test/phases/test_DoorClose.cpp +++ b/rmf_fleet_adapter/test/phases/test_DoorClose.cpp @@ -191,7 +191,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") AND_WHEN("door state is closed and supervisor do not have session") { rmf_rxcpp::subscription_guard sub2 = - rxcpp::observable<>::interval(std::chrono::milliseconds(1)) + rxcpp::observable<>::interval(std::chrono::milliseconds(10)) .subscribe_on(rxcpp::observe_on_new_thread()) .subscribe( [publish_door_state, publish_empty_heartbeat](const auto&) @@ -204,7 +204,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") { std::unique_lock lk(test->m); bool completed = test->status_updates_cv.wait_for( - lk, std::chrono::milliseconds(10), + lk, std::chrono::milliseconds(100), [test]() { return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; @@ -216,7 +216,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") AND_WHEN("door state is open and supervisor do not have session") { rmf_rxcpp::subscription_guard sub2 = - rxcpp::observable<>::interval(std::chrono::milliseconds(1)) + rxcpp::observable<>::interval(std::chrono::milliseconds(10)) .subscribe_on(rxcpp::observe_on_new_thread()) .subscribe([publish_door_state, publish_empty_heartbeat](const auto&) { @@ -228,7 +228,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") { std::unique_lock lk(test->m); bool completed = test->status_updates_cv.wait_for( - lk, std::chrono::milliseconds(10), [test]() + lk, std::chrono::milliseconds(100), [test]() { return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; }); @@ -252,7 +252,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") { std::unique_lock lk(test->m); bool completed = test->status_updates_cv.wait_for( - lk, std::chrono::milliseconds(10), [test]() + lk, std::chrono::milliseconds(100), [test]() { return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; }); @@ -267,7 +267,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") bool completed = test->status_updates_cv.wait_for( - lk, std::chrono::milliseconds(10), [test]() + lk, std::chrono::milliseconds(100), [test]() { for (const auto& status : test->status_updates) { diff --git a/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp b/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp index c9e97fd08..976cba1c0 100644 --- a/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp +++ b/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp @@ -210,7 +210,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") AND_WHEN("door state is open and supervisor has session") { rmf_rxcpp::subscription_guard sub2 = - rxcpp::observable<>::interval(std::chrono::milliseconds(1)) + rxcpp::observable<>::interval(std::chrono::milliseconds(10)) .subscribe_on(rxcpp::observe_on_new_thread()) .subscribe( [publish_door_state, publish_heartbeat_with_session](const auto&) @@ -223,7 +223,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") { std::unique_lock lk(test->m); bool completed = test->status_updates_cv.wait_for( - lk, std::chrono::milliseconds(10), + lk, std::chrono::milliseconds(100), [test]() { return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; @@ -235,7 +235,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") AND_WHEN("door state is open and supervisor do not have session") { rmf_rxcpp::subscription_guard sub2 = - rxcpp::observable<>::interval(std::chrono::milliseconds(1)) + rxcpp::observable<>::interval(std::chrono::milliseconds(10)) .subscribe_on(rxcpp::observe_on_new_thread()) .subscribe([publish_door_state, publish_empty_heartbeat](const auto&) { @@ -247,7 +247,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") { std::unique_lock lk(test->m); bool completed = test->status_updates_cv.wait_for( - lk, std::chrono::milliseconds(10), + lk, std::chrono::milliseconds(100), [test]() { return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; @@ -259,7 +259,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") AND_WHEN("door state is closed and supervisor has session") { rmf_rxcpp::subscription_guard sub2 = - rxcpp::observable<>::interval(std::chrono::milliseconds(1)) + rxcpp::observable<>::interval(std::chrono::milliseconds(10)) .subscribe_on(rxcpp::observe_on_new_thread()) .subscribe([publish_door_state, publish_empty_heartbeat](const auto&) { @@ -271,7 +271,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") { std::unique_lock lk(test->m); bool completed = test->status_updates_cv.wait_for( - lk, std::chrono::milliseconds(10), + lk, std::chrono::milliseconds(100), [test]() { return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; From 1453302fe115a64b97466bc1596b5c230ad944c5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Sun, 13 Feb 2022 02:42:28 +0800 Subject: [PATCH 27/27] Add unit tests for geojson map parsing functions (#144) Signed-off-by: Luca Della Vedova Signed-off-by: Morgan Quigley Co-authored-by: Morgan Quigley --- rmf_traffic_ros2/CMakeLists.txt | 4 + .../src/rmf_traffic_ros2/convert_Graph.cpp | 10 + .../test/resources/office_map.geojson | 1885 +++++++++++++++++ .../test/unit/test_convert_Graph.cpp | 79 +- 4 files changed, 1952 insertions(+), 26 deletions(-) create mode 100644 rmf_traffic_ros2/test/resources/office_map.geojson diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 360258b90..ec4759647 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -55,6 +55,10 @@ if(BUILD_TESTING) test_rmf_traffic_ros2 test/main.cpp ${unit_test_srcs} TIMEOUT 300) + target_compile_definitions(test_rmf_traffic_ros2 + PRIVATE + "-DTEST_RESOURCES_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/test/resources/\"") + target_link_libraries(test_rmf_traffic_ros2 rmf_traffic_ros2 yaml-cpp diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 686a3b8f5..c984824cc 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -228,6 +228,16 @@ rmf_traffic::agv::Graph json_to_graph( double rounded_x = std::round(easting / wp_tolerance) * wp_tolerance; double rounded_y = std::round(northing / wp_tolerance) * wp_tolerance; idx_map[level_idx][rounded_x][rounded_y] = wp.index(); + + // Set waypoint properties + if (feature["properties"].contains("is_holding_point")) + wp.set_holding_point(feature["properties"]["is_holding_point"]); + if (feature["properties"].contains("is_passthrough_point")) + wp.set_passthrough_point(feature["properties"]["is_passthrough_point"]); + if (feature["properties"].contains("is_parking_spot")) + wp.set_parking_spot(feature["properties"]["is_parking_spot"]); + if (feature["properties"].contains("is_charger")) + wp.set_charger(feature["properties"]["is_charger"]); } // now spin through the features again, looking for lanes diff --git a/rmf_traffic_ros2/test/resources/office_map.geojson b/rmf_traffic_ros2/test/resources/office_map.geojson new file mode 100644 index 000000000..a6ce01977 --- /dev/null +++ b/rmf_traffic_ros2/test/resources/office_map.geojson @@ -0,0 +1,1885 @@ +{ + "features": [ + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.5824722729539, + 1.0255337442883958 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58696991419195, + 1.0166571149261503 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58407531956907, + 1.0291685753591946 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60533020732623, + 1.0309638369378948 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60699115466403, + 1.0284743541376067 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60826764742619, + 1.0195994130005688 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59298010391055, + 1.0212985040465252 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59819121447609, + 1.0212168847332959 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60090671761854, + 1.0211836764844966 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60091113172784, + 1.0227707275499787 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60023122722856, + 1.022804754171925 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59895044047363, + 1.0228243391398208 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59821391749033, + 1.0228341243428114 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59669421839008, + 1.0189994400345126 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60688688296449, + 1.01934517244764 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60632039020354, + 1.0227100458218228 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60237145243975, + 1.0227997550577297 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60239599806127, + 1.0268146594204188 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.602390961424, + 1.0277646340974216 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60240580710011, + 1.0285946107993573 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60562458494793, + 1.0283098528167125 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60592777460676, + 1.0255399483570875 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59630106182654, + 1.0290891520501433 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59320153443178, + 1.029033915635053 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59012189541858, + 1.0285586937212876 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.5870610928512, + 1.027688275027746 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58416829601893, + 1.0264405416922064 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58868488016145, + 1.0174864432697321 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59075940431067, + 1.01833989026606 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59249533400411, + 1.0187971021810172 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59428804329656, + 1.0190519582133102 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59468301097623, + 1.0190424647915663 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59076433105538, + 1.0194976955463133 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58972858069353, + 1.021541891814493 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59060016873777, + 1.021984976258748 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59051386160844, + 1.022132642378931 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58725870481062, + 1.0202944115751709 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59013458793966, + 1.0229994410096026 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58764401359441, + 1.0278442134732066 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58907244457035, + 1.0184284976936362 + ], + "type": "Point" + }, + "properties": { + "is_parking_spot": false, + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59262766312183, + 1.0195682948642872 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59245573220846, + 1.0237211235108812 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.5928240196296, + 1.0222209109090254 + ], + "type": "Point" + }, + "properties": { + "is_charger": true, + "is_holding_point": true, + "is_parking_spot": true, + "level_idx": 0, + "name": "tinyRobot1_charger", + "spawn_robot_name": "tinyRobot1", + "spawn_robot_type": "TinyRobot" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59964045557126, + 1.0236168886301638 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59963049810369, + 1.0220387090711036 + ], + "type": "Point" + }, + "properties": { + "is_holding_point": true, + "is_parking_spot": false, + "level_idx": 0, + "name": "pantry", + "pickup_dispenser": "coke_dispenser" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60163964269795, + 1.0236091486194254 + ], + "type": "Point" + }, + "properties": { + "is_parking_spot": false, + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60169763855464, + 1.0273464802569072 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60286696517166, + 1.0199056604981398 + ], + "type": "Point" + }, + "properties": { + "is_holding_point": true, + "is_parking_spot": false, + "level_idx": 0, + "name": "lounge" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.5956870371583, + 1.0205008722840594 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59120951163405, + 1.022868445109988 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.6039837332762, + 1.0242752522146519 + ], + "type": "Point" + }, + "properties": { + "dropoff_ingestor": "coke_ingestor", + "is_holding_point": true, + "is_parking_spot": false, + "level_idx": 0, + "name": "hardware_2" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59015102536746, + 1.0247245330078576 + ], + "type": "Point" + }, + "properties": { + "is_parking_spot": false, + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58742565555747, + 1.021581362335121 + ], + "type": "Point" + }, + "properties": { + "is_holding_point": true, + "is_parking_spot": false, + "level_idx": 0, + "name": "coe" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59783734003983, + 1.0282592460093032 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.5940125631763, + 1.0283539083443207 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60171819465656, + 1.028101305097213 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59022896870967, + 1.0277813245268368 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59170224612089, + 1.0281688280407946 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58839957540249, + 1.0200205328963619 + ], + "type": "Point" + }, + "properties": { + "is_parking_spot": true, + "level_idx": 0, + "name": "supplies" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60162872090821, + 1.0204277841715783 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59402619411426, + 1.0237397993002852 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59798776185056, + 1.023667483433854 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.5940343710202, + 1.0261471664279505 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59784759378708, + 1.0261396375732619 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.58866823454075, + 1.0218550623600697 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.59986477758096, + 1.028121363574092 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60392557834824, + 1.0272782362975372 + ], + "type": "Point" + }, + "properties": { + "level_idx": 0, + "name": "" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_vertex", + "geometry": { + "coordinates": [ + 103.60342718350094, + 1.0219407352685843 + ], + "type": "Point" + }, + "properties": { + "is_charger": true, + "is_holding_point": true, + "is_parking_spot": true, + "level_idx": 0, + "name": "tinyRobot2_charger", + "spawn_robot_name": "tinyRobot2", + "spawn_robot_type": "TinyRobot" + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.58907244457035, + 1.0184284976936362 + ], + [ + 103.59262766312183, + 1.0195682948642872 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59964045557126, + 1.0236168886301638 + ], + [ + 103.60163964269795, + 1.0236091486194254 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59964045557126, + 1.0236168886301638 + ], + [ + 103.59963049810369, + 1.0220387090711036 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.5956870371583, + 1.0205008722840594 + ], + [ + 103.59262766312183, + 1.0195682948642872 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59262766312183, + 1.0195682948642872 + ], + [ + 103.59120951163405, + 1.022868445109988 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59120951163405, + 1.022868445109988 + ], + [ + 103.59245573220846, + 1.0237211235108812 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59120951163405, + 1.022868445109988 + ], + [ + 103.59015102536746, + 1.0247245330078576 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60171819465656, + 1.028101305097213 + ], + [ + 103.60169763855464, + 1.0273464802569072 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59015102536746, + 1.0247245330078576 + ], + [ + 103.59022896870967, + 1.0277813245268368 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59022896870967, + 1.0277813245268368 + ], + [ + 103.59170224612089, + 1.0281688280407946 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.58907244457035, + 1.0184284976936362 + ], + [ + 103.58839957540249, + 1.0200205328963619 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60162872090821, + 1.0204277841715783 + ], + [ + 103.5956870371583, + 1.0205008722840594 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60162872090821, + 1.0204277841715783 + ], + [ + 103.60286696517166, + 1.0199056604981398 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59245573220846, + 1.0237211235108812 + ], + [ + 103.5928240196296, + 1.0222209109090254 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59245573220846, + 1.0237211235108812 + ], + [ + 103.59402619411426, + 1.0237397993002852 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59402619411426, + 1.0237397993002852 + ], + [ + 103.59798776185056, + 1.023667483433854 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59798776185056, + 1.023667483433854 + ], + [ + 103.59964045557126, + 1.0236168886301638 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.5940125631763, + 1.0283539083443207 + ], + [ + 103.5940343710202, + 1.0261471664279505 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.5940343710202, + 1.0261471664279505 + ], + [ + 103.59402619411426, + 1.0237397993002852 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59783734003983, + 1.0282592460093032 + ], + [ + 103.59784759378708, + 1.0261396375732619 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59784759378708, + 1.0261396375732619 + ], + [ + 103.59798776185056, + 1.023667483433854 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59120951163405, + 1.022868445109988 + ], + [ + 103.58866823454075, + 1.0218550623600697 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.58866823454075, + 1.0218550623600697 + ], + [ + 103.58742565555747, + 1.021581362335121 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60163964269795, + 1.0236091486194254 + ], + [ + 103.60169763855464, + 1.0273464802569072 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60171819465656, + 1.028101305097213 + ], + [ + 103.59986477758096, + 1.028121363574092 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60169763855464, + 1.0273464802569072 + ], + [ + 103.60392557834824, + 1.0272782362975372 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60392557834824, + 1.0272782362975372 + ], + [ + 103.6039837332762, + 1.0242752522146519 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59170224612089, + 1.0281688280407946 + ], + [ + 103.5940125631763, + 1.0283539083443207 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.5940125631763, + 1.0283539083443207 + ], + [ + 103.59783734003983, + 1.0282592460093032 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.59783734003983, + 1.0282592460093032 + ], + [ + 103.59986477758096, + 1.028121363574092 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60162872090821, + 1.0204277841715783 + ], + [ + 103.60342718350094, + 1.0219407352685843 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0 + }, + "type": "Feature" + }, + { + "feature_type": "rmf_lane", + "geometry": { + "coordinates": [ + [ + 103.60162872090821, + 1.0204277841715783 + ], + [ + 103.60163964269795, + 1.0236091486194254 + ] + ], + "type": "LineString" + }, + "properties": { + "bidirectional": true, + "demo_mock_floor_name": "", + "demo_mock_lift_name": "", + "graph_idx": 0, + "level_idx": 0, + "orientation": "", + "speed_limit": 0.5 + }, + "type": "Feature" + } + ], + "preferred_crs": "EPSG:3414", + "site_name": "building", + "suggested_offset_x": 123.0, + "suggested_offset_y": 456.0, + "type": "FeatureCollection" +} diff --git a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp index bf870418d..8cd8657ca 100644 --- a/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp +++ b/rmf_traffic_ros2/test/unit/test_convert_Graph.cpp @@ -15,44 +15,71 @@ * */ +#include + #include #include #include -// TODO: Resuscitate these tests when time permits -/* -static auto make_graph_node(const double x, const double y, - const std::string& name, - const std::vector& params) +const std::string MAP_PATH = TEST_RESOURCES_DIR "/office_map.geojson"; + +static auto make_map_message(const std::string& map_path) { - rmf_building_map_msgs::msg::GraphNode node; - node.x = x; - node.y = y; - node.name = name; - for (const auto& param : params) - node.params.push_back(param); - return node; + std::ifstream f(map_path); + std::stringstream string_buffer; + string_buffer << f.rdbuf(); + const std::string contents = string_buffer.str(); + // Make the message + rmf_site_map_msgs::msg::SiteMap msg; + msg.encoding = msg.MAP_DATA_GEOJSON; + msg.data = {contents.begin(), contents.end()}; + return msg; } -static auto make_bool_param(const std::string& name, const bool val) +SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") { - rmf_building_map_msgs::msg::Param param; - param.name = name; - param.type = param.TYPE_BOOL; - param.value_bool = val; - return param; -} + GIVEN("A sample map from an office demo world") + { + auto graph = rmf_traffic_ros2::convert(make_map_message(MAP_PATH), 0); + THEN("Map has all the graph waypoints and lanes") + { + // 68 waypoints in the map + CHECK(graph.num_waypoints() == 68); + // 7 waypoints are named + CHECK(graph.keys().size() == 7); + // 64 lanes (32 bidirectional) + CHECK(graph.num_lanes() == 64); + } + THEN("Waypoint properties are parsed correctly") + { + const auto pantry_wp = graph.find_waypoint("tinyRobot1_charger"); + const auto non_existing_wp = graph.find_waypoint("non_existing_wp"); + CHECK(non_existing_wp == nullptr); + REQUIRE(pantry_wp != nullptr); + CHECK(pantry_wp->get_map_name() == "building"); + // TODO check location + //CHECK(pantry_wp->get_location() == {1, 2}); + CHECK(pantry_wp->is_holding_point() == true); + CHECK(pantry_wp->is_passthrough_point() == false); + CHECK(pantry_wp->is_parking_spot() == true); + CHECK(pantry_wp->is_charger() == true); + } + THEN("Lane connectivity") + { + const auto coe_wp = graph.find_waypoint("coe"); + REQUIRE(coe_wp != nullptr); + const std::size_t wp_idx = coe_wp->index(); + const auto lanes_from_coe = graph.lanes_from(wp_idx); + REQUIRE(lanes_from_coe.size() == 1); + // TODO check this goes to the correct lane + // TODO lane properties + waypoint events + } + } -static auto make_string_param(const std::string& name, const std::string& val) -{ - rmf_building_map_msgs::msg::Param param; - param.name = name; - param.type = param.TYPE_STRING; - param.value_string = val; - return param; } +/* SCENARIO("Test conversion from rmf_building_map_msgs to rmf_traffic") { GIVEN("a graph with two nodes")