From 976abb93b7d6e42e2344aa27d5a6ec7525296392 Mon Sep 17 00:00:00 2001 From: mistry Date: Sat, 20 Sep 2025 02:40:46 -0400 Subject: [PATCH 1/5] 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 2/5] 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 3/5] 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__': From 333c59b2fea99016476ffc21f5ae0f4f539872c5 Mon Sep 17 00:00:00 2001 From: mistry Date: Thu, 25 Dec 2025 21:43:44 -0500 Subject: [PATCH 4/5] runtime logging --- src/kestrel_planning/CMakeLists.txt | 112 +++++ src/kestrel_planning/src/dstar_planner.cpp | 2 +- src/kestrel_planning/src/pathing_node.cpp | 46 +- .../src/waypoint_manager_node.cpp | 4 +- .../test/test_pathing_node.cpp | 184 +++++++ src/kestrel_planning/test/test_waypoints.cpp | 466 ++++++++++++++++++ src/kestrel_planning/test/test_waypoints.py | 99 +--- 7 files changed, 813 insertions(+), 100 deletions(-) create mode 100644 src/kestrel_planning/test/test_pathing_node.cpp create mode 100644 src/kestrel_planning/test/test_waypoints.cpp diff --git a/src/kestrel_planning/CMakeLists.txt b/src/kestrel_planning/CMakeLists.txt index 69ee468..ac5ae0b 100644 --- a/src/kestrel_planning/CMakeLists.txt +++ b/src/kestrel_planning/CMakeLists.txt @@ -67,4 +67,116 @@ install(DIRECTORY include/ DESTINATION include/ ) +# Testing +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + # Create a library from waypoint_manager_node without the main function + # We'll compile the source with a flag to exclude main + add_library(waypoint_manager_lib + src/waypoint_manager_node.cpp + ) + + target_compile_definitions(waypoint_manager_lib PRIVATE EXCLUDE_MAIN) + + target_include_directories(waypoint_manager_lib PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + + ament_target_dependencies(waypoint_manager_lib + rclcpp + nav_msgs + geometry_msgs + std_msgs + tf2 + tf2_ros + tf2_geometry_msgs + kestrel_msgs + mavros_msgs + ) + + # Create a library from pathing_node without the main function + add_library(pathing_node_lib + src/pathing_node.cpp + src/dstar_planner.cpp + ) + + target_compile_definitions(pathing_node_lib PRIVATE EXCLUDE_MAIN) + + target_include_directories(pathing_node_lib PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + + ament_target_dependencies(pathing_node_lib + rclcpp + nav_msgs + geometry_msgs + std_msgs + tf2 + tf2_ros + tf2_geometry_msgs + kestrel_msgs + mavros_msgs + ) + + # Build test executable for waypoint manager + ament_add_gtest(test_waypoint_manager_node + test/test_waypoints.cpp + ) + + target_include_directories(test_waypoint_manager_node PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + + # Link test with the library (not the source file directly) + target_link_libraries(test_waypoint_manager_node + waypoint_manager_lib + pthread + ) + + ament_target_dependencies(test_waypoint_manager_node + rclcpp + nav_msgs + geometry_msgs + std_msgs + tf2 + tf2_ros + tf2_geometry_msgs + kestrel_msgs + mavros_msgs + ) + + # Build test executable for pathing node + ament_add_gtest(test_pathing_node + test/test_pathing_node.cpp + ) + + target_include_directories(test_pathing_node PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + + target_link_libraries(test_pathing_node + pathing_node_lib + pthread + ) + + ament_target_dependencies(test_pathing_node + rclcpp + nav_msgs + geometry_msgs + std_msgs + tf2 + tf2_ros + tf2_geometry_msgs + kestrel_msgs + mavros_msgs + ) + + install(TARGETS + test_waypoint_manager_node + test_pathing_node + DESTINATION lib/${PROJECT_NAME} + ) +endif() + ament_package() \ No newline at end of file diff --git a/src/kestrel_planning/src/dstar_planner.cpp b/src/kestrel_planning/src/dstar_planner.cpp index 9737394..f80a9ef 100644 --- a/src/kestrel_planning/src/dstar_planner.cpp +++ b/src/kestrel_planning/src/dstar_planner.cpp @@ -267,7 +267,7 @@ int DSTARLITE::extractPath(std::vector &waypoin 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_path.insert(point); + visited.insert(point); if (!(point == start)) { geometry_msgs::msg::PoseStamped pose; diff --git a/src/kestrel_planning/src/pathing_node.cpp b/src/kestrel_planning/src/pathing_node.cpp index 06be1a1..7659d40 100644 --- a/src/kestrel_planning/src/pathing_node.cpp +++ b/src/kestrel_planning/src/pathing_node.cpp @@ -4,6 +4,10 @@ #include #include +#include "kestrel_msgs/msg/obstacle_grid.hpp" +#include "mavros_msgs/msg/position_target.hpp" + + #include "kestrel_msgs/msg/obstacle_grid.hpp" #include "mavros_msgs/msg/position_target.hpp" @@ -26,8 +30,7 @@ DStarNode::DStarNode() got_start_(false), got_goal_(false), has_valid_path_(false), - planning_mode_(PlanningMode::NEW_PATH), - costmap_initialized_(false) + planning_mode_(PlanningMode::NEW_PATH) { this->declare_parameter("x_range", 100); this->declare_parameter("y_range", 100); @@ -43,6 +46,11 @@ DStarNode::DStarNode() RCLCPP_INFO(this->get_logger(), "Successfully made node!"); + costmap_sub_ = this->create_subscription( + "perception/obstacle_grid", 10, std::bind(&DStarNode::mapCallback, this, _1)); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); costmap_sub_ = this->create_subscription( "perception/obstacle_grid", 10, std::bind(&DStarNode::mapCallback, this, _1)); @@ -51,15 +59,21 @@ DStarNode::DStarNode() start_sub_ = this->create_subscription( "odometry/local_pose", qos, std::bind(&DStarNode::odomCallback, this, _1)); + "odometry/local_pose", qos, std::bind(&DStarNode::odomCallback, this, _1)); + goal_sub_ = this->create_subscription( + "mavros/setpoint_raw/local", 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( "planning/replan", 10, std::bind(&DStarNode::replanCallback, this, _1)); + "planning/replan", 10, std::bind(&DStarNode::replanCallback, this, _1)); path_pub_ = this->create_publisher("planning/path", 10); status_pub_ = this->create_publisher("planning/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); } @@ -77,6 +91,7 @@ DStarNode::~DStarNode() } } +void DStarNode::mapCallback(const kestrel_msgs::msg::ObstacleGrid::SharedPtr msg) void DStarNode::mapCallback(const kestrel_msgs::msg::ObstacleGrid::SharedPtr msg) { std::lock_guard lk(planner_mutex_); @@ -93,7 +108,6 @@ void DStarNode::mapCallback(const kestrel_msgs::msg::ObstacleGrid::SharedPtr msg 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) { @@ -101,37 +115,37 @@ 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_->setOccupiedStatus(x, y, z, true); - n_obstacles++; - } else { - planner_->setOccupiedStatus(x, y, z, false); + planner_->setOccupied(x, y, z); } - } } } - 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); + 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_(); + //triggerPlanningLocked_(); } +void DStarNode::goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr msg) void DStarNode::goalCallback(const mavros_msgs::msg::PositionTarget::SharedPtr msg) { std::lock_guard lk(planner_mutex_); + planner_->setGoal(msg->position.x, msg->position.y, msg->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->position.x, msg->position.y, msg->position.z); - triggerPlanningLocked_(); + //triggerPlanningLocked_(); } void DStarNode::odomCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) @@ -139,7 +153,9 @@ 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; - triggerPlanningLocked_(); + 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) @@ -291,6 +307,7 @@ void DStarNode::publishStatus() status_pub_->publish(msg); } +#ifndef EXCLUDE_MAIN int main(int argc, char * argv[]) { rclcpp::init(argc, argv); @@ -298,4 +315,5 @@ int main(int argc, char * argv[]) rclcpp::spin(node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/src/kestrel_planning/src/waypoint_manager_node.cpp b/src/kestrel_planning/src/waypoint_manager_node.cpp index 6b42fca..f0e95d7 100644 --- a/src/kestrel_planning/src/waypoint_manager_node.cpp +++ b/src/kestrel_planning/src/waypoint_manager_node.cpp @@ -135,6 +135,7 @@ bool WayPointManagerNode::hasReachedGoal() const } +#ifndef EXCLUDE_MAIN int main(int argc, char * argv[]) { rclcpp::init(argc, argv); @@ -142,4 +143,5 @@ int main(int argc, char * argv[]) rclcpp::spin(node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/src/kestrel_planning/test/test_pathing_node.cpp b/src/kestrel_planning/test/test_pathing_node.cpp new file mode 100644 index 0000000..e691692 --- /dev/null +++ b/src/kestrel_planning/test/test_pathing_node.cpp @@ -0,0 +1,184 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#define private public +#include "pathing_node.hpp" +#undef private + +#include "kestrel_msgs/msg/obstacle_grid.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "mavros_msgs/msg/position_target.hpp" +#include "std_msgs/msg/empty.hpp" + + +#if defined(__linux__) +#include +#endif + +static inline double cpu_seconds_best_effort() { +#if defined(__linux__) + struct timespec ts; + if (clock_gettime(CLOCK_THREAD_CPUTIME_ID, &ts) == 0) { + return double(ts.tv_sec) + 1e-9 * double(ts.tv_nsec); + } +#endif + return double(std::clock()) / double(CLOCKS_PER_SEC); +} + +static inline double wall_seconds() { + using clock = std::chrono::steady_clock; + static const auto t0 = clock::now(); + return std::chrono::duration(clock::now() - t0).count(); +} + +static int getenv_int(const char* n, int d) { + if (auto* s = std::getenv(n)) return std::atoi(s); + return d; +} + +template +static void bench(const char* label, int warmup, int iters, Fn&& fn) { + for (int i = 0; i < warmup; ++i) fn(); + + const double w0 = wall_seconds(); + const double c0 = cpu_seconds_best_effort(); + + for (int i = 0; i < iters; ++i) fn(); + + const double w1 = wall_seconds(); + const double c1 = cpu_seconds_best_effort(); + + const double wall_total = (w1 - w0); + const double cpu_total = (c1 - c0); + + std::cout << "[PERF] " << label + << " iters=" << iters + << " wall_total_s=" << wall_total + << " cpu_total_s=" << cpu_total + << " wall_us/iter=" << (wall_total / double(iters)) * 1e6 + << " cpu_us/iter=" << (cpu_total / double(iters)) * 1e6 + << std::endl; +} + +class DStarPerfFixture : public ::testing::Test { +protected: + static void SetUpTestSuite() { + int argc = 0; + char** argv = nullptr; + rclcpp::init(argc, argv); + + // Silence logs to avoid dominating perf results. + rcutils_logging_set_logger_level("kestrel_planning", RCUTILS_LOG_SEVERITY_FATAL); + rcutils_logging_set_logger_level("rclcpp", RCUTILS_LOG_SEVERITY_FATAL); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void spinFor(std::chrono::milliseconds duration) + { + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < duration) { + exec_->spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + } + + void SetUp() override { + node_ = std::make_shared(); + + exec_ = std::make_shared(); + exec_->add_node(node_); + pub_map_ = node_->create_publisher("perception/obstacle_grid", 10); + pub_odom_ = node_->create_publisher("odometry/local_pose", 10); + pub_goal_ = node_->create_publisher("mavros/setpoint_raw/local", 10); + pub_replan_ = node_->create_publisher("planning/replan", 10); + + spinFor(std::chrono::milliseconds(300)); + } + + void TearDown() override { + if (exec_) exec_->remove_node(node_); + exec_.reset(); + node_.reset(); // triggers DStarNode dtor (should stop/join its planning thread) + } + + // Helper: publish then drain callbacks for a short bounded time + template + void publish_and_drain(rclcpp::Publisher& pub, const MsgT& msg) { + pub.publish(msg); + + spinFor(std::chrono::milliseconds(300)); + } + + std::shared_ptr node_; + std::shared_ptr exec_; + + rclcpp::Publisher::SharedPtr pub_map_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_goal_; + rclcpp::Publisher::SharedPtr pub_replan_; +}; + +TEST_F(DStarPerfFixture, Perf_MapCallback_Throughput) { + const int warmup = getenv_int("DSTAR_PERF_WARMUP", 200); + const int iters = getenv_int("DSTAR_PERF_ITERS", 2000); + + kestrel_msgs::msg::ObstacleGrid msg; + // If your callback loops over grid data, set realistic sizes: + // msg.dim_x = 200; msg.dim_y = 200; msg.dim_z = 10; msg.resolution = 0.05; + // msg.data.resize(msg.dim_x * msg.dim_y * msg.dim_z); + + bench("mapCallback(pub+spin_some)", warmup, iters, [&] { + publish_and_drain(*pub_map_, msg); + }); +} + +TEST_F(DStarPerfFixture, Perf_OdomCallback_Throughput) { + const int warmup = getenv_int("DSTAR_PERF_WARMUP", 200); + const int iters = getenv_int("DSTAR_PERF_ITERS", 5000); + + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "map"; + msg.pose.position.x = 1.0; + msg.pose.position.y = 2.0; + msg.pose.orientation.w = 1.0; + + bench("odomCallback(pub+spin_some)", warmup, iters, [&] { + publish_and_drain(*pub_odom_, msg); + }); +} + +TEST_F(DStarPerfFixture, Perf_GoalCallback_Throughput) { + const int warmup = getenv_int("DSTAR_PERF_WARMUP", 200); + const int iters = getenv_int("DSTAR_PERF_ITERS", 3000); + + mavros_msgs::msg::PositionTarget msg; + msg.position.x = 5.0; + msg.position.y = 6.0; + msg.position.z = 0.0; + + bench("goalCallback(pub+spin_some)", warmup, iters, [&] { + publish_and_drain(*pub_goal_, msg); + }); +} + +TEST_F(DStarPerfFixture, Perf_ReplanCallback_Throughput) { + const int warmup = getenv_int("DSTAR_PERF_WARMUP", 200); + const int iters = getenv_int("DSTAR_PERF_ITERS", 10000); + + std_msgs::msg::Empty msg; + + bench("replanCallback(pub+spin_some)", warmup, iters, [&] { + publish_and_drain(*pub_replan_, msg); + }); +} \ No newline at end of file diff --git a/src/kestrel_planning/test/test_waypoints.cpp b/src/kestrel_planning/test/test_waypoints.cpp new file mode 100644 index 0000000..f7a7c5f --- /dev/null +++ b/src/kestrel_planning/test/test_waypoints.cpp @@ -0,0 +1,466 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "waypoint_manager_node.hpp" + +class WayPointManagerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = std::make_shared(); + + // Create test publisher nodes + test_pub_node_ = std::make_shared("test_publishers"); + test_sub_node_ = std::make_shared("test_subscribers"); + + // Create test publishers + pose_pub_ = test_pub_node_->create_publisher( + "odometry/local_pose", 10); + path_pub_ = test_pub_node_->create_publisher( + "planning/path", 10); + goal_pub_ = test_pub_node_->create_publisher( + "mavros/setpoint_raw/local", 10); + + // Create test subscriber + waypoint_sub_ = test_sub_node_->create_subscription( + "planning/waypoint", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + received_waypoints_.push_back(*msg); + }); + + // Allow time for publishers/subscribers to connect + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + void TearDown() override + { + node_.reset(); + test_pub_node_.reset(); + test_sub_node_.reset(); + pose_pub_.reset(); + path_pub_.reset(); + goal_pub_.reset(); + waypoint_sub_.reset(); + rclcpp::shutdown(); + } + + // Helper function to create a test path + nav_msgs::msg::Path createTestPath(size_t num_waypoints, double spacing = 1.0) + { + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.header.stamp = rclcpp::Clock().now(); + + for (size_t i = 0; i < num_waypoints; ++i) { + geometry_msgs::msg::PoseStamped pose; + pose.header = path.header; + pose.pose.position.x = i * spacing; + pose.pose.position.y = i * spacing; + pose.pose.position.z = 1.0; + pose.pose.orientation.w = 1.0; + path.poses.push_back(pose); + } + + return path; + } + + // Helper function to publish current pose + void publishPose(double x, double y, double z) + { + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = rclcpp::Clock().now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = z; + pose.pose.orientation.w = 1.0; + pose_pub_->publish(pose); + } + + // Helper to spin node for a duration + void spinFor(std::chrono::milliseconds duration) + { + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < duration) { + rclcpp::spin_some(node_); + rclcpp::spin_some(test_pub_node_); + rclcpp::spin_some(test_sub_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + // Get current memory usage in KB + long getCurrentMemoryUsage() + { + std::ifstream status_file("/proc/self/status"); + std::string line; + while (std::getline(status_file, line)) { + if (line.substr(0, 6) == "VmRSS:") { + std::istringstream iss(line.substr(6)); + long memory_kb; + iss >> memory_kb; + return memory_kb; + } + } + return -1; + } + + // Get CPU usage + double getCPUUsage() + { + struct rusage usage; + getrusage(RUSAGE_SELF, &usage); + double user_time = usage.ru_utime.tv_sec + usage.ru_utime.tv_usec / 1e6; + double sys_time = usage.ru_stime.tv_sec + usage.ru_stime.tv_usec / 1e6; + return user_time + sys_time; + } + + std::shared_ptr node_; + std::shared_ptr test_pub_node_; + std::shared_ptr test_sub_node_; + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr goal_pub_; + rclcpp::Subscription::SharedPtr waypoint_sub_; + std::vector received_waypoints_; +}; + +// ==================== PATH FUNCTIONALITY TESTS ==================== + +TEST_F(WayPointManagerTest, EmptyPathHandling) +{ + nav_msgs::msg::Path empty_path; + empty_path.header.frame_id = "map"; + empty_path.header.stamp = rclcpp::Clock().now(); + + path_pub_->publish(empty_path); + spinFor(std::chrono::milliseconds(200)); + + EXPECT_EQ(received_waypoints_.size(), 0); +} + +TEST_F(WayPointManagerTest, MultipleWaypointPath) +{ + const size_t num_waypoints = 5; + auto path = createTestPath(num_waypoints); + path_pub_->publish(path); + spinFor(std::chrono::milliseconds(100)); + + publishPose(0.0, 0.0, 1.0); + spinFor(std::chrono::milliseconds(300)); + + EXPECT_GT(received_waypoints_.size(), 0); + EXPECT_FALSE(node_->hasReachedGoal()); +} + +TEST_F(WayPointManagerTest, WaypointProgression) +{ + auto path = createTestPath(3, 1.0); + path_pub_->publish(path); + spinFor(std::chrono::milliseconds(100)); + + // Start at first waypoint + publishPose(0.0, 0.0, 1.0); + spinFor(std::chrono::milliseconds(200)); + size_t initial_index = node_->getCurrentWaypointIndex(); + + // Move to within tolerance of first waypoint + publishPose(0.1, 0.1, 1.0); + spinFor(std::chrono::milliseconds(200)); + + // Should progress to next waypoint + EXPECT_GE(node_->getCurrentWaypointIndex(), initial_index); +} + +TEST_F(WayPointManagerTest, GoalReached) +{ + auto path = createTestPath(2, 0.5); + path_pub_->publish(path); + spinFor(std::chrono::milliseconds(100)); + + // Move through all waypoints + publishPose(0.0, 0.0, 1.0); + spinFor(std::chrono::milliseconds(200)); + + publishPose(0.5, 0.5, 1.0); + spinFor(std::chrono::milliseconds(200)); + + publishPose(0.5, 0.5, 1.0); + spinFor(std::chrono::milliseconds(200)); + + EXPECT_TRUE(node_->hasReachedGoal()); +} + +TEST_F(WayPointManagerTest, PathUpdate) +{ + auto path1 = createTestPath(3); + path_pub_->publish(path1); + spinFor(std::chrono::milliseconds(100)); + + publishPose(0.5, 0.5, 1.0); + spinFor(std::chrono::milliseconds(200)); + + // Publish new path - should reset + auto path2 = createTestPath(5); + path_pub_->publish(path2); + spinFor(std::chrono::milliseconds(100)); + + EXPECT_EQ(node_->getCurrentWaypointIndex(), 0); +} + +// ==================== RUNTIME PERFORMANCE TESTS ==================== + +TEST_F(WayPointManagerTest, SmallPathRuntime) +{ + const size_t num_waypoints = 10; + auto path = createTestPath(num_waypoints); + + auto start = std::chrono::high_resolution_clock::now(); + + path_pub_->publish(path); + publishPose(0.0, 0.0, 1.0); + spinFor(std::chrono::milliseconds(500)); + + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + + EXPECT_LT(duration.count(), 1000) << "Processing 10 waypoints took too long"; + std::cout << "Small path (10 waypoints) runtime: " << duration.count() << " ms" << std::endl; +} + +TEST_F(WayPointManagerTest, MediumPathRuntime) +{ + const size_t num_waypoints = 100; + auto path = createTestPath(num_waypoints); + + auto start = std::chrono::high_resolution_clock::now(); + + path_pub_->publish(path); + publishPose(0.0, 0.0, 1.0); + spinFor(std::chrono::milliseconds(1000)); + + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + + EXPECT_LT(duration.count(), 2000) << "Processing 100 waypoints took too long"; + std::cout << "Medium path (100 waypoints) runtime: " << duration.count() << " ms" << std::endl; +} + +TEST_F(WayPointManagerTest, LargePathRuntime) +{ + const size_t num_waypoints = 1000; + auto path = createTestPath(num_waypoints); + + auto start = std::chrono::high_resolution_clock::now(); + + path_pub_->publish(path); + publishPose(0.0, 0.0, 1.0); + spinFor(std::chrono::milliseconds(2000)); + + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + + EXPECT_LT(duration.count(), 5000) << "Processing 1000 waypoints took too long"; + std::cout << "Large path (1000 waypoints) runtime: " << duration.count() << " ms" << std::endl; +} + +// ==================== CPU UTILIZATION TESTS ==================== + +TEST_F(WayPointManagerTest, CPUUsageIdle) +{ + double cpu_before = getCPUUsage(); + + // Let node idle + spinFor(std::chrono::seconds(1)); + + double cpu_after = getCPUUsage(); + double cpu_used = cpu_after - cpu_before; + + EXPECT_LT(cpu_used, 0.5) << "CPU usage too high during idle"; + std::cout << "CPU usage (idle): " << cpu_used << " seconds" << std::endl; +} + +TEST_F(WayPointManagerTest, CPUUsageWithPath) +{ + auto path = createTestPath(100); + + double cpu_before = getCPUUsage(); + + path_pub_->publish(path); + publishPose(0.0, 0.0, 1.0); + + // Run for 1 second with active processing + for (int i = 0; i < 100; ++i) { + publishPose(i * 0.01, i * 0.01, 1.0); + spinFor(std::chrono::milliseconds(10)); + } + + double cpu_after = getCPUUsage(); + double cpu_used = cpu_after - cpu_before; + + EXPECT_LT(cpu_used, 1.0) << "CPU usage too high with active path"; + std::cout << "CPU usage (with path): " << cpu_used << " seconds" << std::endl; +} + +TEST_F(WayPointManagerTest, CPUUsageStressTest) +{ + auto path = createTestPath(1000); + + double cpu_before = getCPUUsage(); + + path_pub_->publish(path); + + // Rapid pose updates + for (int i = 0; i < 500; ++i) { + publishPose(i * 0.002, i * 0.002, 1.0); + rclcpp::spin_some(node_); + rclcpp::spin_some(test_pub_node_); + rclcpp::spin_some(test_sub_node_); + } + + double cpu_after = getCPUUsage(); + double cpu_used = cpu_after - cpu_before; + + EXPECT_LT(cpu_used, 2.0) << "CPU usage too high under stress"; + std::cout << "CPU usage (stress test): " << cpu_used << " seconds" << std::endl; +} + +// ==================== MEMORY UTILIZATION TESTS ==================== + +TEST_F(WayPointManagerTest, BaselineMemoryUsage) +{ + long memory = getCurrentMemoryUsage(); + + EXPECT_GT(memory, 0) << "Failed to read memory usage"; + EXPECT_LT(memory, 100000) << "Baseline memory usage too high (>100MB)"; + std::cout << "Baseline memory usage: " << memory << " KB" << std::endl; +} + +TEST_F(WayPointManagerTest, MemoryUsageWithSmallPath) +{ + long memory_before = getCurrentMemoryUsage(); + + auto path = createTestPath(10); + path_pub_->publish(path); + spinFor(std::chrono::milliseconds(500)); + + long memory_after = getCurrentMemoryUsage(); + long memory_increase = memory_after - memory_before; + + EXPECT_LT(memory_increase, 1000) << "Memory increase too large for small path"; + std::cout << "Memory increase (10 waypoints): " << memory_increase << " KB" << std::endl; +} + +TEST_F(WayPointManagerTest, MemoryUsageWithLargePath) +{ + long memory_before = getCurrentMemoryUsage(); + + auto path = createTestPath(1000); + path_pub_->publish(path); + spinFor(std::chrono::milliseconds(500)); + + long memory_after = getCurrentMemoryUsage(); + long memory_increase = memory_after - memory_before; + + EXPECT_LT(memory_increase, 10000) << "Memory increase too large for large path (>10MB)"; + std::cout << "Memory increase (1000 waypoints): " << memory_increase << " KB" << std::endl; +} + +TEST_F(WayPointManagerTest, MemoryLeakTest) +{ + long initial_memory = getCurrentMemoryUsage(); + + // Create and process multiple paths + for (int i = 0; i < 10; ++i) { + auto path = createTestPath(100); + path_pub_->publish(path); + + for (int j = 0; j < 10; ++j) { + publishPose(j * 0.1, j * 0.1, 1.0); + spinFor(std::chrono::milliseconds(10)); + } + } + + // Force garbage collection + spinFor(std::chrono::milliseconds(500)); + + long final_memory = getCurrentMemoryUsage(); + long memory_diff = final_memory - initial_memory; + + EXPECT_LT(memory_diff, 5000) << "Potential memory leak detected (>5MB growth)"; + std::cout << "Memory difference after 10 iterations: " << memory_diff << " KB" << std::endl; +} + +TEST_F(WayPointManagerTest, MemoryUsageUnderLoad) +{ + long memory_before = getCurrentMemoryUsage(); + + // Process many paths in succession + for (int i = 0; i < 100; ++i) { + auto path = createTestPath(50); + path_pub_->publish(path); + publishPose(i * 0.05, i * 0.05, 1.0); + rclcpp::spin_some(node_); + rclcpp::spin_some(test_pub_node_); + rclcpp::spin_some(test_sub_node_); + } + + long memory_after = getCurrentMemoryUsage(); + long memory_increase = memory_after - memory_before; + + EXPECT_LT(memory_increase, 20000) << "Memory usage too high under load (>20MB)"; + std::cout << "Memory increase (100 path updates): " << memory_increase << " KB" << std::endl; +} + +// ==================== COMBINED PERFORMANCE TEST ==================== + +TEST_F(WayPointManagerTest, OverallPerformanceProfile) +{ + std::cout << "\n=== Overall Performance Profile ===" << std::endl; + + long memory_start = getCurrentMemoryUsage(); + double cpu_start = getCPUUsage(); + auto time_start = std::chrono::high_resolution_clock::now(); + + // Simulate realistic usage + auto path = createTestPath(200); + path_pub_->publish(path); + + for (int i = 0; i < 200; ++i) { + publishPose(i * 0.01, i * 0.01, 1.0); + spinFor(std::chrono::milliseconds(50)); + } + + auto time_end = std::chrono::high_resolution_clock::now(); + double cpu_end = getCPUUsage(); + long memory_end = getCurrentMemoryUsage(); + + auto duration = std::chrono::duration_cast(time_end - time_start); + double cpu_used = cpu_end - cpu_start; + long memory_used = memory_end - memory_start; + + std::cout << "Total runtime: " << duration.count() << " ms" << std::endl; + std::cout << "CPU time used: " << cpu_used << " seconds" << std::endl; + std::cout << "Memory used: " << memory_used << " KB" << std::endl; + std::cout << "Waypoints processed: " << node_->getCurrentWaypointIndex() + 1 << std::endl; + + EXPECT_LT(duration.count(), 15000) << "Overall runtime too high"; + EXPECT_LT(cpu_used, 3.0) << "Overall CPU usage too high"; + EXPECT_LT(memory_used, 15000) << "Overall memory usage too high"; +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/src/kestrel_planning/test/test_waypoints.py b/src/kestrel_planning/test/test_waypoints.py index 0e32fc8..774c036 100644 --- a/src/kestrel_planning/test/test_waypoints.py +++ b/src/kestrel_planning/test/test_waypoints.py @@ -15,68 +15,12 @@ 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') @@ -202,9 +146,17 @@ def create_obstacle_grid(self, width=100, height=100, depth=50, obstacles=None): if obstacles: for obstacle in obstacles: - ox, oy, oz = obstacle - idx = oz * (width * height) + oy * width + ox - grid_msg.data[idx] = 100 + 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 @@ -272,9 +224,8 @@ def test_pathing_node_basic_planning(self): self.wait_for_subscribers() self.node.reset_tracking() - 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) + + 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) @@ -284,7 +235,7 @@ def test_pathing_node_basic_planning(self): print("sent current pose") time.sleep(0.5) - goal = self.create_position_target(0.0, 45.0, 0.0) + goal = self.create_position_target(25.0, 25.0, 5.0) self.node.setpoint_pub.publish(goal) print("sent goal") time.sleep(1.0) @@ -294,33 +245,13 @@ 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") - - 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") - - - - + 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 818f1db33ce8ff3658f4c9c743779f549e0070d1 Mon Sep 17 00:00:00 2001 From: mistry Date: Thu, 25 Dec 2025 21:47:11 -0500 Subject: [PATCH 5/5] package --- src/kestrel_planning/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/kestrel_planning/package.xml b/src/kestrel_planning/package.xml index d2ced16..16d6e54 100644 --- a/src/kestrel_planning/package.xml +++ b/src/kestrel_planning/package.xml @@ -21,6 +21,8 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest + ament_cmake