Controller arch development

.gitmodules

@@ -1,3 +1,13 @@
+
+[submodule "driverless_ws/src/HesaiLidar_ROS_2.0"]
+	path = driverless_ws/src/HesaiLidar_ROS_2.0
+	url = https://github.com/HesaiTechnology/HesaiLidar_ROS_2.0
+[submodule "driverless_ws/src/ros2_numpy"]
+	path = driverless_ws/src/ros2_numpy
+	url = https://github.com/Box-Robotics/ros2_numpy.git
+[submodule "driverless_ws/src/zed-ros2-wrapper"]
+	path = driverless_ws/src/zed-ros2-wrapper
+	url = https://github.com/stereolabs/zed-ros2-wrapper.git
 [submodule "driverless_ws/src/eufs_msgs"]
   path = driverless_ws/src/eufs_msgs
   url = https://gitlab.com/eufs/eufs_msgs.git

driverless_ws/src/controls/CMakeLists.txt

@@ -1,6 +1,17 @@
 cmake_minimum_required(VERSION 3.5)
 project(controls)
 
+set(NO_CUDA FALSE)
+
+# disable cuda compilation
+#set(NO_CUDA TRUE)
+
+if(NO_CUDA)
+  add_compile_definitions(CONTROLS_NO_CUDA)
+else()
+  enable_language(CUDA)
+endif()
+
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
@@ -11,25 +22,50 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
+#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+#  add_compile_options(-Wall -Wextra -Wpedantic)
+#endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+find_package(planning REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+include_directories(${PROJECT_SOURCE_DIR}/src)
+include_directories(${PROJECT_SOURCE_DIR}/../)
+
+
+set(LIB_SOURCES
+        src/interface/interface.cpp
+        src/interface/cpu_environment.cpp
+        src/mppi/cpu_mppi.cpp)
+if(NOT NO_CUDA)
+  set(LIB_SOURCES ${LIB_SOURCES}
+          src/interface/cuda_environment.cu
+          src/mppi/cuda_mppi.cu)
 endif()
 
+add_library(controls_lib SHARED ${LIB_SOURCES})
+ament_target_dependencies(controls_lib planning rclcpp std_msgs)
+
+add_executable(controller src/nodes/controller.cpp)
+target_link_libraries(controller controls_lib)
+
+ament_target_dependencies(controller rclcpp)
+
+ament_export_targets(controls_libExport HAS_LIBRARY_TARGET)
+ament_export_dependencies(planning rclcpp std_msgs)
+
+install(
+    TARGETS controls_lib
+    EXPORT controls_libExport
+    DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+    TARGETS controller
+    DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()

driverless_ws/src/controls/conventions.md

@@ -0,0 +1,30 @@
+## Conventions
+
+Conventions for the controls subteam code.
+
+### Naming
+
+Use python naming:
+
+  - ``lower_snake_case`` for values (including functions/methods, 150 moment)
+    - Reason: ``lowerCamelCase`` is just ugly
+  - ``PascalCase`` for types
+    - Reason: prevents declaration issues like ``car car;``
+  - Namespace-level declarations should have namespae corresponding to their directory
+    - e.g. a declaration in ``src/mppi`` should belong to namespace ``controls::mppi``
+
+
+### Code Style
+
+  - Follow the cpp core guidelines, except for anything that uses the GSL. Or is otherwise clearly dumb.
+  - Misc. notes:
+    - use uniform initialization (`{...}` constructor syntax) wherever possible. 
+      - Reason: far more
+        readable than `(...)` in many cases (looks like a function call/declaration)
+    - `m_` prefix should be used for all private fields
+      - Reason: disambiguates fields from parameters before it becomes an issue. If you wait until 
+        you notice ambiguity, you're already too late!
+    - By defualt, prefer immutable data (i.e. `const` lvalues). This also necessitates marking
+      methonds `const` wherever possible.
+      - Reason: _especially_ since we're multithreading, this makes code easier to reason about. Also
+        makes it easier for the compiler to reason about.

driverless_ws/src/controls/package.xml

@@ -9,6 +9,10 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>planning</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

driverless_ws/src/controls/src/constants.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "types.hpp"
+
+
+namespace controls {
+
+    /* ROS moments */
+
+    constexpr const char *controller_node_name = "controller";
+    constexpr const char *control_action_topic_name = "control_action";
+    const rclcpp::QoS control_action_qos (rclcpp::KeepLast(10));
+
+
+    // MPPI stuff
+
+    constexpr Device default_device = Device::Cuda;
+
+    /** Controller target frequency, in Hz */
+    constexpr double controller_freq = 50.;
+
+    /** Controller target period, in sec */
+    constexpr auto controller_period = std::chrono::operator""ms(1000. / controller_freq);
+    constexpr uint num_samples = 1024;
+    constexpr uint num_timesteps = 128;
+}

driverless_ws/src/controls/src/cuda_constants.cuh

@@ -0,0 +1,6 @@
+#include "cuda_types.cuh"
+
+namespace controls {
+    constexpr csize num_spline_interpolations = 512;
+    constexpr cfloat spline_interpolation_separation = 0.5;
+}

driverless_ws/src/controls/src/cuda_types.cuh

@@ -0,0 +1,4 @@
+namespace controls {
+    using cfloat = float;
+    using csize = unsigned int;
+}

driverless_ws/src/controls/src/interface/cpu_environment.cpp

@@ -0,0 +1,23 @@
+#include "cpu_environment.hpp"
+
+namespace  controls {
+    namespace interface {
+        void CpuEnvironment::update_spline(const SplineMsg& msg) {
+        }
+
+        void CpuEnvironment::update_slam(const SlamMsg& msg) {
+        }
+
+        void CpuEnvironment::update_gps(const GpsMsg& msg) {
+        }
+
+        State CpuEnvironment::get_curv_state() const {
+        }
+
+        State CpuEnvironment::get_world_state() const {
+        }
+
+        double CpuEnvironment::get_curvature(double progress_from_current) const {
+        }
+    }
+}

driverless_ws/src/controls/src/interface/cpu_environment.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <planning/src/planning_codebase/raceline/raceline.hpp>
+#include <interfaces/msg/spline_list.hpp>
+
+#include "interface.hpp"
+
+
+namespace controls {
+    namespace interface {
+        class CpuEnvironment : public Environment {
+        public:
+            CpuEnvironment() = default;
+
+            void update_spline(const SplineMsg& msg) override;
+            void update_slam(const SlamMsg& msg) override;
+            void update_gps(const GpsMsg& msg) override;
+
+            State get_curv_state() const override;
+            State get_world_state() const override;
+            double get_curvature(double progress_from_current) const override;
+        };
+    }
+}

driverless_ws/src/controls/src/interface/cuda_environment.cu

@@ -0,0 +1,16 @@
+#include <exception>
+
+#include "cuda_environment.cuh"
+
+
+namespace controls {
+    namespace interface {
+        void CudaEnvironment::update_spline(const controls::SplineMsg &msg) {
+            throw std::runtime_error("cuda spline update not implemented");
+        }
+
+        void CudaEnvironment::update_gps(const controls::SplineMsg &msg) {
+            throw std::runtime_error("cuda spline update not implemented");
+        }
+    }
+}

driverless_ws/src/controls/src/interface/cuda_environment.cuh

@@ -0,0 +1,29 @@
+#include <types.hpp>
+#include <cuda_types.cuh>
+#include <cuda_constants.cuh>
+#include <cuda_types.cuh>
+#include <planning/src/planning_codebase/raceline/raceline.hpp>
+
+#include "interface.hpp"
+
+
+namespace controls {
+    namespace cuda {
+        cfloat* d_spline_curvatures;  // allocated on first CudaEnvironment construction
+    }
+
+    namespace interface {
+        // should be able to be constructed/destructed multiple times without an issue
+        // but there can only be one at once
+        class CudaEnvironment : public Environment {
+        public:
+            void update_spline(const SplineMsg& msg) override;
+            void update_slam(const SlamMsg& msg) override;
+            void update_gps(const GpsMsg& msg) override;
+
+            State get_curv_state() const override;
+            State get_world_state() const override;
+            double get_curvature(double progress_from_current) const override;
+        };
+    }
+}

driverless_ws/src/controls/src/interface/interface.cpp

@@ -0,0 +1,12 @@
+#include "interface.hpp"
+
+
+namespace controls {
+    namespace interface {
+        bool Environment::get_valid() const {
+            return m_valid;
+        }
+
+
+    }
+}

driverless_ws/src/controls/src/interface/interface.hpp

@@ -0,0 +1,43 @@
+#ifndef CONTROLS_INTERFACE_HPP
+#define CONTROLS_INTERFACE_HPP
+
+#include <mutex>
+#include <condition_variable>
+#include <types.hpp>
+
+namespace controls {
+    namespace interface {
+        class Environment {
+        public:
+            virtual ~Environment () =0;
+
+            virtual void update_spline(const SplineMsg& msg) =0;
+            virtual void update_slam(const SlamMsg& msg) =0;
+            virtual void update_gps(const GpsMsg& msg) =0;
+
+            virtual State get_curv_state() const =0;
+            virtual State get_world_state() const =0;
+            virtual double get_curvature(double progress_from_current) const =0;
+
+            bool get_valid() const;
+
+
+            std::mutex mutex;
+
+        protected:
+            bool m_valid {false};
+        };
+    }
+}
+
+
+// included so other classes only need to include one header for all of them
+// at bottom to avoid loop
+
+#include "cpu_environment.hpp"
+
+#ifndef CONTROLS_NO_CUDA
+#include "cuda_environment.cuh"
+#endif
+
+#endif

driverless_ws/src/controls/src/mppi/cpu_mppi.cpp

@@ -0,0 +1 @@
+#include "cpu_mppi.hpp"

driverless_ws/src/controls/src/mppi/cpu_mppi.hpp

@@ -0,0 +1,15 @@
+#pragma once
+
+#include <types.hpp>
+
+#include "mppi.hpp"
+
+
+namespace controls {
+    namespace mppi {
+        class CpuMppiController : public MppiController {
+        public:
+            Action generate_action(const State& current_state) override;
+        };
+    }
+}

driverless_ws/src/controls/src/mppi/cuda_mppi.cu

@@ -0,0 +1 @@
+#include "cuda_mppi.cuh"

driverless_ws/src/controls/src/mppi/cuda_mppi.cuh

@@ -0,0 +1 @@
+#pragma once

driverless_ws/src/controls/src/mppi/mppi.hpp

@@ -0,0 +1,15 @@
+#pragma once
+
+
+namespace controls {
+    namespace mppi {
+        class MppiController : public Controller { };
+    }
+}
+
+
+#include "cpu_mppi.hpp"
+
+#ifndef CONTROLS_NO_CUDA
+#include "cuda_mppi.cuh"
+#endif