diff --git a/CMakeLists.txt b/CMakeLists.txt index b6d8b9e..d9e7643 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,11 @@ add_executable(yet_another_gps_publisher target_include_directories(yet_another_gps_publisher PUBLIC $ $) +set(DATA_FOLDER_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") + +target_compile_definitions(yet_another_gps_publisher PRIVATE + WAYPOINT_FILE="${DATA_FOLDER_PATH}/gps_waypoints_parking_lot_mk1.txt") + target_compile_features(yet_another_gps_publisher PUBLIC c_std_99 cxx_std_17) set(dependencies @@ -57,6 +62,9 @@ ament_target_dependencies(yet_another_gps_publisher ${dependencies}) install(TARGETS yet_another_gps_publisher DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY data + DESTINATION share/${PROJECT_NAME}) + if (BUILD_TESTING) find_package(ament_cmake_clang_format REQUIRED) ament_clang_format(CONFIG_FILE ${CMAKE_CURRENT_SOURCE_DIR}/.clang-format) diff --git a/README.md b/README.md index e4ee53e..162346c 100644 --- a/README.md +++ b/README.md @@ -39,3 +39,10 @@ yet_another_gps_publisher_NODE_NAME_node: Source files for the ROS2 node object yet_another_gps_publisher_NODE_NAME.cpp: Source for the main function of the node, and only the main function tests/unit.cpp: Example file for unit tests. This is linked to the node and ros, so both can be used + +# Testing +(Notes From Elijah) +- to test unit_spline.cpp: + colcon build --packages-select yet_another_gps_publisher --cmake-args -DBUILD_TESTING=ON +./build/yet_another_gps_publisher/unit_spline_test +cat /tmp/circle_arc_output.csv > src/gps_publisher/data/spline_unit_test.csv diff --git a/src/example_waypoints.txt b/data/example_waypoints.txt similarity index 100% rename from src/example_waypoints.txt rename to data/example_waypoints.txt diff --git a/data/example_waypoints_circle.txt b/data/example_waypoints_circle.txt new file mode 100644 index 0000000..2a22861 --- /dev/null +++ b/data/example_waypoints_circle.txt @@ -0,0 +1,6 @@ +# Test waypoints for circle generator +# Format: longitude latitude spline_type [radius] +-83.231343 42.318858 linear +-83.231316 42.318874 circle 5.0 +-83.231286 42.318890 circle -5.0 +-83.231258 42.318905 linear \ No newline at end of file diff --git a/src/gps_waypoints_parking_lot_mk1.txt b/data/gps_waypoints_parking_lot_mk1.txt similarity index 100% rename from src/gps_waypoints_parking_lot_mk1.txt rename to data/gps_waypoints_parking_lot_mk1.txt diff --git a/data/spline_unit_test.csv b/data/spline_unit_test.csv new file mode 100644 index 0000000..69330d2 --- /dev/null +++ b/data/spline_unit_test.csv @@ -0,0 +1,80 @@ +x,y +-3.59972e-16,0 +0.100685,0.00101386 +0.20133,0.00405501 +0.301892,0.00912223 +0.402333,0.0162135 +0.50261,0.0253258 +0.602683,0.0364556 +0.702512,0.0495984 +0.802056,0.0647487 +0.901275,0.0819005 +1.00013,0.101047 +1.09858,0.12218 +1.19658,0.145291 +1.2941,0.170371 +1.39109,0.197409 +1.48752,0.226396 +1.58334,0.257318 +1.67852,0.290163 +1.77302,0.324919 +1.86681,0.36157 +1.95983,0.400103 +2.05206,0.440501 +2.14346,0.482748 +2.23399,0.526827 +2.32362,0.57272 +2.4123,0.620408 +2.5,0.669873 +2.58669,0.721094 +2.67233,0.77405 +2.75689,0.828719 +2.84032,0.885081 +2.92261,0.943111 +3.00371,1.00279 +3.08359,1.06408 +3.16223,1.12698 +3.23958,1.19144 +3.31561,1.25745 +3.3903,1.32497 +3.46362,1.39399 +3.53553,1.46447 +3.60601,1.53638 +3.67503,1.6097 +3.74255,1.68439 +3.80856,1.76042 +3.87302,1.83777 +3.93592,1.91641 +3.99721,1.99629 +4.05689,2.07739 +4.11492,2.15968 +4.17128,2.24311 +4.22595,2.32767 +4.27891,2.41331 +4.33013,2.5 +4.37959,2.5877 +4.42728,2.67638 +4.47317,2.76601 +4.51725,2.85654 +4.5595,2.94794 +4.5999,3.04017 +4.63843,3.13319 +4.67508,3.22698 +4.70984,3.32148 +4.74268,3.41666 +4.7736,3.51248 +4.80259,3.60891 +4.82963,3.7059 +4.85471,3.80342 +4.87782,3.90142 +4.89895,3.99987 +4.9181,4.09872 +4.93525,4.19794 +4.9504,4.29749 +4.96354,4.39732 +4.97467,4.49739 +4.98379,4.59767 +4.99088,4.69811 +4.99594,4.79867 +4.99899,4.89931 +5,5 diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index c8600aa..b702627 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -1,7 +1,7 @@ #pragma once -#include -#include +#include /* for geometry_msg::msg::Pose for position P(x,y,z) and orientation(quanternion) Q(x,y,z,w) */ +#include /* for geometry_msg::msg::PoseStamped for position P(x,y,z) and orientation Q(x,y,z,w), with the header for timestamp and frame_id*/ #include class gps_waypoint { @@ -30,9 +30,9 @@ class gps_waypoint { private: double longitude_ = 0.0; double latitude_ = 0.0; + double radius_ = 0.0; /* determined later on */ std::string method_ = "linear"; - double radius_ = 0.0; - geometry_msgs::msg::PoseStamped utm_pose_; - geometry_msgs::msg::Pose odom_pose; - geometry_msgs::msg::Pose map_pose_; + geometry_msgs::msg::PoseStamped utm_pose_; /* Global, stores the absolute global position UTM*/ + geometry_msgs::msg::Pose map_pose_; /* Global, for path generation */ + geometry_msgs::msg::Pose odom_pose; /* Local, for local robot space position */ }; \ No newline at end of file diff --git a/include/yet_another_gps_publisher/spline_factory.hpp b/include/yet_another_gps_publisher/spline_factory.hpp index 3b65f42..81a3cc0 100644 --- a/include/yet_another_gps_publisher/spline_factory.hpp +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -1,10 +1,10 @@ #pragma once -#include -#include -#include -#include -#include +#include /* for functional paradigme returing of a function */ +#include /* for geoemtry_msg::msg::Pose for points */ +#include /* for map ADT {key: '', value: ''} pair */ +#include /* for method i.e 'linear', 'circular', etc.*/ +#include /* for dynamic array ADT */ #include "gps_waypoint.hpp" @@ -16,16 +16,22 @@ namespace gps_waypoint_spline { // void registerGenerator(std::string name, SplineGenerator gen); using SplineGenerator = std::function(const gps_waypoint& start, const gps_waypoint& end)>; +/* + input: waypoint start P_0, end P_n + output: function that outputs vector of Poses P(x,y,z) vec = +*/ class SplineFactory { public: - static void registerGenerator(const std::string& name, SplineGenerator gen); - static SplineGenerator getGenerator(const std::string& name); - static std::vector generate(const std::string& name, const gps_waypoint& start, - const gps_waypoint& end); + static void registerGenerator(const std::string& name, SplineGenerator gen); /* save function into memory */ + static SplineGenerator getGenerator(const std::string& name); /* looks up function based on key */ + static std::vector generate( + const std::string& name, const gps_waypoint& start, + const gps_waypoint& end); /* looks up the key and executes in one go */ private: - static std::map& registry(); + static std::map& + registry(); /* {key: 'method', value: 'function()'} meyers singleton returing refrence to the map */ }; } // namespace gps_waypoint_spline \ No newline at end of file diff --git a/include/yet_another_gps_publisher/yet_another_gps_publisher_node.hpp b/include/yet_another_gps_publisher/yet_another_gps_publisher_node.hpp index b0e906f..9182af6 100644 --- a/include/yet_another_gps_publisher/yet_another_gps_publisher_node.hpp +++ b/include/yet_another_gps_publisher/yet_another_gps_publisher_node.hpp @@ -1,6 +1,6 @@ #pragma once -#include // for doubly linked list +#include /* for doubly linked lists*/ #include #include @@ -37,22 +37,22 @@ class yet_another_gps_publisher : public rclcpp::Node { double arrival_threshold = 2.0; // Meters // Subscribers - rclcpp::Subscription::SharedPtr odom_sub; + rclcpp::Subscription::SharedPtr odom_sub; /* local ekf */ rclcpp::Subscription::SharedPtr waypoint_file_sub; // GPS subscribers - rclcpp::Subscription::SharedPtr gps_odom_sub; - rclcpp::Subscription::SharedPtr raw_gps_sub; + rclcpp::Subscription::SharedPtr gps_odom_sub; /* global ekf */ + rclcpp::Subscription::SharedPtr raw_gps_sub; /* raw gps points*/ // Publisher - rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr path_pub; /* publishes calculated spline */ // Timer unused ircc - rclcpp::TimerBase::SharedPtr timer; + rclcpp::TimerBase::SharedPtr timer; /* periodic callback (check if robot reached waypoint)*/ // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + tf2_ros::Buffer tf_buffer_; /* local cache that stores used as lookup for transforms */ + tf2_ros::TransformListener tf_listener_; /* listens to broadcasts (utm, map, baselink)*/ // Current robot pose (from odometry) geometry_msgs::msg::Pose current_pose{}; diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index 8c2ddde..f39551b 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -1,5 +1,8 @@ +#include + #include #include +#include #include "yet_another_gps_publisher/spline_factory.hpp" @@ -19,8 +22,10 @@ std::map& SplineFactory::registry() { return reg; } +/* registerGenerator: inputs name (classification of spline geometry), and function of spline geometry stores them in regsitry {key: '', value: ''}*/ void SplineFactory::registerGenerator(const std::string& name, SplineGenerator gen) { registry()[name] = gen; } +/* getGenerator: inputs the name (classification of spline geometry), outputs the actual function maping in regsitry */ SplineGenerator SplineFactory::getGenerator(const std::string& name) { auto it = registry().find(name); if (it == registry().end()) { @@ -29,6 +34,7 @@ SplineGenerator SplineFactory::getGenerator(const std::string& name) { return it->second; } +/* generate: inputs the classification, waypoint W_0 and waypoint W_n, and ouptus the dynamic array of points*/ std::vector SplineFactory::generate(const std::string& name, const gps_waypoint& start, const gps_waypoint& end) { return getGenerator(name)(start, end); @@ -39,6 +45,12 @@ std::vector SplineFactory::generate(const std::string& // ------------------------------------------------------------------ static std::vector linearGenerator(const gps_waypoint& start, const gps_waypoint& end) { + /* + a <- start position + b <- end position + t <- [0, 1] + P(t) = a + t(b - a) + */ const auto& a = start.mapPose().position; const auto& b = end.mapPose().position; @@ -59,8 +71,9 @@ static std::vector linearGenerator(const gps_waypoint& } static std::vector circleGenerator(const gps_waypoint& start, const gps_waypoint& end) { - double R = end.radius(); - if (R <= 0.0) { + double R_val = end.radius(); + + if (std::abs(R_val) <= 0.001) { return linearGenerator(start, end); } @@ -71,11 +84,53 @@ static std::vector circleGenerator(const gps_waypoint& double dy = b.y - a.y; double chord = std::hypot(dx, dy); - double theta = 2.0 * std::asin(chord / (2.0 * R)); + if (std::abs(R_val) < chord / 2.0) { + return linearGenerator(start, end); + } + + double d = std::sqrt(R_val * R_val - (chord / 2.0) * (chord / 2.0)); + double mx = (a.x + b.x) / 2.0; + double my = (a.y + b.y) / 2.0; + double side = (R_val > 0) ? 1.0 : -1.0; + double cx = mx - side * d * (dy / chord); + double cy = my + side * d * (dx / chord); + + double angle_start = std::atan2(a.y - cy, a.x - cx); + double angle_end = std::atan2(b.y - cy, b.x - cx); + + double diff = angle_end - angle_start; + if (side > 0) { + while (diff <= 0.0) diff += 2.0 * M_PI; + while (diff > 2.0 * M_PI) diff -= 2.0 * M_PI; + } else { + while (diff >= 0.0) diff -= 2.0 * M_PI; + while (diff < -2.0 * M_PI) diff += 2.0 * M_PI; + } - // TODO: Replace with actual circular arc interpolation. - // For now, return linear as a fallback. - return linearGenerator(start, end); + double arc_length = std::abs(R_val * diff); + int num_points = std::max(2, static_cast(arc_length / 0.1)); + + std::vector points; + points.reserve(num_points + 1); + + for (int i = 0; i <= num_points; ++i) { + double t = static_cast(i) / num_points; + double current_angle = angle_start + t * diff; + + geometry_msgs::msg::Pose p; + p.position.x = cx + std::abs(R_val) * std::cos(current_angle); + p.position.y = cy + std::abs(R_val) * std::sin(current_angle); + p.position.z = a.z + t * (b.z - a.z); + + double tangent_angle = current_angle + std::copysign(M_PI / 2.0, diff); + + tf2::Quaternion q; + q.setRPY(0, 0, tangent_angle); + p.orientation = tf2::toMsg(q); + + points.push_back(p); + } + return points; } // Register generators (runs before main) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 50d25df..5a19df2 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -12,10 +12,17 @@ #include "yet_another_gps_publisher/spline_factory.hpp" +#ifndef WAYPOINT_FILE +#define WAYPOINT_FILE "/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt" +#endif + // Constructor +/* yet_another_gps_publisher: */ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& options) : Node("yet_another_gps_publisher", options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { - // Declare parameters + /**********************************************************/ + /* PARAMETERS */ + /**********************************************************/ // Threshold for GPS "Confidence". 0.1 means we only trust the GPS if it's within ~30cm precision. // TODO figure out what a good threshold is based on the actual GPS variance we see in testing, and maybe even make it adaptive based on current conditions. @@ -42,16 +49,17 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // This is the robot's body frame (e.g., base_link) – the path is transformed here so the kart is at (0,0) // robot_origin_frame_id = this->declare_parameter("robot_origin_frame_id", "base_link"); // TODO actually set this parameter from launch file or command line, not hardcoded. - // TODO indentify where this file should be stored? - waypoint_file_path = this->declare_parameter( - "waypoint_file_path", - "/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt"); + // TODO indentify where this file should be stored? (Completed by Elijah May 9 check git logs) + waypoint_file_path = this->declare_parameter("waypoint_file_path", WAYPOINT_FILE); - // Publisher + /**********************************************************/ + /* PUBLISHER */ + /**********************************************************/ path_pub = this->create_publisher("/path", 5); - // Subscribers - + /**********************************************************/ + /* SUBSCRIBERS */ + /**********************************************************/ // Subscribe to Raw GPS to check the fix status (VectorNav) raw_gps_sub = this->create_subscription( "/phoenix/navsat", 10, std::bind(&yet_another_gps_publisher::raw_gps_callback, this, std::placeholders::_1)); @@ -76,6 +84,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& } // The Confidence Check + RAW GPS callback +/* raw_gps_callback: takes input of sensor message NavSatFix msg, evaluates signal quality against variance threshold, updates internal is_gps_valid state */ void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { // check if we are even using this if (!do_gps_variance_check) { @@ -111,20 +120,25 @@ void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatF } // Load waypoints from file into std::list +/* load_waypoints: takes input of waypoint file path, loads points as gps_waypoint and stores it in waypoint list, returns wheather file and points are valid or not */ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { + /* load waypoint file */ std::ifstream file(file_path); if (!file.is_open()) { RCLCPP_ERROR(this->get_logger(), "Could not open file: %s", file_path.c_str()); return false; } + /* clear waypoint list and instansiate line */ waypoints.clear(); std::string line; int line_num = 0; + /* while file is not EOF */ while (std::getline(file, line)) { line_num++; - if (line.empty() || line[0] == '#') continue; + if (line.empty() || line[0] == '#') continue; /* ignore comments */ + /* initalize fields*/ std::istringstream iss(line); double lon, lat, radius = 0.0; std::string spline_type; @@ -138,7 +152,7 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { RCLCPP_WARN(this->get_logger(), "Circle method on line %d missing radius, using default 0", line_num); } } - + /* create waypoint with long, lat, type and radius */ gps_waypoint wp(lon, lat, spline_type, radius); // Transform waypoint to odom frame @@ -146,59 +160,79 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { RCLCPP_WARN(this->get_logger(), "Skipping waypoint line %d due to transform failure", line_num); continue; } - + /* push back waypoint into doubly linked list */ waypoints.push_back(wp); RCLCPP_INFO(this->get_logger(), "Loaded waypoint %zu: spline_type=%s at (%.6f, %.6f)", waypoints.size(), spline_type.c_str(), lon, lat); } + /* close file properly */ file.close(); + /* if there are less than lines read return false else return true*/ if (line_num < 5) return false; return true; } // Transform waypoint from lat/lon to UTM and store +/* transformWaypoint: takes coordinates of waypoint, transforms its projection to UTM*/ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { + /* Populates a standard ROS 2 geographic message with the waypoint's coordinate data for projection*/ geographic_msgs::msg::GeoPoint geo; geo.latitude = wp.latitude(); geo.longitude = wp.longitude(); geo.altitude = 0.0; + /* Project the spherical coordinate into flat UTM grid coordinate */ geodesy::UTMPoint utm; geodesy::fromMsg(geo, utm); + /* Package the UTM data into a PossedStamped with the correct UTM frame ID */ geometry_msgs::msg::PoseStamped utm_pose; utm_pose.header.frame_id = utm_frame_id; // Do NOT set the stamp here, we want the TF buffer to grab the newest available transform later utm_pose.pose.position.x = utm.easting; utm_pose.pose.position.y = utm.northing; utm_pose.pose.position.z = 0.0; - utm_pose.pose.orientation.w = 1.0; + utm_pose.pose.orientation.w = + 1.0; /* potential downstream to fix if waypoint needs to be approached at a specific angle */ // Save the UTM pose to the waypoint, but don't do the TF lookup yet + /* Update the waypoint with new UTM pose for future coordinate transformations */ wp.setUtmPose(utm_pose); return true; } // Advance iterator +/* advance_to_next_waypoint: iterates to the next waypoint in the list waypoints */ void yet_another_gps_publisher::advance_to_next_waypoint() { + /* if the current waypoint is not the end */ if (current_waypoint_it_ != waypoints.end()) { + /* ittereate to the next waypoint */ ++current_waypoint_it_; // wrap around for looping since the track is a circuit + /* if waypoint iterator is the end waypoint */ if (current_waypoint_it_ == waypoints.end()) { + /* the current waypoint is now the beginning circular */ current_waypoint_it_ = waypoints.begin(); } } } // generate spline path from robot until we exceed max spline +/* global_ekf_callback: takes input of an Odometry message, transforms robot and + waypoint data into a common map frame, advances mission progress if waypoints + are reached, and generates a multi-segment spline path until a minimum length + requirement is met. */ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { // Guard conditions + /* if the gps point is not valid or the waypoint list is empty */ if (!is_gps_valid || waypoints.empty()) { return; } geometry_msgs::msg::Pose - robot_pose_map; // its using the pose of the center of the map ot the robots pose in the map. + robot_pose_map; // its using the pose of the center of the map not the robots pose in the map. + + /* Attempt to transform the robot's current odometry pose into the map frame */ try { geometry_msgs::msg::PoseStamped ps_in; ps_in.header = msg->header; // frame_id from /odometry/gps (likely "map origin") @@ -208,6 +242,12 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr } catch (tf2::TransformException& ex) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF /odometry/gps -> map failed: %s", ex.what()); + /* + What can cause this to fail? + 1. Broken or missing TF Tree + 2. Timing Issue (ex. one node publishes odom -> base_link then another another utm -> map, nothing is doing map->odom will fail) + 3. Timing Issue (msg->header.stamp transform at the exact moment GPS was recorded if tf_buffer_ hasn't recieved the latest transform tf2 cannot see into future) + */ return; } @@ -216,10 +256,13 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr size_t checked = 0; const size_t N = waypoints.size(); + /* Determine if the robot has entered the arrival_threshold radius of the current target*/ while (checked < N) { // Transform the current waypoint to map frame on demand gps_waypoint& wp = *current_waypoint_it_; geometry_msgs::msg::Pose wp_map_pose; + + /* Transform the stored UTM waypoint into the local map frame for distance comparison*/ try { wp.utmPose().header.stamp = rclcpp::Time(0); geometry_msgs::msg::PoseStamped map_wp = @@ -230,36 +273,48 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr return; } + /* Calculate Euclidean distance; if within threshold, advance mission progress */ double dist = std::hypot(robot_postion.position.x - wp_map_pose.position.x, robot_postion.position.y - wp_map_pose.position.y); - if (dist >= arrival_threshold) break; // not arrived yet do not skipping + if (dist >= arrival_threshold) break; // not arrived yet do not skip RCLCPP_INFO(this->get_logger(), "Passed waypoint (distance %.2f < %.2f)", dist, arrival_threshold); advance_to_next_waypoint(); // ++it, wraps to begin() if at end checked++; } - if (checked >= N) { // all waypoints reached → full lap done + /* All waypoints reached implies full lap done*/ + if (checked >= N) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "All waypoints reached (full lap)"); return; } + /* path_map: Initializes a local path message in the map coordinate frame + to hold the collection of generated spline segments. */ nav_msgs::msg::Path path_map; path_map.header.frame_id = map_frame_id; path_map.header.stamp = msg->header.stamp; + /* cumulative_length: Tracks the total distance of the generated path to + ensure it meets the minimum requirements for the steering controller. */ double cumulative_length = 0.0; // TODO decided if we want to start at the back of the robot pose + /* start_wp: Creates a temporary 'virtual' waypoint at the robot's current + coordinates to act as the anchor for the first spline segment. */ gps_waypoint start_wp(0.0, 0.0, "line"); start_wp.setMapPose(robot_postion); gps_waypoint prev_wp = start_wp; // this will be updated as we drive forward // the look ahead scanner. + /* Segment_it: a look-ahead pointer starting at the current target waypoint + used to scan forward through the list during spline chaining. */ auto segment_it = current_waypoint_it_; size_t processed = 0; const size_t n = waypoints.size(); + /* Generate a continuous path starting from the robot's current position and + chaining together spline segments until the path meets min_spline_length. */ while (cumulative_length < min_spline_length) { gps_waypoint& wp = *segment_it; processed++; @@ -312,6 +367,9 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr // Transform to body frame (odom_frame_id now holds e.g. "base_link") // so that the robot sits at (0,0) and the path extends ahead of it. nav_msgs::msg::Path path_odom; + + /* Transform the entire path from map coordinates to the robot's body frame + (odom_frame_id) so the robot base link acts as the origin (0,0). */ try { auto transform = tf_buffer_.lookupTransform(odom_frame_id, map_frame_id, tf2::TimePointZero); for (const auto& ps : path_map.poses) { @@ -354,6 +412,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr } // gps_waypoint constructor implementation +/* gps_waypoint: */ gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) : longitude_(lon), latitude_(lat), method_(method), radius_(radius) {} diff --git a/tests/unit_spline.cpp b/tests/unit_spline.cpp new file mode 100644 index 0000000..36d83b4 --- /dev/null +++ b/tests/unit_spline.cpp @@ -0,0 +1,107 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "yet_another_gps_publisher/gps_waypoint.hpp" +#include "yet_another_gps_publisher/spline_factory.hpp" + +/* put your own here or fix the CMAKE I am not going to do it. */ +#define WAYPOINT_FILE_TEST "/home/elijahstickel/Desktop/phnx_dev/src/gps_publisher/data/example_waypoints_circle.txt" +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static gps_waypoint make_waypoint(double x, double y, const std::string& method, double radius = 0.0) { + gps_waypoint wp(0.0, 0.0, method, radius); + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = 0.0; + pose.orientation.w = 1.0; + wp.setMapPose(pose); + return wp; +} + +// --------------------------------------------------------------------------- +// GPS file test — mirrors your load_waypoints logic but minimal +// --------------------------------------------------------------------------- + +TEST(yet_another_gps_publisher, gps_file_loads_correctly) { + const std::string path = std::string(WAYPOINT_FILE_TEST); + std::ifstream file(path); + ASSERT_TRUE(file.is_open()) << "Could not open: " << path; + + int count = 0; + std::string line; + while (std::getline(file, line)) { + if (line.empty() || line[0] == '#') continue; + std::istringstream iss(line); + double lon, lat; + std::string method; + ASSERT_TRUE(iss >> lon >> lat >> method) << "Malformed line: " << line; + count++; + } + EXPECT_GT(count, 0) << "Waypoint file is empty"; + RCLCPP_INFO(rclcpp::get_logger("test"), "Loaded %d waypoints from file", count); +} + +// --------------------------------------------------------------------------- +// Circle generator — output to CSV for Desmos +// --------------------------------------------------------------------------- + +TEST(yet_another_gps_publisher, circle_generator_desmos_output) { + // Two points ~7m apart, 5m radius left turn + gps_waypoint start = make_waypoint(0.0, 0.0, "linear"); + gps_waypoint end = make_waypoint(5.0, 5.0, "circle", 5.0); + + auto points = gps_waypoint_spline::SplineFactory::generate("circle", start, end); + + ASSERT_GT(points.size(), 0u) << "Circle generator returned no points"; + + // Dump to CSV so you can paste into Desmos + const std::string csv_path = "/tmp/circle_arc_output.csv"; + std::ofstream csv(csv_path); + ASSERT_TRUE(csv.is_open()) << "Could not open output CSV"; + csv << "x,y\n"; + for (auto& p : points) { + csv << p.position.x << "," << p.position.y << "\n"; + } + csv.close(); + RCLCPP_INFO(rclcpp::get_logger("test"), "Arc points written to %s — paste into Desmos table", csv_path.c_str()); + + // Basic sanity: all points should be within radius*2 of start + for (auto& p : points) { + double dist = std::hypot(p.position.x, p.position.y); + EXPECT_LT(dist, 20.0) << "Point suspiciously far from origin"; + } +} + +// --------------------------------------------------------------------------- +// Circle fallback — bad radius should return linear points +// --------------------------------------------------------------------------- + +TEST(yet_another_gps_publisher, circle_generator_falls_back_on_bad_radius) { + gps_waypoint start = make_waypoint(0.0, 0.0, "linear"); + gps_waypoint end = make_waypoint(10.0, 0.0, "circle", 0.0); // zero radius + + auto points = gps_waypoint_spline::SplineFactory::generate("circle", start, end); + ASSERT_GT(points.size(), 0u); + + // All points should be on y=0 (linear fallback) + for (auto& p : points) { + EXPECT_NEAR(p.position.y, 0.0, 1e-6) << "Expected linear fallback on y=0"; + } +} + +int main(int argc, char** argv) { + rclcpp::init(0, nullptr); + ::testing::InitGoogleTest(&argc, argv); + auto res = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return res; +} \ No newline at end of file