From f6935ee6100262dccdcbb0bec0588c85a2b00742 Mon Sep 17 00:00:00 2001 From: Syndric <65517819+Syndric@users.noreply.github.com> Date: Tue, 16 Sep 2025 20:54:13 -0400 Subject: [PATCH 1/6] feat: add l4t-ros2-docker base image --- .gitmodules | 3 +++ docker/l4t-ros2-docker | 1 + docker/ros_entrypoint.sh | 0 3 files changed, 4 insertions(+) create mode 160000 docker/l4t-ros2-docker delete mode 100644 docker/ros_entrypoint.sh diff --git a/.gitmodules b/.gitmodules index 68dbf7a..9f17e89 100644 --- a/.gitmodules +++ b/.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 diff --git a/docker/l4t-ros2-docker b/docker/l4t-ros2-docker new file mode 160000 index 0000000..e8534a5 --- /dev/null +++ b/docker/l4t-ros2-docker @@ -0,0 +1 @@ +Subproject commit e8534a513feb17d9d6d4112c8e572f21a70b3841 diff --git a/docker/ros_entrypoint.sh b/docker/ros_entrypoint.sh deleted file mode 100644 index e69de29..0000000 From 6c0fc34978fdca9be03c2be037589950f25a59dd Mon Sep 17 00:00:00 2001 From: Syndric <65517819+Syndric@users.noreply.github.com> Date: Tue, 16 Sep 2025 21:07:23 -0400 Subject: [PATCH 2/6] feat: add prod work flow - base --- .github/workflows/build_prod.yaml | 47 +++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 .github/workflows/build_prod.yaml diff --git a/.github/workflows/build_prod.yaml b/.github/workflows/build_prod.yaml new file mode 100644 index 0000000..c686047 --- /dev/null +++ b/.github/workflows/build_prod.yaml @@ -0,0 +1,47 @@ +# 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 + + - 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 From 1e0a8b2fbd3da5a2e2ffa989df125b2f9044a4ab Mon Sep 17 00:00:00 2001 From: Syndric <65517819+Syndric@users.noreply.github.com> Date: Tue, 16 Sep 2025 21:11:16 -0400 Subject: [PATCH 3/6] fix: checkout submodules in checkout --- .github/workflows/build_prod.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/build_prod.yaml b/.github/workflows/build_prod.yaml index c686047..3ae0fd0 100644 --- a/.github/workflows/build_prod.yaml +++ b/.github/workflows/build_prod.yaml @@ -18,6 +18,8 @@ jobs: steps: - name: Checkout Repo uses: actions/checkout@v4 + with: + submodules: 'recursive' - name: Set up QEMU uses: docker/setup-qemu-action@v2 From 976abb93b7d6e42e2344aa27d5a6ec7525296392 Mon Sep 17 00:00:00 2001 From: mistry Date: Sat, 20 Sep 2025 02:40:46 -0400 Subject: [PATCH 4/6] manager node and tests --- src/kestrel_planning/CMakeLists.txt | 34 +- .../include/dstar_planner.hpp | 1 + src/kestrel_planning/include/pathing_node.hpp | 13 +- .../include/waypoint_manager_node.hpp | 56 ++ src/kestrel_planning/package.xml | 2 + src/kestrel_planning/src/dstar_planner.cpp | 1 + src/kestrel_planning/src/pathing_node.cpp | 67 +- .../src/waypoint_manager_node.cpp | 145 +++++ src/kestrel_planning/test/runtest.py | 88 +++ src/kestrel_planning/test/test_pathing.py | 602 ++++++++++++------ src/kestrel_planning/test/test_waypoints.py | 257 ++++++++ 11 files changed, 1031 insertions(+), 235 deletions(-) create mode 100644 src/kestrel_planning/include/waypoint_manager_node.hpp create mode 100644 src/kestrel_planning/src/waypoint_manager_node.cpp create mode 100644 src/kestrel_planning/test/runtest.py create mode 100644 src/kestrel_planning/test/test_waypoints.py diff --git a/src/kestrel_planning/CMakeLists.txt b/src/kestrel_planning/CMakeLists.txt index 224ddf4..69ee468 100644 --- a/src/kestrel_planning/CMakeLists.txt +++ b/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() \ No newline at end of file diff --git a/src/kestrel_planning/include/dstar_planner.hpp b/src/kestrel_planning/include/dstar_planner.hpp index 380c2bf..3189d2f 100644 --- a/src/kestrel_planning/include/dstar_planner.hpp +++ b/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" diff --git a/src/kestrel_planning/include/pathing_node.hpp b/src/kestrel_planning/include/pathing_node.hpp index 629a191..cb2053e 100644 --- a/src/kestrel_planning/include/pathing_node.hpp +++ b/src/kestrel_planning/include/pathing_node.hpp @@ -9,7 +9,7 @@ #include #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" @@ -48,18 +48,17 @@ class DStarNode : public rclcpp::Node { 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::SharedPtr octomap_sub_; - rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr costmap_sub_; rclcpp::Subscription::SharedPtr start_sub_; - rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr replan_sub_; rclcpp::Publisher::SharedPtr path_pub_; rclcpp::Publisher::SharedPtr status_pub_; - rclcpp::Subscription::SharedPtr replan_sub_; std::unique_ptr planner_; std::mutex planner_mutex_; diff --git a/src/kestrel_planning/include/waypoint_manager_node.hpp b/src/kestrel_planning/include/waypoint_manager_node.hpp new file mode 100644 index 0000000..697d1af --- /dev/null +++ b/src/kestrel_planning/include/waypoint_manager_node.hpp @@ -0,0 +1,56 @@ +#ifndef WAYPOINT_MANAGER_NODE_HPP +#define WAYPOINT_MANAGER_NODE_HPP + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include + +class WayPointManagerNode : public rclcpp::Node +{ +public: + WayPointManagerNode(); + + // Optional utility methods + void setWaypointTolerance(double tolerance); + size_t getCurrentWaypointIndex() const; + bool hasReachedGoal() const; + +private: + // Callback methods + void currentPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr msg); + void pathCallback(const nav_msgs::msg::Path::SharedPtr msg); + + // Core logic methods + void updateWaypoint(); + void publishCurrentWaypoint(); + double calculateDistance(const geometry_msgs::msg::PoseStamped& pose1, + const geometry_msgs::msg::PoseStamped& pose2); + + // ROS2 subscribers and publishers + rclcpp::Subscription::SharedPtr start_sub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Publisher::SharedPtr position_pub_; + + // Timer for periodic waypoint updates + rclcpp::TimerBase::SharedPtr timer_; + + // State variables + geometry_msgs::msg::PoseStamped current_pose_; + mavros_msgs::msg::PositionTarget goal_position_; + nav_msgs::msg::Path current_path_; + + size_t current_waypoint_index_; + double waypoint_tolerance_; + + // Status flags + bool has_current_pose_; + bool has_goal_; + bool has_path_; +}; + +#endif // WAYPOINT_MANAGER_NODE_HPP \ No newline at end of file diff --git a/src/kestrel_planning/package.xml b/src/kestrel_planning/package.xml index 3f66c10..d2ced16 100644 --- a/src/kestrel_planning/package.xml +++ b/src/kestrel_planning/package.xml @@ -13,6 +13,8 @@ std_msgs geometry_msgs nav_msgs + kestrel_msgs + nav2_core nav2_costmap_2d diff --git a/src/kestrel_planning/src/dstar_planner.cpp b/src/kestrel_planning/src/dstar_planner.cpp index 7896279..d61d968 100644 --- a/src/kestrel_planning/src/dstar_planner.cpp +++ b/src/kestrel_planning/src/dstar_planner.cpp @@ -269,6 +269,7 @@ int DSTARLITE::extractPath(std::vector &waypoin while (node && visited.find(node->getPoint()) == visited.end()) { vec3 point = node->getPoint(); + std::cout << "[DEBUG] " << point.x << " " << point.y << " " << point.z << std::endl; visited.insert(point); if (!(point == start)) { diff --git a/src/kestrel_planning/src/pathing_node.cpp b/src/kestrel_planning/src/pathing_node.cpp index e1ef89e..9b3f5c4 100644 --- a/src/kestrel_planning/src/pathing_node.cpp +++ b/src/kestrel_planning/src/pathing_node.cpp @@ -3,6 +3,10 @@ #include #include + +#include "kestrel_msgs/msg/obstacle_grid.hpp" +#include "mavros_msgs/msg/position_target.hpp" + #include #include #include @@ -21,7 +25,8 @@ DStarNode::DStarNode() got_costmap_(false), got_start_(false), got_goal_(false), - has_valid_path_(false) + has_valid_path_(false), + planning_mode_(PlanningMode::NEW_PATH) { this->declare_parameter("x_range", 100); this->declare_parameter("y_range", 100); @@ -35,23 +40,25 @@ DStarNode::DStarNode() 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)); + costmap_sub_ = this->create_subscription( + "perception/obstacle_grid", 10, std::bind(&DStarNode::mapCallback, this, _1)); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); start_sub_ = this->create_subscription( - "/current_pose", 10, std::bind(&DStarNode::odomCallback, this, _1)); + "odometry/local_pose", qos, std::bind(&DStarNode::odomCallback, this, _1)); - goal_sub_ = this->create_subscription( - "/goal_pose", 10, std::bind(&DStarNode::goalCallback, this, _1)); + goal_sub_ = this->create_subscription( + "mavros/setpoint_raw/local", 10, std::bind(&DStarNode::goalCallback, this, _1)); replan_sub_ = this->create_subscription( - "/replan", 10, std::bind(&DStarNode::replanCallback, this, _1)); + "planning/replan", 10, std::bind(&DStarNode::replanCallback, this, _1)); - path_pub_ = this->create_publisher("/planned_path", 10); - status_pub_ = this->create_publisher("/planner_status", 10); + path_pub_ = this->create_publisher("planning/path", 10); + status_pub_ = this->create_publisher("planning/planner_status", 10); planning_thread_ = std::thread(&DStarNode::planningWorker, this); } @@ -69,44 +76,48 @@ DStarNode::~DStarNode() } } - -void DStarNode::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +void DStarNode::mapCallback(const kestrel_msgs::msg::ObstacleGrid::SharedPtr msg) { std::lock_guard lk(planner_mutex_); got_costmap_ = true; - - int width = msg->info.width; - int height = msg->info.height; + uint32_t width = msg->width; + uint32_t height = msg->height; + uint32_t depth = msg->depth; - - for (int y = 0; y < height; ++y) { - for (int x = 0; x < width; ++x) { - int idx = y * width + x; - if (msg->data[idx] > 50) { + for (uint32_t z = 0; z < depth; ++z) { + for (uint32_t y = 0; y < height; ++y) { + for (uint32_t x = 0; x < width; ++x) { + size_t idx = z * (width * height) + y * width + x; - planner_->setOccupied(x, y, 0); + if (idx < msg->data.size() && msg->data[idx] > 50) { + planner_->setOccupied(x, y, z); + } } } } - RCLCPP_INFO(this->get_logger(), "Map received: %u x %u (res: %.3f)", - width, height, msg->info.resolution); + RCLCPP_INFO(this->get_logger(), "3D Map received: %u x %u x %u (res: %.3f)", + width, height, depth, msg->resolution); if (has_valid_path_) { RCLCPP_INFO(this->get_logger(), "Map changed, triggering replan"); has_valid_path_ = false; } + //triggerPlanningLocked_(); } -void DStarNode::goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +void DStarNode::goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr msg) { std::lock_guard lk(planner_mutex_); - planner_->setGoal(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + + planner_->setGoal(msg->position.x, msg->position.y, msg->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); + msg->position.x, msg->position.y, msg->position.z); + //triggerPlanningLocked_(); } void DStarNode::odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) @@ -116,7 +127,7 @@ void DStarNode::odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr ms 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); - + //triggerPlanningLocked_(); } void DStarNode::replanCallback(const std_msgs::msg::Empty::SharedPtr msg) @@ -171,7 +182,6 @@ void DStarNode::triggerPlanningLocked_() } } - void DStarNode::planningWorker() { while (rclcpp::ok()) { @@ -232,7 +242,6 @@ void DStarNode::planningWorker() } planning_request_ = false; - planning_mode_ = PlanningMode::NEW_PATH; lk.unlock(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); diff --git a/src/kestrel_planning/src/waypoint_manager_node.cpp b/src/kestrel_planning/src/waypoint_manager_node.cpp new file mode 100644 index 0000000..6b42fca --- /dev/null +++ b/src/kestrel_planning/src/waypoint_manager_node.cpp @@ -0,0 +1,145 @@ +#include "waypoint_manager_node.hpp" + +WayPointManagerNode::WayPointManagerNode() +: rclcpp::Node("kestrel_waypoint"), + current_waypoint_index_(0), + waypoint_tolerance_(0.5), // 0.5 meters tolerance + has_current_pose_(false), + has_goal_(false), + has_path_(false) +{ + start_sub_ = this->create_subscription( + "odometry/local_pose", 10, + std::bind(&WayPointManagerNode::currentPoseCallback, this, std::placeholders::_1)); + + goal_sub_ = this->create_subscription( + "mavros/setpoint_raw/local", 10, + std::bind(&WayPointManagerNode::goalCallback, this, std::placeholders::_1)); + + path_sub_ = this->create_subscription( + "planning/path", 10, + std::bind(&WayPointManagerNode::pathCallback, this, std::placeholders::_1)); + + position_pub_ = this->create_publisher("planning/waypoint", 10); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&WayPointManagerNode::updateWaypoint, this)); + + RCLCPP_INFO(this->get_logger(), "Waypoint Manager Node initialized"); +} + +void WayPointManagerNode::currentPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + current_pose_ = *msg; + has_current_pose_ = true; +} + +void WayPointManagerNode::goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr msg) +{ + goal_position_ = *msg; + has_goal_ = true; +} + +void WayPointManagerNode::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) +{ + if (msg->poses.empty()) { + RCLCPP_WARN(this->get_logger(), "Received empty path"); + return; + } + + current_path_ = *msg; + has_path_ = true; + current_waypoint_index_ = 0; // Reset to start of new path + + RCLCPP_INFO(this->get_logger(), "Received new path with %zu waypoints", + current_path_.poses.size()); +} + +void WayPointManagerNode::updateWaypoint() +{ + if (!has_current_pose_ || !has_path_ || current_path_.poses.empty()) { + return; + } + + // Check if we've reached the current waypoint + if (current_waypoint_index_ < current_path_.poses.size()) { + const auto& current_waypoint = current_path_.poses[current_waypoint_index_]; + + double distance = calculateDistance(current_pose_, current_waypoint); + + if (distance < waypoint_tolerance_) { + // Move to next waypoint + current_waypoint_index_++; + + if (current_waypoint_index_ >= current_path_.poses.size()) { + RCLCPP_INFO(this->get_logger(), "Reached end of path"); + return; + } + } + + // Publish the current target waypoint + publishCurrentWaypoint(); + } +} + +void WayPointManagerNode::publishCurrentWaypoint() +{ + if (current_waypoint_index_ >= current_path_.poses.size()) { + return; + } + + const auto& waypoint_to_publish = current_path_.poses[current_waypoint_index_]; + + geometry_msgs::msg::PoseStamped waypoint_msg = waypoint_to_publish; + waypoint_msg.header.stamp = this->get_clock()->now(); + waypoint_msg.header.frame_id = current_path_.header.frame_id; + + position_pub_->publish(waypoint_msg); + + RCLCPP_DEBUG(this->get_logger(), + "Published waypoint %zu: [%.2f, %.2f, %.2f]", + current_waypoint_index_, + waypoint_msg.pose.position.x, + waypoint_msg.pose.position.y, + waypoint_msg.pose.position.z); +} + +double WayPointManagerNode::calculateDistance( + const geometry_msgs::msg::PoseStamped& pose1, + const geometry_msgs::msg::PoseStamped& pose2) +{ + double dx = pose1.pose.position.x - pose2.pose.position.x; + double dy = pose1.pose.position.y - pose2.pose.position.y; + double dz = pose1.pose.position.z - pose2.pose.position.z; + + return std::sqrt(dx*dx + dy*dy + dz*dz); +} + +// Optional: Method to set waypoint tolerance dynamically +void WayPointManagerNode::setWaypointTolerance(double tolerance) +{ + waypoint_tolerance_ = tolerance; + RCLCPP_INFO(this->get_logger(), "Waypoint tolerance set to %.2f meters", tolerance); +} + +// Optional: Get current waypoint info +size_t WayPointManagerNode::getCurrentWaypointIndex() const +{ + return current_waypoint_index_; +} + +bool WayPointManagerNode::hasReachedGoal() const +{ + return has_path_ && current_waypoint_index_ >= current_path_.poses.size(); +} + + +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/test/runtest.py b/src/kestrel_planning/test/runtest.py new file mode 100644 index 0000000..d3c6618 --- /dev/null +++ b/src/kestrel_planning/test/runtest.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +import rclpy +import time +from rclpy.node import Node + +from nav_msgs.msg import OccupancyGrid +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Empty +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 DStarPublisher(Node): + def __init__(self): + super().__init__('dstar_publisher') + + self.map_pub = self.create_publisher(OccupancyGrid, '/map', 10) + self.start_pub = self.create_publisher(PoseStamped, '/current_pose', 10) + self.goal_pub = self.create_publisher(PoseStamped, '/goal_pose', 10) + self.replan_pub = self.create_publisher(Empty, '/replan', 10) + + self.get_logger().info("DStarPublisher ready to publish test data...") + + def create_map(self, width=50, height=50, obstacle_coords=None): + msg = OccupancyGrid() + msg.info.width = width + msg.info.height = height + msg.info.resolution = 1.0 + msg.data = [0] * (width * height) # free space + + if obstacle_coords: + for (ox, oy) in obstacle_coords: + if 0 <= ox < width and 0 <= oy < height: + idx = oy * width + ox + msg.data[idx] = 100 # obstacle + + return set_header(msg, "map") + + def create_pose(self, x, y, z=0.0): + msg = PoseStamped() + msg.pose.position.x = x + msg.pose.position.y = y + msg.pose.position.z = z + return set_header(msg, "map") + + def publish_test_sequence(self): + self.get_logger().info("Publishing initial empty map...") + map_msg = self.create_map() + self.map_pub.publish(map_msg) + time.sleep(1.0) + + self.get_logger().info("Publishing start and goal poses...") + self.start_pub.publish(self.create_pose(0.0, 0.0, 0.0)) + self.goal_pub.publish(self.create_pose(30.0, 30.0, 0.0)) + time.sleep(1.0) + + self.get_logger().info("Publishing map with obstacle at (10,10)...") + map_msg = self.create_map(obstacle_coords=[(10, 10)]) + self.map_pub.publish(map_msg) + time.sleep(1.0) + + self.get_logger().info("Publishing replan command...") + self.replan_pub.publish(Empty()) + time.sleep(1.0) + + +def main(args=None): + rclpy.init(args=args) + node = DStarPublisher() + + try: + while rclpy.ok(): + node.publish_test_sequence() + time.sleep(5.0) # wait before repeating sequence + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/kestrel_planning/test/test_pathing.py b/src/kestrel_planning/test/test_pathing.py index bcc9067..a8e6bf6 100644 --- a/src/kestrel_planning/test/test_pathing.py +++ b/src/kestrel_planning/test/test_pathing.py @@ -2,58 +2,86 @@ 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 +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped, Point +from mavros_msgs.msg import PositionTarget +from std_msgs.msg import String, Empty, Header +from kestrel_msgs.msg import ObstacleGrid import subprocess import sys import threading from builtin_interfaces.msg import Time - - - +import numpy as np +from rclpy.qos import QoSProfile, ReliabilityPolicy def set_header(msg, frame_id="map"): + """Helper function to set header with current timestamp""" msg.header.frame_id = frame_id - msg.header.stamp = Time(sec=int(time.time())) + msg.header.stamp = Time(sec=int(time.time()), nanosec=int((time.time() % 1) * 1e9)) return msg class PlannerTestNode(Node): def __init__(self): super().__init__('planner_test_node') + # Test state tracking self.received_path = None - self.current_status = None + self.current_planner_status = None self.status_history = [] self.path_count = 0 self.last_path_time = None + self.current_waypoint = None + self.waypoint_count = 0 + + # Publishers for pathing node inputs + self.obstacle_grid_pub = self.create_publisher(ObstacleGrid, 'perception/obstacle_grid', 10) + qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE) + self.local_pose_pub = self.create_publisher(PoseStamped, "odometry/local_pose", qos) + + self.setpoint_pub = self.create_publisher(PositionTarget, 'mavros/setpoint_raw/local', 10) + self.replan_pub = self.create_publisher(Empty, 'planning/replan', 10) + + # Subscribers for pathing node outputs + self.path_sub = self.create_subscription(Path, 'planning/path', self.path_callback, 10) + self.planner_status_sub = self.create_subscription(String, 'planning/planner_status', self.planner_status_callback, 10) - self.create_subscription(Path, '/planned_path', self.path_callback, 10) - self.create_subscription(String, '/planner_status', self.status_callback, 10) + # Subscribers for waypoint manager outputs + self.waypoint_sub = self.create_subscription(PoseStamped, 'planning/waypoint', self.waypoint_callback, 10) def path_callback(self, msg): + """Callback for path messages from pathing node""" 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 + def planner_status_callback(self, msg): + """Callback for planner status messages""" + self.current_planner_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""" + + def waypoint_callback(self, msg): + """Callback for waypoint messages from waypoint manager""" + self.waypoint_count += 1 + self.current_waypoint = msg + self.get_logger().info(f"Received waypoint #{self.waypoint_count}: ({msg.pose.position.x:.2f}, {msg.pose.position.y:.2f}, {msg.pose.position.z:.2f})") + + def reset_tracking(self): + """Reset all tracking variables for new test scenarios""" self.received_path = None self.path_count = 0 self.last_path_time = None self.status_history = [] + self.current_waypoint = None + self.waypoint_count = 0 - -class TestPlanner(unittest.TestCase): +class TestKestrelPlanning(unittest.TestCase): @classmethod def setUpClass(cls): - cls.node_process = subprocess.Popen( + """Set up test environment and start nodes""" + # Start pathing node + cls.pathing_process = subprocess.Popen( ["ros2", "run", "kestrel_planning", "kestrel_planning"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, @@ -61,270 +89,456 @@ def setUpClass(cls): bufsize=1 ) + # Start waypoint manager node + cls.waypoint_process = subprocess.Popen( + ["ros2", "run", "kestrel_planning", "kestrel_waypoint"], + 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 + # Start output threads for both processes + cls.pathing_stdout_thread = threading.Thread( + target=print_stream, args=(cls.pathing_process.stdout, "PATHING-OUT"), daemon=True + ) + cls.pathing_stderr_thread = threading.Thread( + target=print_stream, args=(cls.pathing_process.stderr, "PATHING-ERR"), daemon=True + ) + cls.waypoint_stdout_thread = threading.Thread( + target=print_stream, args=(cls.waypoint_process.stdout, "WAYPOINT-OUT"), daemon=True ) - cls.stderr_thread = threading.Thread( - target=print_stream, args=(cls.node_process.stderr, "STDERR"), daemon=True + cls.waypoint_stderr_thread = threading.Thread( + target=print_stream, args=(cls.waypoint_process.stderr, "WAYPOINT-ERR"), daemon=True ) - cls.stdout_thread.start() - cls.stderr_thread.start() - time.sleep(2) + cls.pathing_stdout_thread.start() + cls.pathing_stderr_thread.start() + cls.waypoint_stdout_thread.start() + cls.waypoint_stderr_thread.start() + # Allow nodes to start + time.sleep(3) + + # Initialize ROS 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) + """Clean up processes and ROS""" + cls.pathing_process.terminate() + cls.waypoint_process.terminate() + cls.pathing_process.wait(timeout=5) + cls.waypoint_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 create_obstacle_grid(self, width=100, height=100, depth=50, obstacles=None): + """Create 3D ObstacleGrid message with optional obstacles""" + grid_msg = ObstacleGrid() + grid_msg.header = Header() + grid_msg = set_header(grid_msg, "map") + + # Set 3D grid parameters based on your ObstacleGrid message + grid_msg.width = width + grid_msg.height = height + grid_msg.depth = depth + grid_msg.resolution = 1.0 + + # Set origin pose + grid_msg.origin.position.x = 0.0 + grid_msg.origin.position.y = 0.0 + grid_msg.origin.position.z = 0.0 + grid_msg.origin.orientation.w = 1.0 + + # Initialize empty 3D grid (row-major order: x + y*width + z*width*height) + total_cells = width * height * depth + grid_msg.data = [0] * total_cells # 0 = free space + + # Add obstacles if specified (assuming obstacles are 3D coordinates) + if obstacles: + for obstacle in obstacles: + if len(obstacle) == 2: + # 2D obstacle - extend through middle layers of 3D grid + ox, oy = obstacle + for oz in range(depth // 4, 3 * depth // 4): # Middle half of depth + if 0 <= ox < width and 0 <= oy < height and 0 <= oz < depth: + idx = ox + oy * width + oz * width * height + grid_msg.data[idx] = 100 # Occupied cell + elif len(obstacle) == 3: + # 3D obstacle + ox, oy, oz = obstacle + if 0 <= ox < width and 0 <= oy < height and 0 <= oz < depth: + idx = ox + oy * width + oz * width * height + grid_msg.data[idx] = 100 # Occupied cell + + return grid_msg + + def create_position_target(self, x, y, z, frame_id="map"): + """Create PositionTarget message for goal setting""" + target = PositionTarget() + target.header = Header() + target = set_header(target, frame_id) + target.coordinate_frame = PositionTarget.FRAME_LOCAL_NED + target.type_mask = PositionTarget.IGNORE_VX | PositionTarget.IGNORE_VY | PositionTarget.IGNORE_VZ | \ + PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | \ + PositionTarget.IGNORE_YAW_RATE + target.position.x = x + target.position.y = y + target.position.z = z + target.yaw = 0.0 + return target + + def create_pose_stamped(self, x, y, z, frame_id="map"): + """Create PoseStamped message for current pose""" + pose = PoseStamped() + pose.header = Header() + pose = set_header(pose, frame_id) + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + pose.pose.orientation.w = 1.0 + return pose def wait_for_subscribers(self, timeout=5): + """Wait for subscribers to connect""" 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: + while (self.node.obstacle_grid_pub.get_subscription_count() == 0 or + self.node.local_pose_pub.get_subscription_count() == 0 or + self.node.setpoint_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): + def wait_for_planner_status(self, expected_status, timeout=10): + """Wait for specific planner status""" start_time = time.time() - while self.node.current_status != expected_status and time.time() - start_time < timeout: + while self.node.current_planner_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 + return self.node.current_planner_status == expected_status - def wait_for_path(self, timeout=5): + def wait_for_path(self, timeout=10): + """Wait for new path to be received""" 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 ===") + def wait_for_waypoint(self, timeout=5): + """Wait for new waypoint to be received""" + initial_count = self.node.waypoint_count + start_time = time.time() + while self.node.waypoint_count <= initial_count and time.time() - start_time < timeout: + rclpy.spin_once(self.node, timeout_sec=0.1) + return self.node.waypoint_count > initial_count + + def test_pathing_node_basic_planning(self): + """Test basic path planning functionality""" + print("\n=== Testing Pathing Node: Basic Planning ===") self.wait_for_subscribers() - self.node.reset_path_tracking() + self.node.reset_tracking() - empty_map = self.create_map_with_obstacles() - self.map_pub.publish(empty_map) + # Publish obstacle grid + obstacle_grid = self.create_obstacle_grid(width=50, height=50, depth=30) + self.node.obstacle_grid_pub.publish(obstacle_grid) time.sleep(0.5) - self.publish_start_goal() + # Publish current pose (start position) + current_pose = self.create_pose_stamped(0.0, 0.0, 0.0) + self.node.local_pose_pub.publish(current_pose) time.sleep(0.5) - map_with_obstacle = self.create_map_with_obstacles(obstacle_coords=[(1, 1)]) - self.map_pub.publish(map_with_obstacle) + # Publish goal + goal = self.create_position_target(25.0, 25.0, 5.0) + self.node.setpoint_pub.publish(goal) + time.sleep(1.0) + + # Wait for path planning to complete + success = self.wait_for_planner_status("SUCCESS", timeout=15) + self.assertTrue(success, f"Planner did not reach SUCCESS status. Current: {self.node.current_planner_status}") + + # Verify path was generated + 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") + + print(f"Basic planning successful: {len(self.node.received_path.poses)} waypoints") + + def test_pathing_node_obstacle_avoidance(self): + """Test path planning with obstacles""" + print("\n=== Testing Pathing Node: Obstacle Avoidance ===") + + self.wait_for_subscribers() + self.node.reset_tracking() + + # Create obstacle grid with obstacles blocking direct path + obstacles_2d = [(12, 12), (12, 13), (13, 12), (13, 13), (14, 12), (14, 13)] # 2D obstacles + obstacles_3d = [(15, 15, 10), (15, 15, 11), (15, 15, 12)] # 3D obstacles + all_obstacles = obstacles_2d + obstacles_3d + obstacle_grid = self.create_obstacle_grid(width=30, height=30, depth=25, obstacles=all_obstacles) + self.node.obstacle_grid_pub.publish(obstacle_grid) time.sleep(0.5) - replan_msg = Empty() - self.replan_pub.publish(replan_msg) + # Set start and goal positions + current_pose = self.create_pose_stamped(10.0, 10.0, 0.0) + self.node.local_pose_pub.publish(current_pose) time.sleep(0.5) + goal = self.create_position_target(20.0, 20.0, 5.0) + self.node.setpoint_pub.publish(goal) + time.sleep(1.0) - success = self.wait_for_status("SUCCESS", timeout=10) - self.assertTrue(success, f"Planner did not reach SUCCESS status. Current: {self.node.current_status}") + # Wait for planning + success = self.wait_for_planner_status("SUCCESS", timeout=15) + self.assertTrue(success, f"Obstacle avoidance planning failed. Status: {self.node.current_planner_status}") + # Verify path avoids obstacles 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") + self.assertTrue(path_received, "No path received") + # Check that path doesn't go through obstacles 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})") + x = int(round(pose.pose.position.x)) + y = int(round(pose.pose.position.y)) + z = int(round(pose.pose.position.z)) + + # Check against 2D obstacles (they extend through depth) + for obs in obstacles_2d: + if (x, y) == obs: + self.fail(f"Path goes through 2D obstacle at ({x}, {y}, {z})") + + # Check against 3D obstacles + for obs in obstacles_3d: + if (x, y, z) == obs: + self.fail(f"Path goes through 3D obstacle at ({x}, {y}, {z})") - print(f"Initial planning successful: {len(self.node.received_path.poses)} waypoints") + print(f"Obstacle avoidance 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() + def test_pathing_node_replan(self): + """Test replanning functionality""" + print("\n=== Testing Pathing Node: Replanning ===") + # First, generate initial path + self.test_pathing_node_basic_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) + # Add new obstacles + new_obstacles = [(5, 5), (6, 6), (7, 7), (8, 8)] # 2D obstacles + obstacle_grid = self.create_obstacle_grid(width=50, height=50, depth=30, obstacles=new_obstacles) + self.node.obstacle_grid_pub.publish(obstacle_grid) time.sleep(0.5) + # Trigger replan print("Triggering replan...") replan_msg = Empty() - self.replan_pub.publish(replan_msg) + self.node.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 replan completion + success = self.wait_for_planner_status("SUCCESS", timeout=15) + self.assertTrue(success, f"Replanning failed. Status: {self.node.current_planner_status}") - # 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 + # Verify new path was generated 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") + print(f"Replanning 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 ===") + def test_waypoint_manager_basic_operation(self): + """Test waypoint manager node basic functionality""" + print("\n=== Testing Waypoint Manager: Basic Operation ===") - self.wait_for_subscribers() - self.node.reset_path_tracking() + self.node.reset_tracking() - # Publish map and positions - empty_map = self.create_map_with_obstacles() - self.map_pub.publish(empty_map) - time.sleep(0.5) + # First generate a path using pathing node + self.test_pathing_node_basic_planning() + time.sleep(1) - self.publish_start_goal(start_pos=(10.0, 10.0, 0.0), goal_pos=(80.0, 80.0, 0.0)) + # Publish current position to waypoint manager + current_pose = self.create_pose_stamped(0.0, 0.0, 0.0) + self.node.local_pose_pub.publish(current_pose) 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) + # Publish the same goal to waypoint manager + goal = self.create_position_target(25.0, 25.0, 5.0) + self.node.setpoint_pub.publish(goal) + time.sleep(1.0) - # 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}") + # Wait for waypoint manager to publish first waypoint + waypoint_received = self.wait_for_waypoint(timeout=10) + self.assertTrue(waypoint_received, "No waypoint received from waypoint manager") + self.assertIsNotNone(self.node.current_waypoint, "Waypoint is None") - 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") + print(f"Waypoint manager operational: First waypoint at ({self.node.current_waypoint.pose.position.x:.2f}, " + f"{self.node.current_waypoint.pose.position.y:.2f}, {self.node.current_waypoint.pose.position.z:.2f})") - def test_replan_with_goal_change(self): - """Test replanning after goal change""" - print("\n=== Testing Replan with Goal Change ===") + def test_waypoint_manager_progression(self): + """Test waypoint manager progression through path""" + print("\n=== Testing Waypoint Manager: Waypoint Progression ===") - # Start with initial planning - self.test_initial_path_planning() - initial_path_count = self.node.path_count + # Setup initial path + self.test_waypoint_manager_basic_operation() + initial_waypoint = self.node.current_waypoint time.sleep(1) - # Change goal position - print("Changing goal position...") - self.publish_start_goal(goal_pos=(20.0, 80.0, 20.0)) + # Simulate moving to first waypoint + if initial_waypoint: + # Move close to the first waypoint + near_waypoint = self.create_pose_stamped( + initial_waypoint.pose.position.x - 0.5, + initial_waypoint.pose.position.y - 0.5, + initial_waypoint.pose.position.z + ) + self.node.local_pose_pub.publish(near_waypoint) + time.sleep(0.5) + + # Move to the waypoint + at_waypoint = self.create_pose_stamped( + initial_waypoint.pose.position.x, + initial_waypoint.pose.position.y, + initial_waypoint.pose.position.z + ) + self.node.local_pose_pub.publish(at_waypoint) + time.sleep(1.0) + + # Check if waypoint manager publishes next waypoint + initial_count = self.node.waypoint_count + waypoint_updated = False + start_time = time.time() + + while time.time() - start_time < 5: + rclpy.spin_once(self.node, timeout_sec=0.1) + if self.node.waypoint_count > initial_count: + waypoint_updated = True + break + + if waypoint_updated: + print(f"Waypoint progression successful: New waypoint at ({self.node.current_waypoint.pose.position.x:.2f}, " + f"{self.node.current_waypoint.pose.position.y:.2f}, {self.node.current_waypoint.pose.position.z:.2f})") + else: + print("Waypoint manager did not progress to next waypoint (might be expected behavior)") + + def test_integrated_planning_and_waypoint_management(self): + """Test integrated operation of both nodes""" + print("\n=== Testing Integrated Operation ===") + + self.wait_for_subscribers() + self.node.reset_tracking() + + # Start with obstacle environment + obstacles_2d = [(15, 10), (15, 11), (15, 12), (15, 13), (15, 14)] + obstacles_3d = [(20, 15, 8), (20, 15, 9), (20, 15, 10)] + all_obstacles = obstacles_2d + obstacles_3d + obstacle_grid = self.create_obstacle_grid(width=40, height=40, depth=20, obstacles=all_obstacles) + self.node.obstacle_grid_pub.publish(obstacle_grid) + time.sleep(0.5) + + # Set initial position + start_pose = self.create_pose_stamped(5.0, 10.0, 0.0) + self.node.local_pose_pub.publish(start_pose) + time.sleep(0.5) + + # Set goal + goal = self.create_position_target(30.0, 15.0, 5.0) + self.node.setpoint_pub.publish(goal) + time.sleep(2.0) + + # Wait for path planning + path_success = self.wait_for_planner_status("SUCCESS", timeout=15) + self.assertTrue(path_success, "Integrated test: Path planning failed") + + # Wait for first waypoint + waypoint_received = self.wait_for_waypoint(timeout=10) + self.assertTrue(waypoint_received, "Integrated test: No waypoint received") + + # Add new obstacles and trigger replan + more_obstacles_2d = [(20, 12), (20, 13), (20, 14)] + more_obstacles_3d = [(25, 15, 5), (25, 15, 6)] + more_obstacles = obstacles_2d + obstacles_3d + more_obstacles_2d + more_obstacles_3d + obstacle_grid = self.create_obstacle_grid(width=40, height=40, depth=20, obstacles=more_obstacles) + self.node.obstacle_grid_pub.publish(obstacle_grid) time.sleep(0.5) - # Trigger replan - print("Triggering replan with new goal...") replan_msg = Empty() - self.replan_pub.publish(replan_msg) + self.node.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}") + # Wait for replan success + replan_success = self.wait_for_planner_status("SUCCESS", timeout=15) + self.assertTrue(replan_success, "Integrated test: Replanning failed") - # 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") + print("Integrated operation test successful") - def test_multiple_replans(self): - """Test multiple consecutive replans""" - print("\n=== Testing Multiple Consecutive Replans ===") + def test_error_conditions(self): + """Test error conditions and edge cases""" + print("\n=== Testing Error Conditions ===") - # 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}") + self.wait_for_subscribers() + self.node.reset_tracking() + + # Test with unreachable goal (surrounded by obstacles in 3D) + obstacles = [] + # Create a 3D cage around position (9,9) at multiple heights + for i in range(8, 12): + for j in range(8, 12): + for k in range(4, 8): # Multiple height levels + if not (i == 9 and j == 9 and k == 5): # Leave center free at one level + obstacles.append((i, j, k)) - print(f"Multiple replans successful: {final_count - initial_count} new paths generated") + obstacle_grid = self.create_obstacle_grid(width=20, height=20, depth=15, obstacles=obstacles) + self.node.obstacle_grid_pub.publish(obstacle_grid) + time.sleep(0.5) - def save_path_to_file(self, filename="test_path.txt"): - """Save the current path to a file for debugging""" + # Set start outside the blocked area + start_pose = self.create_pose_stamped(5.0, 5.0, 0.0) + self.node.local_pose_pub.publish(start_pose) + time.sleep(0.5) + + # Set goal in the blocked center + goal = self.create_position_target(9.0, 9.0, 5.0) # At the one free level + self.node.setpoint_pub.publish(goal) + time.sleep(2.0) + + # Wait for status - might be FAILURE or still processing + start_time = time.time() + final_status = None + while time.time() - start_time < 10: + rclpy.spin_once(self.node, timeout_sec=0.1) + if self.node.current_planner_status in ["SUCCESS", "FAILURE", "NO_PATH"]: + final_status = self.node.current_planner_status + break + + print(f"Error condition test result: {final_status}") + # Don't assert failure here as different planners handle unreachable goals differently + + def save_debug_info(self, test_name): + """Save debug information for failed tests""" + timestamp = int(time.time()) + if self.node.received_path: + filename = f"{test_name}_path_{timestamp}.txt" with open(filename, "w") as f: - f.write(f"# Path with {len(self.node.received_path.poses)} waypoints\n") + f.write(f"# Path for test: {test_name}\n") + f.write(f"# Waypoints: {len(self.node.received_path.poses)}\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}") + print(f"Path debug info saved to {filename}") + if self.node.status_history: + filename = f"{test_name}_status_{timestamp}.txt" + with open(filename, "w") as f: + f.write(f"# Status history for test: {test_name}\n") + for status, timestamp in self.node.status_history: + f.write(f"{timestamp}: {status}\n") + print(f"Status debug info saved to {filename}") if __name__ == '__main__': - # Run tests with more verbose output + # Run tests with verbose output unittest.main(verbosity=2) \ No newline at end of file diff --git a/src/kestrel_planning/test/test_waypoints.py b/src/kestrel_planning/test/test_waypoints.py new file mode 100644 index 0000000..774c036 --- /dev/null +++ b/src/kestrel_planning/test/test_waypoints.py @@ -0,0 +1,257 @@ +import unittest +import time +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped, Point +from mavros_msgs.msg import PositionTarget +from std_msgs.msg import String, Empty, Header +from kestrel_msgs.msg import ObstacleGrid +import subprocess +import sys +import threading +from builtin_interfaces.msg import Time +import numpy as np + +from rclpy.time import Time +from rclpy.clock import Clock + +def set_header(msg, node, frame_id="map"): + msg.header.frame_id = frame_id + msg.header.stamp = node.get_clock().now().to_msg() + return msg + +class PlannerTest(Node): + def __init__(self): + super().__init__('planner_test_node') + + self.received_path = None + self.current_planner_status = None + self.status_history = [] + self.path_count = 0 + self.last_path_time = None + self.current_waypoint = None + self.waypoint_count = 0 + + self.obstacle_grid_pub = self.create_publisher(ObstacleGrid, 'perception/obstacle_grid', 10) + self.local_pose_pub = self.create_publisher(PoseStamped, 'odometry/local_pose', 10) + self.setpoint_pub = self.create_publisher(PositionTarget, 'mavros/setpoint_raw/local', 10) + self.replan_pub = self.create_publisher(Empty, 'planning/replan', 10) + self.path_sub = self.create_subscription(Path, 'planning/path', self.path_callback, 10) + self.planner_status_sub = self.create_subscription(String, 'planning/planner_status', self.planner_status_callback, 10) + self.waypoint_sub = self.create_subscription(PoseStamped, 'planning/waypoint', self.waypoint_callback, 10) + + def path_callback(self, msg): + """Callback for path messages from pathing node""" + 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 planner_status_callback(self, msg): + """Callback for planner status messages""" + self.current_planner_status = msg.data + self.status_history.append((msg.data, time.time())) + print(f"[Planner Status] {msg.data}") + + def waypoint_callback(self, msg): + """Callback for waypoint messages from waypoint manager""" + self.waypoint_count += 1 + self.current_waypoint = msg + self.get_logger().info(f"Received waypoint #{self.waypoint_count}: ({msg.pose.position.x:.2f}, {msg.pose.position.y:.2f}, {msg.pose.position.z:.2f})") + + def reset_tracking(self): + """Reset all tracking variables for new test scenarios""" + self.received_path = None + self.path_count = 0 + self.last_path_time = None + self.status_history = [] + self.current_waypoint = None + self.waypoint_count = 0 + + +class TestKestrelPlanning(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.pathing_process = subprocess.Popen( + ["ros2", "run", "kestrel_planning", "kestrel_planning"], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + bufsize=1 + ) + + cls.waypoint_process = subprocess.Popen( + ["ros2", "run", "kestrel_planning", "kestrel_waypoint"], + 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.pathing_stdout_thread = threading.Thread( + target=print_stream, args=(cls.pathing_process.stdout, "PATHING-OUT"), daemon=True + ) + cls.pathing_stderr_thread = threading.Thread( + target=print_stream, args=(cls.pathing_process.stderr, "PATHING-ERR"), daemon=True + ) + cls.waypoint_stdout_thread = threading.Thread( + target=print_stream, args=(cls.waypoint_process.stdout, "WAYPOINT-OUT"), daemon=True + ) + cls.waypoint_stderr_thread = threading.Thread( + target=print_stream, args=(cls.waypoint_process.stderr, "WAYPOINT-ERR"), daemon=True + ) + + cls.pathing_stdout_thread.start() + cls.pathing_stderr_thread.start() + cls.waypoint_stdout_thread.start() + cls.waypoint_stderr_thread.start() + + time.sleep(3) + + rclpy.init() + cls.node = PlannerTest() + + @classmethod + def tearDownClass(cls): + """Clean up processes and ROS""" + cls.pathing_process.terminate() + cls.waypoint_process.terminate() + cls.pathing_process.wait(timeout=5) + cls.waypoint_process.wait(timeout=5) + rclpy.shutdown() + + def create_obstacle_grid(self, width=100, height=100, depth=50, obstacles=None): + grid_msg = ObstacleGrid() + grid_msg.header = Header() + grid_msg = set_header(grid_msg, self.node, "map") + + grid_msg.width = width + grid_msg.height = height + grid_msg.depth = depth + grid_msg.resolution = 1.0 + + grid_msg.origin.position.x = 0.0 + grid_msg.origin.position.y = 0.0 + grid_msg.origin.position.z = 0.0 + grid_msg.origin.orientation.w = 1.0 + + total_cells = width * height * depth + grid_msg.data = [0] * total_cells + + if obstacles: + for obstacle in obstacles: + if len(obstacle) == 2: + ox, oy = obstacle + for oz in range(depth // 4, 3 * depth // 4): + if 0 <= ox < width and 0 <= oy < height and 0 <= oz < depth: + idx = ox + oy * width + oz * width * height + grid_msg.data[idx] = 100 + elif len(obstacle) == 3: + ox, oy, oz = obstacle + if 0 <= ox < width and 0 <= oy < height and 0 <= oz < depth: + idx = ox + oy * width + oz * width * height + grid_msg.data[idx] = 100 + + return grid_msg + + def create_position_target(self, x, y, z, frame_id="map"): + """Create PositionTarget message for goal setting""" + target = PositionTarget() + target.header = Header() + target = set_header(target, self.node, frame_id) + target.coordinate_frame = PositionTarget.FRAME_LOCAL_NED + target.type_mask = PositionTarget.IGNORE_VX | PositionTarget.IGNORE_VY | PositionTarget.IGNORE_VZ | \ + PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | \ + PositionTarget.IGNORE_YAW_RATE + target.position.x = x + target.position.y = y + target.position.z = z + target.yaw = 0.0 + return target + + def create_pose_stamped(self, x, y, z, frame_id="map"): + """Create PoseStamped message for current pose""" + pose = PoseStamped() + pose.header = Header() + pose = set_header(pose, self.node, frame_id) + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + pose.pose.orientation.w = 1.0 + return pose + + def wait_for_subscribers(self, timeout=100): + start_time = time.time() + while (self.node.obstacle_grid_pub.get_subscription_count() == 0 or + self.node.local_pose_pub.get_subscription_count() == 0 or + self.node.setpoint_pub.get_subscription_count() == 0) and time.time() - start_time < timeout: + print(f"waiting for subscribers {time.time() - start_time}") + rclpy.spin_once(self.node, timeout_sec=0.1) + print("topics subscribed") + + + def wait_for_planner_status(self, expected_status, timeout=10): + start_time = time.time() + while self.node.current_planner_status != expected_status and time.time() - start_time < timeout: + rclpy.spin_once(self.node, timeout_sec=0.1) + return self.node.current_planner_status == expected_status + + def wait_for_path(self, timeout=10): + 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 wait_for_waypoint(self, timeout=100): + initial_count = self.node.waypoint_count + start_time = time.time() + while self.node.waypoint_count <= initial_count and time.time() - start_time < timeout: + rclpy.spin_once(self.node, timeout_sec=0.1) + end_time = time.time() + print(f"took {end_time - start_time} connect") + return self.node.waypoint_count > initial_count + + def test_pathing_node_basic_planning(self): + print("Test #1") + + self.wait_for_subscribers() + self.node.reset_tracking() + + obstacle_grid = self.create_obstacle_grid(width=50, height=50, depth=30) + self.node.obstacle_grid_pub.publish(obstacle_grid) + print("sent obstacle grid") + time.sleep(0.5) + + current_pose = self.create_pose_stamped(0.0, 0.0, 0.0) + self.node.local_pose_pub.publish(current_pose) + print("sent current pose") + time.sleep(0.5) + + goal = self.create_position_target(25.0, 25.0, 5.0) + self.node.setpoint_pub.publish(goal) + print("sent goal") + time.sleep(1.0) + + replan_msg = Empty() + self.node.replan_pub.publish(replan_msg) + print("triggered replan") + time.sleep(0.5) + + + path_received = self.wait_for_path(timeout=500) + 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") + + print(f"Basic planning successful: {len(self.node.received_path.poses)} waypoints") + +if __name__ == '__main__': + unittest.main(verbosity=2) \ No newline at end of file From 7e7c29ae03cbd75edffecf25ab9a57a156744aaa Mon Sep 17 00:00:00 2001 From: mistry Date: Thu, 9 Oct 2025 19:28:33 -0400 Subject: [PATCH 5/6] fixed planner node logic --- .../include/dstar_planner.hpp | 4 +- src/kestrel_planning/include/pathing_node.hpp | 1 + src/kestrel_planning/include/utils.hpp | 125 ++++++++++-------- src/kestrel_planning/src/dstar_planner.cpp | 75 ++++++----- src/kestrel_planning/src/pathing_node.cpp | 53 +++++--- src/kestrel_planning/test/test_waypoints.py | 21 +-- 6 files changed, 152 insertions(+), 127 deletions(-) diff --git a/src/kestrel_planning/include/dstar_planner.hpp b/src/kestrel_planning/include/dstar_planner.hpp index 3189d2f..942679b 100644 --- a/src/kestrel_planning/include/dstar_planner.hpp +++ b/src/kestrel_planning/include/dstar_planner.hpp @@ -41,7 +41,8 @@ class DSTARLITE { void replan(float x, float y, float z); int extractPath(std::vector &waypoints); - void setOccupied(int x, int y, int z); + void setOccupiedStatus(int x, int y, int z, bool value); + void initializeCostmap(); private: float km; @@ -70,6 +71,7 @@ class DSTARLITE { void insertOpenList(std::shared_ptr node); bool isInOpenList(std::shared_ptr node); void removeFromOpenList(std::shared_ptr node); + std::shared_ptr topOpenList(); bool openListEmpty(); diff --git a/src/kestrel_planning/include/pathing_node.hpp b/src/kestrel_planning/include/pathing_node.hpp index cb2053e..c731ecd 100644 --- a/src/kestrel_planning/include/pathing_node.hpp +++ b/src/kestrel_planning/include/pathing_node.hpp @@ -44,6 +44,7 @@ class DStarNode : public rclcpp::Node { bool got_start_; bool got_goal_; bool has_valid_path_; + bool costmap_initialized_; PlanningMode planning_mode_; diff --git a/src/kestrel_planning/include/utils.hpp b/src/kestrel_planning/include/utils.hpp index c37fcbf..1c956ea 100644 --- a/src/kestrel_planning/include/utils.hpp +++ b/src/kestrel_planning/include/utils.hpp @@ -75,68 +75,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 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; - } - + 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; - +private: + vec3 point; + float g, rhs; + std::shared_ptr rhs_from; + std::pair key; + bool occupied; }; diff --git a/src/kestrel_planning/src/dstar_planner.cpp b/src/kestrel_planning/src/dstar_planner.cpp index d61d968..9d38562 100644 --- a/src/kestrel_planning/src/dstar_planner.cpp +++ b/src/kestrel_planning/src/dstar_planner.cpp @@ -1,24 +1,45 @@ - #include "dstar_planner.hpp" +void DSTARLITE::initializeCostmap() { + // Only call this ONCE at the very beginning + std::cout << "[COSTMAP] Initializing costmap structure" << std::endl; + costmap.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()); + } + } + } +} + void DSTARLITE::initialize() { std::cout << "[PATHING START] dstarlite initializing" << std::endl; - costmap.clear(); + + // Reset planner state but DON'T clear costmap (preserves obstacles) km = 0.0f; while (!open_list.empty()) { open_list.pop(); } open_set.clear(); + // Reset all cells' G, RHS values but preserve occupation state 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()); + auto cell = costmap(x, y, z); + if (cell) { + // Reset planning values but keep occupation state + cell->setG(INF_FLOAT); + cell->setRHS(INF_FLOAT); + cell->setNextStep(nullptr); + } } } } @@ -58,28 +79,19 @@ void DSTARLITE::insertOpenList(std::shared_ptr 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) { @@ -95,24 +107,17 @@ std::vector> DSTARLITE::getSuccessors(std::shared_ptr> DSTARLITE::getPredecessors(std::shared_ptr node) { std::vector> preds; - vec3 pos = node->getPoint(); for (int dx = -1; dx <= 1; ++dx) { @@ -130,18 +135,15 @@ std::vector> DSTARLITE::getPredecessors(std::shared_ptr node) { open_set.erase(node); - } int DSTARLITE::computeShortestPath() { @@ -191,9 +191,6 @@ int DSTARLITE::computeShortestPath() { return count; } - - - void DSTARLITE::updateVertex(std::shared_ptr u) { if (u->getPoint() != goal) { float min_rhs = INF_FLOAT; @@ -209,7 +206,6 @@ void DSTARLITE::updateVertex(std::shared_ptr u) { } if (isInOpenList(u)) removeFromOpenList(u); - if (!u->isConsistent()) insertOpenList(u); } @@ -236,7 +232,10 @@ 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; } + if (open_set.find(node) == open_set.end()) { + open_list.pop(); + continue; + } auto newKey = calculateKey(node); if (std::tie(oldKey.first, oldKey.second) < @@ -302,20 +301,20 @@ int DSTARLITE::extractPath(std::vector &waypoin } bool DSTARLITE::isOccupied(int x, int y, int z) { - std::shared_ptr&s = costmap(x, y, 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) { +void DSTARLITE::setOccupiedStatus(int x, int y, int z, bool value) { 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->setOccupation(value); state_occupied->setG(INF_FLOAT); state_occupied->setRHS(INF_FLOAT); state_occupied->setNextStep(nullptr); @@ -338,7 +337,7 @@ void DSTARLITE::replan(float x, float y, float z) { vec3 obstacle_pos(px, py, pz); km += heuristic(start_state, costmap(px, py, pz)); - setOccupied(px, py, pz); + setOccupiedStatus(px, py, pz, true); auto node = costmap(px, py, pz); updateVertex(node); diff --git a/src/kestrel_planning/src/pathing_node.cpp b/src/kestrel_planning/src/pathing_node.cpp index 9b3f5c4..06be1a1 100644 --- a/src/kestrel_planning/src/pathing_node.cpp +++ b/src/kestrel_planning/src/pathing_node.cpp @@ -26,7 +26,8 @@ DStarNode::DStarNode() got_start_(false), got_goal_(false), has_valid_path_(false), - planning_mode_(PlanningMode::NEW_PATH) + planning_mode_(PlanningMode::NEW_PATH), + costmap_initialized_(false) { this->declare_parameter("x_range", 100); this->declare_parameter("y_range", 100); @@ -79,11 +80,20 @@ DStarNode::~DStarNode() void DStarNode::mapCallback(const kestrel_msgs::msg::ObstacleGrid::SharedPtr msg) { std::lock_guard lk(planner_mutex_); + + // Initialize costmap structure on first map callback + if (!costmap_initialized_) { + RCLCPP_INFO(this->get_logger(), "First map received - initializing costmap structure"); + planner_->initializeCostmap(); + costmap_initialized_ = true; + } + got_costmap_ = true; uint32_t width = msg->width; uint32_t height = msg->height; uint32_t depth = msg->depth; + int n_obstacles = 0; for (uint32_t z = 0; z < depth; ++z) { for (uint32_t y = 0; y < height; ++y) { @@ -91,20 +101,24 @@ void DStarNode::mapCallback(const kestrel_msgs::msg::ObstacleGrid::SharedPtr msg size_t idx = z * (width * height) + y * width + x; if (idx < msg->data.size() && msg->data[idx] > 50) { - planner_->setOccupied(x, y, z); + planner_->setOccupiedStatus(x, y, z, true); + n_obstacles++; + } else { + planner_->setOccupiedStatus(x, y, z, false); } + } } } - RCLCPP_INFO(this->get_logger(), "3D Map received: %u x %u x %u (res: %.3f)", - width, height, depth, msg->resolution); + RCLCPP_INFO(this->get_logger(), "3D Map received: %u x %u x %u (res: %.3f) with %d obstacles", + width, height, depth, msg->resolution, n_obstacles); if (has_valid_path_) { RCLCPP_INFO(this->get_logger(), "Map changed, triggering replan"); has_valid_path_ = false; } - //triggerPlanningLocked_(); + triggerPlanningLocked_(); } void DStarNode::goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr msg) @@ -117,7 +131,7 @@ void DStarNode::goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr m RCLCPP_INFO(this->get_logger(), "Goal set: %.3f, %.3f, %.3f", msg->position.x, msg->position.y, msg->position.z); - //triggerPlanningLocked_(); + triggerPlanningLocked_(); } void DStarNode::odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) @@ -125,9 +139,7 @@ void DStarNode::odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr ms 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); - //triggerPlanningLocked_(); + triggerPlanningLocked_(); } void DStarNode::replanCallback(const std_msgs::msg::Empty::SharedPtr msg) @@ -135,6 +147,11 @@ void DStarNode::replanCallback(const std_msgs::msg::Empty::SharedPtr msg) (void)msg; std::lock_guard lk(planner_mutex_); + if (!costmap_initialized_) { + RCLCPP_WARN(this->get_logger(), "Cannot plan - costmap not initialized yet"); + return; + } + if (has_valid_path_) { RCLCPP_INFO(this->get_logger(), "Replan requested - existing path will be updated"); updatePlannerState(PlannerState::REPLANNING); @@ -143,14 +160,12 @@ void DStarNode::replanCallback(const std_msgs::msg::Empty::SharedPtr msg) } if (got_costmap_ && got_start_ && got_goal_) { - if (!has_valid_path_) { - planner_->initialize(); - } + planner_->initialize(); planning_request_ = true; planning_cv_.notify_one(); - RCLCPP_INFO(this->get_logger(), "Replan triggered successfully"); + RCLCPP_INFO(this->get_logger(), "Planning triggered successfully"); } else { - RCLCPP_WARN(this->get_logger(), "Cannot replan - missing required data (costmap=%d, start=%d, goal=%d)", + RCLCPP_WARN(this->get_logger(), "Cannot plan - missing required data (costmap=%d, start=%d, goal=%d)", got_costmap_, got_start_, got_goal_); if (!got_costmap_) updatePlannerState(PlannerState::NO_MAP); @@ -167,10 +182,16 @@ void DStarNode::triggerPlanning() void DStarNode::triggerPlanningLocked_() { - RCLCPP_INFO(this->get_logger(), "Planning check: costmap=%d start=%d goal=%d", - got_costmap_, got_start_, got_goal_); + RCLCPP_INFO(this->get_logger(), "Planning check: costmap=%d start=%d goal=%d initialized=%d", + got_costmap_, got_start_, got_goal_, costmap_initialized_); + + if (!costmap_initialized_) { + updatePlannerState(PlannerState::NO_MAP); + return; + } if (got_costmap_ && got_start_ && got_goal_) { + planner_->initialize(); planning_request_ = true; RCLCPP_INFO(this->get_logger(), "Planning triggered"); planning_cv_.notify_one(); diff --git a/src/kestrel_planning/test/test_waypoints.py b/src/kestrel_planning/test/test_waypoints.py index 774c036..57083b9 100644 --- a/src/kestrel_planning/test/test_waypoints.py +++ b/src/kestrel_planning/test/test_waypoints.py @@ -146,17 +146,9 @@ def create_obstacle_grid(self, width=100, height=100, depth=50, obstacles=None): if obstacles: for obstacle in obstacles: - if len(obstacle) == 2: - ox, oy = obstacle - for oz in range(depth // 4, 3 * depth // 4): - if 0 <= ox < width and 0 <= oy < height and 0 <= oz < depth: - idx = ox + oy * width + oz * width * height - grid_msg.data[idx] = 100 - elif len(obstacle) == 3: - ox, oy, oz = obstacle - if 0 <= ox < width and 0 <= oy < height and 0 <= oz < depth: - idx = ox + oy * width + oz * width * height - grid_msg.data[idx] = 100 + ox, oy, oz = obstacle + idx = oz * (width * height) + oy * width + ox + grid_msg.data[idx] = 100 return grid_msg @@ -224,8 +216,8 @@ def test_pathing_node_basic_planning(self): self.wait_for_subscribers() self.node.reset_tracking() - - obstacle_grid = self.create_obstacle_grid(width=50, height=50, depth=30) + obstacles = [(10,10, 0), (10,13, 5), (5,5, 0), (24,24,4), (10,10,1), (19,19,2)] + obstacle_grid = self.create_obstacle_grid(width=50, height=50, depth=30, obstacles=obstacles) self.node.obstacle_grid_pub.publish(obstacle_grid) print("sent obstacle grid") time.sleep(0.5) @@ -245,13 +237,14 @@ def test_pathing_node_basic_planning(self): print("triggered replan") time.sleep(0.5) - path_received = self.wait_for_path(timeout=500) + 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") print(f"Basic planning successful: {len(self.node.received_path.poses)} waypoints") + if __name__ == '__main__': unittest.main(verbosity=2) \ No newline at end of file From f73071c7ac207c11bc1b23f6505a975eedbccba0 Mon Sep 17 00:00:00 2001 From: mistry Date: Tue, 14 Oct 2025 22:48:29 -0400 Subject: [PATCH 6/6] replan test and optimizations --- .../include/dstar_planner.hpp | 1 + src/kestrel_planning/include/utils.hpp | 3 +- src/kestrel_planning/src/dstar_planner.cpp | 43 +++++----- src/kestrel_planning/test/test_waypoints.py | 80 ++++++++++++++++++- 4 files changed, 102 insertions(+), 25 deletions(-) diff --git a/src/kestrel_planning/include/dstar_planner.hpp b/src/kestrel_planning/include/dstar_planner.hpp index 942679b..fb7a06b 100644 --- a/src/kestrel_planning/include/dstar_planner.hpp +++ b/src/kestrel_planning/include/dstar_planner.hpp @@ -61,6 +61,7 @@ class DSTARLITE { StateComparator> open_list; std::unordered_set> open_set; + std::vector> visited; bool isOccupied(int x, int y, int z); std::vector> getPredecessors(std::shared_ptr node); diff --git a/src/kestrel_planning/include/utils.hpp b/src/kestrel_planning/include/utils.hpp index 1c956ea..acaa377 100644 --- a/src/kestrel_planning/include/utils.hpp +++ b/src/kestrel_planning/include/utils.hpp @@ -1,5 +1,6 @@ #ifndef UTILS_H #define UTILS_H +#pragma once #include #define TOL 1e6 @@ -127,7 +128,7 @@ class state { void setNextStep(std::shared_ptr u) { rhs_from = u; } - void setkey(std::pair k) { + void setKey(std::pair k) { key = k; } std::pair getKey() { diff --git a/src/kestrel_planning/src/dstar_planner.cpp b/src/kestrel_planning/src/dstar_planner.cpp index 9d38562..9737394 100644 --- a/src/kestrel_planning/src/dstar_planner.cpp +++ b/src/kestrel_planning/src/dstar_planner.cpp @@ -4,6 +4,8 @@ void DSTARLITE::initializeCostmap() { // Only call this ONCE at the very beginning std::cout << "[COSTMAP] Initializing costmap structure" << std::endl; costmap.clear(); + km = 0.0f; + visited.clear(); // Clear visited tracking for (uint32_t x = 0; x < X_RANGE; ++x) { for (uint32_t y = 0; y < Y_RANGE; ++y) { @@ -22,27 +24,20 @@ void DSTARLITE::initializeCostmap() { void DSTARLITE::initialize() { std::cout << "[PATHING START] dstarlite initializing" << std::endl; - // Reset planner state but DON'T clear costmap (preserves obstacles) km = 0.0f; while (!open_list.empty()) { open_list.pop(); } open_set.clear(); - // Reset all cells' G, RHS values but preserve occupation state - 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 = costmap(x, y, z); - if (cell) { - // Reset planning values but keep occupation state - cell->setG(INF_FLOAT); - cell->setRHS(INF_FLOAT); - cell->setNextStep(nullptr); - } - } + for (auto& node : visited) { + if (node) { + node->setG(INF_FLOAT); + node->setRHS(INF_FLOAT); + node->setNextStep(nullptr); } } + visited.clear(); goal_state = costmap(goal.x, goal.y, goal.z); start_state = costmap(start.x, start.y, start.z); @@ -55,11 +50,14 @@ void DSTARLITE::initialize() { goal_state->setG(INF_FLOAT); goal_state->setRHS(0.0f); goal_state->setNextStep(nullptr); - goal_state->setkey(calculateKey(goal_state)); + goal_state->setKey(calculateKey(goal_state)); + visited.push_back(goal_state); start_state->setG(INF_FLOAT); start_state->setRHS(INF_FLOAT); start_state->setNextStep(nullptr); + visited.push_back(start_state); + insertOpenList(goal_state); } @@ -176,9 +174,11 @@ int DSTARLITE::computeShortestPath() { if (u->getG() > u->getRHS()) { u->setG(u->getRHS()); + visited.push_back(u); for (auto s : getSuccessors(u)) updateVertex(s); } else { u->setG(INF_FLOAT); + visited.push_back(u); updateVertex(u); for (auto s : getSuccessors(u)) updateVertex(s); } @@ -203,6 +203,7 @@ void DSTARLITE::updateVertex(std::shared_ptr u) { } u->setRHS(min_rhs); u->setNextStep(best); + visited.push_back(u); } if (isInOpenList(u)) removeFromOpenList(u); @@ -232,10 +233,7 @@ 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; - } + if (open_set.find(node) == open_set.end()) { open_list.pop(); continue; } auto newKey = calculateKey(node); if (std::tie(oldKey.first, oldKey.second) < @@ -264,12 +262,12 @@ int DSTARLITE::extractPath(std::vector &waypoin } uint32_t count = 0; - std::set visited; + std::set visited_path; - while (node && visited.find(node->getPoint()) == visited.end()) { + while (node && visited_path.find(node->getPoint()) == visited_path.end()) { vec3 point = node->getPoint(); std::cout << "[DEBUG] " << point.x << " " << point.y << " " << point.z << std::endl; - visited.insert(point); + visited_path.insert(point); if (!(point == start)) { geometry_msgs::msg::PoseStamped pose; @@ -293,7 +291,7 @@ int DSTARLITE::extractPath(std::vector &waypoin node = node->nextStep(); } - if (visited.find(goal) == visited.end()) { + if (visited_path.find(goal) == visited_path.end()) { std::cerr << "Path broken before reaching goal.\n"; } @@ -318,6 +316,7 @@ void DSTARLITE::setOccupiedStatus(int x, int y, int z, bool value) { state_occupied->setG(INF_FLOAT); state_occupied->setRHS(INF_FLOAT); state_occupied->setNextStep(nullptr); + visited.push_back(state_occupied); } void DSTARLITE::replan(float x, float y, float z) { diff --git a/src/kestrel_planning/test/test_waypoints.py b/src/kestrel_planning/test/test_waypoints.py index 57083b9..0e32fc8 100644 --- a/src/kestrel_planning/test/test_waypoints.py +++ b/src/kestrel_planning/test/test_waypoints.py @@ -15,12 +15,68 @@ from rclpy.time import Time from rclpy.clock import Clock +import math + def set_header(msg, node, frame_id="map"): msg.header.frame_id = frame_id msg.header.stamp = node.get_clock().now().to_msg() return msg +def make_sphere(a, b, c, r, resolution=20): + """ + Generate (x, y, z) points on a sphere centered at (a, b, c) with radius r. + + Args: + a, b, c (float): Center of the sphere + r (float): Radius of the sphere + resolution (int): Number of steps for theta and phi (controls density) + + Returns: + list[tuple[float, float, float]]: Points on the sphere surface + """ + points = [] + for i in range(resolution + 1): + theta = math.pi * i / resolution # polar angle (0 to pi) + for j in range(resolution * 2 + 1): + phi = 2 * math.pi * j / (resolution * 2) # azimuthal angle (0 to 2pi) + x = int(a + r * math.sin(theta) * math.cos(phi)) + y = int(b + r * math.sin(theta) * math.sin(phi)) + z = int(c + r * math.cos(theta)) + if x >= 0 and y >= 0 and z >= 0: + points.append((x, y, z)) + return points + + +def make_wall_zy(x, y, z, size, resolution=20): + """ + Generate (x, y, z) points on a flat wall lying on the ZY-plane (x is constant). + + Args: + x, y, z (float): Center of the wall + size (float): Half-size (total height/width = 2*size) + resolution (int): Number of points along each axis + + Returns: + list[tuple[int, int, int]]: Points on the wall surface + """ + points = [] + for i in range(resolution + 1): + for j in range(resolution + 1): + py = int(y - size + 2 * size * (i / resolution)) # vertical (Y) + pz = int(z - size + 2 * size * (j / resolution)) # depth (Z) + px = int(x) # constant X value + if px >= 0 and py >= 0 and pz >= 0: + points.append((px, py, pz)) + return points + +def make_line(x, y, z, size): + points = [] + for i in range(size + 1): + for j in range(size + 1): + points.append((x, y+i, j)) + return points + class PlannerTest(Node): def __init__(self): super().__init__('planner_test_node') @@ -216,7 +272,8 @@ def test_pathing_node_basic_planning(self): self.wait_for_subscribers() self.node.reset_tracking() - obstacles = [(10,10, 0), (10,13, 5), (5,5, 0), (24,24,4), (10,10,1), (19,19,2)] + obstacles = make_line(0, 15, 0, 5) + #obstacles = [(10,10, 0), (10,13, 5), (5,5, 0), (24,24,4), (10,10,1), (19,19,2)] obstacle_grid = self.create_obstacle_grid(width=50, height=50, depth=30, obstacles=obstacles) self.node.obstacle_grid_pub.publish(obstacle_grid) print("sent obstacle grid") @@ -227,7 +284,7 @@ def test_pathing_node_basic_planning(self): print("sent current pose") time.sleep(0.5) - goal = self.create_position_target(25.0, 25.0, 5.0) + goal = self.create_position_target(0.0, 45.0, 0.0) self.node.setpoint_pub.publish(goal) print("sent goal") time.sleep(1.0) @@ -243,7 +300,26 @@ def test_pathing_node_basic_planning(self): self.assertIsNotNone(self.node.received_path, "Path is None") self.assertGreater(len(self.node.received_path.poses), 0, "Path is empty") + current_pose = self.create_pose_stamped(0.0, 45.0, 0.0) + self.node.local_pose_pub.publish(current_pose) + + print(f"Basic planning successful: {len(self.node.received_path.poses)} waypoints") + print("testing a replan") + goal = self.create_position_target(0.0, 0.0, 0.0) + self.node.setpoint_pub.publish(goal) + print("sent goal") + time.sleep(1.0) + + + path_received = self.wait_for_path(timeout=500) + 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") + + + + if __name__ == '__main__':