Slam sim with rules

slam_rules/CMakeLists.txt

@@ -0,0 +1,113 @@
+###################################################
+# 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)
+
+enable_testing()
+
+project(slam_sim)
+
+set(CMAKE_CXX_STANDARD 17)
+
+set(GAIA_COMPILE_FLAGS "-c -Wall -Wextra -ggdb")
+set(GAIA_LINK_FLAGS "-ggdb")
+
+# 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")
+
+# Default compiler/linker flags.
+add_compile_options(-c -Wall -Wextra -ggdb)
+
+# --- Generate Direct Access classes from DDL---
+process_schema(
+  DDL_FILE ${PROJECT_SOURCE_DIR}/gaia/slam.ddl
+  DATABASE_NAME slam
+)
+
+# -- Translate ruleset into CPP --
+translate_ruleset(
+  RULESET_FILE ${PROJECT_SOURCE_DIR}/gaia/slam.ruleset
+  DATABASE_NAME slam
+  CLANG_PARAMS
+    -I ${PROJECT_SOURCE_DIR}/include
+)
+
+# Application
+
+add_executable(slam_sim
+  src/analyze.cpp
+  src/globals.cpp
+  src/navigation.cpp
+  src/occupancy.cpp
+  src/paths.cpp
+  src/slam_sim.cpp
+  src/utils/blob_cache.cpp
+  src/utils/landmark_description.cpp
+  src/utils/line_segment.cpp
+)
+
+target_add_gaia_generated_sources(slam_sim)
+
+target_include_directories(slam_sim PRIVATE
+  ${GAIA_INC}
+  ${PROJECT_SOURCE_DIR}/include
+)
+
+target_link_directories(slam_sim PRIVATE ${GAIA_LIB_DIR})
+target_link_libraries(slam_sim PRIVATE ${GAIA_LIB} rt Threads::Threads)
+
+# Tests
+
+add_executable(test_blob_cache
+  tests/test_blob_cache.cpp
+  src/utils/blob_cache.cpp
+)
+
+set_target_properties(test_blob_cache PROPERTIES COMPILE_FLAGS "${GAIA_COMPILE_FLAGS}")
+set_target_properties(test_blob_cache PROPERTIES LINK_FLAGS "${GAIA_LINK_FLAGS}")
+target_include_directories(test_blob_cache PRIVATE ${PROJECT_SOURCE_DIR}/include)
+target_link_libraries(test_blob_cache PRIVATE rt)
+
+add_test(NAME test_blob_cache COMMAND test_blob_cache)
+
+add_executable(test_line_segment
+  tests/test_line_segment.cpp
+  src/utils/line_segment.cpp
+)
+
+set_target_properties(test_line_segment PROPERTIES COMPILE_FLAGS "${GAIA_COMPILE_FLAGS}")
+set_target_properties(test_line_segment PROPERTIES LINK_FLAGS "${GAIA_LINK_FLAGS}")
+target_include_directories(test_line_segment PRIVATE ${PROJECT_SOURCE_DIR}/include)
+target_link_libraries(test_line_segment PRIVATE rt)
+
+add_test(NAME test_line_segment COMMAND test_line_segment)
+
+add_executable(test_analyze
+  tests/test_analyze.cpp
+  src/analyze.cpp
+  src/globals.cpp
+  src/navigation.cpp
+  src/occupancy.cpp
+  src/paths.cpp
+  src/utils/line_segment.cpp
+  src/utils/landmark_description.cpp
+)
+
+target_add_gaia_generated_sources(test_analyze)
+
+set_target_properties(test_analyze PROPERTIES COMPILE_FLAGS "${GAIA_COMPILE_FLAGS}")
+set_target_properties(test_analyze PROPERTIES LINK_FLAGS "${GAIA_LINK_FLAGS}")
+target_include_directories(test_analyze PRIVATE ${GAIA_INC})
+target_include_directories(test_analyze PRIVATE ${PROJECT_SOURCE_DIR}/include)
+target_link_libraries(test_analyze PRIVATE ${GAIA_LIB} rt)
+
+add_test(NAME test_analyze COMMAND test_analyze)

slam_rules/Makefile

@@ -0,0 +1,17 @@
+
+all:
+	cd gaia && make
+	cd src && make
+
+run: 
+	cd src && make run
+
+drun: 
+	cd src && make drun
+
+refresh: clean all
+
+clean:
+	cd gaia && make clean
+	cd src && make clean
+

slam_rules/Makefile.inc

@@ -0,0 +1,9 @@
+ROOT := $(dir $(lastword $(MAKEFILE_LIST)))
+
+WFLAGS = -Wall -Werror
+DEBUG_FLAG = -g
+#OPTIMIZATION_FLAG = -O3
+MISC_FLAGS = --std=c++17 
+INC = -I/opt/gaia/include/ -I$(ROOT)gaia/ -I$(ROOT)gaia/slam/ -I$(ROOT)include/
+
+CPPFLAGS = $(WFLAGS) $(DEBUG_FLAG) $(OPTIMIZATION_FLAG) $(MISC_FLAGS) $(INC)

slam_rules/README

@@ -0,0 +1,8 @@
+To compile, type:
+  make
+
+To run, type:
+  make run
+
+Output of simulation is stored in image files in src/.
+

slam_rules/data/map.json

@@ -0,0 +1,66 @@
+{
+  "description": "4 rooms, 3 with table",
+  "width": 15.0,
+  "height": 10.0,
+  "world": [
+    {
+      "name": "Walls",
+      "vertices": [
+        { "x": -6.9, "y":  4.9 },
+        { "x": -0.1, "y":  4.9 },
+        { "x": -0.1, "y":  0.1 },
+        { "x": -1.5, "y":  0.1 },
+        { "x": -1.5, "y": -0.1 },
+        { "x": -0.1, "y": -0.1 },
+        { "x": -0.1, "y": -3.0 },
+        { "x":  0.1, "y": -3.0 },
+        { "x":  0.1, "y":  4.9 },
+        { "x":  6.9, "y":  4.9 },
+        { "x":  6.9, "y": -1.9 },
+        { "x":  2.1, "y":  0.1 },
+        { "x":  2.1, "y": -0.1 },
+        { "x":  6.9, "y": -2.1 },
+        { "x":  6.9, "y": -4.9 },
+        { "x": -6.9, "y": -4.9 },
+        { "x": -6.9, "y": -0.1 },
+        { "x": -2.9, "y": -0.1 },
+        { "x": -2.9, "y":  0.1 },
+        { "x": -6.9, "y":  0.1 },
+        { "x": -6.9, "y":  4.9 }
+      ]
+    },
+    {
+      "name": "Table-1",
+      "vertices": [
+        { "x": -5.5, "y":  3.5 },
+        { "x": -4.0, "y":  3.5 },
+        { "x": -4.0, "y":  2.5 },
+        { "x": -5.5, "y":  2.5 },
+        { "x": -5.5, "y":  3.5 }
+      ]
+    },
+    {
+      "name": "Table-2",
+      "vertices": [
+        { "x":  2.5, "y": -2.5 },
+        { "x":  3.8, "y": -3.5 },
+        { "x":  1.2, "y": -3.5 },
+        { "x":  2.5, "y": -2.5 }
+      ]
+    },
+    {
+      "name": "Table-3",
+      "vertices": [
+        { "x":  3.0, "y":  3.0 },
+        { "x":  4.0, "y":  3.0 },
+        { "x":  4.0, "y":  2.0 },
+        { "x":  3.0, "y":  2.0 },
+        { "x":  3.0, "y":  3.0 }
+      ]
+    }
+  ],
+  "destinations": [
+      { "x":  5.0, "y":  0.5 },
+      { "x":  -5.7, "y":  4.1 }
+  ]
+}

slam_rules/data/maze.json

@@ -0,0 +1,47 @@
+{
+  "description": "Simple maze",
+  "width": 18.0,
+  "height": 10.0,
+  "world": [
+    {
+      "name": "Walls",
+      "vertices": [
+          { "x": -8.9, "y":  4.9 },
+          { "x": -6.1, "y":  4.9 },
+          { "x": -6.1, "y": -0.1 },
+          { "x": -2.9, "y": -0.1 },
+          { "x": -2.9, "y":  0.1 },
+          { "x": -5.9, "y":  0.1 },
+          { "x": -5.9, "y":  4.9 },
+          { "x":  2.9, "y":  4.9 },
+          { "x":  2.9, "y":  0.1 },
+          { "x": -0.1, "y":  0.1 },
+          { "x": -0.1, "y": -0.1 },
+          { "x":  3.1, "y": -0.1 },
+          { "x":  3.1, "y":  4.9 },
+          { "x":  5.9, "y":  4.9 },
+          { "x":  5.9, "y": -2.6 },
+          { "x":  6.1, "y": -2.6 },
+          { "x":  6.1, "y":  4.9 },
+          { "x":  8.9, "y":  4.9 },
+          { "x":  8.9, "y": -4.9 },
+          { "x": -8.9, "y": -4.9 },
+          { "x": -8.9, "y":  4.9 }
+      ]
+    },
+    {
+      "name": "Island",
+      "vertices": [
+        { "x": -3.1, "y":  2.6 },
+        { "x":  0.1, "y":  2.6 },
+        { "x":  0.1, "y":  2.4 },
+        { "x": -3.1, "y":  2.4 },
+        { "x": -3.1, "y":  2.6 }
+      ]
+    }
+  ],
+  "destinations": [
+      { "x":  7.5, "y":  4.0 },
+      { "x":  -4.5, "y":  4.0 }
+  ]
+}

slam_rules/data/maze2.json

@@ -0,0 +1,30 @@
+{
+  "description": "Simple maze",
+  "width": 10.0,
+  "height": 8.0,
+  "world": [
+    {
+      "name": "Walls",
+      "vertices": [
+          { "x": -4.9, "y":  3.9 },
+          { "x": -2.1, "y":  3.9 },
+          { "x": -2.1, "y": -2.1 },
+          { "x": -1.9, "y": -2.1 },
+          { "x": -1.9, "y":  3.9 },
+          { "x":  1.9, "y":  3.9 },
+          { "x":  1.9, "y": -2.1 },
+          { "x":  2.1, "y": -2.1 },
+
+          { "x":  2.1, "y":  3.9 },
+          { "x":  4.9, "y":  3.9 },
+          { "x":  4.9, "y": -3.9 },
+          { "x": -4.9, "y": -3.9 },
+          { "x": -4.9, "y":  3.9 }
+      ]
+    }
+  ],
+  "destinations": [
+      { "x":  3.5, "y":  3.0 },
+      { "x":  0.0, "y":  3.0 }
+  ]
+}

slam_rules/data/maze3.json

@@ -0,0 +1,57 @@
+{
+  "description": "Square maze",
+  "width": 15.0,
+  "height": 15.0,
+  "world": [
+    {
+      "name": "Walls",
+      "vertices": [
+        { "x":  0.1, "y":   0.1},
+        { "x": 14.9, "y":   0.1},
+        { "x": 14.9, "y":   2.9},
+        { "x": 11.9, "y":   2.9},
+        { "x": 11.9, "y":  11.9},
+        { "x":  9.1, "y":  11.9},
+        { "x":  9.1, "y":   2.9},
+        { "x":  8.9, "y":   2.9},
+        { "x":  8.9, "y":  11.9},
+        { "x":  2.9, "y":  11.9},
+        { "x":  2.9, "y":  12.1},
+        { "x": 12.1, "y":  12.1},
+        { "x": 12.1, "y":   3.1},
+        { "x": 14.9, "y":   3.1},
+        { "x": 14.9, "y":  14.9},
+        { "x":  0.1, "y":  14.9},
+        { "x":  0.1, "y":   0.1}
+      ]
+    },
+    {
+      "name": "Island",
+      "vertices": [
+        { "x":  2.9, "y":   2.9},
+        { "x":  6.1, "y":   2.9},
+        { "x":  6.1, "y":   3.1},
+        { "x":  2.9, "y":   3.1},
+        { "x":  2.9, "y":   2.9}
+      ]
+    },
+    {
+      "name": "Cup",
+      "vertices": [
+        { "x":  2.9, "y":   5.9},
+        { "x":  3.1, "y":   5.9},
+        { "x":  3.1, "y":   8.9},
+        { "x":  5.9, "y":   8.9},
+        { "x":  5.9, "y":   5.9},
+        { "x":  6.1, "y":   5.9},
+        { "x":  6.1, "y":   9.1},
+        { "x":  2.9, "y":   9.1},
+        { "x":  2.9, "y":   5.9}
+      ]
+    }
+  ],
+  "destinations": [
+    { "x":  13.5, "y":  4.5 },
+    { "x":   4.5, "y":  7.5 }
+  ]
+}

slam_rules/gaia/.gitignore

@@ -0,0 +1,2 @@
+slam_rules.cpp
+slam

slam_rules/gaia/Makefile

@@ -0,0 +1,31 @@
+include ../Makefile.inc
+
+RULES_CPP = slam_rules.cpp
+DB_NAME = slam
+
+OBJS = slam_rules.o slam/gaia_slam.o
+
+all: all_gaia all_procedure_subshell
+
+all_gaia:
+	gaiac slam.ddl -g --db-name $(DB_NAME) -o $(DB_NAME)
+	gaiat slam.ruleset -output $(RULES_CPP) -- -I/usr/lib/clang/10/include $(INC)
+
+
+# when freshly making, slam_rules.cpp may not exist (e.g., if 'make clean'
+# 	was just run). drop into subshell for making local procedural files
+# 	as that will induce a scan of local files
+all_procedure_subshell:
+	make all_procedure
+
+all_procedure: $(OBJS)
+
+refresh: clean all
+
+%.o: %.cpp 
+	$(CXX) $(INC) -c $(CPPFLAGS) $< 
+
+clean:
+	rm -f *.o $(RULES_CPP)
+	rm -rf $(DB_NAME)
+