diff --git a/src/robot/control/CMakeLists.txt b/src/robot/control/CMakeLists.txt index 01191099..84486d2b 100644 --- a/src/robot/control/CMakeLists.txt +++ b/src/robot/control/CMakeLists.txt @@ -13,6 +13,8 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -31,10 +33,13 @@ ament_target_dependencies(control_lib rclcpp ) +ament_target_dependencies(control_lib rclcpp nav_msgs geometry_msgs) + # Create ROS2 node executable from source files add_executable(control_node src/control_node.cpp) # Link to the previously built library to access Transformer classes and methods target_link_libraries(control_node control_lib) +ament_target_dependencies(control_node rclcpp nav_msgs geometry_msgs) # Copy executable to installation location install(TARGETS diff --git a/src/robot/control/include/control_node.hpp b/src/robot/control/include/control_node.hpp index d47f95b5..aad5b095 100644 --- a/src/robot/control/include/control_node.hpp +++ b/src/robot/control/include/control_node.hpp @@ -1,6 +1,51 @@ #ifndef CONTROL_NODE_HPP_ #define CONTROL_NODE_HPP_ +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include +#include + +class PurePursuitController : public rclcpp::Node { +public: + PurePursuitController(); + +private: + void pathCallback(const nav_msgs::msg::Path::SharedPtr msg); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void controlLoop(); + + std::optional findLookaheadPoint(); + geometry_msgs::msg::Twist computeVelocity(const geometry_msgs::msg::PoseStamped &target); + double computeDistance(const geometry_msgs::msg::Point &a, const geometry_msgs::msg::Point &b); + double extractYaw(const geometry_msgs::msg::Quaternion &quat); + + // ROS interfaces + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::TimerBase::SharedPtr control_timer_; + + // Data + nav_msgs::msg::Path::SharedPtr current_path_; + nav_msgs::msg::Odometry::SharedPtr robot_odom_; + std::mutex mutex_; + + // Parameters + double lookahead_distance_ = 1.0; // meters + double goal_tolerance_ = 0.1; // meters + double linear_speed_ = 0.5; // m/s + double max_angular_speed_ = 1.5; // rad/s + double control_rate_hz_ = 10.0; // Hz +}; + +#endif +#ifndef CONTROL_NODE_HPP_ +#define CONTROL_NODE_HPP_ + #include "rclcpp/rclcpp.hpp" #include "control_core.hpp" diff --git a/src/robot/control/package.xml b/src/robot/control/package.xml index b9b7cd6c..bcf067be 100644 --- a/src/robot/control/package.xml +++ b/src/robot/control/package.xml @@ -10,6 +10,8 @@ ament_cmake rclcpp + nav_msgs + geometry_msgs ament_lint_auto ament_lint_common diff --git a/src/robot/control/src/control_node.cpp b/src/robot/control/src/control_node.cpp index c99e2004..6a7e6fc3 100644 --- a/src/robot/control/src/control_node.cpp +++ b/src/robot/control/src/control_node.cpp @@ -1,11 +1,138 @@ #include "control_node.hpp" +#include -ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger())) {} - -int main(int argc, char ** argv) +PurePursuitController::PurePursuitController() + : Node("pure_pursuit_controller") { + path_sub_ = this->create_subscription( + "/path", 10, std::bind(&PurePursuitController::pathCallback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "/odom/filtered", 10, std::bind(&PurePursuitController::odomCallback, this, std::placeholders::_1)); + + cmd_vel_pub_ = this->create_publisher("/cmd_vel", 10); + + auto period = std::chrono::duration(1.0 / control_rate_hz_); + control_timer_ = this->create_wall_timer(period, std::bind(&PurePursuitController::controlLoop, this)); + + RCLCPP_INFO(this->get_logger(), "PurePursuitController started (lookahead=%.2f m)", lookahead_distance_); +} + +void PurePursuitController::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) { + std::lock_guard lock(mutex_); + current_path_ = msg; +} + +void PurePursuitController::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + robot_odom_ = msg; +} + +void PurePursuitController::controlLoop() { + std::lock_guard lock(mutex_); + if (!current_path_ || !robot_odom_) return; + + // Check if the path has poses + if (current_path_->poses.empty()) return; + + // If final goal is within tolerance, stop + const auto &goal_pose = current_path_->poses.back().pose.position; + double dxg = goal_pose.x - robot_odom_->pose.pose.position.x; + double dyg = goal_pose.y - robot_odom_->pose.pose.position.y; + double goal_dist = std::sqrt(dxg*dxg + dyg*dyg); + if (goal_dist < goal_tolerance_) { + geometry_msgs::msg::Twist stop_msg; + cmd_vel_pub_->publish(stop_msg); + return; + } + + auto lookahead = findLookaheadPoint(); + if (!lookahead) return; + + auto cmd = computeVelocity(*lookahead); + cmd_vel_pub_->publish(cmd); +} + +std::optional PurePursuitController::findLookaheadPoint() { + if (!current_path_ || current_path_->poses.empty() || !robot_odom_) return std::nullopt; + + double rx = robot_odom_->pose.pose.position.x; + double ry = robot_odom_->pose.pose.position.y; + double yaw = extractYaw(robot_odom_->pose.pose.orientation); + + // Search along path for first point at least lookahead_distance away + for (const auto &pose_stamped : current_path_->poses) { + double px = pose_stamped.pose.position.x; + double py = pose_stamped.pose.position.y; + double dx = px - rx; + double dy = py - ry; + double dist = std::sqrt(dx*dx + dy*dy); + if (dist >= lookahead_distance_) { + return pose_stamped; + } + } + + // If none found, return the last pose as the lookahead (close to goal) + return current_path_->poses.back(); +} + +geometry_msgs::msg::Twist PurePursuitController::computeVelocity(const geometry_msgs::msg::PoseStamped &target) { + geometry_msgs::msg::Twist cmd; + if (!robot_odom_) return cmd; + + double rx = robot_odom_->pose.pose.position.x; + double ry = robot_odom_->pose.pose.position.y; + double yaw = extractYaw(robot_odom_->pose.pose.orientation); + + double tx = target.pose.position.x; + double ty = target.pose.position.y; + + // Transform target to robot frame (world -> robot) + double dx = tx - rx; + double dy = ty - ry; + double x_r = std::cos(yaw) * dx + std::sin(yaw) * dy; + double y_r = -std::sin(yaw) * dx + std::cos(yaw) * dy; + + // If lookahead point is behind the robot, rotate in place toward it + if (x_r < 0.01) { + cmd.linear.x = 0.0; + double angle_to_target = std::atan2(y_r, x_r); + cmd.angular.z = std::clamp(angle_to_target * 1.0, -max_angular_speed_, max_angular_speed_); + return cmd; + } + + // Pure pursuit curvature + double L = lookahead_distance_; + double curvature = 2.0 * y_r / (L * L); + double angular = linear_speed_ * curvature; + angular = std::clamp(angular, -max_angular_speed_, max_angular_speed_); + + cmd.linear.x = linear_speed_; + cmd.angular.z = angular; + return cmd; +} + +double PurePursuitController::computeDistance(const geometry_msgs::msg::Point &a, const geometry_msgs::msg::Point &b) { + double dx = a.x - b.x; + double dy = a.y - b.y; + return std::sqrt(dx*dx + dy*dy); +} + +double PurePursuitController::extractYaw(const geometry_msgs::msg::Quaternion &quat) { + // yaw (z) from quaternion + double x = quat.x; + double y = quat.y; + double z = quat.z; + double w = quat.w; + double siny_cosp = 2.0 * (w * z + x * y); + double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); + return std::atan2(siny_cosp, cosy_cosp); +} + +int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } + diff --git a/src/robot/costmap/CMakeLists.txt b/src/robot/costmap/CMakeLists.txt index 130ebb88..8a98d0a6 100644 --- a/src/robot/costmap/CMakeLists.txt +++ b/src/robot/costmap/CMakeLists.txt @@ -1,19 +1,22 @@ cmake_minimum_required(VERSION 3.10) project(costmap) - + # Set compiler to use C++ 17 standard if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() - + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() - + # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package - +find_package(std_msgs REQUIRED) # YOU ARE ONLY ADDING THIS TO THE FILE +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + # Compiles source files into a library # A library is not executed, instead other executables can link # against it to access defined methods and classes. @@ -25,21 +28,27 @@ add_library(costmap_lib target_include_directories(costmap_lib PUBLIC include) # Add ROS2 dependencies required by package -ament_target_dependencies(costmap_lib rclcpp) - +ament_target_dependencies(costmap_lib + rclcpp + std_msgs # YOU ARE ONLY ADDING THIS TO THE FILE + sensor_msgs + nav_msgs +) + # Create ROS2 node executable from source files add_executable(costmap_node src/costmap_node.cpp) # Link to the previously built library to access costmap classes and methods target_link_libraries(costmap_node costmap_lib) - +ament_target_dependencies(costmap_node rclcpp sensor_msgs nav_msgs) + # Copy executable to installation location install(TARGETS costmap_node DESTINATION lib/${PROJECT_NAME}) - + # Copy launch and config files to installation location install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) - + ament_package() \ No newline at end of file diff --git a/src/robot/costmap/include/costmap_node.hpp b/src/robot/costmap/include/costmap_node.hpp index df7d5f1b..b116c1a0 100644 --- a/src/robot/costmap/include/costmap_node.hpp +++ b/src/robot/costmap/include/costmap_node.hpp @@ -1,16 +1,54 @@ #ifndef COSTMAP_NODE_HPP_ #define COSTMAP_NODE_HPP_ - + #include "rclcpp/rclcpp.hpp" - +#include "std_msgs/msg/string.hpp" + #include "costmap_core.hpp" - +#include "sensor_msgs/msg/laser_scan.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include +#include + class CostmapNode : public rclcpp::Node { public: CostmapNode(); + + // Place callback function here + void publishMessage(); + // New: laser scan callback will process incoming scans + void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg); + private: robot::CostmapCore costmap_; -}; + // Place these constructs here + rclcpp::Publisher::SharedPtr string_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + // New: lidar subscription and costmap publisher + rclcpp::Subscription::SharedPtr lidar_sub_; + rclcpp::Publisher::SharedPtr costmap_pub_; + // Costmap parameters + double resolution_ = 0.1; // meters per cell + unsigned int width_ = 200; // cells + unsigned int height_ = 200; // cells + double origin_x_ = 0.0; // meters (world frame origin of grid) + double origin_y_ = 0.0; // meters + + // Inflation parameters + double inflation_radius_ = 1.0; // meters + int max_cost_ = 100; + + // Grid storage in row-major (y * width + x) + std::vector grid_data_; + std::mutex grid_mutex_; + + // Helpers + bool worldToGrid(double wx, double wy, int &gx, int &gy); + void inflateObstacles(); + void publishCostmap(const rclcpp::Time &stamp); +}; + #endif \ No newline at end of file diff --git a/src/robot/costmap/package.xml b/src/robot/costmap/package.xml index 4651c557..1366a540 100644 --- a/src/robot/costmap/package.xml +++ b/src/robot/costmap/package.xml @@ -3,18 +3,23 @@ costmap 0.0.0 A sample ROS package for pubsub communication - + Owen Leather Apache2.0 - + ament_cmake rclcpp - + + + std_msgs + sensor_msgs + nav_msgs + ament_lint_auto ament_lint_common ament_cmake_gtest - + ament_cmake diff --git a/src/robot/costmap/src/costmap_node.cpp b/src/robot/costmap/src/costmap_node.cpp index 33a47a77..45dfea63 100644 --- a/src/robot/costmap/src/costmap_node.cpp +++ b/src/robot/costmap/src/costmap_node.cpp @@ -1,10 +1,174 @@ #include #include - +#include +#include +#include +#include + #include "costmap_node.hpp" + +CostmapNode::CostmapNode() : Node("costmap"), costmap_(robot::CostmapCore(this->get_logger())) { + // Initialize the constructs and their parameters + string_pub_ = this->create_publisher("/test_topic", 10); + timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CostmapNode::publishMessage, this)); + + // initialize grid origin to center of the map + origin_x_ = -static_cast(width_) * resolution_ / 2.0; + origin_y_ = -static_cast(height_) * resolution_ / 2.0; + + // initialize grid storage + grid_data_.assign(width_ * height_, 0); + + // Publisher for occupancy grid + costmap_pub_ = this->create_publisher("/costmap", 10); + + // Subscription to lidar scans + lidar_sub_ = this->create_subscription( + "/lidar", + 10, + std::bind(&CostmapNode::laserCallback, this, std::placeholders::_1) + ); +} + +// Define the timer to publish a message every 500ms +void CostmapNode::publishMessage() { + auto message = std_msgs::msg::String(); + message.data = "Hello, ROS 2!"; + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + string_pub_->publish(message); +} + +void CostmapNode::laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { + std::lock_guard lock(grid_mutex_); + + // reset grid to free (0) each scan; alternatively this could be persistent + std::fill(grid_data_.begin(), grid_data_.end(), 0); + + const double angle_min = msg->angle_min; + const double angle_inc = msg->angle_increment; + + // compute robot origin in grid coordinates + int r_gx = 0, r_gy = 0; + worldToGrid(0.0, 0.0, r_gx, r_gy); + + for (size_t i = 0; i < msg->ranges.size(); ++i) { + double r = static_cast(msg->ranges[i]); + if (!std::isfinite(r)) continue; + if (r < msg->range_min || r > msg->range_max) continue; + + double angle = angle_min + static_cast(i) * angle_inc; + double wx = r * std::cos(angle); + double wy = r * std::sin(angle); + + int gx, gy; + if (worldToGrid(wx, wy, gx, gy)) { + // Raytrace from robot cell to endpoint cell using Bresenham's algorithm + int x0 = r_gx; + int y0 = r_gy; + int x1 = gx; + int y1 = gy; + + int dx = std::abs(x1 - x0); + int dy = std::abs(y1 - y0); + int sx = (x0 < x1) ? 1 : -1; + int sy = (y0 < y1) ? 1 : -1; + int err = dx - dy; + + int x = x0; + int y = y0; + // iterate until we reach the endpoint + while (true) { + // mark free space along the ray (0) + size_t idx = static_cast(y) * width_ + static_cast(x); + grid_data_[idx] = 0; + if (x == x1 && y == y1) break; + int e2 = 2 * err; + if (e2 > -dy) { err -= dy; x += sx; } + if (e2 < dx) { err += dx; y += sy; } + } + + // mark endpoint as occupied + size_t eidx = static_cast(gy) * width_ + static_cast(gx); + grid_data_[eidx] = static_cast(std::max(grid_data_[eidx], 100)); + } + } + + // Inflate obstacles to produce costs + inflateObstacles(); + + // Publish occupancy grid + publishCostmap(this->now()); +} + +bool CostmapNode::worldToGrid(double wx, double wy, int &gx, int &gy) { + double rel_x = wx - origin_x_; + double rel_y = wy - origin_y_; + int ix = static_cast(std::floor(rel_x / resolution_)); + int iy = static_cast(std::floor(rel_y / resolution_)); + if (ix < 0 || ix >= static_cast(width_) || iy < 0 || iy >= static_cast(height_)) { + return false; + } + gx = ix; + gy = iy; + return true; +} + +void CostmapNode::inflateObstacles() { + // Make a copy to write inflated values into + std::vector new_grid = grid_data_; + + int inflation_cells = static_cast(std::ceil(inflation_radius_ / resolution_)); + + for (int y = 0; y < static_cast(height_); ++y) { + for (int x = 0; x < static_cast(width_); ++x) { + size_t idx = static_cast(y) * width_ + static_cast(x); + if (grid_data_[idx] >= 100) { + // inflate around this occupied cell + for (int dy = -inflation_cells; dy <= inflation_cells; ++dy) { + for (int dx = -inflation_cells; dx <= inflation_cells; ++dx) { + int nx = x + dx; + int ny = y + dy; + if (nx < 0 || nx >= static_cast(width_) || ny < 0 || ny >= static_cast(height_)) continue; + double dist = std::sqrt((dx * resolution_) * (dx * resolution_) + (dy * resolution_) * (dy * resolution_)); + if (dist > inflation_radius_) continue; + int cost = static_cast(std::round(static_cast(max_cost_) * (1.0 - (dist / inflation_radius_)))); + size_t nidx = static_cast(ny) * width_ + static_cast(nx); + if (cost > new_grid[nidx]) new_grid[nidx] = static_cast(std::min(cost, max_cost_)); + } + } + } + } + } + + grid_data_.swap(new_grid); +} + +void CostmapNode::publishCostmap(const rclcpp::Time &stamp) { + nav_msgs::msg::OccupancyGrid msg; + msg.header.stamp = stamp; + msg.header.frame_id = "map"; + + msg.info.resolution = resolution_; + msg.info.width = width_; + msg.info.height = height_; + msg.info.origin.position.x = origin_x_; + msg.info.origin.position.y = origin_y_; + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation.x = 0.0; + msg.info.origin.orientation.y = 0.0; + msg.info.origin.orientation.z = 0.0; + msg.info.origin.orientation.w = 1.0; -CostmapNode::CostmapNode() : Node("costmap"), costmap_(robot::CostmapCore(this->get_logger())) {} + // Copy grid_data_ into msg.data + msg.data.resize(grid_data_.size()); + for (size_t i = 0; i < grid_data_.size(); ++i) { + // nav_msgs uses int8 for occupancy: -1 unknown, 0 free, [1,100] occupancy + msg.data[i] = grid_data_[i]; + } + costmap_pub_->publish(msg); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/src/robot/map_memory/CMakeLists.txt b/src/robot/map_memory/CMakeLists.txt index 04458703..cdeed4cb 100644 --- a/src/robot/map_memory/CMakeLists.txt +++ b/src/robot/map_memory/CMakeLists.txt @@ -13,6 +13,7 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(nav_msgs REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -27,12 +28,14 @@ target_include_directories(map_memory_lib # Add ROS2 dependencies required by package ament_target_dependencies(map_memory_lib rclcpp + nav_msgs ) # Create ROS2 node executable from source files add_executable(map_memory_node src/map_memory_node.cpp) # Link to the previously built library to access map_memory classes and methods target_link_libraries(map_memory_node map_memory_lib) +ament_target_dependencies(map_memory_node rclcpp nav_msgs) # Copy executable to installation location install(TARGETS diff --git a/src/robot/map_memory/include/map_memory_node.hpp b/src/robot/map_memory/include/map_memory_node.hpp index e55a567c..c13c468f 100644 --- a/src/robot/map_memory/include/map_memory_node.hpp +++ b/src/robot/map_memory/include/map_memory_node.hpp @@ -1,6 +1,71 @@ #ifndef MAP_MEMORY_NODE_HPP_ #define MAP_MEMORY_NODE_HPP_ +#include "rclcpp/rclcpp.hpp" +#include "map_memory_core.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include +#include + +class MapMemoryNode : public rclcpp::Node { +public: + MapMemoryNode(); + +private: + // Core helper + robot::MapMemoryCore core_; + + // ROS interfaces + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + // Latest received local costmap + nav_msgs::msg::OccupancyGrid latest_costmap_; + bool have_costmap_ = false; + + // Robot pose tracking + double current_x_ = 0.0; + double current_y_ = 0.0; + bool have_odom_ = false; + + double last_update_x_ = 0.0; + double last_update_y_ = 0.0; + bool have_last_update_ = false; + + // Global map + std::vector global_grid_; + double global_resolution_ = 0.1; + unsigned int global_width_ = 1000; + unsigned int global_height_ = 1000; + double global_origin_x_ = 0.0; + double global_origin_y_ = 0.0; + bool global_initialized_ = false; + + // Parameters + double update_distance_threshold_ = 5.0; // meters (fuse when robot moved this distance) + double update_timer_period_ = 1.0; // seconds + + // Synchronization + std::mutex mutex_; + + // Callbacks + void costmapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void timerCallback(); + + // Helpers + void initializeGlobalMapFromCostmap(const nav_msgs::msg::OccupancyGrid &costmap); + void integrateCostmapIntoGlobal(const nav_msgs::msg::OccupancyGrid &costmap); + void publishGlobalMap(); +}; + +#endif +#ifndef MAP_MEMORY_NODE_HPP_ +#define MAP_MEMORY_NODE_HPP_ + #include "rclcpp/rclcpp.hpp" #include "map_memory_core.hpp" diff --git a/src/robot/map_memory/package.xml b/src/robot/map_memory/package.xml index 9cc4b5fc..fed6f140 100644 --- a/src/robot/map_memory/package.xml +++ b/src/robot/map_memory/package.xml @@ -11,6 +11,8 @@ ament_cmake rclcpp + nav_msgs + ament_lint_auto ament_lint_common ament_cmake_gtest diff --git a/src/robot/map_memory/src/map_memory_node.cpp b/src/robot/map_memory/src/map_memory_node.cpp index 1ce77741..dba3b6ac 100644 --- a/src/robot/map_memory/src/map_memory_node.cpp +++ b/src/robot/map_memory/src/map_memory_node.cpp @@ -1,9 +1,139 @@ #include "map_memory_node.hpp" +#include +#include -MapMemoryNode::MapMemoryNode() : Node("map_memory"), map_memory_(robot::MapMemoryCore(this->get_logger())) {} - -int main(int argc, char ** argv) +MapMemoryNode::MapMemoryNode() + : Node("map_memory"), core_(robot::MapMemoryCore(this->get_logger())) { + // Subscriptions + costmap_sub_ = this->create_subscription( + "/costmap", 10, std::bind(&MapMemoryNode::costmapCallback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "/odom/filtered", 10, std::bind(&MapMemoryNode::odomCallback, this, std::placeholders::_1)); + + // Publisher + map_pub_ = this->create_publisher("/map", 10); + + // Timer to limit updates + timer_ = this->create_wall_timer(std::chrono::duration(update_timer_period_), + std::bind(&MapMemoryNode::timerCallback, this)); + + RCLCPP_INFO(this->get_logger(), "MapMemoryNode started"); +} + +void MapMemoryNode::costmapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + std::lock_guard lock(mutex_); + latest_costmap_ = *msg; + have_costmap_ = true; + // initialize global map parameters on first costmap + if (!global_initialized_) { + initializeGlobalMapFromCostmap(latest_costmap_); + } +} + +void MapMemoryNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + current_x_ = msg->pose.pose.position.x; + current_y_ = msg->pose.pose.position.y; + have_odom_ = true; +} + +void MapMemoryNode::timerCallback() { + std::lock_guard lock(mutex_); + if (!have_costmap_ || !have_odom_) return; + + double dx = current_x_ - last_update_x_; + double dy = current_y_ - last_update_y_; + double dist = std::sqrt(dx*dx + dy*dy); + + if (!have_last_update_ || dist >= update_distance_threshold_) { + // integrate + integrateCostmapIntoGlobal(latest_costmap_); + publishGlobalMap(); + last_update_x_ = current_x_; + last_update_y_ = current_y_; + have_last_update_ = true; + } +} + +void MapMemoryNode::initializeGlobalMapFromCostmap(const nav_msgs::msg::OccupancyGrid &costmap) { + // adopt the costmap resolution for global map + global_resolution_ = costmap.info.resolution > 0.0 ? costmap.info.resolution : global_resolution_; + + // choose a reasonable global size (meters -> cells) + // default global map spans roughly global_width_ * resolution in x and y + global_grid_.assign(global_width_ * global_height_, static_cast(-1)); + + // center global origin around the incoming costmap origin + global_origin_x_ = costmap.info.origin.position.x - (static_cast(global_width_) * global_resolution_) / 2.0; + global_origin_y_ = costmap.info.origin.position.y - (static_cast(global_height_) * global_resolution_) / 2.0; + + global_initialized_ = true; + RCLCPP_INFO(this->get_logger(), "Global map initialized: res=%.3f width=%u height=%u", global_resolution_, global_width_, global_height_); + // Publish an initial empty global map so other nodes (planner) can start planning immediately + publishGlobalMap(); +} + +void MapMemoryNode::integrateCostmapIntoGlobal(const nav_msgs::msg::OccupancyGrid &costmap) { + if (!global_initialized_) return; + + unsigned int local_w = costmap.info.width; + unsigned int local_h = costmap.info.height; + double local_res = costmap.info.resolution; + + // assume local_res matches global_resolution_; if not, we convert using world coordinates + + for (unsigned int ly = 0; ly < local_h; ++ly) { + for (unsigned int lx = 0; lx < local_w; ++lx) { + size_t lidx = static_cast(ly) * local_w + lx; + int8_t val = costmap.data[lidx]; + // If local cell is unknown, skip it and keep the previous global value. + // If local cell is known (occupied or free), overwrite the global map cell. + if (val == -1) continue; // unknown -> preserve existing global cell + + // compute world coordinates of this local cell (use cell center) + double wx = costmap.info.origin.position.x + (static_cast(lx) + 0.5) * local_res; + double wy = costmap.info.origin.position.y + (static_cast(ly) + 0.5) * local_res; + + // convert to global grid indices + int gx = static_cast(std::floor((wx - global_origin_x_) / global_resolution_)); + int gy = static_cast(std::floor((wy - global_origin_y_) / global_resolution_)); + + if (gx < 0 || gx >= static_cast(global_width_) || gy < 0 || gy >= static_cast(global_height_)) continue; + size_t gidx = static_cast(gy) * global_width_ + static_cast(gx); + + // Merge: linear fusion policy (simple overwrite for known cells) + // Known cell values (0 free, 100 occupied) replace whatever was previously stored. + global_grid_[gidx] = val; + } + } + + RCLCPP_INFO(this->get_logger(), "Integrated costmap into global map at robot pos (%.2f, %.2f)", current_x_, current_y_); +} + +void MapMemoryNode::publishGlobalMap() { + if (!global_initialized_) return; + + nav_msgs::msg::OccupancyGrid msg; + msg.header.stamp = this->now(); + msg.header.frame_id = "map"; + + msg.info.resolution = global_resolution_; + msg.info.width = global_width_; + msg.info.height = global_height_; + msg.info.origin.position.x = global_origin_x_; + msg.info.origin.position.y = global_origin_y_; + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation.w = 1.0; + + msg.data = global_grid_; + + map_pub_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Published global map"); +} + +int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/robot/planner/CMakeLists.txt b/src/robot/planner/CMakeLists.txt index af32e490..9cf6f5bf 100644 --- a/src/robot/planner/CMakeLists.txt +++ b/src/robot/planner/CMakeLists.txt @@ -13,6 +13,8 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -27,10 +29,13 @@ target_include_directories(planner_lib # Add ROS2 dependencies required by package ament_target_dependencies(planner_lib rclcpp) +ament_target_dependencies(planner_lib nav_msgs geometry_msgs) + # Create ROS2 node executable from source files add_executable(planner_node src/planner_node.cpp) # Link to the previously built library to access planner classes and methods target_link_libraries(planner_node planner_lib) +ament_target_dependencies(planner_node rclcpp nav_msgs geometry_msgs) # Copy executable to installation location install(TARGETS diff --git a/src/robot/planner/include/planner_node.hpp b/src/robot/planner/include/planner_node.hpp index 95562ed2..9a801684 100644 --- a/src/robot/planner/include/planner_node.hpp +++ b/src/robot/planner/include/planner_node.hpp @@ -1,6 +1,82 @@ #ifndef PLANNER_NODE_HPP_ #define PLANNER_NODE_HPP_ +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include +#include +#include +#include + +// 2D grid index +struct CellIndex { + int x; + int y; + CellIndex(int xx, int yy) : x(xx), y(yy) {} + CellIndex() : x(0), y(0) {} + bool operator==(const CellIndex &other) const { return x == other.x && y == other.y; } +}; + +struct CellIndexHash { std::size_t operator()(const CellIndex &idx) const { return std::hash()(idx.x) ^ (std::hash()(idx.y) << 1); } }; + +struct AStarNode { + CellIndex index; + double f_score; + AStarNode(CellIndex idx, double f) : index(idx), f_score(f) {} +}; + +struct CompareF { bool operator()(const AStarNode &a, const AStarNode &b) { return a.f_score > b.f_score; } }; + +class PlannerNode : public rclcpp::Node { +public: + PlannerNode(); + +private: + enum class State { WAITING_FOR_GOAL, WAITING_FOR_ROBOT_TO_REACH_GOAL }; + State state_ = State::WAITING_FOR_GOAL; + + // ROS interfaces + rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + // Data + nav_msgs::msg::OccupancyGrid current_map_; + bool have_map_ = false; + bool map_updated_ = false; + + geometry_msgs::msg::PointStamped goal_; + bool goal_received_ = false; + + geometry_msgs::msg::Pose robot_pose_; + + // Parameters + double goal_tolerance_ = 0.5; // meters + double replan_interval_ = 1.0; // seconds + + // Helpers + bool worldToGrid(const nav_msgs::msg::OccupancyGrid &grid, double wx, double wy, int &gx, int &gy); + bool gridToWorld(const nav_msgs::msg::OccupancyGrid &grid, int gx, int gy, double &wx, double &wy); + void planPath(); + bool goalReached(); + + // Callbacks + void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr msg); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void timerCallback(); +}; + +#endif +#ifndef PLANNER_NODE_HPP_ +#define PLANNER_NODE_HPP_ + #include "rclcpp/rclcpp.hpp" #include "planner_core.hpp" diff --git a/src/robot/planner/package.xml b/src/robot/planner/package.xml index b29cb4ed..4e47e378 100644 --- a/src/robot/planner/package.xml +++ b/src/robot/planner/package.xml @@ -10,6 +10,8 @@ ament_cmake rclcpp + nav_msgs + geometry_msgs ament_lint_auto ament_lint_common diff --git a/src/robot/planner/src/planner_node.cpp b/src/robot/planner/src/planner_node.cpp index ed5a3b75..8ec8b3a7 100644 --- a/src/robot/planner/src/planner_node.cpp +++ b/src/robot/planner/src/planner_node.cpp @@ -1,11 +1,210 @@ #include "planner_node.hpp" +#include +#include +#include -PlannerNode::PlannerNode() : Node("planner"), planner_(robot::PlannerCore(this->get_logger())) {} - -int main(int argc, char ** argv) +PlannerNode::PlannerNode() + : Node("planner_node") { + map_sub_ = this->create_subscription( + "/map", 10, std::bind(&PlannerNode::mapCallback, this, std::placeholders::_1)); + + goal_sub_ = this->create_subscription( + "/goal_point", 10, std::bind(&PlannerNode::goalCallback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "/odom/filtered", 10, std::bind(&PlannerNode::odomCallback, this, std::placeholders::_1)); + + path_pub_ = this->create_publisher("/path", 10); + + timer_ = this->create_wall_timer(std::chrono::duration(replan_interval_), + std::bind(&PlannerNode::timerCallback, this)); + + RCLCPP_INFO(this->get_logger(), "PlannerNode started"); +} + +void PlannerNode::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + current_map_ = *msg; + have_map_ = true; + map_updated_ = true; + if (state_ == State::WAITING_FOR_ROBOT_TO_REACH_GOAL && goal_received_) { + planPath(); + } +} + +void PlannerNode::goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr msg) { + goal_ = *msg; + goal_received_ = true; + state_ = State::WAITING_FOR_ROBOT_TO_REACH_GOAL; + planPath(); +} + +void PlannerNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { + robot_pose_ = msg->pose.pose; +} + +void PlannerNode::timerCallback() { + if (state_ == State::WAITING_FOR_ROBOT_TO_REACH_GOAL) { + if (goalReached()) { + RCLCPP_INFO(this->get_logger(), "Goal reached!"); + state_ = State::WAITING_FOR_GOAL; + goal_received_ = false; + return; + } + + // Replan periodically or when map updates + if (map_updated_ && have_map_) { + planPath(); + map_updated_ = false; + } else { + planPath(); + } + } +} + +bool PlannerNode::goalReached() { + if (!goal_received_) return false; + double dx = goal_.point.x - robot_pose_.position.x; + double dy = goal_.point.y - robot_pose_.position.y; + return std::sqrt(dx*dx + dy*dy) < goal_tolerance_; +} + +bool PlannerNode::worldToGrid(const nav_msgs::msg::OccupancyGrid &grid, double wx, double wy, int &gx, int &gy) { + double ox = grid.info.origin.position.x; + double oy = grid.info.origin.position.y; + double res = grid.info.resolution; + int ix = static_cast(std::floor((wx - ox) / res)); + int iy = static_cast(std::floor((wy - oy) / res)); + if (ix < 0 || ix >= static_cast(grid.info.width) || iy < 0 || iy >= static_cast(grid.info.height)) return false; + gx = ix; gy = iy; return true; +} + +bool PlannerNode::gridToWorld(const nav_msgs::msg::OccupancyGrid &grid, int gx, int gy, double &wx, double &wy) { + double ox = grid.info.origin.position.x; + double oy = grid.info.origin.position.y; + double res = grid.info.resolution; + if (gx < 0 || gx >= static_cast(grid.info.width) || gy < 0 || gy >= static_cast(grid.info.height)) return false; + wx = ox + (static_cast(gx) + 0.5) * res; + wy = oy + (static_cast(gy) + 0.5) * res; + return true; +} + +void PlannerNode::planPath() { + if (!have_map_ || !goal_received_) { + RCLCPP_WARN(this->get_logger(), "Cannot plan path: missing map or goal"); + return; + } + + int start_gx, start_gy, goal_gx, goal_gy; + if (!worldToGrid(current_map_, robot_pose_.position.x, robot_pose_.position.y, start_gx, start_gy)) { + RCLCPP_WARN(this->get_logger(), "Robot pose outside map bounds, cannot plan"); + return; + } + if (!worldToGrid(current_map_, goal_.point.x, goal_.point.y, goal_gx, goal_gy)) { + RCLCPP_WARN(this->get_logger(), "Goal outside map bounds, cannot plan"); + return; + } + + const int width = static_cast(current_map_.info.width); + const int height = static_cast(current_map_.info.height); + const double res = current_map_.info.resolution; + + const auto index = [&](int x, int y) { return y * width + x; }; + + const double INF = std::numeric_limits::infinity(); + std::vector g_score(width * height, INF); + std::vector came_from(width * height, -1); + std::priority_queue, CompareF> open_set; + + auto heuristic = [&](int x, int y) { + double dx = (x - goal_gx) * res; + double dy = (y - goal_gy) * res; + return std::sqrt(dx*dx + dy*dy); + }; + + int sidx = index(start_gx, start_gy); + g_score[sidx] = 0.0; + open_set.emplace(CellIndex(start_gx, start_gy), heuristic(start_gx, start_gy)); + + std::vector neighbor_dx = {1, -1, 0, 0}; + std::vector neighbor_dy = {0, 0, 1, -1}; + + int goal_idx = index(goal_gx, goal_gy); + bool found = false; + + auto is_obstacle = [&](int x, int y) { + int idx = index(x, y); + int8_t v = current_map_.data[idx]; + // treat values >=50 as obstacles; unknown (-1) is treated as free + return (v >= 50); + }; + + while (!open_set.empty()) { + AStarNode node = open_set.top(); open_set.pop(); + int cx = node.index.x; + int cy = node.index.y; + int cidx = index(cx, cy); + if (cidx == goal_idx) { found = true; break; } + + for (size_t k = 0; k < neighbor_dx.size(); ++k) { + int nx = cx + neighbor_dx[k]; + int ny = cy + neighbor_dy[k]; + if (nx < 0 || nx >= width || ny < 0 || ny >= height) continue; + if (is_obstacle(nx, ny)) continue; + int nidx = index(nx, ny); + double tentative_g = g_score[cidx] + res; // cost to move one cell + if (tentative_g < g_score[nidx]) { + g_score[nidx] = tentative_g; + came_from[nidx] = cidx; + double f = tentative_g + heuristic(nx, ny); + open_set.emplace(CellIndex(nx, ny), f); + } + } + } + + nav_msgs::msg::Path path; + path.header.stamp = this->now(); + path.header.frame_id = current_map_.header.frame_id.empty() ? std::string("map") : current_map_.header.frame_id; + + if (!found) { + RCLCPP_WARN(this->get_logger(), "A*: no path found to goal"); + path_pub_->publish(path); + return; + } + + // reconstruct path + std::vector rev; + int cur = goal_idx; + while (cur != -1 && cur != sidx) { + rev.push_back(cur); + cur = came_from[cur]; + } + rev.push_back(sidx); + + // convert to PoseStamped + for (auto it = rev.rbegin(); it != rev.rend(); ++it) { + int idxv = *it; + int gx = idxv % width; + int gy = idxv / width; + double wx, wy; + gridToWorld(current_map_, gx, gy, wx, wy); + geometry_msgs::msg::PoseStamped pose; + pose.header = path.header; + pose.pose.position.x = wx; + pose.pose.position.y = wy; + pose.pose.position.z = 0.0; + pose.pose.orientation.w = 1.0; + path.poses.push_back(pose); + } + + path_pub_->publish(path); + RCLCPP_INFO(this->get_logger(), "Published path with %zu poses", path.poses.size()); +} + +int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } + diff --git a/watod-config.sh b/watod-config.sh index bc5da50a..5b826840 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -1,3 +1,6 @@ +# watod-config.sh contents +ACTIVE_MODULES="robot gazebo vis_tools" + ## ----------------------- watod Configuration File Override ---------------------------- ##