Pathing Team - Initial Pathing Logic

.github/workflows/test.yaml

@@ -0,0 +1,59 @@
+name: docker-build-and-push
+
+on:
+  push:
+    branches:
+      - main
+      - pathing_and_workflows
+
+jobs:
+  docker-build-and-push:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: checkout
+        uses: actions/checkout@v4
+
+      - name: login
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+
+      - name: buildx
+        uses: docker/setup-buildx-action@v3
+
+      - name: build
+        uses: docker/build-push-action@v5
+        with:
+          context: ./src         
+          file: ./src/Containerfile 
+          push: true
+          tags: |
+            ${{ secrets.DOCKERHUB_USERNAME }}/kestrel:latest
+            ${{ secrets.DOCKERHUB_USERNAME }}/kestrel:${{ github.sha }}
+
+  colcon-build:
+    needs: docker-build-and-push 
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+
+      - name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+
+      - name: Pull latest image
+        run: docker pull ${{ secrets.DOCKERHUB_USERNAME }}/kestrel:latest
+
+      - name: Colcon build stage
+        run: |
+          docker run --rm \
+            -v ${{ github.workspace }}:/workspace \
+            -w /workspace \
+            ${{ secrets.DOCKERHUB_USERNAME }}/kestrel:latest \
+            bash -c "cd src && colcon build"

.gitignore

@@ -9,7 +9,8 @@ src/_archive_legacy/models/deepSORT/CNN/YOLO Demo/*.mov
 __pycache__/
 *.py[cod]
 *$py.class
-
+.gitignore
+.DS_Store
 # C extensions
 *.so
 

src/Containerfile

@@ -0,0 +1,27 @@
+FROM ros:jazzy-ros-base
+
+ENV DEBIAN_FRONTEND=noninteractive
+ENV ROS_DISTRO=jazzy
+ENV ROS_DOMAIN_ID=0
+
+RUN apt-get update && apt-get install -y \
+    python3-colcon-common-extensions \
+    python3-pip \
+    python3-numpy \
+    git \
+    curl \
+    libgeographiclib-dev \
+    lsb-release \
+    build-essential \
+    ros-${ROS_DISTRO}-nav-msgs \
+    ros-${ROS_DISTRO}-geometry-msgs \
+    ros-${ROS_DISTRO}-std-msgs \
+    ros-${ROS_DISTRO}-control-toolbox \
+    ros-${ROS_DISTRO}-mavros \
+    && rm -rf /var/lib/apt/lists/*
+
+
+RUN mkdir src
+COPY . /src 
+
+

src/kestrel_communication/CMakeLists.txt

@@ -8,9 +8,10 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(kestrel_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(mavros_msgs REQUIRED)
 find_package(rclpy REQUIRED)
 
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/mavros_msgs")
+find_package(mavros_msgs REQUIRED)
 # Include directory for headers
 include_directories(include)
 

src/kestrel_control/CMakeLists.txt

@@ -1,6 +1,9 @@
 cmake_minimum_required(VERSION 3.10)
 project(kestrel_control)
 
+#
+#set(CMAKE_PREFIX_PATH "/opt/ros/${ROS_DISTRO}:${CMAKE_PREFIX_PATH}")
+
 # Core build dependencies
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_gtest REQUIRED)
@@ -9,8 +12,15 @@ find_package(rclpy REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(kestrel_msgs REQUIRED)
+
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib")
 find_package(GeographicLib REQUIRED)
+
+set(control_toolbox_DIR "${ROS_PREFIX}/share/control_toolbox/cmake") 
 find_package(control_toolbox REQUIRED)
+
+
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/mavros_msgs")
 find_package(mavros_msgs REQUIRED)
 
 # Include directory for headers

src/kestrel_planning/CMakeLists.txt

@@ -1,29 +1,40 @@
 cmake_minimum_required(VERSION 3.10)
 project(kestrel_planning)
 
-# Required ROS 2 packages
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
 find_package(ament_cmake REQUIRED)
-find_package(ament_cmake_gtest REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
 
-# Include directory
 include_directories(include)
 
-# Source files
-set(SOURCE_FILES
+add_executable(kestrel_planning
   src/pathing_node.cpp
   src/dstar_planner.cpp
-  src/collision_checker.cpp
-  src/waypoint_manager.cpp
 )
 
-add_executable(pathing_node ${SOURCE_FILES})
-ament_target_dependencies(pathing_node rclcpp std_msgs geometry_msgs)
+ament_target_dependencies(kestrel_planning
+  rclcpp
+  nav_msgs
+  geometry_msgs
+  std_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
 
-# Install the executable
-install(TARGETS pathing_node
+# Install target
+install(TARGETS kestrel_planning
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -32,16 +43,4 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
-# Install launch and config files
-install(DIRECTORY launch/
-  DESTINATION share/${PROJECT_NAME}/launch/
-)
-install(DIRECTORY config/
-  DESTINATION share/${PROJECT_NAME}/config/
-)
-
-# Add GTest unit tests
-ament_add_gtest(test_dstar test/test_dstar.cpp)
-ament_add_gtest(test_collision_checker test/test_collision_checker.cpp)
-
 ament_package()

src/kestrel_planning/Dockerfile

@@ -0,0 +1,28 @@
+FROM ros:jazzy-ros-base
+
+ENV DEBIAN_FRONTEND=noninteractive
+ENV ROS_DISTRO=jazzy
+ENV ROS_DOMAIN_ID=0
+
+# Install tools needed for building & rosdep
+RUN apt-get update && apt-get install -y \
+    python3-colcon-common-extensions \
+    python3-pip \
+    git \
+    curl \
+    lsb-release \
+    build-essential \
+    python3-rosdep \
+ && rm -rf /var/lib/apt/lists/*
+
+# Update rosdep (init usually already done in ROS docker images)
+RUN rosdep update || true
+
+# Copy your workspace
+COPY . /ros_ws
+WORKDIR /ros_ws
+
+# Install dependencies declared in package.xml
+RUN rosdep install --from-paths src --ignore-src -r -y
+
+

src/kestrel_planning/include/Voxel.hpp

@@ -0,0 +1,105 @@
+#ifndef VOXEL_HPP
+#define VOXEL_HPP
+
+#pragma once
+
+#include <vector>
+#include <cstdint>
+#include <memory>
+#include <array>
+#include <stdexcept>
+#include <cmath>
+#include <algorithm>
+
+#include "utils.hpp"
+
+namespace octree {
+
+template <typename T>
+struct Node {
+    std::array<std::shared_ptr<Node<T>>, 8> children{};
+    std::shared_ptr<T> value;
+};
+
+template <typename T>
+class voxel {
+public:
+    voxel(uint32_t X, uint32_t Y, uint32_t Z)
+        : X_RANGE(X), Y_RANGE(Y), Z_RANGE(Z) {
+        uint32_t max_dim = std::max({X_RANGE, Y_RANGE, Z_RANGE});
+        max_depth = static_cast<uint32_t>(std::ceil(std::log2(max_dim)));
+        root = std::make_shared<Node<T>>();
+    }
+    voxel() : X_RANGE(0), Y_RANGE(0), Z_RANGE(0), max_depth(0), root(nullptr) {}
+
+
+
+    std::shared_ptr<T>& operator()(uint32_t x, uint32_t y, uint32_t z) {
+        return access(root, x, y, z, 0);
+    }
+
+    const std::shared_ptr<T>& operator()(uint32_t x, uint32_t y, uint32_t z) const {
+        return access_const(root, x, y, z, 0);
+    }
+
+    void insert(std::shared_ptr<T> val, const vec3& pt) {
+        auto& target = access(root, pt.x, pt.y, pt.z, 0);
+        target = val;
+    }
+
+    void clear() {
+        clearNode(root);
+    }
+
+private:
+    uint32_t X_RANGE, Y_RANGE, Z_RANGE;
+    uint32_t max_depth;
+    std::shared_ptr<Node<T>> root;
+
+    int childIndex(uint32_t x, uint32_t y, uint32_t z, uint32_t level) const {
+        int shift = max_depth - level - 1;
+        return ((x >> shift) & 1) << 2 |
+               ((y >> shift) & 1) << 1 |
+               ((z >> shift) & 1);
+    }
+
+    std::shared_ptr<T>& access(std::shared_ptr<Node<T>> node, uint32_t x, uint32_t y, uint32_t z, uint32_t level) {
+        if (level == max_depth) {
+            if (!node->value)
+                node->value = std::make_shared<T>();
+            return node->value;
+        }
+
+        int idx = childIndex(x, y, z, level);
+        if (!node->children[idx])
+            node->children[idx] = std::make_shared<Node<T>>();
+
+        return access(node->children[idx], x, y, z, level + 1);
+    }
+
+    const std::shared_ptr<T> access_const(std::shared_ptr<Node<T>> node, uint32_t x, uint32_t y, uint32_t z, uint32_t level) const {
+        if (x >= X_RANGE || y >= Y_RANGE || z >= Z_RANGE)
+            return nullptr;
+
+        if (level == max_depth) {
+            return node->value;
+        }
+
+        int idx = childIndex(x, y, z, level);
+        if (!node->children[idx])
+            return nullptr;
+
+        return access_const(node->children[idx], x, y, z, level + 1);
+    }
+
+    void clearNode(std::shared_ptr<Node<T>>& node) {
+        if (!node) return;
+        node->value.reset();
+        for (auto& child : node->children)
+            clearNode(child);
+    }
+};
+
+} 
+
+#endif