Initial Commit (tested training, testing, and TRT conversion)

flightlib/third_party/arc_utilities/src/test_dijkstras.cpp

@@ -0,0 +1,124 @@
+#include <iostream>
+
+#include <arc_utilities/dijkstras.hpp>
+#include <arc_utilities/pretty_print.hpp>
+
+int main(int argc, char* argv[])
+{
+    (void)argc;
+    (void)argv;
+
+    std::cout << "Creating graph with nodes indices:\n\n";
+    std::cout << "  2 | 5 | 8\n"
+              << "  --+---+--\n"
+              << "  1 | 4 | 7\n"
+              << "  --+---+--\n"
+              << "  0 | 3 | 6\n\n";
+
+    arc_dijkstras::Graph<Eigen::Vector2d> graph(9);
+    for (double x = -1; x <= 1; x += 1)
+    {
+        for (double y = -1; y <= 1; y += 1)
+        {
+            graph.AddNode(Eigen::Vector2d(x, y));
+        }
+    }
+
+    // Y-direction edges
+    graph.AddEdgeBetweenNodes(0, 1, 1.0);
+    graph.AddEdgesBetweenNodes(1, 2, 1.0);
+    graph.AddEdgesBetweenNodes(3, 4, 1.0);
+    graph.AddEdgesBetweenNodes(4, 5, 1.0);
+    graph.AddEdgesBetweenNodes(6, 7, 1.0);
+    graph.AddEdgesBetweenNodes(7, 8, 1.0);
+
+    // X-direction edges
+    graph.AddEdgesBetweenNodes(0, 3, 1.0);
+    graph.AddEdgesBetweenNodes(3, 6, 1.0);
+    graph.AddEdgesBetweenNodes(1, 4, 1.0);
+    graph.AddEdgesBetweenNodes(4, 7, 1.0);
+    graph.AddEdgesBetweenNodes(2, 5, 1.0);
+    graph.AddEdgesBetweenNodes(5, 8, 1.0);
+
+    assert(graph.CheckGraphLinkage());
+
+
+    auto dijkstras_result_4connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
+
+    std::cout << "4-connected edges\n"
+              << "Node index            : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
+    std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.first) << std::endl;
+    std::cout << "Distance              : " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.second) << std::endl;
+
+    // Diagonal edges
+    graph.AddEdgesBetweenNodes(0, 4, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(1, 5, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(3, 7, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(4, 8, std::sqrt(2));
+
+    graph.AddEdgesBetweenNodes(1, 3, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(2, 4, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(4, 6, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(5, 7, std::sqrt(2));
+
+    assert(graph.CheckGraphLinkage());
+    auto dijkstras_result_8connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
+
+    std::cout << "\n8-connected edges\n"
+              << "Node index            : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
+    std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.first) << std::endl;
+    std::cout << "Distance              : " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.second) << std::endl;
+
+    std::cout << "\nSerialization test... ";
+
+    arc_dijkstras::Graph<Eigen::Vector2d> serialization_test_graph(2);
+    serialization_test_graph.AddNode(Eigen::Vector2d(0,0));
+    serialization_test_graph.AddNode(Eigen::Vector2d(1,1));
+    serialization_test_graph.AddEdgesBetweenNodes(0, 1, 1.0);
+
+    // Define the graph value serialization function
+    const auto value_serializer_fn = [] (const Eigen::Vector2d& value, std::vector<uint8_t>& buffer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        uint64_t running_total = 0;
+
+        running_total += arc_utilities::SerializeFixedSizePOD<double>(value(0), buffer);
+        running_total += arc_utilities::SerializeFixedSizePOD<double>(value(1), buffer);
+
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+
+        assert(running_total == bytes_written);
+
+        return bytes_written;
+    };
+
+    // Define the graph value serialization function
+    const auto value_deserializer_fn = [] (const std::vector<uint8_t>& buffer, const uint64_t current)
+    {
+        uint64_t current_position = current;
+
+        // Deserialze 2 doubles
+        std::pair<double, uint64_t> x = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+        current_position += x.second;
+        std::pair<double, uint64_t> y = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+        current_position += y.second;
+
+        const Eigen::Vector2d deserialized(x.first, y.first);
+
+        // Figure out how many bytes were read
+        const uint64_t bytes_read = current_position - current;
+        return std::make_pair(deserialized, bytes_read);
+    };
+
+    // Serialze the graph
+    std::vector<uint8_t> buffer;
+    serialization_test_graph.SerializeSelf(buffer, value_serializer_fn);
+
+    auto deserialized_result = arc_dijkstras::Graph<Eigen::Vector2d>::Deserialize(buffer, 0, value_deserializer_fn);
+    assert(deserialized_result.first.CheckGraphLinkage());
+
+    std::cout << "passed" << std::endl;
+
+    return 0;
+}