From 1cd386a098a18680227cc25a6d29ecec48a55bd9 Mon Sep 17 00:00:00 2001 From: mistry Date: Thu, 21 Aug 2025 17:26:09 -0400 Subject: [PATCH] PATHING TEAM - Octree Voxel Costmap + DStarlite implementation + GH Workflows --- .github/workflows/test.yaml | 59 +++ .gitignore | 3 +- src/Containerfile | 27 ++ src/kestrel_communication/CMakeLists.txt | 3 +- src/kestrel_control/CMakeLists.txt | 10 + src/kestrel_planning/CMakeLists.txt | 47 ++- src/kestrel_planning/Dockerfile | 28 ++ src/kestrel_planning/config/dstar_params.yaml | 0 .../config/planning_bounds.yaml | 0 .../config/safety_params.yaml | 0 src/kestrel_planning/include/Voxel.hpp | 105 ++++++ .../include/dstar_planner.hpp | 85 +++++ .../kestrel_planning/collision_checker.hpp | 0 .../kestrel_planning/dstar_planner.hpp | 0 .../include/kestrel_planning/pathing_node.hpp | 0 .../kestrel_planning/waypoint_manager.hpp | 0 src/kestrel_planning/include/pathing_node.hpp | 78 ++++ src/kestrel_planning/include/utils.hpp | 168 +++++++++ .../launch/planning.launch.py | 0 src/kestrel_planning/package.xml | 14 +- .../src/collision_checker.cpp | 0 src/kestrel_planning/src/dstar_planner.cpp | 352 ++++++++++++++++++ src/kestrel_planning/src/pathing_node.cpp | 273 +++++++++++++- src/kestrel_planning/src/waypoint_manager.cpp | 0 .../test/test_collision_checker.cpp | 0 src/kestrel_planning/test/test_dstar.cpp | 0 src/kestrel_planning/test/test_pathing.py | 330 ++++++++++++++++ 27 files changed, 1550 insertions(+), 32 deletions(-) create mode 100644 .github/workflows/test.yaml create mode 100644 src/Containerfile create mode 100644 src/kestrel_planning/Dockerfile delete mode 100644 src/kestrel_planning/config/dstar_params.yaml delete mode 100644 src/kestrel_planning/config/planning_bounds.yaml delete mode 100644 src/kestrel_planning/config/safety_params.yaml create mode 100644 src/kestrel_planning/include/Voxel.hpp create mode 100644 src/kestrel_planning/include/dstar_planner.hpp delete mode 100644 src/kestrel_planning/include/kestrel_planning/collision_checker.hpp delete mode 100644 src/kestrel_planning/include/kestrel_planning/dstar_planner.hpp delete mode 100644 src/kestrel_planning/include/kestrel_planning/pathing_node.hpp delete mode 100644 src/kestrel_planning/include/kestrel_planning/waypoint_manager.hpp create mode 100644 src/kestrel_planning/include/pathing_node.hpp create mode 100644 src/kestrel_planning/include/utils.hpp delete mode 100644 src/kestrel_planning/launch/planning.launch.py delete mode 100644 src/kestrel_planning/src/collision_checker.cpp delete mode 100644 src/kestrel_planning/src/waypoint_manager.cpp delete mode 100644 src/kestrel_planning/test/test_collision_checker.cpp delete mode 100644 src/kestrel_planning/test/test_dstar.cpp create mode 100644 src/kestrel_planning/test/test_pathing.py diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml new file mode 100644 index 0000000..a4d9d1c --- /dev/null +++ b/.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" \ No newline at end of file diff --git a/.gitignore b/.gitignore index f76f010..1b01344 100644 --- a/.gitignore +++ b/.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 diff --git a/src/Containerfile b/src/Containerfile new file mode 100644 index 0000000..aea3de8 --- /dev/null +++ b/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 + + diff --git a/src/kestrel_communication/CMakeLists.txt b/src/kestrel_communication/CMakeLists.txt index e70c31d..b44a99c 100644 --- a/src/kestrel_communication/CMakeLists.txt +++ b/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) diff --git a/src/kestrel_control/CMakeLists.txt b/src/kestrel_control/CMakeLists.txt index c407ef5..2004b74 100644 --- a/src/kestrel_control/CMakeLists.txt +++ b/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 diff --git a/src/kestrel_planning/CMakeLists.txt b/src/kestrel_planning/CMakeLists.txt index 33f7c3b..224ddf4 100644 --- a/src/kestrel_planning/CMakeLists.txt +++ b/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() diff --git a/src/kestrel_planning/Dockerfile b/src/kestrel_planning/Dockerfile new file mode 100644 index 0000000..e265d57 --- /dev/null +++ b/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 + + diff --git a/src/kestrel_planning/config/dstar_params.yaml b/src/kestrel_planning/config/dstar_params.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/config/planning_bounds.yaml b/src/kestrel_planning/config/planning_bounds.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/config/safety_params.yaml b/src/kestrel_planning/config/safety_params.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/include/Voxel.hpp b/src/kestrel_planning/include/Voxel.hpp new file mode 100644 index 0000000..59f6333 --- /dev/null +++ b/src/kestrel_planning/include/Voxel.hpp @@ -0,0 +1,105 @@ +#ifndef VOXEL_HPP +#define VOXEL_HPP + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "utils.hpp" + +namespace octree { + +template +struct Node { + std::array>, 8> children{}; + std::shared_ptr value; +}; + +template +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(std::ceil(std::log2(max_dim))); + root = std::make_shared>(); + } + voxel() : X_RANGE(0), Y_RANGE(0), Z_RANGE(0), max_depth(0), root(nullptr) {} + + + + std::shared_ptr& operator()(uint32_t x, uint32_t y, uint32_t z) { + return access(root, x, y, z, 0); + } + + const std::shared_ptr& operator()(uint32_t x, uint32_t y, uint32_t z) const { + return access_const(root, x, y, z, 0); + } + + void insert(std::shared_ptr 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> 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& access(std::shared_ptr> 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(); + return node->value; + } + + int idx = childIndex(x, y, z, level); + if (!node->children[idx]) + node->children[idx] = std::make_shared>(); + + return access(node->children[idx], x, y, z, level + 1); + } + + const std::shared_ptr access_const(std::shared_ptr> 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) { + if (!node) return; + node->value.reset(); + for (auto& child : node->children) + clearNode(child); + } +}; + +} + +#endif \ No newline at end of file diff --git a/src/kestrel_planning/include/dstar_planner.hpp b/src/kestrel_planning/include/dstar_planner.hpp new file mode 100644 index 0000000..380c2bf --- /dev/null +++ b/src/kestrel_planning/include/dstar_planner.hpp @@ -0,0 +1,85 @@ +#ifndef DSTAR_PLANNER_H +#define DSTAR_PLANNER_H + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "utils.hpp" +#include "Voxel.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + + +const float INF_FLOAT = std::numeric_limits::max(); + +class DSTARLITE { +public: + DSTARLITE(uint32_t X_lim, uint32_t Y_lim, uint32_t Z_lim) + : km(0.0f), + X_RANGE(X_lim), Y_RANGE(Y_lim), Z_RANGE(Z_lim), + costmap(X_lim, Y_lim, Z_lim) {} + + DSTARLITE() + : km(0.0f), + X_RANGE(100), Y_RANGE(100), Z_RANGE(100), + costmap(100, 100, 100) {} + + void setGoal(float x, float y, float z) { goal = vec3(x, y, z); } + void setStart(float x, float y, float z) { start = vec3(x, y, z); } + + void initialize(); + int computeShortestPath(); + + void replan(float x, float y, float z); + int extractPath(std::vector &waypoints); + void setOccupied(int x, int y, int z); + +private: + float km; + vec3 goal; + vec3 start; + std::shared_ptr goal_state; + std::shared_ptr start_state; + + uint32_t X_RANGE, Y_RANGE, Z_RANGE; + octree::voxel costmap; + + std::pair calculateKey(std::shared_ptr u); + + std::priority_queue, std::shared_ptr>, + std::vector, std::shared_ptr>>, + StateComparator> open_list; + + std::unordered_set> open_set; + + bool isOccupied(int x, int y, int z); + std::vector> getPredecessors(std::shared_ptr node); + std::vector> getSuccessors(std::shared_ptr node); + void updateVertex(std::shared_ptr node); + void expand(std::shared_ptr node); + + void insertOpenList(std::shared_ptr node); + bool isInOpenList(std::shared_ptr node); + void removeFromOpenList(std::shared_ptr node); + std::shared_ptr topOpenList(); + bool openListEmpty(); + + + float heuristic(std::shared_ptr s, std::shared_ptr u); + + + float edgeCost(const std::shared_ptr& a, const std::shared_ptr& b); + +}; + + + +#endif diff --git a/src/kestrel_planning/include/kestrel_planning/collision_checker.hpp b/src/kestrel_planning/include/kestrel_planning/collision_checker.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/include/kestrel_planning/dstar_planner.hpp b/src/kestrel_planning/include/kestrel_planning/dstar_planner.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/include/kestrel_planning/pathing_node.hpp b/src/kestrel_planning/include/kestrel_planning/pathing_node.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/include/kestrel_planning/waypoint_manager.hpp b/src/kestrel_planning/include/kestrel_planning/waypoint_manager.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/include/pathing_node.hpp b/src/kestrel_planning/include/pathing_node.hpp new file mode 100644 index 0000000..629a191 --- /dev/null +++ b/src/kestrel_planning/include/pathing_node.hpp @@ -0,0 +1,78 @@ +#ifndef PATHING_NODE_HPP +#define PATHING_NODE_HPP + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include "dstar_planner.hpp" + + +enum class PlanningMode { + NEW_PATH, + REPLAN +}; + + +class DStarNode : public rclcpp::Node { +public: + DStarNode(); + ~DStarNode(); + +private: + enum class PlannerState { + IDLE, + NO_MAP, + AWAITING_GOAL, + PLANNING, + REPLANNING, + SUCCESS, + FAILURE + }; + bool planning_request_ = false; + std::atomic current_state_; + bool got_costmap_; + bool got_start_; + bool got_goal_; + bool has_valid_path_; + + 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 odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void replanCallback(const std_msgs::msg::Empty::SharedPtr msg); + + //rclcpp::Subscription::SharedPtr octomap_sub_; + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr start_sub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Subscription::SharedPtr replan_sub_; + + std::unique_ptr planner_; + std::mutex planner_mutex_; + std::thread planning_thread_; + std::condition_variable planning_cv_; + + void triggerPlanning(); + void triggerPlanningLocked_(); + void planningWorker(); + void updatePlannerState(PlannerState new_state); + void publishPath(const std::vector &waypoints); + void publishStatus(); + +}; + +#endif // PATHING_NODE_HPP diff --git a/src/kestrel_planning/include/utils.hpp b/src/kestrel_planning/include/utils.hpp new file mode 100644 index 0000000..c37fcbf --- /dev/null +++ b/src/kestrel_planning/include/utils.hpp @@ -0,0 +1,168 @@ +#ifndef UTILS_H +#define UTILS_H + +#include +#define TOL 1e6 + +struct vec3 { + int x, y, z; + float cost; + vec3() : x(0), y(0), z(0), cost(0.0f) {} + vec3(int a, int b, int c) : x(a), y(b), z(c) {cost=0;} + vec3(int a, int b, int c, float ct) : x(a), y(b), z(c), cost(ct) {} + + vec3 operator+() const { return *this; } + vec3 operator-() const { return vec3(-x, -y, -z); } + + vec3& operator+=(const vec3& other) { + x += other.x; y += other.y; z += other.z; + return *this; + } + vec3& operator-=(const vec3& other) { + x -= other.x; y -= other.y; z -= other.z; + return *this; + } + vec3& operator*=(int scalar) { + x *= scalar; y *= scalar; z *= scalar; + return *this; + } + vec3& operator/=(int scalar) { + x /= scalar; y /= scalar; z /= scalar; + return *this; + } + + friend vec3 operator+(vec3 lhs, const vec3& rhs) { + return lhs += rhs; + } + friend vec3 operator-(vec3 lhs, const vec3& rhs) { + return lhs -= rhs; + } + friend vec3 operator*(vec3 lhs, int scalar) { + return lhs *= scalar; + } + friend vec3 operator*(int scalar, vec3 rhs) { + return rhs *= scalar; + } + friend vec3 operator/(vec3 lhs, int scalar) { + return lhs /= scalar; + } + + friend bool operator==(const vec3& lhs, const vec3& rhs) { + return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; + } + friend bool operator!=(const vec3& lhs, const vec3& rhs) { + return !(lhs == rhs); + } + friend bool operator<(const vec3& lhs, const vec3& rhs) { + if (lhs.x != rhs.x) return lhs.x < rhs.x; + if (lhs.y != rhs.y) return lhs.y < rhs.y; + return lhs.z < rhs.z; + } + + friend std::ostream& operator<<(std::ostream& os, const vec3& v) { + return os << '(' << v.x << ", " << v.y << ", " << v.z << ')'; + } + + int& operator[](int index) { + if (index == 0) return x; + else if (index == 1) return y; + else return z; + } + const int& operator[](int index) const { + if (index == 0) return x; + else if (index == 1) return y; + else return z; + } +}; + + +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; + } + + bool isOverConsistent() { + return (g > rhs); + } + std::shared_ptr nextStep() { + return rhs_from; + } + void setNextStep(std::shared_ptr u) { + rhs_from = u; + } + void setkey(std::pair k ) { + key = k; + } + std::pair getKey() { + return key; + } + void setOccupation(bool val) { + occupied = val; + } + bool getOccupy() { + return occupied; + } + + + + private: + vec3 point; + float g, rhs; + std::shared_ptr rhs_from; + std::pair key; + bool occupied; + +}; + + +inline bool isEqual(float a, float b) { + if (abs(a - b) <= TOL) { + return true; + } + return false; +} + + + +inline bool compareNodes(std::shared_ptr n1, std::shared_ptr n2) { + return n1->getRHS() > n2->getRHS(); +} + + +struct StateComparator { + bool operator()(const std::pair, std::shared_ptr>& a, + const std::pair, std::shared_ptr>& b) const { + if (a.first.first != b.first.first) { + return a.first.first > b.first.first; + } + return a.first.second > b.first.second; + } +}; + + +#endif \ No newline at end of file diff --git a/src/kestrel_planning/launch/planning.launch.py b/src/kestrel_planning/launch/planning.launch.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/package.xml b/src/kestrel_planning/package.xml index 74448a4..3f66c10 100644 --- a/src/kestrel_planning/package.xml +++ b/src/kestrel_planning/package.xml @@ -1,9 +1,10 @@ kestrel_planning - 0.1.0 - Path planning and collision avoidance for autonomous drone navigation - David Orjuela + 0.0.1 + Pathing team - D* Lite ROS2 Node + + nicholas mistry MIT ament_cmake @@ -11,8 +12,13 @@ rclcpp std_msgs geometry_msgs + nav_msgs - ament_cmake_gtest + nav2_core + nav2_costmap_2d + + ament_lint_auto + ament_lint_common ament_cmake diff --git a/src/kestrel_planning/src/collision_checker.cpp b/src/kestrel_planning/src/collision_checker.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/src/dstar_planner.cpp b/src/kestrel_planning/src/dstar_planner.cpp index e69de29..7896279 100644 --- a/src/kestrel_planning/src/dstar_planner.cpp +++ b/src/kestrel_planning/src/dstar_planner.cpp @@ -0,0 +1,352 @@ + +#include "dstar_planner.hpp" + +void DSTARLITE::initialize() { + std::cout << "[PATHING START] dstarlite initializing" << std::endl; + costmap.clear(); + km = 0.0f; + while (!open_list.empty()) { + open_list.pop(); + } + open_set.clear(); + + for (uint32_t x = 0; x < X_RANGE; ++x) { + for (uint32_t y = 0; y < Y_RANGE; ++y) { + for (uint32_t z = 0; z < Z_RANGE; ++z) { + auto cell_state = std::make_shared(); + cell_state->setPoint(vec3(x, y, z)); + cell_state->setG(INF_FLOAT); + cell_state->setRHS(INF_FLOAT); + cell_state->setNextStep(nullptr); + costmap.insert(cell_state, cell_state->getPoint()); + } + } + } + + goal_state = costmap(goal.x, goal.y, goal.z); + start_state = costmap(start.x, start.y, start.z); + + if (!goal_state || !start_state) { + std::cerr << "[ERROR]: Start or goal not found in costmap\n"; + return; + } + + goal_state->setG(INF_FLOAT); + goal_state->setRHS(0.0f); + goal_state->setNextStep(nullptr); + goal_state->setkey(calculateKey(goal_state)); + + start_state->setG(INF_FLOAT); + start_state->setRHS(INF_FLOAT); + start_state->setNextStep(nullptr); + insertOpenList(goal_state); +} + +float DSTARLITE::edgeCost(const std::shared_ptr& a, const std::shared_ptr& b) { + const auto& A = a->getPoint(); + const auto& B = b->getPoint(); + if (isOccupied(B.x, B.y, B.z)) return INF_FLOAT; + + float dx = float(B.x - A.x), dy = float(B.y - A.y), dz = float(B.z - A.z); + return std::sqrt(dx*dx + dy*dy + dz*dz); +} + +void DSTARLITE::insertOpenList(std::shared_ptr node) { + auto key = calculateKey(node); + + if (isInOpenList(node)) open_set.erase(node); + + open_list.push(std::make_pair(key, node)); + open_set.insert(node); + + std::cout << "[DEBUG] Inserted ("<getPoint().x<<","<getPoint().y<<","<getPoint().z + <<") key=("< node) { + return open_set.find(node) != open_set.end(); +} + + + +std::pair DSTARLITE::calculateKey(std::shared_ptr u) { + float val = std::min(u->getG(), u->getRHS()); + + return { val + heuristic(start_state, u) + km, val }; +} + + +std::vector> DSTARLITE::getSuccessors(std::shared_ptr node) { + std::vector> succs; + + vec3 pos = node->getPoint(); + + for (int dx = -1; dx <= 1; ++dx) { + for (int dy = -1; dy <= 1; ++dy) { + for (int dz = -1; dz <= 1; ++dz) { + if (dx == 0 && dy == 0 && dz == 0) continue; + + vec3 neighbor_pos(pos.x + dx, pos.y + dy, pos.z + dz); + if (neighbor_pos.x < 0 || neighbor_pos.x >= static_cast(X_RANGE) || + neighbor_pos.y < 0 || neighbor_pos.y >= static_cast(Y_RANGE) || + neighbor_pos.z < 0 || neighbor_pos.z >= static_cast(Z_RANGE)) { + continue; + } + + auto neighbor = costmap(neighbor_pos.x, neighbor_pos.y, neighbor_pos.z); + + if (neighbor && !isOccupied(neighbor_pos.x, neighbor_pos.y, neighbor_pos.z)) { + succs.push_back(neighbor); + } else { + if (isOccupied(neighbor_pos.x, neighbor_pos.y, neighbor_pos.z)) { + std::cout << neighbor_pos.x << " " << neighbor_pos.y << " " << neighbor_pos.z << " is occupied! \n"; + } + } + } + } + } + + return succs; +} + +std::vector> DSTARLITE::getPredecessors(std::shared_ptr node) { + std::vector> preds; + + vec3 pos = node->getPoint(); + + for (int dx = -1; dx <= 1; ++dx) { + for (int dy = -1; dy <= 1; ++dy) { + for (int dz = -1; dz <= 1; ++dz) { + if (dx == 0 && dy == 0 && dz == 0) continue; + + int nx = pos.x + dx; + int ny = pos.y + dy; + int nz = pos.z + dz; + + if (nx < 0 || nx >= static_cast(X_RANGE) || + ny < 0 || ny >= static_cast(Y_RANGE) || + nz < 0 || nz >= static_cast(Z_RANGE)) + continue; + + auto neighbor = costmap(nx, ny, nz); + + if (neighbor && heuristic(neighbor, node) < INF_FLOAT) { + preds.push_back(neighbor); + } + } + } + } + + return preds; +} + + +bool DSTARLITE::openListEmpty() { + while (!open_list.empty() && + open_set.find(open_list.top().second) == open_set.end()) { + open_list.pop(); + } + return open_list.empty(); +} + + +void DSTARLITE::removeFromOpenList(std::shared_ptr node) { + open_set.erase(node); + +} + +int DSTARLITE::computeShortestPath() { + uint32_t count = 0; + + while (!openListEmpty()) { + auto topPair = open_list.top(); + auto u = topPair.second; + + auto topKey = calculateKey(u); + auto startKey = calculateKey(start_state); + + if (!(topKey < startKey || start_state->getRHS() != start_state->getG())) + break; + + u = topOpenList(); + if (!u) break; + + removeFromOpenList(u); + + if (u->getG() > u->getRHS()) { + u->setG(u->getRHS()); + for (auto s : getSuccessors(u)) updateVertex(s); + } else { + u->setG(INF_FLOAT); + updateVertex(u); + for (auto s : getSuccessors(u)) updateVertex(s); + } + + if (++count > X_RANGE * Y_RANGE * Z_RANGE) { + std::cerr << "[ERROR] Max iterations reached in computeShortestPath\n"; + break; + } + } + return count; +} + + + + +void DSTARLITE::updateVertex(std::shared_ptr u) { + if (u->getPoint() != goal) { + float min_rhs = INF_FLOAT; + std::shared_ptr best = nullptr; + for (auto s : getSuccessors(u)) { + float c = edgeCost(u, s); + if (c >= INF_FLOAT) continue; + float val = s->getG() + c; + if (val < min_rhs) { min_rhs = val; best = s; } + } + u->setRHS(min_rhs); + u->setNextStep(best); + } + + if (isInOpenList(u)) removeFromOpenList(u); + + if (!u->isConsistent()) insertOpenList(u); +} + +float DSTARLITE::heuristic(std::shared_ptr s, std::shared_ptr u) { + vec3 svec = s->getPoint(); + vec3 uvec = u->getPoint(); + + double dx = std::abs(svec.x - uvec.x); + double dy = std::abs(svec.y - uvec.y); + double dz = std::abs(svec.z - uvec.z); + + if (dx < dy) std::swap(dx, dy); + if (dx < dz) std::swap(dx, dz); + if (dy < dz) std::swap(dy, dz); + + return (float)( + (dx - dy) + + (dy - dz) * std::sqrt(2.0) + + dz * std::sqrt(3.0) + ); +} + +std::shared_ptr DSTARLITE::topOpenList() { + while (!open_list.empty()) { + auto [oldKey, node] = open_list.top(); + + if (open_set.find(node) == open_set.end()) { open_list.pop(); continue; } + + auto newKey = calculateKey(node); + if (std::tie(oldKey.first, oldKey.second) < + std::tie(newKey.first, newKey.second)) { + open_set.erase(node); + open_list.pop(); + insertOpenList(node); + continue; + } + + return node; + } + return nullptr; +} + +int DSTARLITE::extractPath(std::vector &waypoints) { + std::shared_ptr node = costmap(start.x, start.y, start.z); + + if (!node) { + return 0; + } + + if (!node->nextStep()) { + std::cout << "Start node has no parent!\n"; + return 0; + } + + uint32_t count = 0; + std::set visited; + + while (node && visited.find(node->getPoint()) == visited.end()) { + vec3 point = node->getPoint(); + visited.insert(point); + + if (!(point == start)) { + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "map"; + + pose.pose.position.x = point.x; + pose.pose.position.y = point.y; + pose.pose.position.z = point.z; + + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + waypoints.push_back(pose); + } + + count++; + if (point == goal) break; + + node = node->nextStep(); + } + + if (visited.find(goal) == visited.end()) { + std::cerr << "Path broken before reaching goal.\n"; + } + + return count; +} + +bool DSTARLITE::isOccupied(int x, int y, int z) { + std::shared_ptr&s = costmap(x, y, z); + if (!s) { + return false; + } + return s->getOccupy(); +} + +void DSTARLITE::setOccupied(int x, int y, int z) { + auto state_occupied = costmap(x, y, z); + if (!state_occupied) { + std::cerr << "Error: No state found at (" << x << ", " << y << ", " << z << ")\n"; + return; + } + state_occupied->setOccupation(true); + state_occupied->setG(INF_FLOAT); + state_occupied->setRHS(INF_FLOAT); + state_occupied->setNextStep(nullptr); +} + +void DSTARLITE::replan(float x, float y, float z) { + int px = static_cast(x + X_RANGE / 2); + int py = static_cast(y + Y_RANGE / 2); + int pz = static_cast(z); + + if (px < 0 || px >= (int)X_RANGE || + py < 0 || py >= (int)Y_RANGE || + pz < 0 || pz >= (int)Z_RANGE) { + return; + } + + auto old_cost = costmap(px, py, pz); + if (!old_cost) return; + + vec3 obstacle_pos(px, py, pz); + km += heuristic(start_state, costmap(px, py, pz)); + + setOccupied(px, py, pz); + auto node = costmap(px, py, pz); + + updateVertex(node); + for (auto neighbor : getSuccessors(node)) { + updateVertex(neighbor); + } + for (auto neighbor : getPredecessors(node)) { + updateVertex(neighbor); + } + + computeShortestPath(); +} \ No newline at end of file diff --git a/src/kestrel_planning/src/pathing_node.cpp b/src/kestrel_planning/src/pathing_node.cpp index 99bfbd7..e1ef89e 100644 --- a/src/kestrel_planning/src/pathing_node.cpp +++ b/src/kestrel_planning/src/pathing_node.cpp @@ -1,2 +1,271 @@ -int main(int argc, char **argv) { return 0; } -// TEMP MAIN() TO MAKE CMAKE HAPPY — REMOVE WHEN NODE IS IMPLEMENTED \ No newline at end of file +#include "pathing_node.hpp" +#include "dstar_planner.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using std::placeholders::_1; + +DStarNode::DStarNode() +: rclcpp::Node("kestrel_planning"), + planner_(nullptr), + planning_request_(false), + current_state_(PlannerState::IDLE), + got_costmap_(false), + got_start_(false), + got_goal_(false), + has_valid_path_(false) +{ + this->declare_parameter("x_range", 100); + this->declare_parameter("y_range", 100); + this->declare_parameter("z_range", 100); + + int x = this->get_parameter("x_range").as_int(); + int y = this->get_parameter("y_range").as_int(); + int z = this->get_parameter("z_range").as_int(); + + RCLCPP_INFO(this->get_logger(), "Creating DSTARLITE planner with ranges X:%d Y:%d Z:%d", x, y, z); + + planner_ = std::make_unique(x, y, z); + + + RCLCPP_INFO(this->get_logger(), "Successfully made node!"); + + costmap_sub_ = this->create_subscription( + "/map", 10, std::bind(&DStarNode::mapCallback, this, _1)); + + start_sub_ = this->create_subscription( + "/current_pose", 10, std::bind(&DStarNode::odomCallback, this, _1)); + + goal_sub_ = this->create_subscription( + "/goal_pose", 10, std::bind(&DStarNode::goalCallback, this, _1)); + + replan_sub_ = this->create_subscription( + "/replan", 10, std::bind(&DStarNode::replanCallback, this, _1)); + + path_pub_ = this->create_publisher("/planned_path", 10); + status_pub_ = this->create_publisher("/planner_status", 10); + + planning_thread_ = std::thread(&DStarNode::planningWorker, this); +} + +DStarNode::~DStarNode() +{ + { + std::lock_guard lk(planner_mutex_); + planning_request_ = false; + } + planning_cv_.notify_all(); + + if (planning_thread_.joinable()) { + planning_thread_.join(); + } +} + + +void DStarNode::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + std::lock_guard lk(planner_mutex_); + got_costmap_ = true; + + + int width = msg->info.width; + int height = msg->info.height; + + + for (int y = 0; y < height; ++y) { + for (int x = 0; x < width; ++x) { + int idx = y * width + x; + if (msg->data[idx] > 50) { + + planner_->setOccupied(x, y, 0); + } + } + } + + RCLCPP_INFO(this->get_logger(), "Map received: %u x %u (res: %.3f)", + width, height, msg->info.resolution); + + if (has_valid_path_) { + RCLCPP_INFO(this->get_logger(), "Map changed, triggering replan"); + has_valid_path_ = false; + } +} + +void DStarNode::goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + std::lock_guard lk(planner_mutex_); + planner_->setGoal(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + got_goal_ = true; + has_valid_path_ = false; + RCLCPP_INFO(this->get_logger(), "Goal set: %.3f, %.3f, %.3f", + msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); +} + +void DStarNode::odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + std::lock_guard lk(planner_mutex_); + planner_->setStart(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + got_start_ = true; + RCLCPP_INFO(this->get_logger(), "Start set: %.3f, %.3f, %.3f", + msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + +} + +void DStarNode::replanCallback(const std_msgs::msg::Empty::SharedPtr msg) +{ + (void)msg; + std::lock_guard lk(planner_mutex_); + + if (has_valid_path_) { + RCLCPP_INFO(this->get_logger(), "Replan requested - existing path will be updated"); + updatePlannerState(PlannerState::REPLANNING); + } else { + RCLCPP_INFO(this->get_logger(), "Replan requested - will compute new path"); + } + + if (got_costmap_ && got_start_ && got_goal_) { + if (!has_valid_path_) { + planner_->initialize(); + } + planning_request_ = true; + planning_cv_.notify_one(); + RCLCPP_INFO(this->get_logger(), "Replan triggered successfully"); + } else { + RCLCPP_WARN(this->get_logger(), "Cannot replan - missing required data (costmap=%d, start=%d, goal=%d)", + got_costmap_, got_start_, got_goal_); + + if (!got_costmap_) updatePlannerState(PlannerState::NO_MAP); + else if (!got_goal_) updatePlannerState(PlannerState::AWAITING_GOAL); + else if (!got_start_) updatePlannerState(PlannerState::IDLE); + } +} + +void DStarNode::triggerPlanning() +{ + std::lock_guard lk(planner_mutex_); + triggerPlanningLocked_(); +} + +void DStarNode::triggerPlanningLocked_() +{ + RCLCPP_INFO(this->get_logger(), "Planning check: costmap=%d start=%d goal=%d", + got_costmap_, got_start_, got_goal_); + + if (got_costmap_ && got_start_ && got_goal_) { + planning_request_ = true; + RCLCPP_INFO(this->get_logger(), "Planning triggered"); + planning_cv_.notify_one(); + } else { + if (!got_costmap_) updatePlannerState(PlannerState::NO_MAP); + else if (!got_goal_) updatePlannerState(PlannerState::AWAITING_GOAL); + else if (!got_start_) updatePlannerState(PlannerState::IDLE); + else updatePlannerState(PlannerState::FAILURE); + } +} + + +void DStarNode::planningWorker() +{ + while (rclcpp::ok()) { + std::unique_lock lk(planner_mutex_); + planning_cv_.wait(lk, [this]{ return planning_request_ || !rclcpp::ok(); }); + + if (!rclcpp::ok()) break; + if (!planning_request_) continue; + + bool was_replanning = (planning_mode_ == PlanningMode::REPLAN); + + RCLCPP_INFO(this->get_logger(), "Planning worker running (%s)", + was_replanning ? "REPLANNING" : "NEW_PATH"); + + if (!was_replanning) { + updatePlannerState(PlannerState::PLANNING); + } + + try { + int status = planner_->computeShortestPath(); + RCLCPP_INFO(this->get_logger(), "computeShortestPath returned: %d", status); + + if (status >= 0) { + std::vector waypoints; + int extracted = planner_->extractPath(waypoints); + RCLCPP_INFO(this->get_logger(), "extractPath returned: %d", extracted); + + if (extracted >= 0) { + nav_msgs::msg::Path path_msg; + path_msg.header.stamp = this->now(); + path_msg.header.frame_id = "map"; + path_msg.poses = std::move(waypoints); + path_pub_->publish(path_msg); + has_valid_path_ = true; + updatePlannerState(PlannerState::SUCCESS); + + if (was_replanning) { + RCLCPP_INFO(this->get_logger(), "Replan successful - path updated with %zu waypoints", + path_msg.poses.size()); + } else { + RCLCPP_INFO(this->get_logger(), "New path computed with %zu waypoints", + path_msg.poses.size()); + } + } else { + RCLCPP_WARN(this->get_logger(), "extractPath failed"); + has_valid_path_ = false; + updatePlannerState(PlannerState::FAILURE); + } + } else { + RCLCPP_WARN(this->get_logger(), "computeShortestPath failed"); + has_valid_path_ = false; + updatePlannerState(PlannerState::FAILURE); + } + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Exception during planning: %s", e.what()); + has_valid_path_ = false; + updatePlannerState(PlannerState::FAILURE); + } + + planning_request_ = false; + + planning_mode_ = PlanningMode::NEW_PATH; + lk.unlock(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +void DStarNode::updatePlannerState(PlannerState new_state) +{ + current_state_.store(new_state); + publishStatus(); +} + +void DStarNode::publishStatus() +{ + std_msgs::msg::String msg; + switch (current_state_.load()) { + case PlannerState::IDLE: msg.data = "IDLE"; break; + case PlannerState::NO_MAP: msg.data = "NO_MAP"; break; + case PlannerState::AWAITING_GOAL: msg.data = "AWAITING_GOAL"; break; + case PlannerState::PLANNING: msg.data = "PLANNING"; break; + case PlannerState::REPLANNING: msg.data = "REPLANNING"; break; + case PlannerState::SUCCESS: msg.data = "SUCCESS"; break; + case PlannerState::FAILURE: msg.data = "FAILURE"; break; + default: msg.data = "UNKNOWN"; break; + } + status_pub_->publish(msg); +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/kestrel_planning/src/waypoint_manager.cpp b/src/kestrel_planning/src/waypoint_manager.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/test/test_collision_checker.cpp b/src/kestrel_planning/test/test_collision_checker.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/test/test_dstar.cpp b/src/kestrel_planning/test/test_dstar.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/kestrel_planning/test/test_pathing.py b/src/kestrel_planning/test/test_pathing.py new file mode 100644 index 0000000..bcc9067 --- /dev/null +++ b/src/kestrel_planning/test/test_pathing.py @@ -0,0 +1,330 @@ +import unittest +import time +import rclpy +from rclpy.node import Node +from nav_msgs.msg import OccupancyGrid, Path +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import String, Empty +import subprocess +import sys +import threading +from builtin_interfaces.msg import Time + + + + +def set_header(msg, frame_id="map"): + msg.header.frame_id = frame_id + msg.header.stamp = Time(sec=int(time.time())) + return msg + +class PlannerTestNode(Node): + def __init__(self): + super().__init__('planner_test_node') + + self.received_path = None + self.current_status = None + self.status_history = [] + self.path_count = 0 + self.last_path_time = None + + self.create_subscription(Path, '/planned_path', self.path_callback, 10) + self.create_subscription(String, '/planner_status', self.status_callback, 10) + + def path_callback(self, msg): + self.path_count += 1 + self.last_path_time = time.time() + self.get_logger().info(f"Received path #{self.path_count} with {len(msg.poses)} waypoints") + self.received_path = msg + + def status_callback(self, msg): + self.current_status = msg.data + self.status_history.append((msg.data, time.time())) + print(f"[Planner Status] {msg.data}") + + def reset_path_tracking(self): + """Reset path tracking for new test scenarios""" + self.received_path = None + self.path_count = 0 + self.last_path_time = None + self.status_history = [] + + +class TestPlanner(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.node_process = subprocess.Popen( + ["ros2", "run", "kestrel_planning", "kestrel_planning"], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + bufsize=1 + ) + + def print_stream(stream, prefix): + for line in iter(stream.readline, ''): + sys.stdout.write(f"[{prefix}] {line}") + stream.close() + + cls.stdout_thread = threading.Thread( + target=print_stream, args=(cls.node_process.stdout, "STDOUT"), daemon=True + ) + cls.stderr_thread = threading.Thread( + target=print_stream, args=(cls.node_process.stderr, "STDERR"), daemon=True + ) + cls.stdout_thread.start() + cls.stderr_thread.start() + + time.sleep(2) + + rclpy.init() + cls.node = PlannerTestNode() + + cls.map_pub = cls.node.create_publisher(OccupancyGrid, '/map', 10) + cls.start_pub = cls.node.create_publisher(PoseStamped, '/current_pose', 10) + cls.goal_pub = cls.node.create_publisher(PoseStamped, '/goal_pose', 10) + cls.replan_pub = cls.node.create_publisher(Empty, '/replan', 10) + + @classmethod + def tearDownClass(cls): + cls.node_process.terminate() + cls.node_process.wait(timeout=5) + rclpy.shutdown() + + def create_map_with_obstacles(self, width=100, height=100, obstacle_coords=None): + map_msg = OccupancyGrid() + map_msg.info.width = width + map_msg.info.height = height + map_msg.info.resolution = 1.0 + map_msg.data = [0] * (width * height) + + if obstacle_coords: + for (ox, oy) in obstacle_coords: + if 0 <= ox < width and 0 <= oy < height: + idx = oy * width + ox + map_msg.data[idx] = 100 + + map_msg = set_header(map_msg, "map") + return map_msg + + def publish_start_goal(self, start_pos=(0.0, 0.0, 0.0), goal_pos=(50.0, 5.0, 50.0)): + start_msg = PoseStamped() + start_msg.pose.position.x = start_pos[0] + start_msg.pose.position.y = start_pos[1] + start_msg.pose.position.z = start_pos[2] + start_msg = set_header(start_msg, "map") + self.start_pub.publish(start_msg) + + goal_msg = PoseStamped() + goal_msg.pose.position.x = goal_pos[0] + goal_msg.pose.position.y = goal_pos[1] + goal_msg.pose.position.z = goal_pos[2] + goal_msg = set_header(goal_msg, "map") + self.goal_pub.publish(goal_msg) + + def wait_for_subscribers(self, timeout=5): + start_time = time.time() + while (self.map_pub.get_subscription_count() == 0 or + self.start_pub.get_subscription_count() == 0 or + self.goal_pub.get_subscription_count() == 0) and time.time() - start_time < timeout: + rclpy.spin_once(self.node, timeout_sec=0.1) + + def wait_for_status(self, expected_status, timeout=10): + start_time = time.time() + while self.node.current_status != expected_status and time.time() - start_time < timeout: + rclpy.spin_once(self.node, timeout_sec=0.1) + return self.node.current_status == expected_status + + def wait_for_path(self, timeout=5): + initial_count = self.node.path_count + start_time = time.time() + while self.node.path_count <= initial_count and time.time() - start_time < timeout: + rclpy.spin_once(self.node, timeout_sec=0.1) + return self.node.path_count > initial_count + + def test_initial_path_planning(self): + print("\n=== Testing Initial Path Planning ===") + + self.wait_for_subscribers() + self.node.reset_path_tracking() + + empty_map = self.create_map_with_obstacles() + self.map_pub.publish(empty_map) + time.sleep(0.5) + + self.publish_start_goal() + time.sleep(0.5) + + map_with_obstacle = self.create_map_with_obstacles(obstacle_coords=[(1, 1)]) + self.map_pub.publish(map_with_obstacle) + time.sleep(0.5) + + replan_msg = Empty() + self.replan_pub.publish(replan_msg) + time.sleep(0.5) + + + success = self.wait_for_status("SUCCESS", timeout=10) + self.assertTrue(success, f"Planner did not reach SUCCESS status. Current: {self.node.current_status}") + + path_received = self.wait_for_path(timeout=5) + self.assertTrue(path_received, "No path received from planner") + self.assertIsNotNone(self.node.received_path, "Path is None") + self.assertGreater(len(self.node.received_path.poses), 0, "Path is empty") + + for pose in self.node.received_path.poses: + x = round(pose.pose.position.x) + y = round(pose.pose.position.y) + self.assertFalse((x, y) == (1, 1), f"Path includes blocked cell (1,1): ({x},{y})") + + print(f"Initial planning successful: {len(self.node.received_path.poses)} waypoints") + + def test_replan_with_new_obstacles(self): + """Test replanning when new obstacles are added""" + print("\n=== Testing Replan with New Obstacles ===") + + self.test_initial_path_planning() + + initial_path_count = self.node.path_count + time.sleep(1) + + map_with_more_obstacles = self.create_map_with_obstacles( + obstacle_coords=[(1, 1), (25, 2), (25, 3), (25, 4), (25, 5)] + ) + self.map_pub.publish(map_with_more_obstacles) + time.sleep(0.5) + + print("Triggering replan...") + replan_msg = Empty() + self.replan_pub.publish(replan_msg) + time.sleep(0.5) + + # Wait for replanning status or success + replanning_detected = False + start_time = time.time() + while time.time() - start_time < 10: + rclpy.spin_once(self.node, timeout_sec=0.1) + if self.node.current_status == "REPLANNING": + replanning_detected = True + print("Replanning status detected") + break + elif self.node.current_status == "SUCCESS" and self.node.path_count > initial_path_count: + print("Replan completed directly to SUCCESS") + break + + # Wait for final success + success = self.wait_for_status("SUCCESS", timeout=15) + self.assertTrue(success, f"Replanning did not complete successfully. Status: {self.node.current_status}") + + # Verify we got a new path + self.assertGreater(self.node.path_count, initial_path_count, "No new path generated during replan") + + print(f"Replan successful: {len(self.node.received_path.poses)} waypoints") + + def test_replan_without_existing_path(self): + """Test replan command when no existing path exists""" + print("\n=== Testing Replan without Existing Path ===") + + self.wait_for_subscribers() + self.node.reset_path_tracking() + + # Publish map and positions + empty_map = self.create_map_with_obstacles() + self.map_pub.publish(empty_map) + time.sleep(0.5) + + self.publish_start_goal(start_pos=(10.0, 10.0, 0.0), goal_pos=(80.0, 80.0, 0.0)) + time.sleep(0.5) + + # Trigger replan before any initial planning + print("Triggering replan without existing path...") + replan_msg = Empty() + self.replan_pub.publish(replan_msg) + time.sleep(0.5) + + # Should result in new path planning + success = self.wait_for_status("SUCCESS", timeout=10) + self.assertTrue(success, f"Replan without existing path failed. Status: {self.node.current_status}") + + path_received = self.wait_for_path(timeout=5) + self.assertTrue(path_received, "No path received from replan command") + + print(f"Replan without existing path successful: {len(self.node.received_path.poses)} waypoints") + + def test_replan_with_goal_change(self): + """Test replanning after goal change""" + print("\n=== Testing Replan with Goal Change ===") + + # Start with initial planning + self.test_initial_path_planning() + initial_path_count = self.node.path_count + time.sleep(1) + + # Change goal position + print("Changing goal position...") + self.publish_start_goal(goal_pos=(20.0, 80.0, 20.0)) + time.sleep(0.5) + + # Trigger replan + print("Triggering replan with new goal...") + replan_msg = Empty() + self.replan_pub.publish(replan_msg) + time.sleep(0.5) + + # Wait for completion + success = self.wait_for_status("SUCCESS", timeout=10) + self.assertTrue(success, f"Goal change replan failed. Status: {self.node.current_status}") + + # Verify new path + self.assertGreater(self.node.path_count, initial_path_count, "No new path after goal change") + + print(f"Goal change replan successful: {len(self.node.received_path.poses)} waypoints") + + def test_multiple_replans(self): + """Test multiple consecutive replans""" + print("\n=== Testing Multiple Consecutive Replans ===") + + # Start with initial path + self.test_initial_path_planning() + initial_count = self.node.path_count + + # Perform multiple replans + for i in range(3): + print(f"Performing replan #{i+1}") + + # Add different obstacles each time + obstacles = [(10+i*5, 10+i*5), (15+i*5, 15+i*5)] + map_msg = self.create_map_with_obstacles(obstacle_coords=obstacles) + self.map_pub.publish(map_msg) + time.sleep(0.3) + + # Trigger replan + replan_msg = Empty() + self.replan_pub.publish(replan_msg) + time.sleep(0.3) + + # Wait for success + success = self.wait_for_status("SUCCESS", timeout=8) + self.assertTrue(success, f"Multiple replan #{i+1} failed. Status: {self.node.current_status}") + + # Verify we got multiple new paths + final_count = self.node.path_count + self.assertGreaterEqual(final_count - initial_count, 3, + f"Expected at least 3 new paths, got {final_count - initial_count}") + + print(f"Multiple replans successful: {final_count - initial_count} new paths generated") + + def save_path_to_file(self, filename="test_path.txt"): + """Save the current path to a file for debugging""" + if self.node.received_path: + with open(filename, "w") as f: + f.write(f"# Path with {len(self.node.received_path.poses)} waypoints\n") + for i, pose in enumerate(self.node.received_path.poses): + p = pose.pose.position + f.write(f"{i}: {p.x:.3f}, {p.y:.3f}, {p.z:.3f}\n") + print(f"Path saved to {filename}") + + +if __name__ == '__main__': + # Run tests with more verbose output + unittest.main(verbosity=2) \ No newline at end of file