diff --git a/gaia_slam/.gitignore b/gaia_slam/.gitignore new file mode 100644 index 0000000..192fcfd --- /dev/null +++ b/gaia_slam/.gitignore @@ -0,0 +1,6 @@ +logs/ +.gaia_slam_db +cmake-* +.idea +.clang-tidy +.clang-format diff --git a/gaia_slam/CMakeLists.txt b/gaia_slam/CMakeLists.txt new file mode 100644 index 0000000..a0d8e51 --- /dev/null +++ b/gaia_slam/CMakeLists.txt @@ -0,0 +1,74 @@ +################################################### +# Copyright (c) Gaia Platform LLC +# +# Use of this source code is governed by the MIT +# license that can be found in the LICENSE.txt file +# or at https://opensource.org/licenses/MIT. +################################################### + +cmake_minimum_required(VERSION 3.16) + +project(gaia_slam) + +set(CMAKE_CXX_STANDARD 17) + +# We need pthreads support. +set(CMAKE_THREAD_PREFER_PTHREAD TRUE) +set(THREADS_PREFER_PTHREAD_FLAG TRUE) +find_package(Threads REQUIRED) + +include("/opt/gaia/cmake/gaia.cmake") + +# --- Generate Direct Access classes from DDL--- +process_schema( + DDL_FILE ${PROJECT_SOURCE_DIR}/gaia/gaia_slam.ddl + DATABASE_NAME gaia_slam +) + +# -- Translate ruleset into CPP -- +translate_ruleset( + RULESET_FILE ${PROJECT_SOURCE_DIR}/gaia/gaia_slam.ruleset + DATABASE_NAME gaia_slam + CLANG_PARAMS + -I ${PROJECT_SOURCE_DIR}/include +) + +# +# Direct Access Example +# +add_executable(gaia_slam_direct_access + src/main_direct_access.cpp + src/graph.cpp +) + +target_add_gaia_generated_sources(gaia_slam_direct_access) + +target_include_directories(gaia_slam_direct_access PRIVATE + ${GAIA_INC} + ${PROJECT_SOURCE_DIR}/include +) + +target_link_libraries(gaia_slam_direct_access PRIVATE + ${GAIA_LIB} + Threads::Threads +) + +# +# Rules Example +# +add_executable(gaia_slam_rules + src/main_rules.cpp + src/graph.cpp +) + +target_add_gaia_generated_sources(gaia_slam_rules) + +target_include_directories(gaia_slam_rules PRIVATE + ${GAIA_INC} + ${PROJECT_SOURCE_DIR}/include +) + +target_link_libraries(gaia_slam_rules PRIVATE + ${GAIA_LIB} + Threads::Threads +) diff --git a/gaia_slam/README.md b/gaia_slam/README.md new file mode 100644 index 0000000..ee73318 --- /dev/null +++ b/gaia_slam/README.md @@ -0,0 +1,11 @@ +This example aims at teaching Gaia concepts using a real-world use-case scenario (SLAM). This is only for demonstration +purposes and does not aim to be a real solution for SLAM. + +Still, you can appreciate some elements of Graph-based SLAM such as the graph data structure (`graph`, `vertex`, `edge`), +observation of the external world (`incoming_data_event`), and building of the graph elements when new observations come in. + +The data model (the SLAM graph) is defined in `gaia/gaia_slam.ddl`, while the rules are defined in `gaia/gaia_slam.ruleset`. + +There are two executables: +1. `src/main_rules.cpp`: Shows how mutating the database triggers the rules in `gaia/gaia_slam.ruleset`. +2. `src/main_direct_access.cpp`: Show how to manipulate the Graph using the direct_access API. This code does not use rules. diff --git a/gaia_slam/gaia/gaia_slam.ddl b/gaia_slam/gaia/gaia_slam.ddl new file mode 100644 index 0000000..4b0d65c --- /dev/null +++ b/gaia_slam/gaia/gaia_slam.ddl @@ -0,0 +1,69 @@ +---------------------------------------------------- +-- Copyright (c) Gaia Platform LLC +-- +-- Use of this source code is governed by the MIT +-- license that can be found in the LICENSE.txt file +-- or at https://opensource.org/licenses/MIT. +---------------------------------------------------- + +database gaia_slam + +-- SLAM graph. +table graph +( + id string unique, + vertices references vertex[], + edges references edge[] +) + +-- A new vertex is created every time a new observation from a sensor is done. +table vertex +( + id uint64 unique, + type uint8, + data uint8[], + pose_x double, + pose_y double, + + -- Relationships + graph_id string, + graph references graph + using vertices + where vertex.graph_id = graph.id, + + in_edges references edge[], + out_edges references edge[] +) + +-- A new edge is created between every new created vertex and the previous one. +table edge +( + id uint64 unique, + + -- Relationships + graph_id string, + graph references graph + where edge.graph_id = graph.id, + + dest_id uint64, + dest references vertex + using in_edges + where edge.dest_id = vertex.id, + + src_id uint64, + src references vertex + using out_edges + where edge.src_id = vertex.id +) + +-- This table represents a generic input from a sensor. +-- i.e. Lidar sensor, image, etc.. +-- Note: this is just an example. +table incoming_data_event +( + id uint64 unique, + type uint8, + data uint8[], + pose_x double, + pose_y double +) diff --git a/gaia_slam/gaia/gaia_slam.ruleset b/gaia_slam/gaia/gaia_slam.ruleset new file mode 100644 index 0000000..7a2cb02 --- /dev/null +++ b/gaia_slam/gaia/gaia_slam.ruleset @@ -0,0 +1,91 @@ +//////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////// + +#include + +#include + +constexpr uint64_t c_first_vertex_id = 0; +std::atomic c_last_edge_id = 0; + +// The serial_group() keyword makes the rules in the ruleset run serially: +// only one rule is executed at a time. This prevents transaction +// conflicts when modifying the same data. +ruleset data_processing : serial_group() +{ + + // Process an incoming data event by creating a new vertex in the graph. + on_insert(incoming_data_event) + { + gaia_log::app().info("Processing incoming_data_event(id: {})", id); + + // The loop below finds the maximum id value for existing edge ids. + // In practice, we wouldn't search the table each time to look for + // a new ID. This is only done to provide an example of searching + // a table. + // The leading slash '/' means to search through all records in the + // specified table. + uint64_t max_vertex_id = c_first_vertex_id; + for (/v:vertex) + { + if (v.id > max_vertex_id) + { + max_vertex_id = v.id; + } + } + + uint64_t new_vertex_id = max_vertex_id + 1; + gaia_log::app().info("Creating new vertex(id: {})", new_vertex_id); + + // Create a vertex record + std::string graph_id_string("graph_1"); + vertex.insert( + id: new_vertex_id, + type: incoming_data_event.type, + data: incoming_data_event.data, + pose_x: incoming_data_event.pose_x, + pose_y: incoming_data_event.pose_y, + graph_id: graph_id_string.c_str() + ); + } + + // Process each new vertex by creating an edge with the previous existing vertex. + on_insert(vertex) + { + gaia_log::app().info("Processing vertex(id: {})", id); + + if (vertex.id == c_first_vertex_id) + { + return; + } + + uint64_t prev_vertex_id = vertex.id - 1; + uint64_t new_edge_id = c_last_edge_id.fetch_add(1); + gaia_log::app().info( + "Creating edge(id: {}) with between vertex(id: {}) and vertex(id: {})", + new_edge_id, prev_vertex_id, vertex.id); + + edge.insert( + id: new_edge_id, + graph_id: vertex.graph_id, + src_id: prev_vertex_id, + dest_id: vertex.id); + } +} + +ruleset graph_processing +{ + // Reacts to the creation of an edge by just printing it. + // This rule could contain part of the SALM algorithm. + on_insert(edge) + { + gaia_log::app().info( + "Created edge(id: {}) vertex(id: {}) -> vertex(id: {})", + edge.id, edge.src_id, edge.dest_id); + } +} diff --git a/gaia_slam/include/graph.hpp b/gaia_slam/include/graph.hpp new file mode 100644 index 0000000..e71c640 --- /dev/null +++ b/gaia_slam/include/graph.hpp @@ -0,0 +1,24 @@ +//////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////// + +#pragma once + +#include + +namespace gaia::gaia_slam::graph +{ + +graph_t create_graph(const std::string& uuid); + +vertex_t create_vertex(const graph_t& graph, int64_t id, int64_t vertex_type); + +edge_t create_edge(int64_t id, const vertex_t& src, const vertex_t& dest); + +void clear_data(); + +} // namespace gaia::gaia_slam::graph diff --git a/gaia_slam/include/vertex_types.hpp b/gaia_slam/include/vertex_types.hpp new file mode 100644 index 0000000..fb34671 --- /dev/null +++ b/gaia_slam/include/vertex_types.hpp @@ -0,0 +1,20 @@ +//////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////// + +#pragma once + +#include + +namespace gaia::gaia_slam::vertex_type +{ + +static constexpr uint8_t c_lidar_scan = 0; +static constexpr uint8_t c_image_keyframe = 1; +static constexpr uint8_t c_visual_landmark = 2; + +} // namespace gaia::gaia_slam::vertex_type diff --git a/gaia_slam/src/graph.cpp b/gaia_slam/src/graph.cpp new file mode 100644 index 0000000..f116b92 --- /dev/null +++ b/gaia_slam/src/graph.cpp @@ -0,0 +1,64 @@ +//////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////// + +#include "graph.hpp" + +namespace gaia::gaia_slam::graph +{ + +graph_t create_graph(const std::string& id) +{ + return graph_t::get(graph_t::insert_row(id.c_str())); +} + +vertex_t create_vertex(const graph_t& graph, int64_t id, int64_t vertex_type) +{ + vertex_writer vertex_w; + vertex_w.id = id; + vertex_w.type = vertex_type; + vertex_w.graph_id = graph.id(); + + return vertex_t::get(vertex_w.insert_row()); +} + +edge_t create_edge(int64_t id, const vertex_t& src, const vertex_t& dest) +{ + if (src.graph_id() == dest.graph_id()) + { + throw std::runtime_error("You cannot create an edge from vertices in different graphs!"); + } + + edge_writer edge_w; + edge_w.id = id; + edge_w.src_id = src.id(); + edge_w.dest_id = dest.id(); + edge_w.graph_id = src.graph_id(); + + return edge_t::get(edge_w.insert_row()); +} + +template +void clear_table() +{ + for (auto obj_it = T_type::list().begin(); + obj_it != T_type::list().end();) + { + auto next_obj_it = obj_it++; + next_obj_it->delete_row(); + } +} + +void clear_data() +{ + clear_table(); + clear_table(); + clear_table(); + clear_table(); +} + +} // namespace gaia::gaia_slam::graph diff --git a/gaia_slam/src/main_direct_access.cpp b/gaia_slam/src/main_direct_access.cpp new file mode 100644 index 0000000..a5ced8e --- /dev/null +++ b/gaia_slam/src/main_direct_access.cpp @@ -0,0 +1,104 @@ +//////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////// + +#include +#include +#include +#include +#include + +#include "gaia_gaia_slam.h" +#include "graph.hpp" +#include "vertex_types.hpp" + +using namespace gaia::direct_access; +using namespace gaia::gaia_slam; +using namespace gaia::gaia_slam::graph; + +/** + * Showcase the usage of the direct_access API to create/read data from the database. + * This example does not make usage of Rules. + */ +int main() +{ + gaia::system::initialize(); + + // This examples wants to focus on direct_access hence the rules are disabled. + gaia::rules::unsubscribe_rules(); + + gaia::db::begin_transaction(); + + clear_data(); + + graph_t graph = create_graph("graph_1"); + + vertex_t v1 = create_vertex(graph, 1, vertex_type::c_lidar_scan); + vertex_t v2 = create_vertex(graph, 2, vertex_type::c_lidar_scan); + vertex_t v3 = create_vertex(graph, 3, vertex_type::c_visual_landmark); + + edge_t e1 = create_edge(1, v1, v2); + edge_t e2 = create_edge(2, v2, v3); + + // Get all graph vertices and edges. + + gaia_log::app().info("=== Printing all vertices from Graph({}) ===", graph.id()); + + for (const vertex_t& v : graph.vertices()) + { + gaia_log::app().info(" Vertex({})", v.id()); + } + + gaia_log::app().info("=== Printing all edges from Graph({}) ===", graph.id()); + + for (const edge_t& e : graph.edges()) + { + gaia_log::app().info(" Edge({}): ({}) --> ({})", e.id(), e.src().id(), e.dest().id()); + } + + gaia_log::app().info("=== Get all vertices with type lidar_scan ===", graph.id()); + + for (const vertex_t& v : vertex_t::list() + .where(vertex_expr::type == vertex_type::c_lidar_scan)) + { + gaia_log::app().info(" Vertex({})", v.id()); + } + + gaia_log::app().info("=== Get the vertex with id 2 and type lidar_scan ===", graph.id()); + + auto vertex_it = vertex_t::list() + .where( + vertex_expr::id == 2 && + vertex_expr::type == vertex_type::c_lidar_scan); + + if (vertex_it.begin() == vertex_it.end()) + { + throw std::runtime_error("Cannot find vertex with id 1 and type lidar_scan"); + } + + gaia_log::app().info(" Vertex({})", vertex_it.begin()->id()); + + gaia_log::app().info("=== Get all edges with src_id or dest_id equals 1 ===", graph.id()); + + auto edge_it = edge_t::list() + .where( + edge_expr::dest == v1 || + edge_expr::src == v1); + + for (const edge_t& e : edge_it) + { + gaia_log::app().info(" Edge({}): ({}) --> ({})", e.id(), e.src().id(), e.dest().id()); + } + + gaia::db::commit_transaction(); + + /// + /// Now you can write your additional logic to use the direct_access API. + /// + + gaia::system::shutdown(); +} diff --git a/gaia_slam/src/main_rules.cpp b/gaia_slam/src/main_rules.cpp new file mode 100644 index 0000000..a29eb4b --- /dev/null +++ b/gaia_slam/src/main_rules.cpp @@ -0,0 +1,95 @@ +//////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////// + +#include + +#include + +#include +#include +#include + +#include "gaia_gaia_slam.h" +#include "graph.hpp" +#include "vertex_types.hpp" + +using namespace gaia::direct_access; +using namespace gaia::gaia_slam; +using namespace gaia::gaia_slam::graph; + +using std::this_thread::sleep_for; + +constexpr size_t c_num_data_events = 5; +constexpr uint32_t c_rule_wait_millis = 100; + +/** + * Wait an arbitrary amount of time for rule execution to terminate. + * Rules are triggered after commit and can take some time to fully execute. + */ +void wait_for_rules() +{ + sleep_for(std::chrono::milliseconds(c_rule_wait_millis)); +} + +/** + * Shows how mutating the database triggers the rules in `gaia/gaia_slam.ruleset`. + */ +int main() +{ + gaia::system::initialize(); + + // We explicitly handle the transactions with begin_transaction() and commit_transaction() + // to trigger the rules. + gaia::db::begin_transaction(); + clear_data(); + gaia::db::commit_transaction(); + + gaia_log::app().info("=== Creates a new Graph ==="); + + gaia::db::begin_transaction(); + graph_t graph = create_graph("graph_1"); + gaia::db::commit_transaction(); + + gaia_log::app().info("=== Inserting {} incoming_data_event of type {} ===", c_num_data_events, vertex_type::c_lidar_scan); + + // Insert some incoming_data_event rows to simulate data arriving from sensors. + // This should trigger the following rule: + // - on_insert(incoming_data_event): triggered when an incoming_data_event is inserted. This insert a new vertex. + gaia::db::begin_transaction(); + for (int i = 0; i < c_num_data_events; i++) + { + incoming_data_event_writer data_w; + data_w.id = i; + data_w.data = {1, 2, 3, 4, 5}; + data_w.pose_x = i * 2; + data_w.pose_y = i * 3; + data_w.type = vertex_type::c_image_keyframe; + data_w.insert_row(); + } + gaia::db::commit_transaction(); + + wait_for_rules(); + + gaia_log::app().info("=== Retrieving vertices/edges with type {} ===", vertex_type::c_image_keyframe); + + // This code verifies that the vertex with type vertex_type::c_image_keyframe has been created. + gaia::db::begin_transaction(); + for (const vertex_t& v : vertex_t::list() + .where(vertex_expr::type == vertex_type::c_image_keyframe)) + { + gaia_log::app().info("Vertex(id:{} type:{} pose_x:{} pose_y:{})", v.id(), v.type(), v.pose_x(), v.pose_y()); + } + gaia::db::commit_transaction(); + + /// + /// Now you can write your additional logic to trigger the other rules, + /// or write some new rules! + /// + + gaia::system::shutdown(); +}