finished moving octree into main lib

README.md

 
 # Main repo for the Schpin project.
 
-The packages can be compiled with traditional ROS tools, but you need a compiler that handles C++17.
-Keep in mind however that you also need a schpin-robot repository with data.
 
-As of now, only thing presentable is the motion planner.
-Use:
-
-```
-$ roslaunch repos/schpin-ros/schpin_motion/launch/debug_env.launch
-```
-
-This opens rviz with a model of our robot and prepares necessary parameters.
-Use:
-
-```
-$ rosrun schpin_motion manual
-```
-
-To get access to the control console, write "help" for basic manual for control.
-You have to start the motion planner at first: local start
-
-You can get some feedback from the motion planner on standart ros topics, however we recommend our logger utility:
-
-```
-$ rosrun schpin_motion logger                                     
-```
-Please keep in mind that this is under heavy development and state of the code is sometimes ... well I warned you!

schpin_control/CMakeLists.txt

@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.7.2)
 
 project(schpin_control)
 
+option(SCHPIN_EMBEDDED_ENABLED "Decides whenever embedded firmware should also be compiled" OFF)
+
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(schpin_lib REQUIRED)
@@ -140,19 +142,21 @@ endif()
 
 include(ExternalProject)
 
-ExternalProject_Add(
-  blob_stm32
-  SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/stm32"
-  CMAKE_ARGS
-    -DCMAKE_VERBOSE_BUILD=1
-    -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}
-  BUILD_ALWAYS 1
-  USES_TERMINAL_DOWNLOAD 1
-  USES_TERMINAL_UPDATE 1
-  USES_TERMINAL_CONFIGURE 1
-  USES_TERMINAL_BUILD 1)
-
-ExternalProject_Get_Property(blob_stm32 BINARY_DIR)
+if(SCHPIN_EMBEDDED_ENABLED)
+  ExternalProject_Add(
+    blob_stm32
+    SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/stm32"
+    CMAKE_ARGS
+      -DCMAKE_VERBOSE_BUILD=1
+      -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}
+    BUILD_ALWAYS 1
+    USES_TERMINAL_DOWNLOAD 1
+    USES_TERMINAL_UPDATE 1
+    USES_TERMINAL_CONFIGURE 1
+    USES_TERMINAL_BUILD 1)
+
+  ExternalProject_Get_Property(blob_stm32 BINARY_DIR)
+endif()
 
 # TODO analyze properly where stuff should be installed and fix in all packges
 install(

schpin_control/package.xml

@@ -14,7 +14,6 @@
   <depend>urdf</depend>
   <depend>rclcpp</depend>
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_clang_tidy</test_depend>
   <!--<test_depend>ament_cmake_copyright</test_depend>-->
     <!--<test_depend>ament_cmake_lint_cmake</test_depend>-->

schpin_lib/include/schpin_lib/octree/node.h

@@ -134,7 +134,7 @@ public:
                 return &content_;
         }
 
-        friend constexpr bool
+        friend bool
         operator<=>( const oc_node< Content >& lh, const oc_node< Content >& rh ) = default;
 
         void force_update()
@@ -186,7 +186,6 @@ public:
                 return res;
         }
 
-
 private:
         std::unique_ptr< container > children_ptr_;
         Content                      content_;

schpin_lib/include/schpin_lib/octree/node_view.h

@@ -123,7 +123,7 @@ public:
         }
 
         template < typename FilterFunction >
-        constexpr std::optional< em::min_max< point< 3, oc_frame > > >
+        std::optional< em::min_max< point< 3, oc_frame > > >
         recursive_matched_area( FilterFunction&& f ) const
         {
                 if ( f( *this ) ) {

schpin_lib/include/schpin_lib/octree/tree.h

 #include "schpin_lib/octree/node.h"
 #include "schpin_lib/octree/visitors.h"
 
+#include <schpin_lib/logging.h>
+
 #pragma once
 
 namespace schpin
@@ -124,11 +126,13 @@ private:
             const Visitor&                     vis,
             const typename Visitor::info_type& parent_info )
         {
+                SCHPIN_ASSERT( nview.edge_length() != em::length{ 0.f } );
                 const const_node_view&      cnview{ *nview, nview.context() };
                 typename Visitor::info_type new_info;
 
                 if ( nview.has_max_depth() ) {
                         vis.on_leaf( cnview, parent_info, **nview );
+                        return;
                 }
 
                 if ( cnview.empty() ) {

schpin_lib/include/schpin_lib/octree/visitors.h

@@ -93,7 +93,7 @@ private:
 
 public:
         template < typename T >
-        oc_collision_visitor( T batch, token tok )
+        oc_collision_visitor( T&& batch, token tok )
           : checkers_()
           , token_( tok )
         {

schpin_lib/package.xml

@@ -12,12 +12,13 @@
   <depend>schpin_msgs</depend>
   <depend>urdf</depend>
   <depend>eigen</depend>
+  <depend>assimp</depend>
+  <depend>libreadline</depend>
   <depend>nlohmann-json-dev</depend>
   <depend>tbb</depend>
   <depend>geometry_msgs</depend>
   
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_clang_tidy</test_depend>
   <!--<test_depend>ament_cmake_copyright</test_depend>-->
     <!--<test_depend>ament_cmake_lint_cmake</test_depend>-->

schpin_motion/CMakeLists.txt

@@ -28,7 +28,6 @@ endfunction()
 
 add_library(
   schpin_motion_always_fast STATIC
-  src/body.cpp
   src/leg/sidepath.cpp
   src/planner.cpp
   src/leg/model.cpp
@@ -40,15 +39,17 @@ add_library(
 motion_setup_base(schpin_motion_always_fast)
 
 target_include_directories(schpin_motion_always_fast PUBLIC src/)
-target_compile_options(schpin_motion_always_fast PRIVATE -O3)
+target_compile_options(schpin_motion_always_fast PRIVATE -O3 -g)
 target_link_libraries(schpin_motion_always_fast PUBLIC stdc++fs)
 
-add_library(schpin_motion STATIC src/robot/storage.cpp)
+add_library(schpin_motion STATIC src/robot/storage.cpp
+  src/body.cpp
+  )
 motion_setup_base(schpin_motion)
 
 target_include_directories(schpin_motion PUBLIC src/)
 target_link_libraries(schpin_motion PUBLIC stdc++fs schpin_motion_always_fast)
-target_compile_options(schpin_motion PRIVATE -O3 -g)
+target_compile_options(schpin_motion PRIVATE -O0 -g)
 
 function(motion_setup target)
   motion_setup_base(${target})

schpin_motion/package.xml

@@ -17,7 +17,6 @@
   <depend>interactive_markers</depend>
   
   <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_clang_tidy</test_depend>
   <!--<test_depend>ament_cmake_copyright</test_depend>-->
     <!--<test_depend>ament_cmake_lint_cmake</test_depend>-->

schpin_motion/test/unit_test.cmake

@@ -23,7 +23,7 @@ if(BUILD_TESTING)
 
   motion_setup(schpin_test_lib)
   target_include_directories(schpin_test_lib PUBLIC test/include/ src/)
-  target_compile_options(schpin_test_lib PRIVATE -O3)
+  target_compile_options(schpin_test_lib PRIVATE -O3 -g)
 
   file(
     GLOB_RECURSE gtest_cpp_files