Pathing updates

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
 )
 
-# Install target
-install(TARGETS kestrel_planning
+ament_target_dependencies(kestrel_waypoint
+  rclcpp
+  nav_msgs
+  geometry_msgs
+  std_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  kestrel_msgs
+  mavros_msgs
+)
+
+# Install executables
+install(TARGETS 
+  kestrel_planning
+  kestrel_waypoint
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -43,4 +67,116 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
-ament_package()
+# Testing
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  # Create a library from waypoint_manager_node without the main function
+  # We'll compile the source with a flag to exclude main
+  add_library(waypoint_manager_lib
+    src/waypoint_manager_node.cpp
+  )
+
+  target_compile_definitions(waypoint_manager_lib PRIVATE EXCLUDE_MAIN)
+
+  target_include_directories(waypoint_manager_lib PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+  )
+
+  ament_target_dependencies(waypoint_manager_lib
+    rclcpp
+    nav_msgs
+    geometry_msgs
+    std_msgs
+    tf2
+    tf2_ros
+    tf2_geometry_msgs
+    kestrel_msgs
+    mavros_msgs
+  )
+
+  # Create a library from pathing_node without the main function
+  add_library(pathing_node_lib
+    src/pathing_node.cpp
+    src/dstar_planner.cpp
+  )
+
+  target_compile_definitions(pathing_node_lib PRIVATE EXCLUDE_MAIN)
+
+  target_include_directories(pathing_node_lib PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+  )
+
+  ament_target_dependencies(pathing_node_lib
+    rclcpp
+    nav_msgs
+    geometry_msgs
+    std_msgs
+    tf2
+    tf2_ros
+    tf2_geometry_msgs
+    kestrel_msgs
+    mavros_msgs
+  )
+
+  # Build test executable for waypoint manager
+  ament_add_gtest(test_waypoint_manager_node
+    test/test_waypoints.cpp
+  )
+
+  target_include_directories(test_waypoint_manager_node PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+  )
+
+  # Link test with the library (not the source file directly)
+  target_link_libraries(test_waypoint_manager_node
+    waypoint_manager_lib
+    pthread
+  )
+
+  ament_target_dependencies(test_waypoint_manager_node
+    rclcpp
+    nav_msgs
+    geometry_msgs
+    std_msgs
+    tf2
+    tf2_ros
+    tf2_geometry_msgs
+    kestrel_msgs
+    mavros_msgs
+  )
+
+  # Build test executable for pathing node
+  ament_add_gtest(test_pathing_node
+    test/test_pathing_node.cpp
+  )
+
+  target_include_directories(test_pathing_node PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+  )
+
+  target_link_libraries(test_pathing_node
+    pathing_node_lib
+    pthread
+  )
+
+  ament_target_dependencies(test_pathing_node
+    rclcpp
+    nav_msgs
+    geometry_msgs
+    std_msgs
+    tf2
+    tf2_ros
+    tf2_geometry_msgs
+    kestrel_msgs
+    mavros_msgs
+  )
+
+  install(TARGETS
+    test_waypoint_manager_node
+    test_pathing_node
+    DESTINATION lib/${PROJECT_NAME}
+  )
+endif()
+
+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;
 };