Waypoint Manager Node + Pathing Topics Modifications

.github/workflows/build_prod.yaml

@@ -0,0 +1,49 @@
+# Process for building and pushing the full Jetson Orin Nano docker image
+
+name: Docker Build and Push - Prod
+
+on:
+  pull_request:
+    branches:
+      - master
+    types:
+      - opened
+      - synchronize
+  workflow_dispatch:
+
+jobs:
+  build-base:
+    name: Build Base Image
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout Repo
+        uses: actions/checkout@v4
+        with:
+          submodules: 'recursive'
+
+      - name: Set up QEMU
+        uses: docker/setup-qemu-action@v2
+        with:
+          platforms: arm64,amd64
+
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+
+      - name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+
+      - name: Build and Push Base Image
+        uses: docker/build-push-action@v5
+        with:
+          context: ./docker/l4t-ros2-docker/jazzy
+          file: ./docker/l4t-ros2-docker/jazzy/Dockerfile
+          push: true
+          platforms: linux/arm64
+          tags: kestreldev/l4t-ros2-docker:jazzy
+          cache-from: |
+            type=registry,ref=kestreldev/l4t-ros2-docker:cache-jazzy
+          cache-to: |
+            type=registry,ref=kestreldev/l4t-ros2-docker:cache-jazzy,mode=max

.gitmodules

@@ -22,3 +22,6 @@
 [submodule "src/libraries/geographiclib"]
 	path = src/libraries/geographiclib
 	url = https://github.com/geographiclib/geographiclib.git
+[submodule "docker/l4t-ros2-docker"]
+	path = docker/l4t-ros2-docker
+	url = https://github.com/atinfinity/l4t-ros2-docker.git

src/kestrel_planning/CMakeLists.txt

@@ -4,25 +4,33 @@ project(kestrel_planning)
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-add_compile_options(-Wall -Wextra -Wpedantic)
+#add_compile_options(-Wall -Wextra -Wpedantic)
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclpy 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)
-
+find_package(kestrel_msgs REQUIRED)
+find_package(mavros_msgs REQUIRED)
 
 include_directories(include)
 
+# Create executables with all source files included directly
 add_executable(kestrel_planning
   src/pathing_node.cpp
   src/dstar_planner.cpp
 )
 
+add_executable(kestrel_waypoint
+  src/waypoint_manager_node.cpp
+)
+
+# Set dependencies for executables
 ament_target_dependencies(kestrel_planning
   rclcpp
   nav_msgs
@@ -31,10 +39,26 @@ ament_target_dependencies(kestrel_planning
   tf2
   tf2_ros
   tf2_geometry_msgs
+  kestrel_msgs
+  mavros_msgs
+)
+
+ament_target_dependencies(kestrel_waypoint
+  rclcpp
+  nav_msgs
+  geometry_msgs
+  std_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  kestrel_msgs
+  mavros_msgs
 )
 
-# Install target
-install(TARGETS kestrel_planning
+# Install executables
+install(TARGETS 
+  kestrel_planning
+  kestrel_waypoint
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -43,4 +67,4 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
-ament_package()
+ament_package()

src/kestrel_planning/include/dstar_planner.hpp

@@ -4,6 +4,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "mavros_msgs/msg/position_target.hpp"
 
 #include "utils.hpp"
 #include "Voxel.hpp"
@@ -40,7 +41,8 @@ class DSTARLITE {
 
     void replan(float x, float y, float z);
     int extractPath(std::vector<geometry_msgs::msg::PoseStamped> &waypoints);
-    void setOccupied(int x, int y, int z);
+    void setOccupiedStatus(int x, int y, int z, bool value);
+    void initializeCostmap(); 
 
 private:
     float km;                
@@ -59,6 +61,7 @@ class DSTARLITE {
                        StateComparator> open_list;
 
     std::unordered_set<std::shared_ptr<state>> open_set;
+    std::vector<std::shared_ptr<state>> visited;
 
     bool isOccupied(int x, int y, int z);
     std::vector<std::shared_ptr<state>> getPredecessors(std::shared_ptr<state> node);
@@ -69,6 +72,7 @@ class DSTARLITE {
     void insertOpenList(std::shared_ptr<state> node);
     bool isInOpenList(std::shared_ptr<state> node);
     void removeFromOpenList(std::shared_ptr<state> node);
+
     std::shared_ptr<state> topOpenList();
     bool openListEmpty();
 

src/kestrel_planning/include/pathing_node.hpp

@@ -9,7 +9,7 @@
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
-#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "kestrel_msgs/msg/obstacle_grid.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "std_msgs/msg/string.hpp"
@@ -44,22 +44,22 @@ class DStarNode : public rclcpp::Node {
     bool got_start_;
     bool got_goal_;
     bool has_valid_path_;
+    bool costmap_initialized_;
 
     PlanningMode planning_mode_;
 
     //void octomapCallback(const octomap_msgs::msg::Octomap::SharedPtr msg);
-    void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
-    void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+    void mapCallback(const kestrel_msgs::msg::ObstacleGrid::SharedPtr msg);
+    void goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr msg);
     void odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
     void replanCallback(const std_msgs::msg::Empty::SharedPtr msg);
 
-    //rclcpp::Subscription<octomap_msgs::msg::Octomap>::SharedPtr octomap_sub_;
-    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_sub_;
+    rclcpp::Subscription<kestrel_msgs::msg::ObstacleGrid>::SharedPtr costmap_sub_;
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr start_sub_;
-    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
+    rclcpp::Subscription<mavros_msgs::msg::PositionTarget>::SharedPtr goal_sub_;
+    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr replan_sub_;
     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr status_pub_;
-    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr replan_sub_;
 
     std::unique_ptr<DSTARLITE> planner_;
     std::mutex planner_mutex_;

src/kestrel_planning/include/utils.hpp

@@ -1,5 +1,6 @@
 #ifndef UTILS_H
 #define UTILS_H
+#pragma once
 
 #include <iostream>
 #define TOL 1e6
@@ -75,68 +76,77 @@ struct vec3 {
     }
 };
 
-
 class state {
-    public:
-        state() {};
-        state(int a, int b, int c) {
-            point = vec3(a,b,c);
-        }
-
-        void setG(float _g) {
-            g = _g;
-        }
-        void setRHS(float _rhs) {
-            rhs = _rhs;
-        }
-        float getG() {
-            return g;
-        }
-        float getRHS() {
-            return rhs;
-        }
-        void setPoint(vec3 pt) {
-            point = pt;
-        }
-        vec3 getPoint() const {
-            return point;
-        }
-        bool isConsistent() {
-            const float EPS = 1e-5f;
-            return std::abs(g - rhs) < EPS;
-        }
+public:
+    state() 
+        : point(vec3(0, 0, 0))
+        , g(0.0f)
+        , rhs(0.0f)
+        , rhs_from(nullptr)
+        , key({0.0f, 0.0f})
+        , occupied(false) 
+    {}
+
+    state(int a, int b, int c) 
+        : point(vec3(a, b, c))
+        , g(0.0f)
+        , rhs(0.0f)
+        , rhs_from(nullptr)
+        , key({0.0f, 0.0f})
+        , occupied(false) 
+    {}
 
-        bool isOverConsistent() {
-            return (g > rhs);
-        }
-        std::shared_ptr<state> nextStep() {
-            return rhs_from;
-        }
-        void setNextStep(std::shared_ptr<state> u) {
-            rhs_from = u;
-        }
-        void setkey(std::pair<float,float> k ) {
-            key = k;
-        }
-        std::pair<float,float> getKey() {
-            return key;
-        }
-        void setOccupation(bool val) {
-            occupied = val;
-        }
-        bool getOccupy() {
-            return occupied;
-        }
-        
+    void setG(float _g) {
+        g = _g;
+    }
+    void setRHS(float _rhs) {
+        rhs = _rhs;
+    }
+    float getG() {
+        return g;
+    }
+    float getRHS() {
+        return rhs;
+    }
+    void setPoint(vec3 pt) {
+        point = pt;
+    }
+    vec3 getPoint() const {
+        return point;
+    }
+    bool isConsistent() {
+        const float EPS = 1e-5f;
+        return std::abs(g - rhs) < EPS;
+    }
 
+    bool isOverConsistent() {
+        return (g > rhs);
+    }
+    std::shared_ptr<state> nextStep() {
+        return rhs_from;
+    }
+    void setNextStep(std::shared_ptr<state> u) {
+        rhs_from = u;
+    }
+    void setKey(std::pair<float,float> k) {
+        key = k;
+    }
+    std::pair<float,float> getKey() {
+        return key;
+    }
+    void setOccupation(bool val) {
+        occupied = val;
+    }
+    bool getOccupy() {
+        return occupied;
+    }
 
-    private:
-        vec3 point;
-        float g, rhs;
-        std::shared_ptr<state> rhs_from;
-        std::pair<float,float> key;
-        bool occupied;
-
+private:
+    vec3 point;
+    float g, rhs;
+    std::shared_ptr<state> rhs_from;
+    std::pair<float,float> key;
+    bool occupied;
 };