From 3d55f2520d0eb058b52a0cac9e28e8be28fafb77 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Feb 2026 14:55:08 -0500 Subject: [PATCH 01/65] gps waypoint struct --- .../gps_waypoint.hpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 include/yet_another_gps_publisher/gps_waypoint.hpp diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp new file mode 100644 index 0000000..1024a1c --- /dev/null +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +class gps_waypoint +{ +public: + gps_waypoint() = default; + gps_waypoint(double lon, double lat, const std::string& method, double radius = 0.0); + + double longitude() const { return longitude_; } + double latitude() const { return latitude_; } + const std::string& method() const { return method_; } + double radius() const { return radius_; } + bool enabled() const { return enabled_; } + void setEnabled(bool e) { enabled_ = e; } + + // Odom pose after transformation (set when waypoint is loaded) + void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose_ = pose; } + const geometry_msgs::msg::Pose& odomPose() const { return odom_pose_; } + +private: + double longitude_ = 0.0; + double latitude_ = 0.0; + std::string method_ = "linear"; + double radius_ = 0.0; + bool enabled_ = true; + geometry_msgs::msg::Pose odom_pose_; +}; \ No newline at end of file From 5ca920fddf730c6cee4370ea3029267902213e81 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Feb 2026 14:55:31 -0500 Subject: [PATCH 02/65] spline holder --- .../spline_factory.hpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 include/yet_another_gps_publisher/spline_factory.hpp diff --git a/include/yet_another_gps_publisher/spline_factory.hpp b/include/yet_another_gps_publisher/spline_factory.hpp new file mode 100644 index 0000000..dc0057a --- /dev/null +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "gps_waypoint.hpp" + +namespace gps_waypoint_spline { + +using SplineGenerator = std::function( + const gps_waypoint& start, + const gps_waypoint& end)>; + +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); + +private: + static std::map& registry(); +}; + +} // namespace gps_waypoint_spline \ No newline at end of file From 939b43d16f8db58bc22c425410278c76d8830fd8 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Feb 2026 14:55:52 -0500 Subject: [PATCH 03/65] spline factory cpp --- src/spline_methods.cpp | 93 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 src/spline_methods.cpp diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp new file mode 100644 index 0000000..9f0ec32 --- /dev/null +++ b/src/spline_methods.cpp @@ -0,0 +1,93 @@ +#include "yet_another_gps_publisher/spline_factory.hpp" +#include +#include + +namespace gps_waypoint_spline { + +// Factory registry +std::map& SplineFactory::registry() +{ + static std::map reg; + return reg; +} + +void SplineFactory::registerGenerator(const std::string& name, SplineGenerator gen) +{ + registry()[name] = gen; +} + +SplineGenerator SplineFactory::getGenerator(const std::string& name) +{ + auto it = registry().find(name); + if (it == registry().end()) { + throw std::runtime_error("Unknown spline method: " + name); + } + return it->second; +} + +std::vector SplineFactory::generate( + const std::string& name, + const gps_waypoint& start, + const gps_waypoint& end) +{ + return getGenerator(name)(start, end); +} + +// ------------------------------------------------------------------ +// Concrete generators +// ------------------------------------------------------------------ + +static std::vector linearGenerator( + const gps_waypoint& start, + const gps_waypoint& end) +{ + const auto& a = start.odomPose().position; + const auto& b = end.odomPose().position; + + const int num_points = 10; + std::vector points; + points.reserve(num_points + 1); + + for (int i = 0; i <= num_points; ++i) { + double t = static_cast(i) / num_points; + geometry_msgs::msg::Pose p; + p.position.x = a.x + t * (b.x - a.x); + p.position.y = a.y + t * (b.y - a.y); + p.position.z = a.z + t * (b.z - a.z); + p.orientation.w = 1.0; + points.push_back(p); + } + return points; +} + +static std::vector circleGenerator( + const gps_waypoint& start, + const gps_waypoint& end) +{ + double R = end.radius(); + if (R <= 0.0) { + return linearGenerator(start, end); + } + + const auto& a = start.odomPose().position; + const auto& b = end.odomPose().position; + + double dx = b.x - a.x; + double dy = b.y - a.y; + double chord = std::hypot(dx, dy); + + double theta = 2.0 * std::asin(chord / (2.0 * R)); + + // TODO: Replace with actual circular arc interpolation. + // For now, return linear as a fallback. + return linearGenerator(start, end); +} + +// Register generators (runs before main) +static bool registered = []() { + SplineFactory::registerGenerator("linear", linearGenerator); + SplineFactory::registerGenerator("circle", circleGenerator); + return true; +}(); + +} // namespace gps_waypoint_spline \ No newline at end of file From 2aa0ac0aac53f38c0831b1c41c39b6b867707b98 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Feb 2026 14:56:22 -0500 Subject: [PATCH 04/65] node logic --- .../yet_another_gps_publisher_node.hpp | 54 ++++- src/yet_another_gps_publisher_node.cpp | 208 ++++++++++++++++-- 2 files changed, 235 insertions(+), 27 deletions(-) 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 1c7d6b0..41df73a 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 @@ -2,19 +2,53 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include "geodesy/utm.h" +#include "gps_waypoint.hpp" +#include + +class yet_another_gps_publisher : public rclcpp::Node +{ +public: + explicit yet_another_gps_publisher(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); -// yet_another_gps_publisher_node.cpp -class yet_another_gps_publisher : public rclcpp::Node { private: - rclcpp::Publisher::SharedPtr pub; + // Parameters + double min_spline_length_; + std::string odom_topic_; + std::string waypoint_file_topic_; + std::string utm_frame_id_; + std::string odom_frame_id_; - rclcpp::Subscription::SharedPtr sub; - geodesy::UTMPose currentPose; + // Subscribers + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr waypoint_file_sub_; -public: - yet_another_gps_publisher(const rclcpp::NodeOptions& options); + // Publisher + rclcpp::Publisher::SharedPtr path_pub_; + + // Timer for periodic spline generation + rclcpp::TimerBase::SharedPtr timer_; + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // Current robot pose in odom frame (from odometry) + geometry_msgs::msg::Pose current_pose_; + + // List of pending waypoints (already transformed to odom) + std::deque waypoints_; + + // Callbacks + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + void waypoint_file_callback(const std_msgs::msg::String::SharedPtr msg); + void timer_callback(); - /// subscriber callback - void sub_cb(std_msgs::msg::String::SharedPtr msg); -}; + // Helper: transform a waypoint from lat/lon to odom frame using TF + bool transformWaypoint(gps_waypoint& wp); +}; \ No newline at end of file diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 16919da..92e00a5 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -1,27 +1,201 @@ #include "yet_another_gps_publisher/yet_another_gps_publisher_node.hpp" +#include "yet_another_gps_publisher/spline_factory.hpp" +#include +#include +#include +#include +#include -// For _1 using namespace std::placeholders; -yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& options) : Node("yet_another_gps_publisher", options) { - // Parameters - float x = this->declare_parameter("foo", -10.0); +// Constructor +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 + min_spline_length_ = this->declare_parameter("min_spline_length", 10.0); + odom_topic_ = this->declare_parameter("odom_topic", "/odometry/filtered"); + waypoint_file_topic_ = this->declare_parameter("waypoint_file_topic", "/waypoint_file"); + utm_frame_id_ = this->declare_parameter("utm_frame_id", "utm"); + odom_frame_id_ = this->declare_parameter("odom_frame_id", "odom"); - // Pub Sub - this->sub = - this->create_subscription("/str", 1, std::bind(&yet_another_gps_publisher::sub_cb, this, _1)); - this->pub = this->create_publisher("/run_folder", 1); + // Subscribers + odom_sub_ = this->create_subscription( + odom_topic_, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, _1)); + waypoint_file_sub_ = this->create_subscription( + waypoint_file_topic_, 1, std::bind(&yet_another_gps_publisher::waypoint_file_callback, this, _1)); - // Log a sample log - RCLCPP_INFO(this->get_logger(), "You passed %f", x); + // Publisher + path_pub_ = this->create_publisher("spline_path", 10); - // Send a sample message - std_msgs::msg::String msg{}; - msg.data = std::string{"Hello World!"}; - pub->publish(msg); + // Timer (1 Hz) + timer_ = this->create_wall_timer( + std::chrono::seconds(1), std::bind(&yet_another_gps_publisher::timer_callback, this)); + + RCLCPP_INFO(this->get_logger(), "yet_another_gps_publisher started"); +} + +// Odom callback +void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + current_pose_ = msg->pose.pose; +} + +// Waypoint file callback +void yet_another_gps_publisher::waypoint_file_callback(const std_msgs::msg::String::SharedPtr msg) +{ + std::string file_path = msg->data; + std::ifstream file(file_path); + if (!file.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Could not open file: %s", file_path.c_str()); + return; + } + + std::string line; + int line_num = 0; + while (std::getline(file, line)) { + line_num++; + if (line.empty() || line[0] == '#') continue; + + std::istringstream iss(line); + double lon, lat, radius = 0.0; + std::string method; + if (!(iss >> lon >> lat >> method)) { + RCLCPP_WARN(this->get_logger(), "Skipping malformed line %d", line_num); + continue; + } + if (method == "circle") { + if (!(iss >> radius)) { + RCLCPP_WARN(this->get_logger(), "Circle method on line %d missing radius, using default 0", line_num); + } + } + + gps_waypoint wp(lon, lat, method, radius); + + // Transform waypoint to odom frame + if (!transformWaypoint(wp)) { + RCLCPP_WARN(this->get_logger(), "Skipping waypoint line %d due to transform failure", line_num); + continue; + } + + waypoints_.push_back(wp); + RCLCPP_INFO(this->get_logger(), "Loaded waypoint %zu: method=%s at (%.6f, %.6f)", + waypoints_.size(), method.c_str(), lon, lat); + } + file.close(); +} + +// Transform waypoint from lat/lon to odom +bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) +{ + // Create a GeoPoint from lat/lon + geographic_msgs::msg::GeoPoint geo; + geo.latitude = wp.latitude(); + geo.longitude = wp.longitude(); + geo.altitude = 0.0; // assume ground level; could be extended + + // Convert to UTM using geodesy + geodesy::UTMPoint utm; + geodesy::fromMsg(geo, utm); // This populates easting, northing, zone, etc. + + geometry_msgs::msg::PoseStamped utm_pose; + utm_pose.header.frame_id = utm_frame_id_; + utm_pose.header.stamp = this->now(); + 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; + + try { + geometry_msgs::msg::PoseStamped odom_pose = tf_buffer_.transform(utm_pose, odom_frame_id_); + wp.setOdomPose(odom_pose.pose); + return true; + } catch (tf2::TransformException& ex) { + RCLCPP_WARN(this->get_logger(), "Transform failed: %s", ex.what()); + return false; + } } -void yet_another_gps_publisher::sub_cb(const std_msgs::msg::String::SharedPtr msg) { - // Echo message - this->pub->publish(*msg); +// Timer callback: generate and publish spline +void yet_another_gps_publisher::timer_callback() +{ + if (waypoints_.empty()) { + return; + } + + nav_msgs::msg::Path path; + path.header.frame_id = odom_frame_id_; + path.header.stamp = this->now(); + + // Start with current pose + geometry_msgs::msg::PoseStamped start_pose; + start_pose.header = path.header; + start_pose.pose = current_pose_; + path.poses.push_back(start_pose); + + double cumulative_length = 0.0; + // Temporary waypoint for current pose (method irrelevant) + gps_waypoint current_wp; + current_wp.setOdomPose(current_pose_); + current_wp.setEnabled(true); + + size_t used_count = 0; + for (size_t i = 0; i < waypoints_.size(); ++i) { + const auto& wp = waypoints_[i]; + if (!wp.enabled()) continue; + + const gps_waypoint& start_ref = (i == 0) ? current_wp : waypoints_[i-1]; + + std::vector segment; + try { + segment = gps_waypoint_spline::SplineFactory::generate(wp.method(), start_ref, wp); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Spline generation failed for method %s: %s", + wp.method().c_str(), e.what()); + break; + } + + // Add segment points (skip first to avoid duplicate with previous end) + for (size_t j = 1; j < segment.size(); ++j) { + geometry_msgs::msg::PoseStamped ps; + ps.header = path.header; + ps.pose = segment[j]; + path.poses.push_back(ps); + } + + // Update cumulative length + for (size_t j = 1; j < segment.size(); ++j) { + const auto& a = segment[j-1].position; + const auto& b = segment[j].position; + cumulative_length += std::hypot(b.x - a.x, b.y - a.y); + } + + used_count = i + 1; + + if (cumulative_length >= min_spline_length_) { + break; + } + } + + if (cumulative_length >= min_spline_length_) { + path_pub_->publish(path); + RCLCPP_INFO(this->get_logger(), "Published spline path, length = %.2f m using %zu waypoints", + cumulative_length, used_count); + // Optionally remove used waypoints: + // waypoints_.erase(waypoints_.begin(), waypoints_.begin() + used_count); + } else { + RCLCPP_DEBUG(this->get_logger(), "Path too short (%.2f < %.2f), not publishing", + cumulative_length, min_spline_length_); + } } + +// gps_waypoint constructor implementation +gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) + : longitude_(lon), latitude_(lat), method_(method), radius_(radius), enabled_(true) +{} + +// Register node as a component +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) \ No newline at end of file From b461bd9810122557b6e87f22c420fb11a4d189d7 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Feb 2026 14:56:56 -0500 Subject: [PATCH 05/65] building hopefully --- CMakeLists.txt | 56 +++++++++++++++++++++++++++----------------------- package.xml | 13 ++++++++++-- 2 files changed, 41 insertions(+), 28 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a42f6ce..b6d8b9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,58 +9,62 @@ endif () # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -# Messages TODO_EXTRA find_package(ackermann_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) -# GPS packages find_package(geodesy REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) -# Add source for node executable (link non-ros dependencies here) -add_executable(yet_another_gps_publisher src/yet_another_gps_publisher.cpp src/yet_another_gps_publisher_node.cpp) +# Optional: print found packages for debugging +message(STATUS "Found tf2: ${tf2_DIR}") +message(STATUS "Found tf2_ros: ${tf2_ros_DIR}") +message(STATUS "Found tf2_geometry_msgs: ${tf2_geometry_msgs_DIR}") + +# Add source for node executable +add_executable(yet_another_gps_publisher + src/yet_another_gps_publisher.cpp + src/yet_another_gps_publisher_node.cpp + src/spline_methods.cpp +) target_include_directories(yet_another_gps_publisher PUBLIC $ $) -target_compile_features(yet_another_gps_publisher PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(yet_another_gps_publisher PUBLIC c_std_99 cxx_std_17) -# Make ros deps a variable so they get linked to tests as well set(dependencies rclcpp - # Messages TODO_EXTRA ackermann_msgs sensor_msgs std_msgs - # GPS Packages geodesy - ) - -# Link ros dependencies -ament_target_dependencies( - yet_another_gps_publisher - ${dependencies} + geographic_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + rclcpp_components ) +ament_target_dependencies(yet_another_gps_publisher ${dependencies}) + install(TARGETS yet_another_gps_publisher DESTINATION lib/${PROJECT_NAME}) -# Uncomment below to make launch files available if created -#install( -# DIRECTORY launch config -# DESTINATION share/${PROJECT_NAME}/ -#) - if (BUILD_TESTING) - # Manually invoke clang format so it actually uses our file find_package(ament_cmake_clang_format REQUIRED) ament_clang_format(CONFIG_FILE ${CMAKE_CURRENT_SOURCE_DIR}/.clang-format) - find_package(ament_cmake_gtest REQUIRED) - - # Add unit tests ament_add_gtest(${PROJECT_NAME}-test tests/unit.cpp - # Remember to add node source files src/yet_another_gps_publisher_node.cpp + src/spline_methods.cpp ) ament_target_dependencies(${PROJECT_NAME}-test ${dependencies}) target_include_directories(${PROJECT_NAME}-test PUBLIC @@ -68,4 +72,4 @@ if (BUILD_TESTING) $) endif () -ament_package() +ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml index 09bb41c..0c26058 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,7 @@ rclcpp - + ackermann_msgs sensor_msgs std_msgs @@ -19,6 +19,15 @@ geodesy + + geographic_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + rclcpp_components + ament_lint_auto ament_cmake_flake8 ament_cmake_xmllint @@ -32,4 +41,4 @@ ament_cmake - + \ No newline at end of file From 977b9e277d98ed794d73e2e58d3a91589f05406a Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 18 Feb 2026 23:53:59 -0500 Subject: [PATCH 06/65] making derek happy again + changing the waypoiny file to a parameter not a topic lol --- .../gps_waypoint.hpp | 5 +- .../spline_factory.hpp | 17 ++-- .../yet_another_gps_publisher_node.hpp | 18 +++-- src/spline_methods.cpp | 31 +++---- src/yet_another_gps_publisher_node.cpp | 81 +++++++++---------- 5 files changed, 67 insertions(+), 85 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 1024a1c..ed9f1aa 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -1,10 +1,9 @@ #pragma once -#include #include +#include -class gps_waypoint -{ +class gps_waypoint { public: gps_waypoint() = default; gps_waypoint(double lon, double lat, const std::string& method, double radius = 0.0); diff --git a/include/yet_another_gps_publisher/spline_factory.hpp b/include/yet_another_gps_publisher/spline_factory.hpp index dc0057a..e9abbb0 100644 --- a/include/yet_another_gps_publisher/spline_factory.hpp +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -1,27 +1,24 @@ #pragma once #include +#include #include #include #include -#include + #include "gps_waypoint.hpp" namespace gps_waypoint_spline { -using SplineGenerator = std::function( - const gps_waypoint& start, - const gps_waypoint& end)>; +using SplineGenerator = + std::function(const gps_waypoint& start, const gps_waypoint& end)>; -class SplineFactory -{ +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 std::vector generate(const std::string& name, const gps_waypoint& start, + const gps_waypoint& end); private: static std::map& registry(); 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 41df73a..aa83b8f 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,18 +1,19 @@ #pragma once +#include +#include + +#include "geodesy/utm.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "gps_waypoint.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "geodesy/utm.h" -#include "gps_waypoint.hpp" -#include -class yet_another_gps_publisher : public rclcpp::Node -{ +class yet_another_gps_publisher : public rclcpp::Node { public: explicit yet_another_gps_publisher(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); @@ -23,6 +24,7 @@ class yet_another_gps_publisher : public rclcpp::Node std::string waypoint_file_topic_; std::string utm_frame_id_; std::string odom_frame_id_; + std::string waypoint_file_path; // Subscribers rclcpp::Subscription::SharedPtr odom_sub_; diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index 9f0ec32..5548a0d 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -1,23 +1,19 @@ -#include "yet_another_gps_publisher/spline_factory.hpp" #include #include +#include "yet_another_gps_publisher/spline_factory.hpp" + namespace gps_waypoint_spline { // Factory registry -std::map& SplineFactory::registry() -{ +std::map& SplineFactory::registry() { static std::map reg; return reg; } -void SplineFactory::registerGenerator(const std::string& name, SplineGenerator gen) -{ - registry()[name] = gen; -} +void SplineFactory::registerGenerator(const std::string& name, SplineGenerator gen) { registry()[name] = gen; } -SplineGenerator SplineFactory::getGenerator(const std::string& name) -{ +SplineGenerator SplineFactory::getGenerator(const std::string& name) { auto it = registry().find(name); if (it == registry().end()) { throw std::runtime_error("Unknown spline method: " + name); @@ -25,11 +21,8 @@ SplineGenerator SplineFactory::getGenerator(const std::string& name) return it->second; } -std::vector SplineFactory::generate( - const std::string& name, - const gps_waypoint& start, - const gps_waypoint& end) -{ +std::vector SplineFactory::generate(const std::string& name, const gps_waypoint& start, + const gps_waypoint& end) { return getGenerator(name)(start, end); } @@ -37,10 +30,7 @@ std::vector SplineFactory::generate( // Concrete generators // ------------------------------------------------------------------ -static std::vector linearGenerator( - const gps_waypoint& start, - const gps_waypoint& end) -{ +static std::vector linearGenerator(const gps_waypoint& start, const gps_waypoint& end) { const auto& a = start.odomPose().position; const auto& b = end.odomPose().position; @@ -60,10 +50,7 @@ static std::vector linearGenerator( return points; } -static std::vector circleGenerator( - const gps_waypoint& start, - const gps_waypoint& end) -{ +static std::vector circleGenerator(const gps_waypoint& start, const gps_waypoint& end) { double R = end.radius(); if (R <= 0.0) { return linearGenerator(start, end); diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 92e00a5..02105b3 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -1,56 +1,54 @@ #include "yet_another_gps_publisher/yet_another_gps_publisher_node.hpp" -#include "yet_another_gps_publisher/spline_factory.hpp" -#include -#include -#include + #include +#include #include +#include +#include + +#include "yet_another_gps_publisher/spline_factory.hpp" using namespace std::placeholders; // Constructor 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_) -{ + : Node("yet_another_gps_publisher", options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // Declare parameters min_spline_length_ = this->declare_parameter("min_spline_length", 10.0); odom_topic_ = this->declare_parameter("odom_topic", "/odometry/filtered"); - waypoint_file_topic_ = this->declare_parameter("waypoint_file_topic", "/waypoint_file"); utm_frame_id_ = this->declare_parameter("utm_frame_id", "utm"); odom_frame_id_ = this->declare_parameter("odom_frame_id", "odom"); + // TODO actually set this parameter from launch file or command line, not hardcoded. + waypoint_file_path = this->declare_parameter("waypoint_file", "waypoints.txt"); // Subscribers odom_sub_ = this->create_subscription( odom_topic_, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, _1)); - waypoint_file_sub_ = this->create_subscription( - waypoint_file_topic_, 1, std::bind(&yet_another_gps_publisher::waypoint_file_callback, this, _1)); // Publisher path_pub_ = this->create_publisher("spline_path", 10); // Timer (1 Hz) - timer_ = this->create_wall_timer( - std::chrono::seconds(1), std::bind(&yet_another_gps_publisher::timer_callback, this)); + timer_ = + this->create_wall_timer(std::chrono::seconds(1), std::bind(&yet_another_gps_publisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "yet_another_gps_publisher started"); + + // Load waypoints directly on startup! + load_waypoints(waypoint_file_path); } // Odom callback -void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) -{ +void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_pose_ = msg->pose.pose; } -// Waypoint file callback -void yet_another_gps_publisher::waypoint_file_callback(const std_msgs::msg::String::SharedPtr msg) -{ - std::string file_path = msg->data; +// Normal function to load waypoints from file on startup +bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { std::ifstream file(file_path); if (!file.is_open()) { RCLCPP_ERROR(this->get_logger(), "Could not open file: %s", file_path.c_str()); - return; + return false; } std::string line; @@ -61,18 +59,19 @@ void yet_another_gps_publisher::waypoint_file_callback(const std_msgs::msg::Stri std::istringstream iss(line); double lon, lat, radius = 0.0; - std::string method; - if (!(iss >> lon >> lat >> method)) { + std::string spline_type; + + if (!(iss >> lon >> lat >> spline_type)) { RCLCPP_WARN(this->get_logger(), "Skipping malformed line %d", line_num); continue; } - if (method == "circle") { + if (spline_type == "circle") { if (!(iss >> radius)) { RCLCPP_WARN(this->get_logger(), "Circle method on line %d missing radius, using default 0", line_num); } } - gps_waypoint wp(lon, lat, method, radius); + gps_waypoint wp(lon, lat, spline_type, radius); // Transform waypoint to odom frame if (!transformWaypoint(wp)) { @@ -81,24 +80,24 @@ void yet_another_gps_publisher::waypoint_file_callback(const std_msgs::msg::Stri } waypoints_.push_back(wp); - RCLCPP_INFO(this->get_logger(), "Loaded waypoint %zu: method=%s at (%.6f, %.6f)", - waypoints_.size(), method.c_str(), lon, lat); + RCLCPP_INFO(this->get_logger(), "Loaded waypoint %zu: spline_type=%s at (%.6f, %.6f)", waypoints_.size(), + spline_type.c_str(), lon, lat); } file.close(); + return true; } // Transform waypoint from lat/lon to odom -bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) -{ +bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { // Create a GeoPoint from lat/lon geographic_msgs::msg::GeoPoint geo; geo.latitude = wp.latitude(); geo.longitude = wp.longitude(); - geo.altitude = 0.0; // assume ground level; could be extended + geo.altitude = 0.0; // assume ground level; could be extended // Convert to UTM using geodesy geodesy::UTMPoint utm; - geodesy::fromMsg(geo, utm); // This populates easting, northing, zone, etc. + geodesy::fromMsg(geo, utm); // This populates easting, northing, zone, etc. geometry_msgs::msg::PoseStamped utm_pose; utm_pose.header.frame_id = utm_frame_id_; @@ -119,8 +118,7 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) } // Timer callback: generate and publish spline -void yet_another_gps_publisher::timer_callback() -{ +void yet_another_gps_publisher::timer_callback() { if (waypoints_.empty()) { return; } @@ -146,14 +144,14 @@ void yet_another_gps_publisher::timer_callback() const auto& wp = waypoints_[i]; if (!wp.enabled()) continue; - const gps_waypoint& start_ref = (i == 0) ? current_wp : waypoints_[i-1]; + const gps_waypoint& start_ref = (i == 0) ? current_wp : waypoints_[i - 1]; std::vector segment; try { segment = gps_waypoint_spline::SplineFactory::generate(wp.method(), start_ref, wp); } catch (const std::exception& e) { - RCLCPP_ERROR(this->get_logger(), "Spline generation failed for method %s: %s", - wp.method().c_str(), e.what()); + RCLCPP_ERROR(this->get_logger(), "Spline generation failed for method %s: %s", wp.method().c_str(), + e.what()); break; } @@ -167,7 +165,7 @@ void yet_another_gps_publisher::timer_callback() // Update cumulative length for (size_t j = 1; j < segment.size(); ++j) { - const auto& a = segment[j-1].position; + const auto& a = segment[j - 1].position; const auto& b = segment[j].position; cumulative_length += std::hypot(b.x - a.x, b.y - a.y); } @@ -181,20 +179,19 @@ void yet_another_gps_publisher::timer_callback() if (cumulative_length >= min_spline_length_) { path_pub_->publish(path); - RCLCPP_INFO(this->get_logger(), "Published spline path, length = %.2f m using %zu waypoints", - cumulative_length, used_count); + RCLCPP_INFO(this->get_logger(), "Published spline path, length = %.2f m using %zu waypoints", cumulative_length, + used_count); // Optionally remove used waypoints: // waypoints_.erase(waypoints_.begin(), waypoints_.begin() + used_count); } else { - RCLCPP_DEBUG(this->get_logger(), "Path too short (%.2f < %.2f), not publishing", - cumulative_length, min_spline_length_); + RCLCPP_DEBUG(this->get_logger(), "Path too short (%.2f < %.2f), not publishing", cumulative_length, + min_spline_length_); } } // gps_waypoint constructor implementation gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) - : longitude_(lon), latitude_(lat), method_(method), radius_(radius), enabled_(true) -{} + : longitude_(lon), latitude_(lat), method_(method), radius_(radius), enabled_(true) {} // Register node as a component #include "rclcpp_components/register_node_macro.hpp" From f72062602a90a687c1efc13db614efbc73a5b71f Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 18 Feb 2026 23:56:00 -0500 Subject: [PATCH 07/65] =?UTF-8?q?adding=20in=20the=20load=20waypoints=20to?= =?UTF-8?q?=20hpp=20again=20=F0=9F=98=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../yet_another_gps_publisher_node.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 aa83b8f..229ca4e 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 @@ -48,9 +48,11 @@ class yet_another_gps_publisher : public rclcpp::Node { // Callbacks void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - void waypoint_file_callback(const std_msgs::msg::String::SharedPtr msg); void timer_callback(); + // helper to load waypoints from file + bool load_waypoints(const std::string& file_path); + // Helper: transform a waypoint from lat/lon to odom frame using TF bool transformWaypoint(gps_waypoint& wp); }; \ No newline at end of file From dccdac66e2aef22568c718882dcf64a1d23abce3 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Feb 2026 00:07:37 -0500 Subject: [PATCH 08/65] TODO comments lol --- src/yet_another_gps_publisher_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 02105b3..1e6e44e 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -29,6 +29,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& path_pub_ = this->create_publisher("spline_path", 10); // Timer (1 Hz) + // TODO remove this the call back will be the navsat transform from the gps lol. timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&yet_another_gps_publisher::timer_callback, this)); @@ -182,6 +183,7 @@ void yet_another_gps_publisher::timer_callback() { RCLCPP_INFO(this->get_logger(), "Published spline path, length = %.2f m using %zu waypoints", cumulative_length, used_count); // Optionally remove used waypoints: + // DO NOT REMOVE WATPOINTS CHAT // waypoints_.erase(waypoints_.begin(), waypoints_.begin() + used_count); } else { RCLCPP_DEBUG(this->get_logger(), "Path too short (%.2f < %.2f), not publishing", cumulative_length, @@ -194,5 +196,6 @@ gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, do : longitude_(lon), latitude_(lat), method_(method), radius_(radius), enabled_(true) {} // Register node as a component +// todo chat why are we evening using this #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) \ No newline at end of file From 8d07dc7ef67ec080355f796b17eb04a61cd43d16 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Feb 2026 00:59:54 -0500 Subject: [PATCH 09/65] =?UTF-8?q?updates=20to=20the=20slop=20=F0=9F=98=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/yet_another_gps_publisher_node.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 1e6e44e..bdc6e81 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -8,8 +8,6 @@ #include "yet_another_gps_publisher/spline_factory.hpp" -using namespace std::placeholders; - // Constructor 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_) { @@ -23,19 +21,20 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // Subscribers odom_sub_ = this->create_subscription( - odom_topic_, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, _1)); + odom_topic_, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, std::placeholders::_1)); // Publisher - path_pub_ = this->create_publisher("spline_path", 10); + path_pub_ = this->create_publisher("/path", 5); // Timer (1 Hz) - // TODO remove this the call back will be the navsat transform from the gps lol. + // TODO remove this the call back will be the navsat transform from the gps lol. timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&yet_another_gps_publisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "yet_another_gps_publisher started"); // Load waypoints directly on startup! + // TODO catch failure and maybe retry later if file not found, instead of just crashing or doing nothing. load_waypoints(waypoint_file_path); } @@ -44,7 +43,7 @@ void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::Sha current_pose_ = msg->pose.pose; } -// Normal function to load waypoints from file on startup +// standard function to load waypoints from file on startup bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { std::ifstream file(file_path); if (!file.is_open()) { @@ -94,7 +93,8 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { geographic_msgs::msg::GeoPoint geo; geo.latitude = wp.latitude(); geo.longitude = wp.longitude(); - geo.altitude = 0.0; // assume ground level; could be extended + geo.altitude = 0.0; // assume ground level. Will not support altiude, ever. + // this is a GROUND nav robot lmao -redtoo // Convert to UTM using geodesy geodesy::UTMPoint utm; @@ -196,6 +196,6 @@ gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, do : longitude_(lon), latitude_(lat), method_(method), radius_(radius), enabled_(true) {} // Register node as a component -// todo chat why are we evening using this +// todo chat why are we evening using this #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) \ No newline at end of file From e9480317757316ef3153bceef6834e88e2202e94 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 26 Feb 2026 19:12:13 -0500 Subject: [PATCH 10/65] removing _; because I dont like them --- .../gps_waypoint.hpp | 14 +++++----- .../yet_another_gps_publisher_node.hpp | 26 +++++++++---------- src/yet_another_gps_publisher_node.cpp | 6 ++--- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index ed9f1aa..2527257 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -8,16 +8,16 @@ class gps_waypoint { gps_waypoint() = default; gps_waypoint(double lon, double lat, const std::string& method, double radius = 0.0); - double longitude() const { return longitude_; } - double latitude() const { return latitude_; } - const std::string& method() const { return method_; } - double radius() const { return radius_; } - bool enabled() const { return enabled_; } + double longitude() const { return longitude; } + double latitude() const { return latitude; } + const std::string& method() const { return method; } + double radius() const { return radius; } + bool enabled() const { return enabled; } void setEnabled(bool e) { enabled_ = e; } // Odom pose after transformation (set when waypoint is loaded) void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose_ = pose; } - const geometry_msgs::msg::Pose& odomPose() const { return odom_pose_; } + const geometry_msgs::msg::Pose& odomPose() const { return odom_pose; } private: double longitude_ = 0.0; @@ -25,5 +25,5 @@ class gps_waypoint { std::string method_ = "linear"; double radius_ = 0.0; bool enabled_ = true; - geometry_msgs::msg::Pose odom_pose_; + geometry_msgs::msg::Pose odom_pose; }; \ 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 229ca4e..d70d25f 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 @@ -19,32 +19,32 @@ class yet_another_gps_publisher : public rclcpp::Node { private: // Parameters - double min_spline_length_; - std::string odom_topic_; - std::string waypoint_file_topic_; - std::string utm_frame_id_; - std::string odom_frame_id_; + double min_spline_length; + std::string odom_topic; + std::string waypoint_file_topic; + std::string utm_frame_id; + std::string odom_frame_id; std::string waypoint_file_path; // Subscribers - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr waypoint_file_sub_; + rclcpp::Subscription::SharedPtr odom_sub; + rclcpp::Subscription::SharedPtr waypoint_file_sub; // Publisher - rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr path_pub; // Timer for periodic spline generation - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr timer; // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener; // Current robot pose in odom frame (from odometry) - geometry_msgs::msg::Pose current_pose_; + geometry_msgs::msg::Pose current_pose; // List of pending waypoints (already transformed to odom) - std::deque waypoints_; + std::deque waypoints; // Callbacks void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index bdc6e81..b88d0f1 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -101,7 +101,7 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { geodesy::fromMsg(geo, utm); // This populates easting, northing, zone, etc. geometry_msgs::msg::PoseStamped utm_pose; - utm_pose.header.frame_id = utm_frame_id_; + utm_pose.header.frame_id = utm_frame_id; utm_pose.header.stamp = this->now(); utm_pose.pose.position.x = utm.easting; utm_pose.pose.position.y = utm.northing; @@ -125,13 +125,13 @@ void yet_another_gps_publisher::timer_callback() { } nav_msgs::msg::Path path; - path.header.frame_id = odom_frame_id_; + path.header.frame_id = odom_frame_id; path.header.stamp = this->now(); // Start with current pose geometry_msgs::msg::PoseStamped start_pose; start_pose.header = path.header; - start_pose.pose = current_pose_; + start_pose.pose = current_pose; path.poses.push_back(start_pose); double cumulative_length = 0.0; From 0f8e4e2de8c20ff3675af6fef9aff7bbc4e9e72e Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 26 Feb 2026 20:24:50 -0500 Subject: [PATCH 11/65] DOCS with leon --- README.md | 33 ++++++++++++--------------------- 1 file changed, 12 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 8870800..0b424ca 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,5 @@ An opinionated ROS2 C++ node template, optimised for ISC. -# Instructions - -1. Clone repo inside your workspaces src directory (Ex. phnx_ws/src) -2. `rosdep install --from-paths . --ignore-src -r -y` to install deps -3. `colcon build` to make sure the repo builds before you mess with it -4. Replace the following in both file names and code exactly and consistently. - 1. yet_another_gps_publisher: Replace with the package name. Use snake case. Ex. `data_logger` - 2. yet_another_gps_publisher: Replace with the node name. Use Pascal case. Ex. `DataLogger` -5. `colcon build` again. If it builds, you are done -6. Rename outer folder -7. Review the optional dependencies, and remove what you do not need - # Dependencies Some common extra dependencies are included. Review them and remove what you don't need. These are marked with yet_another_gps_publisher. @@ -29,15 +17,18 @@ These are marked with yet_another_gps_publisher. ``` . ├── include -│   └── yet_another_gps_publisher -│   └── yet_another_gps_publisher_node.hpp -├── package.xml -├── README.md -├── src -│   ├── yet_another_gps_publisher.cpp -│   └── yet_another_gps_publisher.cpp -└── tests - └── unit.cpp +│ └── yet_another_gps_publisher +│ ├── gps_waypoint.hpp // this is the header for the GPS CLASSES +│ ├── spline_factory.hpp // this holds the spline generation methonds +│ └── yet_another_gps_publisher_node.hpp // this is the header for teh node specficlly. +├── package.xml // ros building files +├── README.md // this file 😆 +├── src // the main file for logicing codes +│ ├── spline_methods.cpp // this holds the spline generation methonds +│ ├── yet_another_gps_publisher.cpp // this holds the main logic for the node. THis is where the callbacks are +│ └── yet_another_gps_publisher_node.cpp // this is file that ROS launches. +└── tests // we donts use these tbh + └── unit.cpp ``` yet_another_gps_publisher_NODE_NAME_node: Source files for the ROS2 node object itself, and only itself From 0d6979c56982de578a7fa585a448a2a932063347 Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 27 Feb 2026 00:16:31 -0500 Subject: [PATCH 12/65] removing unused class options --- include/yet_another_gps_publisher/gps_waypoint.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 2527257..1db783f 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -12,8 +12,6 @@ class gps_waypoint { double latitude() const { return latitude; } const std::string& method() const { return method; } double radius() const { return radius; } - bool enabled() const { return enabled; } - void setEnabled(bool e) { enabled_ = e; } // Odom pose after transformation (set when waypoint is loaded) void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose_ = pose; } From cf0ee0864a17566b1e87a8aa8b22e65eecf0ce91 Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 27 Feb 2026 00:16:51 -0500 Subject: [PATCH 13/65] Spline Comments --- include/yet_another_gps_publisher/spline_factory.hpp | 1 + src/spline_methods.cpp | 11 ++++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/include/yet_another_gps_publisher/spline_factory.hpp b/include/yet_another_gps_publisher/spline_factory.hpp index e9abbb0..6903830 100644 --- a/include/yet_another_gps_publisher/spline_factory.hpp +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -10,6 +10,7 @@ namespace gps_waypoint_spline { + // what does using mean compared to normal spline generator. using SplineGenerator = std::function(const gps_waypoint& start, const gps_waypoint& end)>; diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index 5548a0d..f5e4fab 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -3,6 +3,15 @@ #include "yet_another_gps_publisher/spline_factory.hpp" + +// ------------------------------------------------------------------------------------------ +// +// todo this file is for storing the splines functions we want to generate with +// we need to store the functions either here or in the GPS classes. +// up too ya'll +// +// ------------------------------------------------------------------------------------------ + namespace gps_waypoint_spline { // Factory registry @@ -34,7 +43,7 @@ static std::vector linearGenerator(const gps_waypoint& const auto& a = start.odomPose().position; const auto& b = end.odomPose().position; - const int num_points = 10; + const int num_points = 10; // TODO this shouldnt be hardcoded like EVER std::vector points; points.reserve(num_points + 1); From e00d781142cb4a7c06fc1e3b3367536a7a1eaa20 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 8 Mar 2026 14:21:19 -0400 Subject: [PATCH 14/65] formating --- .../gps_waypoint.hpp | 10 ++--- .../spline_factory.hpp | 5 ++- src/yet_another_gps_publisher_node.cpp | 44 +++++++++---------- 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 1db783f..307100a 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -14,14 +14,14 @@ class gps_waypoint { double radius() const { return radius; } // Odom pose after transformation (set when waypoint is loaded) + // TODO if we are still doing ODOM frame for storing maps or just as temp frame for controls void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose_ = pose; } const geometry_msgs::msg::Pose& odomPose() const { return odom_pose; } private: - double longitude_ = 0.0; - double latitude_ = 0.0; - std::string method_ = "linear"; - double radius_ = 0.0; - bool enabled_ = true; + double longitude = 0.0; + double latitude = 0.0; + std::string method = "linear"; + double radius = 0.0; geometry_msgs::msg::Pose odom_pose; }; \ 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 6903830..e3f86b1 100644 --- a/include/yet_another_gps_publisher/spline_factory.hpp +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -10,7 +10,10 @@ namespace gps_waypoint_spline { - // what does using mean compared to normal spline generator. +// 'using' just creates a shorthand nickname. Instead of typing out +// this massive 'std::function<...>' every time, we can just type 'SplineGenerator'. +// when called write this! +// void registerGenerator(std::string name, SplineGenerator gen); using SplineGenerator = std::function(const gps_waypoint& start, const gps_waypoint& end)>; diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index b88d0f1..0a786dd 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -12,23 +12,23 @@ 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 - min_spline_length_ = this->declare_parameter("min_spline_length", 10.0); - odom_topic_ = this->declare_parameter("odom_topic", "/odometry/filtered"); - utm_frame_id_ = this->declare_parameter("utm_frame_id", "utm"); - odom_frame_id_ = this->declare_parameter("odom_frame_id", "odom"); + min_spline_length = this->declare_parameter("min_spline_length", 10.0); + odom_topic = this->declare_parameter("odom_topic", "/odometry/filtered"); + utm_frame_id = this->declare_parameter("utm_frame_id", "utm"); + odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); // TODO actually set this parameter from launch file or command line, not hardcoded. waypoint_file_path = this->declare_parameter("waypoint_file", "waypoints.txt"); // Subscribers - odom_sub_ = this->create_subscription( - odom_topic_, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, std::placeholders::_1)); + odom_sub = this->create_subscription( + odom_topic, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, std::placeholders::_1)); // Publisher - path_pub_ = this->create_publisher("/path", 5); + path_pub = this->create_publisher("/path", 5); // Timer (1 Hz) // TODO remove this the call back will be the navsat transform from the gps lol. - timer_ = + timer = this->create_wall_timer(std::chrono::seconds(1), std::bind(&yet_another_gps_publisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "yet_another_gps_publisher started"); @@ -40,7 +40,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // Odom callback void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - current_pose_ = msg->pose.pose; + current_pose = msg->pose.pose; } // standard function to load waypoints from file on startup @@ -79,8 +79,8 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { continue; } - waypoints_.push_back(wp); - RCLCPP_INFO(this->get_logger(), "Loaded waypoint %zu: spline_type=%s at (%.6f, %.6f)", waypoints_.size(), + 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); } file.close(); @@ -109,7 +109,7 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { utm_pose.pose.orientation.w = 1.0; try { - geometry_msgs::msg::PoseStamped odom_pose = tf_buffer_.transform(utm_pose, odom_frame_id_); + geometry_msgs::msg::PoseStamped odom_pose = tf_buffer_.transform(utm_pose, odom_frame_id); wp.setOdomPose(odom_pose.pose); return true; } catch (tf2::TransformException& ex) { @@ -120,7 +120,7 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { // Timer callback: generate and publish spline void yet_another_gps_publisher::timer_callback() { - if (waypoints_.empty()) { + if (waypoints.empty()) { return; } @@ -137,15 +137,15 @@ void yet_another_gps_publisher::timer_callback() { double cumulative_length = 0.0; // Temporary waypoint for current pose (method irrelevant) gps_waypoint current_wp; - current_wp.setOdomPose(current_pose_); + current_wp.setOdomPose(current_pose); current_wp.setEnabled(true); size_t used_count = 0; - for (size_t i = 0; i < waypoints_.size(); ++i) { - const auto& wp = waypoints_[i]; + for (size_t i = 0; i < waypoints.size(); ++i) { + const auto& wp = waypoints[i]; if (!wp.enabled()) continue; - const gps_waypoint& start_ref = (i == 0) ? current_wp : waypoints_[i - 1]; + const gps_waypoint& start_ref = (i == 0) ? current_wp : waypoints[i - 1]; std::vector segment; try { @@ -173,21 +173,21 @@ void yet_another_gps_publisher::timer_callback() { used_count = i + 1; - if (cumulative_length >= min_spline_length_) { + if (cumulative_length >= min_spline_length) { break; } } - if (cumulative_length >= min_spline_length_) { - path_pub_->publish(path); + if (cumulative_length >= min_spline_length) { + path_pub->publish(path); RCLCPP_INFO(this->get_logger(), "Published spline path, length = %.2f m using %zu waypoints", cumulative_length, used_count); // Optionally remove used waypoints: // DO NOT REMOVE WATPOINTS CHAT - // waypoints_.erase(waypoints_.begin(), waypoints_.begin() + used_count); + // waypoints.erase(waypoints.begin(), waypoints.begin() + used_count); } else { RCLCPP_DEBUG(this->get_logger(), "Path too short (%.2f < %.2f), not publishing", cumulative_length, - min_spline_length_); + min_spline_length); } } From fd42cca7071edd736d80545448ff72699fe57444 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Mar 2026 22:27:43 -0400 Subject: [PATCH 15/65] FIX VarNames Clean up - added _ to the end of vars with setters/getter of the same name - removed the check for the enabled option on waypoints. - reason being we arent going to have a disable/enable system. --- .../yet_another_gps_publisher/gps_waypoint.hpp | 18 +++++++++--------- .../spline_factory.hpp | 4 ++-- .../yet_another_gps_publisher_node.hpp | 7 ++++--- src/spline_methods.cpp | 5 ++--- src/yet_another_gps_publisher_node.cpp | 4 +--- 5 files changed, 18 insertions(+), 20 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 307100a..9235c07 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -8,20 +8,20 @@ class gps_waypoint { gps_waypoint() = default; gps_waypoint(double lon, double lat, const std::string& method, double radius = 0.0); - double longitude() const { return longitude; } - double latitude() const { return latitude; } - const std::string& method() const { return method; } - double radius() const { return radius; } + double longitude() const { return longitude_; } + double latitude() const { return latitude_; } + const std::string& method() const { return method_; } + double radius() const { return radius_; } // Odom pose after transformation (set when waypoint is loaded) // TODO if we are still doing ODOM frame for storing maps or just as temp frame for controls - void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose_ = pose; } + void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose = pose; } const geometry_msgs::msg::Pose& odomPose() const { return odom_pose; } private: - double longitude = 0.0; - double latitude = 0.0; - std::string method = "linear"; - double radius = 0.0; + double longitude_ = 0.0; + double latitude_ = 0.0; + std::string method_ = "linear"; + double radius_ = 0.0; geometry_msgs::msg::Pose odom_pose; }; \ 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 e3f86b1..3b65f42 100644 --- a/include/yet_another_gps_publisher/spline_factory.hpp +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -10,9 +10,9 @@ namespace gps_waypoint_spline { -// 'using' just creates a shorthand nickname. Instead of typing out +// 'using' just creates a shorthand nickname. Instead of typing out // this massive 'std::function<...>' every time, we can just type 'SplineGenerator'. -// when called write this! +// when called write this! // void registerGenerator(std::string name, SplineGenerator gen); using SplineGenerator = std::function(const gps_waypoint& start, const gps_waypoint& end)>; 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 d70d25f..856cae5 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 @@ -37,11 +37,12 @@ class yet_another_gps_publisher : public rclcpp::Node { rclcpp::TimerBase::SharedPtr timer; // TF - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; // Current robot pose in odom frame (from odometry) - geometry_msgs::msg::Pose current_pose; + // Brace initialization zeros it out + geometry_msgs::msg::Pose current_pose{}; // List of pending waypoints (already transformed to odom) std::deque waypoints; diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index f5e4fab..f88fedb 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -3,11 +3,10 @@ #include "yet_another_gps_publisher/spline_factory.hpp" - // ------------------------------------------------------------------------------------------ // // todo this file is for storing the splines functions we want to generate with -// we need to store the functions either here or in the GPS classes. +// we need to store the functions either here or in the GPS classes. // up too ya'll // // ------------------------------------------------------------------------------------------ @@ -43,7 +42,7 @@ static std::vector linearGenerator(const gps_waypoint& const auto& a = start.odomPose().position; const auto& b = end.odomPose().position; - const int num_points = 10; // TODO this shouldnt be hardcoded like EVER + const int num_points = 10; // TODO this shouldnt be hardcoded like EVER std::vector points; points.reserve(num_points + 1); diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 0a786dd..73c2e8c 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -138,12 +138,10 @@ void yet_another_gps_publisher::timer_callback() { // Temporary waypoint for current pose (method irrelevant) gps_waypoint current_wp; current_wp.setOdomPose(current_pose); - current_wp.setEnabled(true); size_t used_count = 0; for (size_t i = 0; i < waypoints.size(); ++i) { const auto& wp = waypoints[i]; - if (!wp.enabled()) continue; const gps_waypoint& start_ref = (i == 0) ? current_wp : waypoints[i - 1]; @@ -193,7 +191,7 @@ void yet_another_gps_publisher::timer_callback() { // gps_waypoint constructor implementation gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) - : longitude_(lon), latitude_(lat), method_(method), radius_(radius), enabled_(true) {} + : longitude_(lon), latitude_(lat), method_(method), radius_(radius) {} // Register node as a component // todo chat why are we evening using this From 4f3e9a51b304973c366e934fe1ddb61ccff82a50 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Mar 2026 23:12:54 -0400 Subject: [PATCH 16/65] Added comments for parameters. - aded comments on parameters. - added example waypoint file. --- src/example_waypoints.txt | 15 +++++++++++++++ src/yet_another_gps_publisher_node.cpp | 6 ++++++ 2 files changed, 21 insertions(+) create mode 100644 src/example_waypoints.txt diff --git a/src/example_waypoints.txt b/src/example_waypoints.txt new file mode 100644 index 0000000..5559021 --- /dev/null +++ b/src/example_waypoints.txt @@ -0,0 +1,15 @@ +# GPS Waypoints Configuration +# Format: longitude latitude spline_type [radius] + +# 1. Standard linear waypoints +-83.109121 42.486211 linear +-83.110452 42.487122 cubic + +# 2. Circle waypoint with a defined radius of 15.5 +-83.111883 42.488001 circle 15.5 + +# 3. Circle waypoint missing a radius (will trigger the line 26 warning and default to 0) +-83.112994 42.489332 circle + +# 4. Another standard waypoint +-83.114105 42.490111 spline \ No newline at end of file diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 73c2e8c..fa60de0 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -12,11 +12,17 @@ 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 + + // This is the mimium size of the spline as required by the controls team. If its too short they cannot plan ahead of corners enough. min_spline_length = this->declare_parameter("min_spline_length", 10.0); + // why the odom topic is a parameter: in sim we use the filtered odometry from the sim, but on the real robot we might want to use a different topic or maybe even have it remapped from the sim topic to the real topic. odom_topic = this->declare_parameter("odom_topic", "/odometry/filtered"); + // This is the utm Frame. Keep in might dearborn and purdue have different utm zones, so this might be necessary to change when we switch between the two or where ever you are. utm_frame_id = this->declare_parameter("utm_frame_id", "utm"); + // This is the odom frame we will translate the waypoints to. odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); // 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", "waypoints.txt"); // Subscribers From a2aa4ffcbd91b94229641fafd8cf3c9c7f2fb820 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 15 Mar 2026 00:42:49 -0400 Subject: [PATCH 17/65] QoL: Added Launch Parameters to Readme - added there name and type - TODO work on the descriptions more --- README.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/README.md b/README.md index 0b424ca..d94a890 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,14 @@ These are marked with yet_another_gps_publisher. - A selection of sane lints - A single node setup in a multithreaded executor +# Parameters + +- min_spline_length This is just the minium length the spline needs to be. This is Determened by what the Controls team needs. this is a hard coding trick for now but ideally this a topic from Hybird Pure Pursuit so it can be dymatic. +- odom_topic "odom_topic" +- utm_frame_id "utm_frame_id". Keep in might this changes between Dearborn and Indina +- odom_frame_id "odom_frame_id" +- waypoint_file_path "waypoint_file" path + # File structure ``` From 9908953b5943b0da066b7c9fe750cdff2acc6475 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Wed, 18 Mar 2026 22:27:25 -0400 Subject: [PATCH 18/65] Revise README by removing dependencies and features Removed sections on dependencies and features from README. Signed-off-by: Alexander Boccaccio --- README.md | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/README.md b/README.md index d94a890..0bd30eb 100644 --- a/README.md +++ b/README.md @@ -1,16 +1,3 @@ -An opinionated ROS2 C++ node template, optimised for ISC. - -# Dependencies -Some common extra dependencies are included. Review them and remove what you don't need. -These are marked with yet_another_gps_publisher. - -# Features - -- Unit tests -- ROS-Industrial github CI (will test units and lints) -- C++ formatting via clangformat -- A selection of sane lints -- A single node setup in a multithreaded executor # Parameters @@ -43,4 +30,4 @@ 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 \ No newline at end of file +tests/unit.cpp: Example file for unit tests. This is linked to the node and ros, so both can be used From 101f4254f5270c91c22e9b3c4543bc83a889c617 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 01:10:35 -0400 Subject: [PATCH 19/65] FEAT updating headers - added new parameters and functions --- .../gps_waypoint.hpp | 9 +++++++++ .../yet_another_gps_publisher_node.hpp | 19 ++++++++++++++++--- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 9235c07..91a7e5a 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -18,10 +18,19 @@ class gps_waypoint { void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose = pose; } const geometry_msgs::msg::Pose& odomPose() const { return odom_pose; } + // Store the absolute UTM pose + void setUtmPose(const geometry_msgs::msg::PoseStamped& pose) { utm_pose_ = pose; } + geometry_msgs::msg::PoseStamped& utmPose() { return utm_pose_; } + + // Odom pose after transformation (set when waypoint is loaded) + void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose = pose; } + const geometry_msgs::msg::Pose& odomPose() const { return odom_pose; } + private: double longitude_ = 0.0; double latitude_ = 0.0; std::string method_ = "linear"; double radius_ = 0.0; + geometry_msgs::msg::PoseStamped utm_pose_; geometry_msgs::msg::Pose odom_pose; }; \ 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 856cae5..2556474 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 @@ -18,18 +18,27 @@ class yet_another_gps_publisher : public rclcpp::Node { explicit yet_another_gps_publisher(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: - // Parameters - double min_spline_length; + // Parameters for locatization and topics. std::string odom_topic; - std::string waypoint_file_topic; std::string utm_frame_id; std::string odom_frame_id; + // paramters for waypoint file loading std::string waypoint_file_path; + std::string waypoint_file_topic; + // parameters for spline configs + double min_spline_length; + /// Parameters for confidence + double max_gps_variance; // Threshold in meters squared + bool is_gps_valid = false; // Subscribers rclcpp::Subscription::SharedPtr odom_sub; rclcpp::Subscription::SharedPtr waypoint_file_sub; + // New GPS subscribers + rclcpp::Subscription::SharedPtr gps_odom_sub; + rclcpp::Subscription::SharedPtr raw_gps_sub; + // Publisher rclcpp::Publisher::SharedPtr path_pub; @@ -51,6 +60,10 @@ class yet_another_gps_publisher : public rclcpp::Node { void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); void timer_callback(); + // GPS specific Callbacks + void gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + void raw_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + // helper to load waypoints from file bool load_waypoints(const std::string& file_path); From e369cfa336b3e91ac04a6e29a12bbd592d30aa0f Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 01:11:17 -0400 Subject: [PATCH 20/65] FEAT updating to use GPS call back and covarance check - remved timer call back - now using the gps call back. - added paramter for GPS variane allowed in CM - added funcitons to check against GPS varance! y --- src/yet_another_gps_publisher_node.cpp | 142 ++++++++++++++----------- 1 file changed, 77 insertions(+), 65 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index fa60de0..3d2e0a8 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -13,8 +13,13 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& : Node("yet_another_gps_publisher", options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // Declare 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. + max_gps_variance = this->declare_parameter("max_gps_variance", 0.1); + // This is the mimium size of the spline as required by the controls team. If its too short they cannot plan ahead of corners enough. min_spline_length = this->declare_parameter("min_spline_length", 10.0); + // why the odom topic is a parameter: in sim we use the filtered odometry from the sim, but on the real robot we might want to use a different topic or maybe even have it remapped from the sim topic to the real topic. odom_topic = this->declare_parameter("odom_topic", "/odometry/filtered"); // This is the utm Frame. Keep in might dearborn and purdue have different utm zones, so this might be necessary to change when we switch between the two or where ever you are. @@ -24,20 +29,21 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // 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", "waypoints.txt"); + + // Publisher + path_pub = this->create_publisher("/path", 5); // Subscribers odom_sub = this->create_subscription( odom_topic, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, std::placeholders::_1)); - // Publisher - path_pub = this->create_publisher("/path", 5); - - // Timer (1 Hz) - // TODO remove this the call back will be the navsat transform from the gps lol. - timer = - this->create_wall_timer(std::chrono::seconds(1), std::bind(&yet_another_gps_publisher::timer_callback, this)); + // 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, _1)); - RCLCPP_INFO(this->get_logger(), "yet_another_gps_publisher started"); + // Subscribe to NavSat Transform output to trigger spline generation + gps_odom_sub = this->create_subscription( + "/odometry/gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, _1)); // Load waypoints directly on startup! // TODO catch failure and maybe retry later if file not found, instead of just crashing or doing nothing. @@ -49,6 +55,25 @@ void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::Sha current_pose = msg->pose.pose; } +// The Confidence Check + RAW GPS callback +void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + // Status < 0 means NO_FIX. + // We also check the covariance (diagonal [0] is Easting, [7] is Northing) + if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS Lost Fix!"); + is_gps_valid = false; + return; + } + + if (msg->position_covariance[0] > max_gps_variance) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS Variance too high: %f", msg->position_covariance[0]); + is_gps_valid = false; + return; + } + + is_gps_valid = true; +} + // standard function to load waypoints from file on startup bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { std::ifstream file(file_path); @@ -95,106 +120,93 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { // Transform waypoint from lat/lon to odom bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { - // Create a GeoPoint from lat/lon geographic_msgs::msg::GeoPoint geo; geo.latitude = wp.latitude(); geo.longitude = wp.longitude(); - geo.altitude = 0.0; // assume ground level. Will not support altiude, ever. - // this is a GROUND nav robot lmao -redtoo + geo.altitude = 0.0; - // Convert to UTM using geodesy geodesy::UTMPoint utm; - geodesy::fromMsg(geo, utm); // This populates easting, northing, zone, etc. + geodesy::fromMsg(geo, utm); geometry_msgs::msg::PoseStamped utm_pose; utm_pose.header.frame_id = utm_frame_id; - utm_pose.header.stamp = this->now(); + // 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; - try { - geometry_msgs::msg::PoseStamped odom_pose = tf_buffer_.transform(utm_pose, odom_frame_id); - wp.setOdomPose(odom_pose.pose); - return true; - } catch (tf2::TransformException& ex) { - RCLCPP_WARN(this->get_logger(), "Transform failed: %s", ex.what()); - return false; - } + // Save the UTM pose to the waypoint, but don't do the TF lookup yet + wp.setUtmPose(utm_pose); + return true; } -// Timer callback: generate and publish spline -void yet_another_gps_publisher::timer_callback() { - if (waypoints.empty()) { +// gps callback: generate and publish spline +void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + // Guard: Don't calculate paths if waypoints aren't loaded or GPS is unreliable + if (!is_gps_valid || waypoints.empty()) { return; } + // Update current robot pose from the GPS-corrected odometry message + current_pose = msg->pose.pose; + + // --- STEP 1: DYNAMIC TRANSFORM --- + // We transform our static UTM waypoints into the current (drifting) ODOM frame + for (auto& wp : waypoints) { + try { + // Use time 0 to get the latest available transform + wp.utmPose().header.stamp = rclcpp::Time(0); + geometry_msgs::msg::PoseStamped odom_wp = tf_buffer_.transform(wp.utmPose(), odom_frame_id); + wp.setOdomPose(odom_wp.pose); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF Link UTM->ODOM failed: %s", ex.what()); + return; + } + } + + // --- STEP 2: PATH GENERATION --- nav_msgs::msg::Path path; path.header.frame_id = odom_frame_id; - path.header.stamp = this->now(); - - // Start with current pose - geometry_msgs::msg::PoseStamped start_pose; - start_pose.header = path.header; - start_pose.pose = current_pose; - path.poses.push_back(start_pose); + path.header.stamp = msg->header.stamp; // Sync path time to the GPS update time double cumulative_length = 0.0; - // Temporary waypoint for current pose (method irrelevant) + size_t used_count = 0; + + // Start the spline from the robot's current position gps_waypoint current_wp; current_wp.setOdomPose(current_pose); - size_t used_count = 0; - for (size_t i = 0; i < waypoints.size(); ++i) { - const auto& wp = waypoints[i]; - - const gps_waypoint& start_ref = (i == 0) ? current_wp : waypoints[i - 1]; + const gps_waypoint* start_ptr = ¤t_wp; - std::vector segment; - try { - segment = gps_waypoint_spline::SplineFactory::generate(wp.method(), start_ref, wp); - } catch (const std::exception& e) { - RCLCPP_ERROR(this->get_logger(), "Spline generation failed for method %s: %s", wp.method().c_str(), - e.what()); - break; - } + for (size_t i = 0; i < waypoints.size(); ++i) { + auto segment = gps_waypoint_spline::SplineFactory::generate(waypoints[i].method(), *start_ptr, waypoints[i]); + start_ptr = &waypoints[i]; - // Add segment points (skip first to avoid duplicate with previous end) - for (size_t j = 1; j < segment.size(); ++j) { + for (const auto& pose : segment) { geometry_msgs::msg::PoseStamped ps; ps.header = path.header; - ps.pose = segment[j]; + ps.pose = pose; path.poses.push_back(ps); } - // Update cumulative length + // Calculate length of this segment to see if we've met the min_spline_length requirement for (size_t j = 1; j < segment.size(); ++j) { - const auto& a = segment[j - 1].position; - const auto& b = segment[j].position; - cumulative_length += std::hypot(b.x - a.x, b.y - a.y); + cumulative_length += std::hypot(segment[j].position.x - segment[j - 1].position.x, + segment[j].position.y - segment[j - 1].position.y); } used_count = i + 1; - - if (cumulative_length >= min_spline_length) { - break; - } + if (cumulative_length >= min_spline_length) break; } + // --- STEP 3: PUBLISH --- if (cumulative_length >= min_spline_length) { path_pub->publish(path); - RCLCPP_INFO(this->get_logger(), "Published spline path, length = %.2f m using %zu waypoints", cumulative_length, - used_count); - // Optionally remove used waypoints: - // DO NOT REMOVE WATPOINTS CHAT - // waypoints.erase(waypoints.begin(), waypoints.begin() + used_count); } else { - RCLCPP_DEBUG(this->get_logger(), "Path too short (%.2f < %.2f), not publishing", cumulative_length, - min_spline_length); + RCLCPP_DEBUG(this->get_logger(), "GPS path too short (%.2f m), waiting for more waypoints", cumulative_length); } } - // gps_waypoint constructor implementation gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) : longitude_(lon), latitude_(lat), method_(method), radius_(radius) {} From 098dda8f17ae2605fae95b381f00c438e3c14362 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 01:22:42 -0400 Subject: [PATCH 21/65] FIX code clean up --- include/yet_another_gps_publisher/gps_waypoint.hpp | 5 +---- src/yet_another_gps_publisher_node.cpp | 4 ++-- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 91a7e5a..4fa101e 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include class gps_waypoint { @@ -22,10 +23,6 @@ class gps_waypoint { void setUtmPose(const geometry_msgs::msg::PoseStamped& pose) { utm_pose_ = pose; } geometry_msgs::msg::PoseStamped& utmPose() { return utm_pose_; } - // Odom pose after transformation (set when waypoint is loaded) - void setOdomPose(const geometry_msgs::msg::Pose& pose) { odom_pose = pose; } - const geometry_msgs::msg::Pose& odomPose() const { return odom_pose; } - private: double longitude_ = 0.0; double latitude_ = 0.0; diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 3d2e0a8..f03bcec 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -39,11 +39,11 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // 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, _1)); + "/phoenix/navsat", 10, std::bind(&yet_another_gps_publisher::raw_gps_callback, this, std::placeholders::_1)); // Subscribe to NavSat Transform output to trigger spline generation gps_odom_sub = this->create_subscription( - "/odometry/gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, _1)); + "/odometry/gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, std::placeholders::_1)); // Load waypoints directly on startup! // TODO catch failure and maybe retry later if file not found, instead of just crashing or doing nothing. From e407bf9497f7cf7a0c811d27bb33975bda2b2a26 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 11:55:32 -0400 Subject: [PATCH 22/65] FIX: Readme upsates - added in fumctionally section --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 0bd30eb..ed8c629 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ +# Functionality + +This node is designed to broadcast a spline between your Eagle and a waypoint(s). This is a ground truth full stack planning and navigation Node, desigened to be used with an ackerman control setup downstream. This requires a MAP and ODOM frame and NavSat for transforms with a GNSS to robotspace. The algorythm takes in defined GPS waypoints and uses a spline generating function, which are created in a abract factory spline_methods. This allows for different type of spline gen functions to be used with mininal downstream changes. Each point will spefic a function type. # Parameters @@ -6,6 +9,7 @@ - utm_frame_id "utm_frame_id". Keep in might this changes between Dearborn and Indina - odom_frame_id "odom_frame_id" - waypoint_file_path "waypoint_file" path +- max_gps_variance so basically this is the amount of varanice allowed in gps readings between gnss readings. Ideally we dont want to be running if the GPS is jumping between METERS of points compared to odom. # File structure From 8fefd3bdfbcd141c5450a6730cc97b7612f73737 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 11:56:15 -0400 Subject: [PATCH 23/65] FIX: spelling updates in Readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ed8c629..fdcaa0e 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Functionality -This node is designed to broadcast a spline between your Eagle and a waypoint(s). This is a ground truth full stack planning and navigation Node, desigened to be used with an ackerman control setup downstream. This requires a MAP and ODOM frame and NavSat for transforms with a GNSS to robotspace. The algorythm takes in defined GPS waypoints and uses a spline generating function, which are created in a abract factory spline_methods. This allows for different type of spline gen functions to be used with mininal downstream changes. Each point will spefic a function type. +This node is designed to broadcast a spline between your Eagle and a waypoint(s). This is a ground truth full stack planning and navigation Node, desigened to be used with an Ackermann control setup downstream. This requires a MAP and ODOM frame and NavSat for transforms with a GNSS to robotspace. The algorythm takes in defined GPS waypoints and uses a spline generating function, which are created in a abstract factory spline_methods. This allows for different type of spline gen functions to be used with mininal downstream changes. Each point will specify a function type. # Parameters From 9ea87e392927187bdb9e5dab4cd14ff7428884fe Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 11:58:12 -0400 Subject: [PATCH 24/65] FIS: More spellling fixes --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index fdcaa0e..30e4330 100644 --- a/README.md +++ b/README.md @@ -18,15 +18,15 @@ This node is designed to broadcast a spline between your Eagle and a waypoint(s) ├── include │ └── yet_another_gps_publisher │ ├── gps_waypoint.hpp // this is the header for the GPS CLASSES -│ ├── spline_factory.hpp // this holds the spline generation methonds -│ └── yet_another_gps_publisher_node.hpp // this is the header for teh node specficlly. +│ ├── spline_factory.hpp // this holds the spline generation methods +│ └── yet_another_gps_publisher_node.hpp // this is the header for the node specficlly. ├── package.xml // ros building files ├── README.md // this file 😆 -├── src // the main file for logicing codes +├── src // the main file for core logic │ ├── spline_methods.cpp // this holds the spline generation methonds │ ├── yet_another_gps_publisher.cpp // this holds the main logic for the node. THis is where the callbacks are │ └── yet_another_gps_publisher_node.cpp // this is file that ROS launches. -└── tests // we donts use these tbh +└── tests // Placeholders for future unit testing └── unit.cpp ``` From 620e0fbf6e0c96addd7678119b37f3df66112fcf Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 11:59:26 -0400 Subject: [PATCH 25/65] Fix: More spelling fixes --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 30e4330..bc58882 100644 --- a/README.md +++ b/README.md @@ -4,12 +4,12 @@ This node is designed to broadcast a spline between your Eagle and a waypoint(s) # Parameters -- min_spline_length This is just the minium length the spline needs to be. This is Determened by what the Controls team needs. this is a hard coding trick for now but ideally this a topic from Hybird Pure Pursuit so it can be dymatic. +- min_spline_length This is just the minium length the spline needs to be. This is determined by what the Controls team needs. This is a hard coding trick for now but ideally this a topic from Hybird Pure Pursuit so it can be dynamic. - odom_topic "odom_topic" -- utm_frame_id "utm_frame_id". Keep in might this changes between Dearborn and Indina +- utm_frame_id "utm_frame_id". Keep in mind this changes between Dearborn and Indina - odom_frame_id "odom_frame_id" - waypoint_file_path "waypoint_file" path -- max_gps_variance so basically this is the amount of varanice allowed in gps readings between gnss readings. Ideally we dont want to be running if the GPS is jumping between METERS of points compared to odom. +- max_gps_variance so basically this is the amount of variance allowed in gps readings between gnss readings. Ideally we dont want to be running if the GPS is jumping between METERS of points compared to odom. # File structure From 7ebfa6eb2c7ce6914a9f1991a4e213569861d6ce Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 12:04:08 -0400 Subject: [PATCH 26/65] TODO Reame me topic to cover --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index bc58882..e4ee53e 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,10 @@ This node is designed to broadcast a spline between your Eagle and a waypoint(s) - waypoint_file_path "waypoint_file" path - max_gps_variance so basically this is the amount of variance allowed in gps readings between gnss readings. Ideally we dont want to be running if the GPS is jumping between METERS of points compared to odom. +# Subscribed Topics + +# Published Topics + # File structure ``` From 727fa8397f03b2f2e816dc722095a35011595392 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 9 Apr 2026 20:13:52 -0400 Subject: [PATCH 27/65] reformat to make derek happy --- .../yet_another_gps_publisher_node.hpp | 4 ++-- src/example_waypoints.txt | 2 +- src/yet_another_gps_publisher_node.cpp | 18 ++++++++++-------- 3 files changed, 13 insertions(+), 11 deletions(-) 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 2556474..b19080a 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 @@ -18,7 +18,7 @@ class yet_another_gps_publisher : public rclcpp::Node { explicit yet_another_gps_publisher(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: - // Parameters for locatization and topics. + // Parameters for locatization and topics. std::string odom_topic; std::string utm_frame_id; std::string odom_frame_id; @@ -28,7 +28,7 @@ class yet_another_gps_publisher : public rclcpp::Node { // parameters for spline configs double min_spline_length; /// Parameters for confidence - double max_gps_variance; // Threshold in meters squared + double max_gps_variance; // Threshold in meters squared bool is_gps_valid = false; // Subscribers diff --git a/src/example_waypoints.txt b/src/example_waypoints.txt index 5559021..f529760 100644 --- a/src/example_waypoints.txt +++ b/src/example_waypoints.txt @@ -12,4 +12,4 @@ -83.112994 42.489332 circle # 4. Another standard waypoint --83.114105 42.490111 spline \ No newline at end of file +-83.114105 42.490111 spline \ No newline at end of file diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index f03bcec..2fd3813 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -29,7 +29,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // 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", "waypoints.txt"); - + // Publisher path_pub = this->create_publisher("/path", 5); @@ -39,11 +39,11 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // 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)); + "/phoenix/navsat", 10, std::bind(&yet_another_gps_publisher::raw_gps_callback, this, std::placeholders::_1)); // Subscribe to NavSat Transform output to trigger spline generation gps_odom_sub = this->create_subscription( - "/odometry/gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, std::placeholders::_1)); + "/odometry/gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, std::placeholders::_1)); // Load waypoints directly on startup! // TODO catch failure and maybe retry later if file not found, instead of just crashing or doing nothing. @@ -57,7 +57,7 @@ void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::Sha // The Confidence Check + RAW GPS callback void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - // Status < 0 means NO_FIX. + // Status < 0 means NO_FIX. // We also check the covariance (diagonal [0] is Easting, [7] is Northing) if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS Lost Fix!"); @@ -66,7 +66,8 @@ void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatF } if (msg->position_covariance[0] > max_gps_variance) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS Variance too high: %f", msg->position_covariance[0]); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS Variance too high: %f", + msg->position_covariance[0]); is_gps_valid = false; return; } @@ -156,11 +157,12 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: for (auto& wp : waypoints) { try { // Use time 0 to get the latest available transform - wp.utmPose().header.stamp = rclcpp::Time(0); + wp.utmPose().header.stamp = rclcpp::Time(0); geometry_msgs::msg::PoseStamped odom_wp = tf_buffer_.transform(wp.utmPose(), odom_frame_id); wp.setOdomPose(odom_wp.pose); } catch (tf2::TransformException& ex) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF Link UTM->ODOM failed: %s", ex.what()); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF Link UTM->ODOM failed: %s", + ex.what()); return; } } @@ -168,7 +170,7 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: // --- STEP 2: PATH GENERATION --- nav_msgs::msg::Path path; path.header.frame_id = odom_frame_id; - path.header.stamp = msg->header.stamp; // Sync path time to the GPS update time + path.header.stamp = msg->header.stamp; // Sync path time to the GPS update time double cumulative_length = 0.0; size_t used_count = 0; From cb45bd1733204d5998232d0c3df51bb4c61cd53a Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 12 Apr 2026 12:45:43 -0400 Subject: [PATCH 28/65] GPS waypoints for parking lot test --- src/gps_waypoints_parking_lot_mk1.txt | 97 +++++++++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100644 src/gps_waypoints_parking_lot_mk1.txt diff --git a/src/gps_waypoints_parking_lot_mk1.txt b/src/gps_waypoints_parking_lot_mk1.txt new file mode 100644 index 0000000..39ce9e1 --- /dev/null +++ b/src/gps_waypoints_parking_lot_mk1.txt @@ -0,0 +1,97 @@ +# GPS Waypoints Configuration +# Format: longitude latitude spline_type [radius] +-83.231343 42.318858 linear +-83.231316 42.318874 linear +-83.231286 42.31889 linear +-83.231258 42.318905 linear +-83.231232 42.318915 linear +-83.231203 42.318927 linear +-83.23117 42.31894 linear +-83.231136 42.318955 linear +-83.231104 42.318971 linear +-83.23107 42.318985 linear +-83.231038 42.319001 linear +-83.231005 42.319013 linear +-83.230991 42.31902 linear +-83.230959 42.319033 linear +-83.230925 42.319047 linear +-83.23089 42.31906 linear +-83.230855 42.319074 linear +-83.230818 42.319087 linear +-83.230781 42.319103 linear +-83.230745 42.31912 linear +-83.230707 42.319136 linear +-83.230666 42.319145 linear +-83.230628 42.319132 linear +-83.230602 42.319108 linear +-83.230578 42.319082 linear +-83.23056 42.319045 linear +-83.230572 42.319017 linear +-83.230605 42.318997 linear +-83.230641 42.318982 linear +-83.230679 42.318965 linear +-83.230717 42.318947 linear +-83.230753 42.318929 linear +-83.230792 42.318915 linear +-83.23083 42.318899 linear +-83.230869 42.318884 linear +-83.230906 42.318867 linear +-83.230945 42.318852 linear +-83.230994 42.318827 linear +-83.231031 42.318812 linear +-83.231066 42.318795 linear +-83.231099 42.318778 linear +-83.231129 42.318764 linear +-83.231141 42.31874 linear +-83.231127 42.318711 linear +-83.231105 42.318683 linear +-83.231081 42.318657 linear +-83.231046 42.318644 linear +-83.231007 42.318649 linear +-83.230968 42.318661 linear +-83.23092 42.318678 linear +-83.230883 42.318693 linear +-83.230846 42.318708 linear +-83.230813 42.318723 linear +-83.230779 42.318737 linear +-83.230747 42.318747 linear +-83.230708 42.318761 linear +-83.230671 42.318776 linear +-83.230634 42.318792 linear +-83.230595 42.318809 linear +-83.230557 42.318824 linear +-83.230516 42.318839 linear +-83.230457 42.318854 linear +-83.230417 42.318839 linear +-83.230393 42.318813 linear +-83.230369 42.318784 linear +-83.230358 42.318754 linear +-83.230375 42.318724 linear +-83.230409 42.318701 linear +-83.230447 42.318681 linear +-83.230484 42.318664 linear +-83.230524 42.318649 linear +-83.230564 42.318635 linear +-83.230605 42.31862 linear +-83.230656 42.3186 linear +-83.230694 42.318585 linear +-83.230734 42.318569 linear +-83.230773 42.318557 linear +-83.230816 42.318547 linear +-83.230859 42.318542 linear +-83.230903 42.318542 linear +-83.230944 42.318549 linear +-83.23098 42.318564 linear +-83.231012 42.318583 linear +-83.231043 42.318604 linear +-83.231071 42.318627 linear +-83.231105 42.318657 linear +-83.231132 42.31868 linear +-83.231156 42.318698 linear +-83.231181 42.318719 linear +-83.231208 42.318741 linear +-83.231236 42.318761 linear +-83.231263 42.318783 linear +-83.231289 42.318804 linear +-83.231317 42.318824 linear +-83.231344 42.318841 linear From 77a52cda6f4122e370f2d08ad2b00f9de3cc6b9d Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 12 Apr 2026 13:22:49 -0400 Subject: [PATCH 29/65] added waypoint file --- src/yet_another_gps_publisher_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 2fd3813..6db9568 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -28,7 +28,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); // 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", "waypoints.txt"); + waypoint_file_path = this->declare_parameter("src", "gps_waypoints_parking_lot_mk1.txt"); // Publisher path_pub = this->create_publisher("/path", 5); From 8af48866a96d835b92712224ad893331a481cbe9 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 12 Apr 2026 13:24:29 -0400 Subject: [PATCH 30/65] added waypoint file --- src/yet_another_gps_publisher_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 6db9568..ed3d914 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -28,7 +28,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); // 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("src", "gps_waypoints_parking_lot_mk1.txt"); + waypoint_file_path = this->declare_parameter("gps_waypoints_parking_lot_mk1.txt"); // Publisher path_pub = this->create_publisher("/path", 5); From ace4fcadd22832aba38611ecbee7b228c742a162 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 12 Apr 2026 13:28:06 -0400 Subject: [PATCH 31/65] added waypoint file --- src/yet_another_gps_publisher_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index ed3d914..553125a 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -28,7 +28,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); // 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("gps_waypoints_parking_lot_mk1.txt"); + waypoint_file_path = this->declare_parameter("","gps_waypoints_parking_lot_mk1.txt"); // Publisher path_pub = this->create_publisher("/path", 5); From e403ac97e3e0a0b4362f597f6640f8701bf30ea8 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 12 Apr 2026 16:24:43 -0400 Subject: [PATCH 32/65] Added in radus fro moving past waypoints - modified odom callback to have radus check - set a paramter for monium radus --- .../yet_another_gps_publisher_node.hpp | 4 ++ src/yet_another_gps_publisher_node.cpp | 68 ++++++++++++------- 2 files changed, 46 insertions(+), 26 deletions(-) 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 b19080a..d4dbfc6 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 @@ -31,6 +31,10 @@ class yet_another_gps_publisher : public rclcpp::Node { double max_gps_variance; // Threshold in meters squared bool is_gps_valid = false; + + size_t current_waypoint_index_global = 0; + double arrival_threshold = 2.0; // Meters; adjust based on robot size/speed + // Subscribers rclcpp::Subscription::SharedPtr odom_sub; rclcpp::Subscription::SharedPtr waypoint_file_sub; diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 553125a..52c49f8 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -20,6 +20,10 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // This is the mimium size of the spline as required by the controls team. If its too short they cannot plan ahead of corners enough. min_spline_length = this->declare_parameter("min_spline_length", 10.0); + // this is the mimium radus for the kart to have considered "arrived" at a particular waypoint. + // as is the norm for ROS2 this unit is in meters. + arrival_threshold = this->declare_parameter("arrival_threshold", 2.0); + // why the odom topic is a parameter: in sim we use the filtered odometry from the sim, but on the real robot we might want to use a different topic or maybe even have it remapped from the sim topic to the real topic. odom_topic = this->declare_parameter("odom_topic", "/odometry/filtered"); // This is the utm Frame. Keep in might dearborn and purdue have different utm zones, so this might be necessary to change when we switch between the two or where ever you are. @@ -145,46 +149,57 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { // gps callback: generate and publish spline void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { // Guard: Don't calculate paths if waypoints aren't loaded or GPS is unreliable - if (!is_gps_valid || waypoints.empty()) { + if (!is_gps_valid || waypoints.empty() || current_waypoint_index_global >= waypoints.size()) { return; } // Update current robot pose from the GPS-corrected odometry message current_pose = msg->pose.pose; - // --- STEP 1: DYNAMIC TRANSFORM --- - // We transform our static UTM waypoints into the current (drifting) ODOM frame - for (auto& wp : waypoints) { + // --- STEP 1: CHECK FOR ARRIVAL --- + // Get the current target waypoint's position in Odom frame + auto& target_wp = waypoints[current_waypoint_index_global]; + double dist_to_target = std::hypot( + current_pose.position.x - target_wp.odomPose().position.x, + current_pose.position.y - target_wp.odomPose().position.y + ); + // If we are close enough, "pass" it by moving the pointer forward + if (dist_to_target < arrival_threshold) { + RCLCPP_INFO(this->get_logger(), "Passed waypoint %zu!", current_waypoint_index_global); + current_waypoint_index_global++; + + // Guard against finishing the list + if (current_waypoint_index_global >= waypoints.size()) return; + } + + // --- STEP 2: DYNAMIC TRANSFORM --- + // Note: You only really need to transform the waypoints from current_waypoint_index_global onwards + for (int i = current_waypoint_index_global; i < waypoints.size(); ++i) { try { - // Use time 0 to get the latest available transform - wp.utmPose().header.stamp = rclcpp::Time(0); - geometry_msgs::msg::PoseStamped odom_wp = tf_buffer_.transform(wp.utmPose(), odom_frame_id); - wp.setOdomPose(odom_wp.pose); + waypoints[i].utmPose().header.stamp = rclcpp::Time(0); + geometry_msgs::msg::PoseStamped odom_wp = tf_buffer_.transform(waypoints[i].utmPose(), odom_frame_id); + waypoints[i].setOdomPose(odom_wp.pose); } catch (tf2::TransformException& ex) { RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF Link UTM->ODOM failed: %s", ex.what()); - return; + return; } } - // --- STEP 2: PATH GENERATION --- + // --- STEP 3: PATH GENERATION and Satisfying min_spline_length --- nav_msgs::msg::Path path; path.header.frame_id = odom_frame_id; path.header.stamp = msg->header.stamp; // Sync path time to the GPS update time double cumulative_length = 0.0; - size_t used_count = 0; - - // Start the spline from the robot's current position - gps_waypoint current_wp; - current_wp.setOdomPose(current_pose); - - const gps_waypoint* start_ptr = ¤t_wp; + gps_waypoint start_wp; + start_wp.setOdomPose(current_pose); + const gps_waypoint* start_ptr = &start_wp; - for (size_t i = 0; i < waypoints.size(); ++i) { + // Start building from the current_waypoint_index_global forward + for (int i = current_waypoint_index_global; i < waypoints.size(); ++i) { auto segment = gps_waypoint_spline::SplineFactory::generate(waypoints[i].method(), *start_ptr, waypoints[i]); - start_ptr = &waypoints[i]; - + for (const auto& pose : segment) { geometry_msgs::msg::PoseStamped ps; ps.header = path.header; @@ -192,23 +207,24 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: path.poses.push_back(ps); } - // Calculate length of this segment to see if we've met the min_spline_length requirement - for (size_t j = 1; j < segment.size(); ++j) { - cumulative_length += std::hypot(segment[j].position.x - segment[j - 1].position.x, + // Calculate length of this segment to see if we've met the min_spline_length parameter + for (int j = 1; j < segment.size(); ++j) { + cumulative_length += std::hypot(segment[j].position.x - segment[j - 1].position.x, segment[j].position.y - segment[j - 1].position.y); } - used_count = i + 1; + start_ptr = &waypoints[i]; if (cumulative_length >= min_spline_length) break; - } + } // end for loop - // --- STEP 3: PUBLISH --- + // --- STEP 4: PUBLISH --- if (cumulative_length >= min_spline_length) { path_pub->publish(path); } else { RCLCPP_DEBUG(this->get_logger(), "GPS path too short (%.2f m), waiting for more waypoints", cumulative_length); } } + // gps_waypoint constructor implementation gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) : longitude_(lon), latitude_(lat), method_(method), radius_(radius) {} From bc4cb2b62032b93d0fb70091c179597c189902c6 Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 17 Apr 2026 22:03:52 -0400 Subject: [PATCH 33/65] refactor: generate GPS paths in map frame and transform to odom for controller - Waypoints are now stored in fixed UTM frame and dynamically transformed to map - Path generation occurs in map frame to maintain global consistency - Final path is transformed to odom frame before publishing to match controller expectations - This pattern mirrors the PolynomialPlannerAi node for consistent frame handling across the stack --- .../gps_waypoint.hpp | 5 + .../yet_another_gps_publisher_node.hpp | 7 +- src/yet_another_gps_publisher_node.cpp | 130 +++++++++++------- 3 files changed, 89 insertions(+), 53 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 4fa101e..c8600aa 100644 --- a/include/yet_another_gps_publisher/gps_waypoint.hpp +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -23,6 +23,10 @@ class gps_waypoint { void setUtmPose(const geometry_msgs::msg::PoseStamped& pose) { utm_pose_ = pose; } geometry_msgs::msg::PoseStamped& utmPose() { return utm_pose_; } + // Store the map-frame pose (for path generation) + void setMapPose(const geometry_msgs::msg::Pose& pose) { map_pose_ = pose; } + const geometry_msgs::msg::Pose& mapPose() const { return map_pose_; } + private: double longitude_ = 0.0; double latitude_ = 0.0; @@ -30,4 +34,5 @@ class gps_waypoint { double radius_ = 0.0; geometry_msgs::msg::PoseStamped utm_pose_; geometry_msgs::msg::Pose odom_pose; + geometry_msgs::msg::Pose map_pose_; }; \ 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 d4dbfc6..c79614f 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 @@ -22,6 +22,7 @@ class yet_another_gps_publisher : public rclcpp::Node { std::string odom_topic; std::string utm_frame_id; std::string odom_frame_id; + std::string map_frame_id; // paramters for waypoint file loading std::string waypoint_file_path; std::string waypoint_file_topic; @@ -30,10 +31,10 @@ class yet_another_gps_publisher : public rclcpp::Node { /// Parameters for confidence double max_gps_variance; // Threshold in meters squared bool is_gps_valid = false; + bool do_gps_variance_check = false; - - size_t current_waypoint_index_global = 0; - double arrival_threshold = 2.0; // Meters; adjust based on robot size/speed + size_t current_waypoint_index_global = 0; + double arrival_threshold = 2.0; // Meters; adjust based on robot size/speed // Subscribers rclcpp::Subscription::SharedPtr odom_sub; diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 52c49f8..4f46f47 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "yet_another_gps_publisher/spline_factory.hpp" @@ -17,22 +18,27 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // 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. max_gps_variance = this->declare_parameter("max_gps_variance", 0.1); + // true for do GPS varance check false if not. + do_gps_variance_check = this->declare_parameter("do_gps_variance_check", false); + // This is the mimium size of the spline as required by the controls team. If its too short they cannot plan ahead of corners enough. min_spline_length = this->declare_parameter("min_spline_length", 10.0); // this is the mimium radus for the kart to have considered "arrived" at a particular waypoint. - // as is the norm for ROS2 this unit is in meters. + // as is the norm for ROS2 this unit is in meters. arrival_threshold = this->declare_parameter("arrival_threshold", 2.0); // why the odom topic is a parameter: in sim we use the filtered odometry from the sim, but on the real robot we might want to use a different topic or maybe even have it remapped from the sim topic to the real topic. odom_topic = this->declare_parameter("odom_topic", "/odometry/filtered"); // This is the utm Frame. Keep in might dearborn and purdue have different utm zones, so this might be necessary to change when we switch between the two or where ever you are. utm_frame_id = this->declare_parameter("utm_frame_id", "utm"); - // This is the odom frame we will translate the waypoints to. + // This is the odom frame we will translate the spline into. odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); + // this is the MAP frame that we will store the waypoints in over time. + map_frame_id = this->declare_parameter("map_frame_id", "map"); // 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("","gps_waypoints_parking_lot_mk1.txt"); + waypoint_file_path = this->declare_parameter("gps_points_files", "gps_waypoints_parking_lot_mk1.txt"); // Publisher path_pub = this->create_publisher("/path", 5); @@ -51,6 +57,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // Load waypoints directly on startup! // TODO catch failure and maybe retry later if file not found, instead of just crashing or doing nothing. + // TODO this should happen after the GPS is loaded! load_waypoints(waypoint_file_path); } @@ -61,6 +68,12 @@ void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::Sha // The Confidence Check + RAW GPS callback 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) { + is_gps_valid = true; + return; + } + // Status < 0 means NO_FIX. // We also check the covariance (diagonal [0] is Easting, [7] is Northing) if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { @@ -153,78 +166,95 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: return; } - // Update current robot pose from the GPS-corrected odometry message - current_pose = msg->pose.pose; + // Robot pose from GPS is in map frame + geometry_msgs::msg::Pose robot_pose_map = msg->pose.pose; - // --- STEP 1: CHECK FOR ARRIVAL --- - // Get the current target waypoint's position in Odom frame - auto& target_wp = waypoints[current_waypoint_index_global]; + // Transform waypoints from UTM to map + std::vector waypoints_in_map; + for (size_t i = current_waypoint_index_global; i < waypoints.size(); ++i) { + try { + waypoints[i].utmPose().header.stamp = rclcpp::Time(0); + geometry_msgs::msg::PoseStamped map_wp = tf_buffer_.transform( + waypoints[i].utmPose(), map_frame_id, std::chrono::milliseconds(100)); + waypoints_in_map.push_back(map_wp.pose); + } catch (tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "TF UTM->MAP failed: %s", ex.what()); + return; + } + } + + // Arrival check in map frame + const auto& target_wp_map = waypoints_in_map[0]; double dist_to_target = std::hypot( - current_pose.position.x - target_wp.odomPose().position.x, - current_pose.position.y - target_wp.odomPose().position.y - ); - // If we are close enough, "pass" it by moving the pointer forward + robot_pose_map.position.x - target_wp_map.position.x, + robot_pose_map.position.y - target_wp_map.position.y); if (dist_to_target < arrival_threshold) { RCLCPP_INFO(this->get_logger(), "Passed waypoint %zu!", current_waypoint_index_global); current_waypoint_index_global++; - - // Guard against finishing the list if (current_waypoint_index_global >= waypoints.size()) return; + return; // Wait for next callback to process new target } - // --- STEP 2: DYNAMIC TRANSFORM --- - // Note: You only really need to transform the waypoints from current_waypoint_index_global onwards - for (int i = current_waypoint_index_global; i < waypoints.size(); ++i) { - try { - waypoints[i].utmPose().header.stamp = rclcpp::Time(0); - geometry_msgs::msg::PoseStamped odom_wp = tf_buffer_.transform(waypoints[i].utmPose(), odom_frame_id); - waypoints[i].setOdomPose(odom_wp.pose); - } catch (tf2::TransformException& ex) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF Link UTM->ODOM failed: %s", - ex.what()); - return; - } - } - - // --- STEP 3: PATH GENERATION and Satisfying min_spline_length --- - nav_msgs::msg::Path path; - path.header.frame_id = odom_frame_id; - path.header.stamp = msg->header.stamp; // Sync path time to the GPS update time + // Generate path in map frame + nav_msgs::msg::Path path_map; + path_map.header.frame_id = map_frame_id; + path_map.header.stamp = msg->header.stamp; double cumulative_length = 0.0; gps_waypoint start_wp; - start_wp.setOdomPose(current_pose); + + // TODO start from robot or use the raw path? + start_wp.setMapPose(robot_pose_map); const gps_waypoint* start_ptr = &start_wp; - // Start building from the current_waypoint_index_global forward - for (int i = current_waypoint_index_global; i < waypoints.size(); ++i) { - auto segment = gps_waypoint_spline::SplineFactory::generate(waypoints[i].method(), *start_ptr, waypoints[i]); - + for (size_t i = 0; i < waypoints_in_map.size(); ++i) { + gps_waypoint end_wp = waypoints[current_waypoint_index_global + i]; + end_wp.setMapPose(waypoints_in_map[i]); + auto segment = gps_waypoint_spline::SplineFactory::generate(end_wp.method(), *start_ptr, end_wp); + for (const auto& pose : segment) { geometry_msgs::msg::PoseStamped ps; - ps.header = path.header; + ps.header = path_map.header; ps.pose = pose; - path.poses.push_back(ps); + path_map.poses.push_back(ps); } - // Calculate length of this segment to see if we've met the min_spline_length parameter - for (int j = 1; j < segment.size(); ++j) { - cumulative_length += std::hypot(segment[j].position.x - segment[j - 1].position.x, - segment[j].position.y - segment[j - 1].position.y); + // Length calculation + for (size_t j = 1; j < segment.size(); ++j) { + cumulative_length += std::hypot(segment[j].position.x - segment[j-1].position.x, + segment[j].position.y - segment[j-1].position.y); } - - start_ptr = &waypoints[i]; + start_ptr = &end_wp; if (cumulative_length >= min_spline_length) break; - } // end for loop + } + + // Transform entire path to odom frame + nav_msgs::msg::Path path_odom; + try { + auto transform = tf_buffer_.lookupTransform(odom_frame_id, map_frame_id, tf2::TimePointZero); + for (const auto& pose_stamped : path_map.poses) { + geometry_msgs::msg::PoseStamped ps_out; + tf2::doTransform(pose_stamped, ps_out, transform); + ps_out.header.frame_id = odom_frame_id; + ps_out.header.stamp = path_map.header.stamp; + path_odom.poses.push_back(ps_out); + } + path_odom.header.frame_id = odom_frame_id; + path_odom.header.stamp = path_map.header.stamp; + } catch (tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "TF MAP->ODOM failed: %s", ex.what()); + return; + } - // --- STEP 4: PUBLISH --- if (cumulative_length >= min_spline_length) { - path_pub->publish(path); + path_pub->publish(path_odom); } else { - RCLCPP_DEBUG(this->get_logger(), "GPS path too short (%.2f m), waiting for more waypoints", cumulative_length); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "GPS path too short (%.2f m)", cumulative_length); } } - // gps_waypoint constructor implementation gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) : longitude_(lon), latitude_(lat), method_(method), radius_(radius) {} From c3ddb8bdafe65e5c5f437765a5b1ed43e86174f2 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 19 Apr 2026 13:32:50 -0400 Subject: [PATCH 34/65] waypoints in list to avoid floating pointer issues --- .../yet_another_gps_publisher_node.hpp | 15 ++- src/yet_another_gps_publisher_node.cpp | 111 ++++++++++++------ 2 files changed, 86 insertions(+), 40 deletions(-) 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 c79614f..7e80a14 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,8 @@ #pragma once -#include +#include // for doubly linked list +#include +#include // not used anymore, but you can keep includes #include #include "geodesy/utm.h" @@ -58,8 +60,15 @@ class yet_another_gps_publisher : public rclcpp::Node { // Brace initialization zeros it out geometry_msgs::msg::Pose current_pose{}; - // List of pending waypoints (already transformed to odom) - std::deque waypoints; + // Doubly linked list of pending waypoints (already transformed to UTM in memory) + std::list waypoint_list_; + + // Iterator pointing to the next waypoint the robot should head toward. + // Initially set to waypoint_list_.begin() after loading. + std::list::iterator current_waypoint_it_; + + // Helper to advance the iterator safely + void advance_to_next_waypoint(); // Callbacks void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 4f46f47..2ca12cf 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -55,10 +55,12 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& gps_odom_sub = this->create_subscription( "/odometry/gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, std::placeholders::_1)); - // Load waypoints directly on startup! - // TODO catch failure and maybe retry later if file not found, instead of just crashing or doing nothing. - // TODO this should happen after the GPS is loaded! - load_waypoints(waypoint_file_path); + // Load waypoints and initialize iterator + if (load_waypoints(waypoint_file_path)) { + current_waypoint_it_ = waypoint_list_.begin(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to load waypoints. Node will not publish paths."); + } } // Odom callback @@ -92,7 +94,7 @@ void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatF is_gps_valid = true; } -// standard function to load waypoints from file on startup +// Load waypoints from file into std::list bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { std::ifstream file(file_path); if (!file.is_open()) { @@ -100,6 +102,7 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { return false; } + waypoint_list_.clear(); std::string line; int line_num = 0; while (std::getline(file, line)) { @@ -136,7 +139,7 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { return true; } -// Transform waypoint from lat/lon to odom +// Transform waypoint from lat/lon to UTM and store bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { geographic_msgs::msg::GeoPoint geo; geo.latitude = wp.latitude(); @@ -159,24 +162,31 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { return true; } -// gps callback: generate and publish spline +// Advance iterator to next waypoint (safe) +void yet_another_gps_publisher::advance_to_next_waypoint() { + if (current_waypoint_it_ != waypoint_list_.end()) { + ++current_waypoint_it_; + } +} + +// GPS odom callback: generate spline path from robot to remaining waypoints void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - // Guard: Don't calculate paths if waypoints aren't loaded or GPS is unreliable - if (!is_gps_valid || waypoints.empty() || current_waypoint_index_global >= waypoints.size()) { + // Guard conditions + if (!is_gps_valid || waypoint_list_.empty() || current_waypoint_it_ == waypoint_list_.end()) { return; } // Robot pose from GPS is in map frame geometry_msgs::msg::Pose robot_pose_map = msg->pose.pose; - // Transform waypoints from UTM to map - std::vector waypoints_in_map; - for (size_t i = current_waypoint_index_global; i < waypoints.size(); ++i) { + // Transform all remaining waypoints from UTM to map (we'll need them for arrival check and spline) + std::vector> remaining_waypoints; + for (auto it = current_waypoint_it_; it != waypoint_list_.end(); ++it) { try { - waypoints[i].utmPose().header.stamp = rclcpp::Time(0); + it->utmPose().header.stamp = rclcpp::Time(0); geometry_msgs::msg::PoseStamped map_wp = tf_buffer_.transform( - waypoints[i].utmPose(), map_frame_id, std::chrono::milliseconds(100)); - waypoints_in_map.push_back(map_wp.pose); + it->utmPose(), map_frame_id, std::chrono::milliseconds(100)); + remaining_waypoints.emplace_back(&(*it), map_wp.pose); } catch (tf2::TransformException& ex) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF UTM->MAP failed: %s", ex.what()); @@ -184,35 +194,56 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: } } - // Arrival check in map frame - const auto& target_wp_map = waypoints_in_map[0]; - double dist_to_target = std::hypot( - robot_pose_map.position.x - target_wp_map.position.x, - robot_pose_map.position.y - target_wp_map.position.y); - if (dist_to_target < arrival_threshold) { - RCLCPP_INFO(this->get_logger(), "Passed waypoint %zu!", current_waypoint_index_global); - current_waypoint_index_global++; - if (current_waypoint_index_global >= waypoints.size()) return; - return; // Wait for next callback to process new target + // Skip waypoints that we've already arrived at + while (!remaining_waypoints.empty()) { + const auto& target_pose = remaining_waypoints[0].second; + double dist_to_target = std::hypot( + robot_pose_map.position.x - target_pose.position.x, + robot_pose_map.position.y - target_pose.position.y); + if (dist_to_target >= arrival_threshold) { + break; // first non-arrived waypoint found + } + // Arrived at this waypoint; advance and remove from list + RCLCPP_INFO(this->get_logger(), "Passed waypoint (arrival distance %.2f < %.2f)", + dist_to_target, arrival_threshold); + advance_to_next_waypoint(); + remaining_waypoints.erase(remaining_waypoints.begin()); + } + + // If no remaining waypoints, we're done + if (remaining_waypoints.empty()) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "All waypoints reached."); + return; } - // Generate path in map frame + // Build path by daisy-chaining splines from robot to first waypoint, then between waypoints nav_msgs::msg::Path path_map; path_map.header.frame_id = map_frame_id; path_map.header.stamp = msg->header.stamp; double cumulative_length = 0.0; - gps_waypoint start_wp; - // TODO start from robot or use the raw path? + // Create a virtual start waypoint at robot's current pose with a default method + gps_waypoint start_wp(0.0, 0.0, "line"); // use "line" method for first segment start_wp.setMapPose(robot_pose_map); - const gps_waypoint* start_ptr = &start_wp; - for (size_t i = 0; i < waypoints_in_map.size(); ++i) { - gps_waypoint end_wp = waypoints[current_waypoint_index_global + i]; - end_wp.setMapPose(waypoints_in_map[i]); - auto segment = gps_waypoint_spline::SplineFactory::generate(end_wp.method(), *start_ptr, end_wp); + gps_waypoint prev_wp = start_wp; // copy, not pointer + + for (auto& pair : remaining_waypoints) { + gps_waypoint* wp_ptr = pair.first; + const geometry_msgs::msg::Pose& map_pose = pair.second; + + // Set map pose into the waypoint for spline generation + wp_ptr->setMapPose(map_pose); + + // Generate spline segment from prev_wp to current waypoint + auto segment = gps_waypoint_spline::SplineFactory::generate(wp_ptr->method(), prev_wp, *wp_ptr); + if (segment.empty()) { + RCLCPP_WARN(this->get_logger(), "Spline generation failed, stopping chain."); + break; + } + // Add segment poses to path for (const auto& pose : segment) { geometry_msgs::msg::PoseStamped ps; ps.header = path_map.header; @@ -220,16 +251,22 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: path_map.poses.push_back(ps); } - // Length calculation + // Calculate cumulative length for (size_t j = 1; j < segment.size(); ++j) { cumulative_length += std::hypot(segment[j].position.x - segment[j-1].position.x, segment[j].position.y - segment[j-1].position.y); } - start_ptr = &end_wp; - if (cumulative_length >= min_spline_length) break; + + // Update previous waypoint for next iteration (copy) + prev_wp = *wp_ptr; + + // Stop if we've reached the minimum required path length + if (cumulative_length >= min_spline_length) { + break; + } } - // Transform entire path to odom frame + // Transform path to odom frame and publish if it works nav_msgs::msg::Path path_odom; try { auto transform = tf_buffer_.lookupTransform(odom_frame_id, map_frame_id, tf2::TimePointZero); From 7c955bb5a2eb222aa502565d129db56f9ed17f78 Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 1 May 2026 23:19:59 -0400 Subject: [PATCH 35/65] FIX: waypoints loop now instead of moving around. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Replace temporary vector with direct iteration over std::list - Use global iterator and arrival check with wrap‑around for closed circuits - Build spline path in a single forward pass (two‑pointer sliding window) - Prevent infinite loop when all waypoints are reached --- .../yet_another_gps_publisher_node.hpp | 25 ++-- src/yet_another_gps_publisher_node.cpp | 139 ++++++++++-------- 2 files changed, 85 insertions(+), 79 deletions(-) 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 7e80a14..99a95fc 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,8 +1,7 @@ #pragma once -#include // for doubly linked list +#include // for doubly linked list #include -#include // not used anymore, but you can keep includes #include #include "geodesy/utm.h" @@ -20,12 +19,12 @@ class yet_another_gps_publisher : public rclcpp::Node { explicit yet_another_gps_publisher(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: - // Parameters for locatization and topics. + // Parameters for localization and topics. std::string odom_topic; std::string utm_frame_id; std::string odom_frame_id; std::string map_frame_id; - // paramters for waypoint file loading + // parameters for waypoint file loading std::string waypoint_file_path; std::string waypoint_file_topic; // parameters for spline configs @@ -35,36 +34,33 @@ class yet_another_gps_publisher : public rclcpp::Node { bool is_gps_valid = false; bool do_gps_variance_check = false; - size_t current_waypoint_index_global = 0; - double arrival_threshold = 2.0; // Meters; adjust based on robot size/speed + double arrival_threshold = 2.0; // Meters // Subscribers rclcpp::Subscription::SharedPtr odom_sub; rclcpp::Subscription::SharedPtr waypoint_file_sub; - // New GPS subscribers + // GPS subscribers rclcpp::Subscription::SharedPtr gps_odom_sub; rclcpp::Subscription::SharedPtr raw_gps_sub; // Publisher rclcpp::Publisher::SharedPtr path_pub; - // Timer for periodic spline generation + // Timer unused ircc rclcpp::TimerBase::SharedPtr timer; // TF tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - // Current robot pose in odom frame (from odometry) - // Brace initialization zeros it out + // Current robot pose (from odometry) geometry_msgs::msg::Pose current_pose{}; - // Doubly linked list of pending waypoints (already transformed to UTM in memory) - std::list waypoint_list_; + // Doubly linked list of waypoints + std::list waypoints; - // Iterator pointing to the next waypoint the robot should head toward. - // Initially set to waypoint_list_.begin() after loading. + // Iterator to next target waypoint std::list::iterator current_waypoint_it_; // Helper to advance the iterator safely @@ -82,5 +78,6 @@ class yet_another_gps_publisher : public rclcpp::Node { bool load_waypoints(const std::string& file_path); // Helper: transform a waypoint from lat/lon to odom frame using TF + // unused ircc bool transformWaypoint(gps_waypoint& wp); }; \ No newline at end of file diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 2ca12cf..2b7c334 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -1,11 +1,11 @@ #include "yet_another_gps_publisher/yet_another_gps_publisher_node.hpp" +#include #include #include #include #include #include -#include #include "yet_another_gps_publisher/spline_factory.hpp" @@ -57,7 +57,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // Load waypoints and initialize iterator if (load_waypoints(waypoint_file_path)) { - current_waypoint_it_ = waypoint_list_.begin(); + current_waypoint_it_ = waypoints.begin(); } else { RCLCPP_ERROR(this->get_logger(), "Failed to load waypoints. Node will not publish paths."); } @@ -102,7 +102,7 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { return false; } - waypoint_list_.clear(); + waypoints.clear(); std::string line; int line_num = 0; while (std::getline(file, line)) { @@ -162,88 +162,96 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { return true; } -// Advance iterator to next waypoint (safe) +// Advance iterator void yet_another_gps_publisher::advance_to_next_waypoint() { - if (current_waypoint_it_ != waypoint_list_.end()) { + if (current_waypoint_it_ != waypoints.end()) { ++current_waypoint_it_; + // wrap around for looping since the track is a circuit + if (current_waypoint_it_ == waypoints.end()) { + current_waypoint_it_ = waypoints.begin(); + } } } -// GPS odom callback: generate spline path from robot to remaining waypoints +// generate spline path from robot until we exceed max spline void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { // Guard conditions - if (!is_gps_valid || waypoint_list_.empty() || current_waypoint_it_ == waypoint_list_.end()) { + if (!is_gps_valid || waypoints.empty()) { return; } + geometry_msgs::msg::Pose robot_postion = msg->pose.pose; - // Robot pose from GPS is in map frame - geometry_msgs::msg::Pose robot_pose_map = msg->pose.pose; + size_t checked = 0; + const size_t N = waypoints.size(); - // Transform all remaining waypoints from UTM to map (we'll need them for arrival check and spline) - std::vector> remaining_waypoints; - for (auto it = current_waypoint_it_; it != waypoint_list_.end(); ++it) { + 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; try { - it->utmPose().header.stamp = rclcpp::Time(0); - geometry_msgs::msg::PoseStamped map_wp = tf_buffer_.transform( - it->utmPose(), map_frame_id, std::chrono::milliseconds(100)); - remaining_waypoints.emplace_back(&(*it), map_wp.pose); + wp.utmPose().header.stamp = rclcpp::Time(0); + geometry_msgs::msg::PoseStamped map_wp = + tf_buffer_.transform(wp.utmPose(), map_frame_id, std::chrono::milliseconds(100)); + wp_map_pose = map_wp.pose; } catch (tf2::TransformException& ex) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, - "TF UTM->MAP failed: %s", ex.what()); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF UTM->MAP failed: %s", ex.what()); return; } - } - // Skip waypoints that we've already arrived at - while (!remaining_waypoints.empty()) { - const auto& target_pose = remaining_waypoints[0].second; - double dist_to_target = std::hypot( - robot_pose_map.position.x - target_pose.position.x, - robot_pose_map.position.y - target_pose.position.y); - if (dist_to_target >= arrival_threshold) { - break; // first non-arrived waypoint found - } - // Arrived at this waypoint; advance and remove from list - RCLCPP_INFO(this->get_logger(), "Passed waypoint (arrival distance %.2f < %.2f)", - dist_to_target, arrival_threshold); - advance_to_next_waypoint(); - remaining_waypoints.erase(remaining_waypoints.begin()); + 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 → stop skipping + + 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 no remaining waypoints, we're done - if (remaining_waypoints.empty()) { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "All waypoints reached."); + if (checked >= N) { // all waypoints reached → full lap done + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "All waypoints reached (full lap)"); return; } - // Build path by daisy-chaining splines from robot to first waypoint, then between waypoints nav_msgs::msg::Path path_map; path_map.header.frame_id = map_frame_id; path_map.header.stamp = msg->header.stamp; double cumulative_length = 0.0; - // Create a virtual start waypoint at robot's current pose with a default method - gps_waypoint start_wp(0.0, 0.0, "line"); // use "line" method for first segment - start_wp.setMapPose(robot_pose_map); + // TODO decided if we want to start at the back of the robot pose + 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 - gps_waypoint prev_wp = start_wp; // copy, not pointer + // the look ahead scanner. + auto segment_it = current_waypoint_it_; + size_t processed = 0; - for (auto& pair : remaining_waypoints) { - gps_waypoint* wp_ptr = pair.first; - const geometry_msgs::msg::Pose& map_pose = pair.second; + while (segment_it != waypoints.end() && cumulative_length < min_spline_length) { + gps_waypoint& wp = *segment_it; - // Set map pose into the waypoint for spline generation - wp_ptr->setMapPose(map_pose); + // Transform this waypoint to map frame + geometry_msgs::msg::Pose map_pose; + try { + wp.utmPose().header.stamp = rclcpp::Time(0); + geometry_msgs::msg::PoseStamped map_wp = + tf_buffer_.transform(wp.utmPose(), map_frame_id, std::chrono::milliseconds(100)); + map_pose = map_wp.pose; + } catch (tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "TF transform failed during spline build: %s", ex.what()); + break; + } + wp.setMapPose(map_pose); - // Generate spline segment from prev_wp to current waypoint - auto segment = gps_waypoint_spline::SplineFactory::generate(wp_ptr->method(), prev_wp, *wp_ptr); + // Generate spline segment between prev_wp and wp + auto segment = gps_waypoint_spline::SplineFactory::generate(wp.method(), prev_wp, wp); if (segment.empty()) { RCLCPP_WARN(this->get_logger(), "Spline generation failed, stopping chain."); break; } - // Add segment poses to path + // Add segment poses to the path for (const auto& pose : segment) { geometry_msgs::msg::PoseStamped ps; ps.header = path_map.header; @@ -253,26 +261,27 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: // Calculate cumulative length for (size_t j = 1; j < segment.size(); ++j) { - cumulative_length += std::hypot(segment[j].position.x - segment[j-1].position.x, - segment[j].position.y - segment[j-1].position.y); + cumulative_length += std::hypot(segment[j].position.x - segment[j - 1].position.x, + segment[j].position.y - segment[j - 1].position.y); } - // Update previous waypoint for next iteration (copy) - prev_wp = *wp_ptr; + prev_wp = wp; // shift anchor + ++segment_it; // advance the scanning pointer + processed++; - // Stop if we've reached the minimum required path length - if (cumulative_length >= min_spline_length) { - break; - } + // stop if we’ve walked the whole list without hitting the length. + // TODO decide is we still wnat to publish? + // should probably warn though. I dont see this ever being a problem though. + if (processed >= waypoints.size()) break; } - // Transform path to odom frame and publish if it works + // Transform to odom and publish nav_msgs::msg::Path path_odom; try { auto transform = tf_buffer_.lookupTransform(odom_frame_id, map_frame_id, tf2::TimePointZero); - for (const auto& pose_stamped : path_map.poses) { + for (const auto& ps : path_map.poses) { geometry_msgs::msg::PoseStamped ps_out; - tf2::doTransform(pose_stamped, ps_out, transform); + tf2::doTransform(ps, ps_out, transform); ps_out.header.frame_id = odom_frame_id; ps_out.header.stamp = path_map.header.stamp; path_odom.poses.push_back(ps_out); @@ -280,23 +289,23 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: path_odom.header.frame_id = odom_frame_id; path_odom.header.stamp = path_map.header.stamp; } catch (tf2::TransformException& ex) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, - "TF MAP->ODOM failed: %s", ex.what()); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF MAP->ODOM failed: %s", ex.what()); return; } if (cumulative_length >= min_spline_length) { path_pub->publish(path_odom); } else { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "GPS path too short (%.2f m)", cumulative_length); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS path too short (%.2f m)", + cumulative_length); } } + // gps_waypoint constructor implementation gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) : longitude_(lon), latitude_(lat), method_(method), radius_(radius) {} // Register node as a component -// todo chat why are we evening using this +// todo chat why are we evening using this here #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) \ No newline at end of file From a80bcdbf18ce959f74e8e7414761c05df8a7d3fc Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 3 May 2026 18:46:09 -0400 Subject: [PATCH 36/65] file path cnage --- src/yet_another_gps_publisher_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 2b7c334..a9b0223 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -38,7 +38,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& map_frame_id = this->declare_parameter("map_frame_id", "map"); // 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("gps_points_files", "gps_waypoints_parking_lot_mk1.txt"); + waypoint_file_path = this->declare_parameter("gps_points_files", "/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt"); // Publisher path_pub = this->create_publisher("/path", 5); @@ -308,4 +308,4 @@ gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, do // Register node as a component // todo chat why are we evening using this here #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) From cf159bf137353a74987159017d1b937c957b05dd Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 5 May 2026 15:12:11 -0400 Subject: [PATCH 37/65] comments update --- src/yet_another_gps_publisher_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index a9b0223..618807d 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -200,7 +200,7 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: 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 → stop skipping + if (dist >= arrival_threshold) break; // not arrived yet do not skipping RCLCPP_INFO(this->get_logger(), "Passed waypoint (distance %.2f < %.2f)", dist, arrival_threshold); advance_to_next_waypoint(); // ++it, wraps to begin() if at end @@ -289,6 +289,7 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: path_odom.header.frame_id = odom_frame_id; path_odom.header.stamp = path_map.header.stamp; } catch (tf2::TransformException& ex) { + // apparently the map look up has failed! RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF MAP->ODOM failed: %s", ex.what()); return; } From 7b5664ba4be3817dba09ca1ec5e09a95612c25ca Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 5 May 2026 19:07:17 -0400 Subject: [PATCH 38/65] making the starting pose in the map frame not ODOM --- src/yet_another_gps_publisher_node.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 618807d..e6f4ade 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -179,7 +179,21 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: if (!is_gps_valid || waypoints.empty()) { return; } - geometry_msgs::msg::Pose robot_postion = msg->pose.pose; + + geometry_msgs::msg::Pose robot_pose_map; + try { + geometry_msgs::msg::PoseStamped ps_in; + ps_in.header = msg->header; // frame_id from /odometry/gps (likely "odom") + ps_in.pose = msg->pose.pose; + auto ps_out = tf_buffer_.transform(ps_in, map_frame_id, std::chrono::milliseconds(100)); + robot_pose_map = ps_out.pose; + } catch (tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "TF /odometry/gps -> map failed: %s", ex.what()); + return; + } + + geometry_msgs::msg::Pose robot_postion = robot_pose_map; //msg->pose.pose; size_t checked = 0; const size_t N = waypoints.size(); From cb02e05bbc96ad2d7db8dfa41517b18e700b06f5 Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 5 May 2026 19:50:30 -0400 Subject: [PATCH 39/65] varance check updates --- src/yet_another_gps_publisher_node.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index e6f4ade..c7ee6d4 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -19,7 +19,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& max_gps_variance = this->declare_parameter("max_gps_variance", 0.1); // true for do GPS varance check false if not. - do_gps_variance_check = this->declare_parameter("do_gps_variance_check", false); + do_gps_variance_check = this->declare_parameter("do_gps_variance_check", true); // This is the mimium size of the spline as required by the controls team. If its too short they cannot plan ahead of corners enough. min_spline_length = this->declare_parameter("min_spline_length", 10.0); @@ -78,12 +78,21 @@ void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatF // Status < 0 means NO_FIX. // We also check the covariance (diagonal [0] is Easting, [7] is Northing) + // int8 STATUS_NO_FIX = -1 # unable to fix position + // int8 STATUS_FIX = 0 # unaugmented fix + // int8 STATUS_SBAS_FIX = 1 # with satellite-based augmentation + // int8 STATUS_GBAS_FIX = 2 # with ground-based augmentation + // https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatStatus.html if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS Lost Fix!"); is_gps_valid = false; return; } + if (max_gps_variance <= 0) { + return; + } + if (msg->position_covariance[0] > max_gps_variance) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS Variance too high: %f", msg->position_covariance[0]); From de818429bd2ce4c04233e2e62734fa29e9411136 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 15:23:51 -0400 Subject: [PATCH 40/65] ahh changes --- src/yet_another_gps_publisher_node.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index c7ee6d4..bf50bd5 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -19,7 +19,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& max_gps_variance = this->declare_parameter("max_gps_variance", 0.1); // true for do GPS varance check false if not. - do_gps_variance_check = this->declare_parameter("do_gps_variance_check", true); + do_gps_variance_check = this->declare_parameter("do_gps_variance_check", false); // This is the mimium size of the spline as required by the controls team. If its too short they cannot plan ahead of corners enough. min_spline_length = this->declare_parameter("min_spline_length", 10.0); @@ -29,7 +29,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& arrival_threshold = this->declare_parameter("arrival_threshold", 2.0); // why the odom topic is a parameter: in sim we use the filtered odometry from the sim, but on the real robot we might want to use a different topic or maybe even have it remapped from the sim topic to the real topic. - odom_topic = this->declare_parameter("odom_topic", "/odometry/filtered"); + odom_topic = this->declare_parameter("odom_topic", "/odometry/gps/filtered"); // This is the utm Frame. Keep in might dearborn and purdue have different utm zones, so this might be necessary to change when we switch between the two or where ever you are. utm_frame_id = this->declare_parameter("utm_frame_id", "utm"); // This is the odom frame we will translate the spline into. @@ -53,7 +53,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // Subscribe to NavSat Transform output to trigger spline generation gps_odom_sub = this->create_subscription( - "/odometry/gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, std::placeholders::_1)); + "/odometry/navsat_gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, std::placeholders::_1)); // Load waypoints and initialize iterator if (load_waypoints(waypoint_file_path)) { @@ -227,7 +227,7 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: 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; + checked++; } if (checked >= N) { // all waypoints reached → full lap done @@ -249,10 +249,12 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: // the look ahead scanner. auto segment_it = current_waypoint_it_; size_t processed = 0; + const size_t n = waypoints.size(); - while (segment_it != waypoints.end() && cumulative_length < min_spline_length) { + while (cumulative_length < min_spline_length) { gps_waypoint& wp = *segment_it; - + processed++; + // Transform this waypoint to map frame geometry_msgs::msg::Pose map_pose; try { @@ -284,8 +286,8 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: // Calculate cumulative length for (size_t j = 1; j < segment.size(); ++j) { - cumulative_length += std::hypot(segment[j].position.x - segment[j - 1].position.x, - segment[j].position.y - segment[j - 1].position.y); + cumulative_length += std::abs( std::hypot(segment[j].position.x - segment[j - 1].position.x, + segment[j].position.y - segment[j - 1].position.y) ); } prev_wp = wp; // shift anchor From 618fb5f32b3944129cae84b7bb0bc4c30ba370ca Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 15:31:07 -0400 Subject: [PATCH 41/65] bullshit fr fr --- .../yet_another_gps_publisher_node.hpp | 2 +- src/yet_another_gps_publisher_node.cpp | 40 ++++++++++--------- 2 files changed, 22 insertions(+), 20 deletions(-) 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 99a95fc..b0e906f 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 @@ -71,7 +71,7 @@ class yet_another_gps_publisher : public rclcpp::Node { void timer_callback(); // GPS specific Callbacks - void gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + void global_ekf_callback(const nav_msgs::msg::Odometry::SharedPtr msg); void raw_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); // helper to load waypoints from file diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index bf50bd5..f93e984 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -38,15 +38,15 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& map_frame_id = this->declare_parameter("map_frame_id", "map"); // 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("gps_points_files", "/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt"); + 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"); // Publisher path_pub = this->create_publisher("/path", 5); // Subscribers - odom_sub = this->create_subscription( - odom_topic, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, std::placeholders::_1)); - + // 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)); @@ -58,16 +58,17 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // Load waypoints and initialize iterator if (load_waypoints(waypoint_file_path)) { current_waypoint_it_ = waypoints.begin(); + RCLCPP_INFO(this->get_logger(), "Loaded %zu waypoints from file.", waypoints.size()); } else { - RCLCPP_ERROR(this->get_logger(), "Failed to load waypoints. Node will not publish paths."); + while (true){ + // FAIL LOUDLY + RCLCPP_ERROR(this->get_logger(), "Failed to load waypoints. Node will not publish paths."); + RCLCPP_INFO(this->get_logger(), "Loaded %zu waypoints from file.", waypoints.size()); + rclcpp::sleep_for(std::chrono::milliseconds(2000)); + } } } -// Odom callback -void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - current_pose = msg->pose.pose; -} - // The Confidence Check + RAW GPS callback void yet_another_gps_publisher::raw_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { // check if we are even using this @@ -145,6 +146,7 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { spline_type.c_str(), lon, lat); } file.close(); + if ( line_num < 5 ) return false; return true; } @@ -183,7 +185,7 @@ void yet_another_gps_publisher::advance_to_next_waypoint() { } // generate spline path from robot until we exceed max spline -void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { +void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { // Guard conditions if (!is_gps_valid || waypoints.empty()) { return; @@ -192,17 +194,17 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: geometry_msgs::msg::Pose robot_pose_map; try { geometry_msgs::msg::PoseStamped ps_in; - ps_in.header = msg->header; // frame_id from /odometry/gps (likely "odom") + ps_in.header = msg->header; // frame_id from /odometry/gps (likely "odom") ps_in.pose = msg->pose.pose; auto ps_out = tf_buffer_.transform(ps_in, map_frame_id, std::chrono::milliseconds(100)); robot_pose_map = ps_out.pose; } catch (tf2::TransformException& ex) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, - "TF /odometry/gps -> map failed: %s", ex.what()); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF /odometry/gps -> map failed: %s", + ex.what()); return; } - geometry_msgs::msg::Pose robot_postion = robot_pose_map; //msg->pose.pose; + geometry_msgs::msg::Pose robot_postion = robot_pose_map; //msg->pose.pose; size_t checked = 0; const size_t N = waypoints.size(); @@ -294,7 +296,7 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: ++segment_it; // advance the scanning pointer processed++; - // stop if we’ve walked the whole list without hitting the length. + // to stop if we’ve walked the whole list without hitting the length. // TODO decide is we still wnat to publish? // should probably warn though. I dont see this ever being a problem though. if (processed >= waypoints.size()) break; @@ -314,15 +316,15 @@ void yet_another_gps_publisher::gps_odom_callback(const nav_msgs::msg::Odometry: path_odom.header.frame_id = odom_frame_id; path_odom.header.stamp = path_map.header.stamp; } catch (tf2::TransformException& ex) { - // apparently the map look up has failed! - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF MAP->ODOM failed: %s", ex.what()); + // apparently the map look up has failed! + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF MAP->ODOM failed: %s", ex.what()); return; } if (cumulative_length >= min_spline_length) { path_pub->publish(path_odom); } else { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "GPS path too short (%.2f m)", + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", cumulative_length); } } From f2fd5b5f6c72860311b81353ea77a46fd87fcf50 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 15:55:27 -0400 Subject: [PATCH 42/65] testing changes --- src/yet_another_gps_publisher_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index f93e984..b471aca 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -53,7 +53,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // Subscribe to NavSat Transform output to trigger spline generation gps_odom_sub = this->create_subscription( - "/odometry/navsat_gps", 10, std::bind(&yet_another_gps_publisher::gps_odom_callback, this, std::placeholders::_1)); + "/odometry/navsat_gps", 10, std::bind(&yet_another_gps_publisher::global_ekf_callback, this, std::placeholders::_1)); // Load waypoints and initialize iterator if (load_waypoints(waypoint_file_path)) { @@ -325,7 +325,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr path_pub->publish(path_odom); } else { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", - cumulative_length); + (double)N); } } From da733c1a0c5e568f2caa413fb9b9d3c4750a1458 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 15:59:07 -0400 Subject: [PATCH 43/65] send over using map pose instead of the normal poses --- src/spline_methods.cpp | 8 ++++---- src/yet_another_gps_publisher_node.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index f88fedb..8c2ddde 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -39,8 +39,8 @@ std::vector SplineFactory::generate(const std::string& // ------------------------------------------------------------------ static std::vector linearGenerator(const gps_waypoint& start, const gps_waypoint& end) { - const auto& a = start.odomPose().position; - const auto& b = end.odomPose().position; + const auto& a = start.mapPose().position; + const auto& b = end.mapPose().position; const int num_points = 10; // TODO this shouldnt be hardcoded like EVER std::vector points; @@ -64,8 +64,8 @@ static std::vector circleGenerator(const gps_waypoint& return linearGenerator(start, end); } - const auto& a = start.odomPose().position; - const auto& b = end.odomPose().position; + const auto& a = start.mapPose().position; + const auto& b = end.mapPose().position; double dx = b.x - a.x; double dy = b.y - a.y; diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index b471aca..56584df 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -325,7 +325,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr path_pub->publish(path_odom); } else { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", - (double)N); + (double)cumulative_length); } } From d9825695e0996478c8bc2fce56cdb17d9f6707a1 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 16:47:05 -0400 Subject: [PATCH 44/65] RPY transforms lmaooo ig --- src/yet_another_gps_publisher_node.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 56584df..17d7dfc 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -7,6 +7,9 @@ #include #include +#include +#include + #include "yet_another_gps_publisher/spline_factory.hpp" // Constructor @@ -309,6 +312,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr for (const auto& ps : path_map.poses) { geometry_msgs::msg::PoseStamped ps_out; tf2::doTransform(ps, ps_out, transform); + ps_out.header.frame_id = odom_frame_id; ps_out.header.stamp = path_map.header.stamp; path_odom.poses.push_back(ps_out); @@ -322,7 +326,20 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr } if (cumulative_length >= min_spline_length) { + // Apply +90 deg pitch (rotate around Y axis) + tf2::Quaternion pitch_rotation; + pitch_rotation.setRPY(0.0, M_PI / 2.0, 0.0); // roll=0, pitch=+90°, yaw=0 + + for (auto& pose_stamped : path_odom.poses) { + tf2::Quaternion original_q, rotated_q; + tf2::fromMsg(pose_stamped.pose.orientation, original_q); + rotated_q = pitch_rotation * original_q; // rotate the heading by +90° pitch + pose_stamped.pose.orientation = tf2::toMsg(rotated_q); + } + + // Now publish the rotated path path_pub->publish(path_odom); + } else { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", (double)cumulative_length); From ff97d4a1798001c675e59bd27c6d36766b4f7b4e Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 9 May 2026 15:25:46 -0400 Subject: [PATCH 45/65] path origin is now robot origin not map origin TEST --- src/yet_another_gps_publisher_node.cpp | 58 ++++++++++++++------------ 1 file changed, 31 insertions(+), 27 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 17d7dfc..ee002ec 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -1,13 +1,13 @@ #include "yet_another_gps_publisher/yet_another_gps_publisher_node.hpp" +#include + #include #include #include #include #include #include - -#include #include #include "yet_another_gps_publisher/spline_factory.hpp" @@ -49,21 +49,22 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& path_pub = this->create_publisher("/path", 5); // 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)); // Subscribe to NavSat Transform output to trigger spline generation gps_odom_sub = this->create_subscription( - "/odometry/navsat_gps", 10, std::bind(&yet_another_gps_publisher::global_ekf_callback, this, std::placeholders::_1)); + "/odometry/navsat_gps", 10, + std::bind(&yet_another_gps_publisher::global_ekf_callback, this, std::placeholders::_1)); // Load waypoints and initialize iterator if (load_waypoints(waypoint_file_path)) { current_waypoint_it_ = waypoints.begin(); RCLCPP_INFO(this->get_logger(), "Loaded %zu waypoints from file.", waypoints.size()); } else { - while (true){ + while (true) { // FAIL LOUDLY RCLCPP_ERROR(this->get_logger(), "Failed to load waypoints. Node will not publish paths."); RCLCPP_INFO(this->get_logger(), "Loaded %zu waypoints from file.", waypoints.size()); @@ -149,7 +150,7 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { spline_type.c_str(), lon, lat); } file.close(); - if ( line_num < 5 ) return false; + if (line_num < 5) return false; return true; } @@ -194,10 +195,11 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr return; } - geometry_msgs::msg::Pose robot_pose_map; + geometry_msgs::msg::Pose + robot_pose_map; // its using the pose of the center of the map ot the robots pose in the map. try { geometry_msgs::msg::PoseStamped ps_in; - ps_in.header = msg->header; // frame_id from /odometry/gps (likely "odom") + ps_in.header = msg->header; // frame_id from /odometry/gps (likely "map origin") ps_in.pose = msg->pose.pose; auto ps_out = tf_buffer_.transform(ps_in, map_frame_id, std::chrono::milliseconds(100)); robot_pose_map = ps_out.pose; @@ -258,8 +260,8 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr while (cumulative_length < min_spline_length) { gps_waypoint& wp = *segment_it; - processed++; - + processed++; + // Transform this waypoint to map frame geometry_msgs::msg::Pose map_pose; try { @@ -291,8 +293,8 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr // Calculate cumulative length for (size_t j = 1; j < segment.size(); ++j) { - cumulative_length += std::abs( std::hypot(segment[j].position.x - segment[j - 1].position.x, - segment[j].position.y - segment[j - 1].position.y) ); + cumulative_length += std::abs(std::hypot(segment[j].position.x - segment[j - 1].position.x, + segment[j].position.y - segment[j - 1].position.y)); } prev_wp = wp; // shift anchor @@ -305,40 +307,42 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr if (processed >= waypoints.size()) break; } - // Transform to odom and publish - nav_msgs::msg::Path path_odom; + // Transform to robot's body frame (e.g., base_link) so that the robot sits at (0,0) + // Using the odometry message's child_frame_id to get the correct body frame name. + std::string body_frame = msg->child_frame_id; + nav_msgs::msg::Path path_body; try { - auto transform = tf_buffer_.lookupTransform(odom_frame_id, map_frame_id, tf2::TimePointZero); + auto transform = tf_buffer_.lookupTransform(body_frame, map_frame_id, tf2::TimePointZero); for (const auto& ps : path_map.poses) { geometry_msgs::msg::PoseStamped ps_out; tf2::doTransform(ps, ps_out, transform); - - ps_out.header.frame_id = odom_frame_id; + ps_out.header.frame_id = body_frame; ps_out.header.stamp = path_map.header.stamp; - path_odom.poses.push_back(ps_out); + path_body.poses.push_back(ps_out); } - path_odom.header.frame_id = odom_frame_id; - path_odom.header.stamp = path_map.header.stamp; + path_body.header.frame_id = body_frame; + path_body.header.stamp = path_map.header.stamp; } catch (tf2::TransformException& ex) { - // apparently the map look up has failed! - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF MAP->ODOM failed: %s", ex.what()); + // TF lookup from map to body frame failed + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF MAP->%s failed: %s", body_frame.c_str(), + ex.what()); return; } if (cumulative_length >= min_spline_length) { - // Apply +90 deg pitch (rotate around Y axis) + // Apply rotoations (but we dont have too at all) tf2::Quaternion pitch_rotation; - pitch_rotation.setRPY(0.0, M_PI / 2.0, 0.0); // roll=0, pitch=+90°, yaw=0 + pitch_rotation.setRPY(0.0, M_PI / 2.0, 0.0); // roll=0, pitch=+0°, yaw=0 - for (auto& pose_stamped : path_odom.poses) { + for (auto& pose_stamped : path_body.poses) { tf2::Quaternion original_q, rotated_q; tf2::fromMsg(pose_stamped.pose.orientation, original_q); - rotated_q = pitch_rotation * original_q; // rotate the heading by +90° pitch + rotated_q = pitch_rotation * original_q; // rotate the heading by +90° pitch pose_stamped.pose.orientation = tf2::toMsg(rotated_q); } // Now publish the rotated path - path_pub->publish(path_odom); + path_pub->publish(path_body); } else { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", From b7142d60b8439a152f975df18c63a406d93db4a9 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 9 May 2026 15:38:51 -0400 Subject: [PATCH 46/65] rpy updates --- src/yet_another_gps_publisher_node.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index ee002ec..443b2b0 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -330,11 +330,13 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr } if (cumulative_length >= min_spline_length) { - // Apply rotoations (but we dont have too at all) - tf2::Quaternion pitch_rotation; - pitch_rotation.setRPY(0.0, M_PI / 2.0, 0.0); // roll=0, pitch=+0°, yaw=0 + // Apply +90 deg pitch (rotate around Y axis) + tf2::Quaternion pitch_rotation; // M_PI + pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 + + for (auto& pose_stamped : path_odom.poses) { + pose_stamped.pose.position.z = 0.0; - for (auto& pose_stamped : path_body.poses) { tf2::Quaternion original_q, rotated_q; tf2::fromMsg(pose_stamped.pose.orientation, original_q); rotated_q = pitch_rotation * original_q; // rotate the heading by +90° pitch From 0225c172b0c31de6b89792ba173ba64412225e9a Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 9 May 2026 15:42:47 -0400 Subject: [PATCH 47/65] fixing bug missnamed path var --- src/yet_another_gps_publisher_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 443b2b0..f23f55c 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -334,7 +334,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr tf2::Quaternion pitch_rotation; // M_PI pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 - for (auto& pose_stamped : path_odom.poses) { + for (auto& pose_stamped : path_body.poses) { pose_stamped.pose.position.z = 0.0; tf2::Quaternion original_q, rotated_q; From 4f82c3fa21a08c8407196b911effcdba9b54a483 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 9 May 2026 16:28:31 -0400 Subject: [PATCH 48/65] new fame id parameter for frame look ups! --- src/yet_another_gps_publisher_node.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index f23f55c..c9bfa6f 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -39,6 +39,8 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); // this is the MAP frame that we will store the waypoints in over time. map_frame_id = this->declare_parameter("map_frame_id", "map"); + // 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( @@ -201,7 +203,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr geometry_msgs::msg::PoseStamped ps_in; ps_in.header = msg->header; // frame_id from /odometry/gps (likely "map origin") ps_in.pose = msg->pose.pose; - auto ps_out = tf_buffer_.transform(ps_in, map_frame_id, std::chrono::milliseconds(100)); + auto ps_out = tf_buffer_.transform(robot_origin_frame_id, map_frame_id, std::chrono::milliseconds(100)); robot_pose_map = ps_out.pose; } catch (tf2::TransformException& ex) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF /odometry/gps -> map failed: %s", @@ -334,7 +336,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr tf2::Quaternion pitch_rotation; // M_PI pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 - for (auto& pose_stamped : path_body.poses) { + for (auto& pose_stamped : path_map.poses) { pose_stamped.pose.position.z = 0.0; tf2::Quaternion original_q, rotated_q; From 884d850db1d9cc2479528c259c152a37da9966b3 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 9 May 2026 16:42:14 -0400 Subject: [PATCH 49/65] fix --- src/yet_another_gps_publisher_node.cpp | 29 +++++++++++++------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index c9bfa6f..b2ad4b6 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -203,7 +203,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr geometry_msgs::msg::PoseStamped ps_in; ps_in.header = msg->header; // frame_id from /odometry/gps (likely "map origin") ps_in.pose = msg->pose.pose; - auto ps_out = tf_buffer_.transform(robot_origin_frame_id, map_frame_id, std::chrono::milliseconds(100)); + auto ps_out = tf_buffer_.transform(ps_in, map_frame_id, std::chrono::milliseconds(100)); robot_pose_map = ps_out.pose; } catch (tf2::TransformException& ex) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "TF /odometry/gps -> map failed: %s", @@ -309,35 +309,34 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr if (processed >= waypoints.size()) break; } - // Transform to robot's body frame (e.g., base_link) so that the robot sits at (0,0) - // Using the odometry message's child_frame_id to get the correct body frame name. - std::string body_frame = msg->child_frame_id; - nav_msgs::msg::Path path_body; + // 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; try { - auto transform = tf_buffer_.lookupTransform(body_frame, map_frame_id, tf2::TimePointZero); + auto transform = tf_buffer_.lookupTransform(odom_frame_id, map_frame_id, tf2::TimePointZero); for (const auto& ps : path_map.poses) { geometry_msgs::msg::PoseStamped ps_out; tf2::doTransform(ps, ps_out, transform); - ps_out.header.frame_id = body_frame; + ps_out.header.frame_id = odom_frame_id; ps_out.header.stamp = path_map.header.stamp; - path_body.poses.push_back(ps_out); + path_odom.poses.push_back(ps_out); } - path_body.header.frame_id = body_frame; - path_body.header.stamp = path_map.header.stamp; + path_odom.header.frame_id = odom_frame_id; + path_odom.header.stamp = path_map.header.stamp; } catch (tf2::TransformException& ex) { - // TF lookup from map to body frame failed - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF MAP->%s failed: %s", body_frame.c_str(), + // TF lookup from map to odom_frame_id (body frame) failed + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF MAP->%s failed: %s", odom_frame_id.c_str(), ex.what()); return; } if (cumulative_length >= min_spline_length) { // Apply +90 deg pitch (rotate around Y axis) - tf2::Quaternion pitch_rotation; // M_PI + tf2::Quaternion pitch_rotation; // M_PI pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 - for (auto& pose_stamped : path_map.poses) { - pose_stamped.pose.position.z = 0.0; + for (auto& pose_stamped : path_odom.poses) { + pose_stamped.pose.position.z = 0.0; tf2::Quaternion original_q, rotated_q; tf2::fromMsg(pose_stamped.pose.orientation, original_q); From efbe48155f64a1cc303132773b56e15ded3985c3 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 9 May 2026 16:44:44 -0400 Subject: [PATCH 50/65] fix --- src/yet_another_gps_publisher_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index b2ad4b6..50d25df 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -40,7 +40,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // this is the MAP frame that we will store the waypoints in over time. map_frame_id = this->declare_parameter("map_frame_id", "map"); // 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"); + // 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( @@ -345,7 +345,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr } // Now publish the rotated path - path_pub->publish(path_body); + path_pub->publish(path_odom); } else { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", From 8e7f157c9100af8121d266a28dfd6c5e6d3bec5a Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 19:16:47 -0400 Subject: [PATCH 51/65] Documentation: Annotated gps_waypoint.hpp --- include/yet_another_gps_publisher/gps_waypoint.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index c8600aa..64211a6 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,10 @@ 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 From cd8243718bd64628639a844a6d66abd42c862f63 Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 19:28:54 -0400 Subject: [PATCH 52/65] Documentation: Annotated spline_factory.hpp --- .../spline_factory.hpp | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/include/yet_another_gps_publisher/spline_factory.hpp b/include/yet_another_gps_publisher/spline_factory.hpp index 3b65f42..f0d2a74 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 geoemtry_msg::msg::Pose for points */ +#include /* for functional paradigme returing of a function */ +#include /* for map ADT {key: '', value: ''} pair */ +#include /* for method i.e 'linear', 'circular', etc.*/ +#include /* for dynamic array ADT */ #include "gps_waypoint.hpp" @@ -14,18 +14,21 @@ namespace gps_waypoint_spline { // this massive 'std::function<...>' every time, we can just type 'SplineGenerator'. // when called write this! // void registerGenerator(std::string name, SplineGenerator gen); -using SplineGenerator = - std::function(const gps_waypoint& start, const gps_waypoint& end)>; +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 void registerGenerator(const std::string& name, SplineGenerator gen); /* save function into memory */ + static SplineGenerator getGenerator(const std::string& name); /* looks up based on key */ static std::vector generate(const std::string& name, const gps_waypoint& start, - const gps_waypoint& end); + 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 From 9a13b48d7707a738c6178a838960b17c451095c4 Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 19:40:03 -0400 Subject: [PATCH 53/65] Documentation: Annotated yet_another_gps_publisher_node.hpp --- .../yet_another_gps_publisher_node.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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..6ab32de 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,25 +37,25 @@ class yet_another_gps_publisher : public rclcpp::Node { double arrival_threshold = 2.0; // Meters // Subscribers - rclcpp::Subscription::SharedPtr odom_sub; - rclcpp::Subscription::SharedPtr waypoint_file_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{}; + geometry_msgs::msg::Pose current_pose{}; // Doubly linked list of waypoints std::list waypoints; From 393fe0536255b0ec94a70064aaef2686e63e6cdd Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 19:54:38 -0400 Subject: [PATCH 54/65] Documentation: Annotated src/spline_methods.cpp --- src/spline_methods.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index 8c2ddde..9e25961 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -19,8 +19,12 @@ std::map& SplineFactory::registry() { return reg; } -void SplineFactory::registerGenerator(const std::string& name, SplineGenerator gen) { registry()[name] = gen; } +/* 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,8 +33,8 @@ SplineGenerator SplineFactory::getGenerator(const std::string& name) { return it->second; } -std::vector SplineFactory::generate(const std::string& name, const gps_waypoint& start, - const gps_waypoint& end) { +/* 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); } @@ -38,7 +42,14 @@ std::vector SplineFactory::generate(const std::string& // Concrete generators // ------------------------------------------------------------------ + 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,6 +70,11 @@ static std::vector linearGenerator(const gps_waypoint& } static std::vector circleGenerator(const gps_waypoint& start, const gps_waypoint& end) { + /* + Chord = sqrt[(x_b - x_a)^2 + (y_b - y_a)^2] + theta <- 2 * arcsin(chord/2R) + + */ double R = end.radius(); if (R <= 0.0) { return linearGenerator(start, end); From 1f8bac8d261a39aaaccf91e0c189c7657851bf70 Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 21:13:09 -0400 Subject: [PATCH 55/65] Documentation: Annotated src/yet_another_gps_publisher_node.cpp --- .../spline_factory.hpp | 2 +- src/spline_methods.cpp | 1 - src/yet_another_gps_publisher_node.cpp | 79 ++++++++++++++++--- 3 files changed, 68 insertions(+), 14 deletions(-) diff --git a/include/yet_another_gps_publisher/spline_factory.hpp b/include/yet_another_gps_publisher/spline_factory.hpp index f0d2a74..b9f204e 100644 --- a/include/yet_another_gps_publisher/spline_factory.hpp +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -23,7 +23,7 @@ using SplineGenerator = std::function(cons class SplineFactory { public: static void registerGenerator(const std::string& name, SplineGenerator gen); /* save function into memory */ - static SplineGenerator getGenerator(const std::string& name); /* looks up based on key */ + 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 */ diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index 9e25961..51a1847 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -73,7 +73,6 @@ static std::vector circleGenerator(const gps_waypoint& /* Chord = sqrt[(x_b - x_a)^2 + (y_b - y_a)^2] theta <- 2 * arcsin(chord/2R) - */ double R = end.radius(); if (R <= 0.0) { diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 50d25df..91f00cc 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -13,9 +13,12 @@ #include "yet_another_gps_publisher/spline_factory.hpp" // 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. @@ -47,11 +50,14 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& "waypoint_file_path", "/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt"); - // 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 +82,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 +118,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 +150,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 +158,77 @@ 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. + geometry_msgs::msg::Pose 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 +238,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 +252,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 +269,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 +363,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 +408,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) {} From eb2db4d1ca4ffe0cfed5d688352c9b73bfa12b4f Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 21:34:42 -0400 Subject: [PATCH 56/65] Restructure/Added: The data/ was added to store gps_waypoints.txt, WAYPOINT_FILE macro added to yagpn.cpp, Cmakelist has the fallback path for data to declare where it is --- CMakeLists.txt | 8 ++++++++ {src => data}/example_waypoints.txt | 0 {src => data}/gps_waypoints_parking_lot_mk1.txt | 0 src/yet_another_gps_publisher_node.cpp | 6 +++++- 4 files changed, 13 insertions(+), 1 deletion(-) rename {src => data}/example_waypoints.txt (100%) rename {src => data}/gps_waypoints_parking_lot_mk1.txt (100%) 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/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/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/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 91f00cc..1e56e58 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -12,6 +12,10 @@ #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) @@ -48,7 +52,7 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // 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"); + WAYPOINT_FILE); /**********************************************************/ /* PUBLISHER */ From 19fe919d7fbc21dbaebbdd7f59fb60a0e6e3d587 Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 22:17:25 -0400 Subject: [PATCH 57/65] Fixed: Implemented the circleGenerator correctly so that it is not just a linearGenerator awaiting Test Confirmation --- src/spline_methods.cpp | 70 +++++++++++++++++++++----- src/yet_another_gps_publisher_node.cpp | 2 +- 2 files changed, 59 insertions(+), 13 deletions(-) diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index 51a1847..5ea0a40 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -3,6 +3,9 @@ #include "yet_another_gps_publisher/spline_factory.hpp" +#include +#include + // ------------------------------------------------------------------------------------------ // // todo this file is for storing the splines functions we want to generate with @@ -70,27 +73,70 @@ static std::vector linearGenerator(const gps_waypoint& } static std::vector circleGenerator(const gps_waypoint& start, const gps_waypoint& end) { - /* - Chord = sqrt[(x_b - x_a)^2 + (y_b - y_a)^2] - theta <- 2 * arcsin(chord/2R) - */ - double R = end.radius(); - if (R <= 0.0) { + double R_val = end.radius(); + + if (std::abs(R_val) <= 0.001) { return linearGenerator(start, end); } - const auto& a = start.mapPose().position; - const auto& b = end.mapPose().position; + const auto &a = start.mapPose().position; + const auto &b = end.mapPose().position; double dx = b.x - a.x; 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 1e56e58..27d0b2d 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -49,7 +49,7 @@ 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? + // 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); From 50b07e35f9b27c9bfe27ea2b2a16f769d55733ce Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sat, 9 May 2026 22:48:51 -0400 Subject: [PATCH 58/65] Unit Test: The spline circleGenerator works as intended, note CmakeList for Unit Test not added as it was claude AI generated and I do not trust that --- README.md | 7 ++ data/example_waypoints_circle.txt | 6 ++ data/spline_unit_test.csv | 80 ++++++++++++++++++++++ tests/unit_spline.cpp | 106 ++++++++++++++++++++++++++++++ 4 files changed, 199 insertions(+) create mode 100644 data/example_waypoints_circle.txt create mode 100644 data/spline_unit_test.csv create mode 100644 tests/unit_spline.cpp 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/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/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/tests/unit_spline.cpp b/tests/unit_spline.cpp new file mode 100644 index 0000000..8f45f93 --- /dev/null +++ b/tests/unit_spline.cpp @@ -0,0 +1,106 @@ +#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 From 49397f06c7324b486575857a0a13345af237c248 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 10 May 2026 16:33:18 -0400 Subject: [PATCH 59/65] reformat for CI/CD --- .../gps_waypoint.hpp | 5 ++-- .../spline_factory.hpp | 21 +++++++------ .../yet_another_gps_publisher_node.hpp | 12 ++++---- src/spline_methods.cpp | 30 ++++++++----------- src/yet_another_gps_publisher_node.cpp | 18 +++++------ tests/unit_spline.cpp | 9 +++--- 6 files changed, 46 insertions(+), 49 deletions(-) diff --git a/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp index 64211a6..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 /* 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 /* 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 { @@ -35,5 +35,4 @@ class gps_waypoint { 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 b9f204e..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 /* for functional paradigme returing of a function */ #include /* for geoemtry_msg::msg::Pose for points */ -#include /* for functional paradigme returing of a function */ -#include /* for map ADT {key: '', value: ''} pair */ -#include /* for method i.e 'linear', 'circular', etc.*/ -#include /* for dynamic array ADT */ +#include /* for map ADT {key: '', value: ''} pair */ +#include /* for method i.e 'linear', 'circular', etc.*/ +#include /* for dynamic array ADT */ #include "gps_waypoint.hpp" @@ -14,7 +14,8 @@ namespace gps_waypoint_spline { // this massive 'std::function<...>' every time, we can just type 'SplineGenerator'. // when called write this! // void registerGenerator(std::string name, SplineGenerator gen); -using SplineGenerator = std::function(const gps_waypoint& start, const gps_waypoint& end)>; +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 = @@ -23,12 +24,14 @@ using SplineGenerator = std::function(cons class SplineFactory { public: 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 */ + 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(); /* {key: 'method', value: 'function()'} meyers singleton returing refrence to the map */ + 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 6ab32de..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 lists*/ +#include /* for doubly linked lists*/ #include #include @@ -38,10 +38,10 @@ class yet_another_gps_publisher : public rclcpp::Node { // Subscribers rclcpp::Subscription::SharedPtr odom_sub; /* local ekf */ - rclcpp::Subscription::SharedPtr waypoint_file_sub; + rclcpp::Subscription::SharedPtr waypoint_file_sub; // GPS subscribers - rclcpp::Subscription::SharedPtr gps_odom_sub; /* global ekf */ + rclcpp::Subscription::SharedPtr gps_odom_sub; /* global ekf */ rclcpp::Subscription::SharedPtr raw_gps_sub; /* raw gps points*/ // Publisher @@ -51,11 +51,11 @@ class yet_another_gps_publisher : public rclcpp::Node { rclcpp::TimerBase::SharedPtr timer; /* periodic callback (check if robot reached waypoint)*/ // TF - 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)*/ + 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{}; + geometry_msgs::msg::Pose current_pose{}; // Doubly linked list of waypoints std::list waypoints; diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp index 5ea0a40..f39551b 100644 --- a/src/spline_methods.cpp +++ b/src/spline_methods.cpp @@ -1,11 +1,11 @@ +#include + #include #include +#include #include "yet_another_gps_publisher/spline_factory.hpp" -#include -#include - // ------------------------------------------------------------------------------------------ // // todo this file is for storing the splines functions we want to generate with @@ -23,9 +23,7 @@ std::map& SplineFactory::registry() { } /* 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; -} +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) { @@ -37,7 +35,8 @@ SplineGenerator SplineFactory::getGenerator(const std::string& name) { } /* 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) { +std::vector SplineFactory::generate(const std::string& name, const gps_waypoint& start, + const gps_waypoint& end) { return getGenerator(name)(start, end); } @@ -45,7 +44,6 @@ std::vector SplineFactory::generate(const std::string& // Concrete generators // ------------------------------------------------------------------ - static std::vector linearGenerator(const gps_waypoint& start, const gps_waypoint& end) { /* a <- start position @@ -79,8 +77,8 @@ static std::vector circleGenerator(const gps_waypoint& return linearGenerator(start, end); } - const auto &a = start.mapPose().position; - const auto &b = end.mapPose().position; + const auto& a = start.mapPose().position; + const auto& b = end.mapPose().position; double dx = b.x - a.x; double dy = b.y - a.y; @@ -102,15 +100,11 @@ static std::vector circleGenerator(const gps_waypoint& 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; + 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; + while (diff >= 0.0) diff -= 2.0 * M_PI; + while (diff < -2.0 * M_PI) diff += 2.0 * M_PI; } double arc_length = std::abs(R_val * diff); diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 27d0b2d..5a19df2 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -13,7 +13,7 @@ #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" +#define WAYPOINT_FILE "/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt" #endif // Constructor @@ -21,7 +21,7 @@ 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_) { /**********************************************************/ - /* PARAMETERS */ + /* PARAMETERS */ /**********************************************************/ // Threshold for GPS "Confidence". 0.1 means we only trust the GPS if it's within ~30cm precision. @@ -50,17 +50,15 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& // 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? (Completed by Elijah May 9 check git logs) - waypoint_file_path = this->declare_parameter( - "waypoint_file_path", - WAYPOINT_FILE); + 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( @@ -194,7 +192,8 @@ bool yet_another_gps_publisher::transformWaypoint(gps_waypoint& wp) { 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; /* potential downstream to fix if waypoint needs to be approached at a specific angle */ + 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 */ @@ -230,7 +229,8 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr return; } - geometry_msgs::msg::Pose robot_pose_map; // its using the pose of the center of the map not the robots pose in the map. + geometry_msgs::msg::Pose + 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 { diff --git a/tests/unit_spline.cpp b/tests/unit_spline.cpp index 8f45f93..36d83b4 100644 --- a/tests/unit_spline.cpp +++ b/tests/unit_spline.cpp @@ -1,10 +1,11 @@ #include -#include + +#include #include +#include #include #include #include -#include #include "yet_another_gps_publisher/gps_waypoint.hpp" #include "yet_another_gps_publisher/spline_factory.hpp" @@ -56,7 +57,7 @@ TEST(yet_another_gps_publisher, gps_file_loads_correctly) { 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); + gps_waypoint end = make_waypoint(5.0, 5.0, "circle", 5.0); auto points = gps_waypoint_spline::SplineFactory::generate("circle", start, end); @@ -86,7 +87,7 @@ TEST(yet_another_gps_publisher, circle_generator_desmos_output) { 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 + 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); From 3c8d05538ba60f49a1abb5a7e8268fe6ab16b8ba Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 12 May 2026 01:41:40 -0400 Subject: [PATCH 60/65] data --- data/gps_waypoints_purdue_sim_mk1.txt | 172 +++++++++++ data/gps_waypoints_purdue_sim_mk2.txt | 429 ++++++++++++++++++++++++++ 2 files changed, 601 insertions(+) create mode 100644 data/gps_waypoints_purdue_sim_mk1.txt create mode 100644 data/gps_waypoints_purdue_sim_mk2.txt diff --git a/data/gps_waypoints_purdue_sim_mk1.txt b/data/gps_waypoints_purdue_sim_mk1.txt new file mode 100644 index 0000000..bb4334b --- /dev/null +++ b/data/gps_waypoints_purdue_sim_mk1.txt @@ -0,0 +1,172 @@ +-86.9443603632717 40.43766692173833 linear +-86.94433602103761 40.437655973339524 linear +-86.9443117068889 40.437644988570995 linear +-86.94428760400798 40.43763372963704 linear +-86.94426371379639 40.43762221449797 linear +-86.94423986363003 40.437610651097216 linear +-86.94421605368333 40.43759903942589 linear +-86.94419228458715 40.43758737900027 linear +-86.9441685566014 40.43757566979858 linear +-86.94414485834322 40.437563925509025 linear +-86.94412115723075 40.43755218459804 linear +-86.94409745239295 40.43754044806121 linear +-86.94407262797307 40.43753027806319 linear +-86.944045063164 40.437525355493214 linear +-86.94401686174733 40.437526810685796 linear +-86.9439901670049 40.43753393881501 linear +-86.94396450088647 40.43754283431089 linear +-86.94394035857299 40.43755406160901 linear +-86.94391768101751 40.43756692639304 linear +-86.94389572445475 40.437580513787935 linear +-86.94387518246248 40.437595340514775 linear +-86.94385459069514 40.437610114038286 linear +-86.94383399193921 40.437624881803714 linear +-86.94381340786175 40.43763966155279 linear +-86.94379396187936 40.437655352812016 linear +-86.94377733434716 40.43767284129635 linear +-86.94376429231815 40.4376920385607 linear +-86.94375602837825 40.437712725354395 linear +-86.94375170399384 40.43773410245633 linear +-86.9437475166857 40.43775542500003 linear +-86.9437451979484 40.43777693646938 linear +-86.94374334312914 40.437798406784786 linear +-86.9437424830546 40.437819967965176 linear +-86.94374150037585 40.437841537502386 linear +-86.94374051702684 40.437863107021826 linear +-86.94373953367435 40.4378846765411 linear +-86.94373882300876 40.437906252441955 linear +-86.94373826828699 40.43792783089168 linear +-86.9437377001632 40.437949409127455 linear +-86.94373771782051 40.437970963454916 linear +-86.943737783356 40.43799252566561 linear +-86.94373934275124 40.438014015684885 linear +-86.94374058304034 40.438035577496755 linear +-86.94374124938808 40.43805715469112 linear +-86.94374198204036 40.438078730031464 linear +-86.9437427159562 40.43810030534626 linear +-86.94374344987864 40.43812188066083 linear +-86.94374418380183 40.43814345597533 linear +-86.94374491772575 40.438165031289735 linear +-86.94374565165036 40.43818660660405 linear +-86.94374638557561 40.43820818191828 linear +-86.94374712088573 40.43822975720525 linear +-86.94374820085929 40.438251319429625 linear +-86.94375233706121 40.43827268408659 linear +-86.94375898161773 40.43829371205175 linear +-86.94376836181314 40.43831410657221 linear +-86.94378367575227 40.43833229451222 linear +-86.9438046686705 40.43834683862676 linear +-86.9438294246054 40.438357372733286 linear +-86.94385677156079 40.4383629964754 linear +-86.94388509288278 40.43836318639173 linear +-86.94391209903429 40.438356952648505 linear +-86.94393375037198 40.438342858533105 linear +-86.94394668906051 40.438323543482085 linear +-86.94395429466077 40.438302665290735 linear +-86.94396142905009 40.43828178104184 linear +-86.94396868943258 40.43826092357294 linear +-86.94397595064453 40.43824006627286 linear +-86.94398347460026 40.43821927469621 linear +-86.94399424072627 40.43819933028764 linear +-86.94400885861953 40.43818099800855 linear +-86.94402620131785 40.43816393427198 linear +-86.94404498650654 40.438147793212366 linear +-86.94406488000571 40.43813246804079 linear +-86.94408473200586 40.43811711460465 linear +-86.94410462074727 40.43810179563465 linear +-86.94411687851337 40.43808242952682 linear +-86.94411835146255 40.43806117092542 linear +-86.94411445443203 40.438040146247005 linear +-86.94410693830847 40.438019628042724 linear +-86.94409489718161 40.438000254531914 linear +-86.94407876587702 40.43798261989495 linear +-86.94406480496384 40.43796391499347 linear +-86.94405353928845 40.437944219176316 linear +-86.94404818964328 40.437923251559546 linear +-86.94404911079583 40.4379020081509 linear +-86.94406120023949 40.43788256646727 linear +-86.94408200754673 40.437867658879426 linear +-86.94410754741702 40.43785821825041 linear +-86.94413452674165 40.43785248998453 linear +-86.94416213418455 40.437848964591936 linear +-86.94419005186822 40.43784811536207 linear +-86.94421784428407 40.43785023498827 linear +-86.94424491946127 40.437855248595845 linear +-86.94427078407597 40.43786356925765 linear +-86.94429508878137 40.43787459090574 linear +-86.94431773062854 40.43788754210985 linear +-86.94433768433298 40.43790282531801 linear +-86.94435450774056 40.43792019249511 linear +-86.94436922826722 40.437938578060596 linear +-86.94438065188326 40.43795833299983 linear +-86.94439021049264 40.437978559706295 linear +-86.94439719368792 40.4379993423131 linear +-86.94440028818526 40.438020635353105 linear +-86.94440050021397 40.43804209560165 linear +-86.94439774794367 40.438063492381644 linear +-86.94439400895122 40.43808486479197 linear +-86.94439038981685 40.43810626950635 linear +-86.94438677155952 40.43812767430675 linear +-86.94438315330426 40.43814907910741 linear +-86.94437953504705 40.43817048390791 linear +-86.94437591678782 40.43819188870824 linear +-86.94437229852656 40.4382132935084 linear +-86.94436868026315 40.438234698308385 linear +-86.94436590745828 40.438256103515265 linear +-86.94436871535629 40.438277629828086 linear +-86.94437685530174 40.438298356652915 linear +-86.94438850357078 40.4383180712101 linear +-86.94440579139025 40.438335215292376 linear +-86.94442834169777 40.43834832750734 linear +-86.94445303605464 40.438358854760146 linear +-86.94447930746685 40.43836670630556 linear +-86.9445061897152 40.43837304355127 linear +-86.94453438502937 40.43837521603241 linear +-86.94456263838309 40.43837576536707 linear +-86.94459070247191 40.43837295522452 linear +-86.94461825880485 40.43836827269103 linear +-86.94464546547445 40.43836238805813 linear +-86.94467138554667 40.43835370980435 linear +-86.94469466543373 40.4383414023391 linear +-86.94471333478846 40.4383251173752 linear +-86.94472576111781 40.43830568664648 linear +-86.94473029547267 40.43828431468079 linear +-86.94472820938094 40.43826274248664 linear +-86.94472454058393 40.43824137284906 linear +-86.94471879972151 40.43822022400832 linear +-86.94471281899223 40.43819913036147 linear +-86.9447068551544 40.43817803429622 linear +-86.94469940477472 40.43815731219449 linear +-86.94468858782483 40.438137336934844 linear +-86.94467818617693 40.43811728722631 linear +-86.94467267382778 40.43809612654611 linear +-86.9446748404134 40.438074575520865 linear +-86.94468634031992 40.43805476957031 linear +-86.94470459926197 40.438038181879 linear +-86.94472640430303 40.43802442090376 linear +-86.94474975725274 40.43801231931812 linear +-86.94477386374567 40.43800105598526 linear +-86.94479785794033 40.437989665906706 linear +-86.94481977414463 40.437976039760095 linear +-86.94483149184617 40.4379563369291 linear +-86.94483431291725 40.437934874765396 linear +-86.94482748934243 40.437913884697466 linear +-86.94481218585248 40.43789559455943 linear +-86.94479254652168 40.43788003379515 linear +-86.94477293748011 40.43786449672798 linear +-86.94475163938566 40.43785027624814 linear +-86.94472996469236 40.43783643072518 linear +-86.94470763097857 40.43782321247245 linear +-86.94468504166807 40.43781025435755 linear +-86.94466172782494 40.43779805466494 linear +-86.94463803259018 40.437786301379326 linear +-86.94461389832686 40.437775084092586 linear +-86.94458959506107 40.437764085324375 linear +-86.94456532339734 40.43775304579642 linear +-86.94454090225042 40.437742214471 linear +-86.94451632080984 40.437731582910146 linear +-86.94449199524021 40.43772061214761 linear +-86.94446773978417 40.4377095517875 linear +-86.94444354496044 40.43769841420607 linear +-86.94441941424206 40.437687195728245 linear +-86.94439629337734 40.43767634182225 linear diff --git a/data/gps_waypoints_purdue_sim_mk2.txt b/data/gps_waypoints_purdue_sim_mk2.txt new file mode 100644 index 0000000..a723d14 --- /dev/null +++ b/data/gps_waypoints_purdue_sim_mk2.txt @@ -0,0 +1,429 @@ +-86.9443603632717 40.43766692173833 linear +-86.94435224607882 40.43766327631573 linear +-86.94434007612873 40.43765780059952 linear +-86.94433196672657 40.43765414506935 linear +-86.94432386044672 40.4376504854989 linear +-86.9443117068889 40.437644988570995 linear +-86.94430361227717 40.437641313940304 linear +-86.94429157945692 40.437635656349784 linear +-86.94428362353025 40.43763180893977 linear +-86.94427565798722 40.43762797314209 linear +-86.94426371379639 40.43762221449797 linear +-86.94425575822896 40.43761836666844 linear +-86.94424383580892 40.43761258175648 linear +-86.94423589250789 40.4376087191696 linear +-86.94422795358892 40.43760485132763 linear +-86.94421605368333 40.43759903942589 linear +-86.9442081260942 40.43759515804193 linear +-86.9441962432531 40.437589325793816 linear +-86.94418832706344 40.43758543085146 linear +-86.94418041544482 40.437581530491066 linear +-86.9441685566014 40.43757566979858 linear +-86.94416065643821 40.437571755906745 linear +-86.94414880841772 40.43756588245873 linear +-86.94414090819447 40.437561968646975 linear +-86.94413300782401 40.437558055009305 linear +-86.94412115723075 40.43755218459804 linear +-86.94411325683294 40.43754827099394 linear +-86.94410140623728 40.43754240058755 linear +-86.94409346753379 40.43753853233459 linear +-86.94408539458387 40.43753483002543 linear +-86.94407262797307 40.43753027806319 linear +-86.94406363681136 40.43752802895856 linear +-86.94404975977288 40.43752579198946 linear +-86.94404035100169 40.43752507777037 linear +-86.94403091691395 40.437525141536874 linear +-86.94401686174733 40.437526810685796 linear +-86.94400777024582 40.43752891848313 linear +-86.94399458354543 40.43753273957331 linear +-86.94398572441933 40.43753511821614 linear +-86.9439770280294 40.437537815058974 linear +-86.94396450088647 40.43754283431089 linear +-86.9439562716277 40.43754631668834 linear +-86.9439442806732 40.437552055564815 linear +-86.94393642421404 40.43755602497887 linear +-86.94392869694954 40.43756017315919 linear +-86.94391768101751 40.43756692639304 linear +-86.94391034280795 40.437571430505194 linear +-86.9438992998687 40.43757815998262 linear +-86.94389219242589 40.437582900287744 linear +-86.94388539708511 40.43758789291142 linear +-86.94387518246248 40.437595340514775 linear +-86.9438683220399 40.43760026791485 linear +-86.94385802377678 40.4376076527082 linear +-86.94385115758466 40.43761257534491 linear +-86.94384429133355 40.43761749793388 linear +-86.94383399193921 40.437624881803714 linear +-86.94382712567409 40.437629804382276 linear +-86.9438168266831 40.43763718858207 linear +-86.94381004231629 40.43764217955013 linear +-86.94380346724938 40.43764734358349 linear +-86.94379396187936 40.437655352812016 linear +-86.94378794807156 40.437660908223826 linear +-86.94377981529 40.4376697686362 linear +-86.94377494453492 40.437675951973226 linear +-86.94377039104423 40.43768227360082 linear +-86.94376429231815 40.4376920385607 linear +-86.94376094250974 40.43769878932651 linear +-86.94375703336772 40.43770919910126 linear +-86.94375517160705 40.43771627958912 linear +-86.94375376539566 40.437723414253455 linear +-86.94375170399384 40.43773410245633 linear +-86.9437502551495 40.43774121066486 linear +-86.94374808431093 40.43775186262092 linear +-86.94374709867725 40.437759009130076 linear +-86.94374642158857 40.437766189283835 linear +-86.9437451979484 40.43777693646938 linear +-86.94374435090661 40.43778410150057 linear +-86.94374339001996 40.43779480941762 linear +-86.94374324765695 40.437801996999625 linear +-86.94374296566387 40.437809182934096 linear +-86.9437424830546 40.437819967965176 linear +-86.94374215583366 40.437827157819854 linear +-86.94374166426377 40.43783794258237 linear +-86.9437413364863 40.437845132422346 linear +-86.94374100870341 40.43785232226217 linear +-86.94374051702684 40.437863107021826 linear +-86.94374018924272 40.43787029686159 linear +-86.94373969756727 40.437881081621256 linear +-86.94373936978302 40.43788827146099 linear +-86.9437390529224 40.437895461613465 linear +-86.94373882300876 40.437906252441955 linear +-86.94373864487679 40.43791344536687 linear +-86.94373836289331 40.43792423451777 linear +-86.94373817362741 40.437931427264736 linear +-86.94373798425329 40.437938620010016 linear +-86.9437377001632 40.437949409127455 linear +-86.94373757609674 40.43795657465184 linear +-86.94373770562464 40.437967366361626 linear +-86.94373772632805 40.437974560548064 linear +-86.94373773969411 40.43798175474005 linear +-86.943737783356 40.43799252566561 linear +-86.94373837054951 40.43799965791413 linear +-86.94373911799279 40.43801042266412 linear +-86.9437395650804 40.438017608775326 linear +-86.94374000732509 40.43802479503663 linear +-86.94374058304034 40.438035577496755 linear +-86.94374079766663 40.43804277036738 linear +-86.94374112890075 40.43805355876784 linear +-86.94374137095573 40.438060750592165 linear +-86.94374161516107 40.43806794237248 linear +-86.94374198204036 40.438078730031464 linear +-86.94374222667591 40.438085921803136 linear +-86.94374259363605 40.43809670946048 linear +-86.94374283827679 40.43810390123202 linear +-86.94374308291763 40.43811109300356 linear +-86.94374344987864 40.43812188066083 linear +-86.94374369451961 40.43812907243235 linear +-86.94374406148164 40.43813986008959 linear +-86.94374430612278 40.438147051861066 linear +-86.943744550764 40.438154243632546 linear +-86.94374491772575 40.438165031289735 linear +-86.94374516236721 40.43817222306118 linear +-86.94374552932942 40.43818301071834 linear +-86.94374577397096 40.43819020248977 linear +-86.94374601861256 40.4381973942612 linear +-86.94374638557561 40.43820818191828 linear +-86.94374663021749 40.43821537368967 linear +-86.94374699717972 40.438226161346755 linear +-86.94374725645031 40.43823335282766 linear +-86.94374757114808 40.4382405430893 linear +-86.94374820085929 40.438251319429625 linear +-86.94374914204506 40.438258463631364 linear +-86.94375144311616 40.43826914117255 linear +-86.94375328968269 40.43827621717914 linear +-86.94375537619938 40.43828325129432 linear +-86.94375898161773 40.43829371205175 linear +-86.94376172709221 40.438300611794666 linear +-86.94376651799232 40.438310795726274 linear +-86.94377043194126 40.438317345333786 linear +-86.94377520161747 40.43832359107926 linear +-86.94378367575227 40.43833229451222 linear +-86.94379011692143 40.43833759833143 linear +-86.94380083995948 40.43834471001548 linear +-86.94380860706947 40.43834884548153 linear +-86.94381670857476 40.43835258991793 linear +-86.9438294246054 40.438357372733286 linear +-86.94383832130772 40.43835984424754 linear +-86.94385209808073 40.43836243287532 linear +-86.94386147205454 40.43836340892218 linear +-86.94387092236452 40.43836377698024 linear +-86.94388509288278 40.43836318639173 linear +-86.94389440750247 40.43836199382509 linear +-86.94390785652882 40.43835857884845 linear +-86.94391622638038 40.43835509514432 linear +-86.94392389178425 40.43835075847932 linear +-86.94393375037198 40.438342858533105 linear +-86.94393905918773 40.43833682045681 linear +-86.9439450510473 40.43832693613359 linear +-86.94394815802082 40.43832010090118 linear +-86.94395076460374 40.43831314837671 linear +-86.94395429466077 40.438302665290735 linear +-86.94395661677018 40.4382956916539 linear +-86.9439602200589 40.43828525750484 linear +-86.94396263875824 40.438278304724776 linear +-86.94396505887791 40.43827135223355 linear +-86.94396868943258 40.43826092357294 linear +-86.9439711098351 40.43825397113924 linear +-86.94397474044234 40.438243542489474 linear +-86.9439771608465 40.43823659005621 linear +-86.9439795820691 40.43822963778931 linear +-86.94398347460026 40.43821927469621 linear +-86.94398684116227 40.438212558009795 linear +-86.94399239542457 40.438202639324814 linear +-86.94399636259078 40.43819619609117 linear +-86.94400141391318 40.438190150786426 linear +-86.94400885861953 40.43818099800855 linear +-86.94401464964945 40.43817527138378 linear +-86.9440232905728 40.43816676496926 linear +-86.94402929983059 40.43816120994242 linear +-86.9440354957413 40.438155791237534 linear +-86.94404498650654 40.438147793212366 linear +-86.94405168555419 40.438142733684224 linear +-86.94406158257225 40.43813503542284 linear +-86.94406818009996 40.43812990266768 linear +-86.94407479020784 40.43812477939252 linear +-86.94408473200586 40.43811711460465 linear +-86.9440913746139 40.43811201594389 linear +-86.9441013348614 40.43810436520177 linear +-86.94410762567234 40.43809904020365 linear +-86.94411211014214 40.438092685697434 linear +-86.94411687851337 40.43808242952682 linear +-86.94411933999629 40.43807554975714 linear +-86.94411919177742 40.43806475548226 linear +-86.94411778267859 40.43805768949553 linear +-86.94411693168064 40.43805058040099 linear +-86.94411445443203 40.438040146247005 linear +-86.94411267026261 40.438033120931216 linear +-86.94410882276283 40.438022828149286 linear +-86.944105047136 40.43801631783794 linear +-86.94410127109464 40.43800981292304 linear +-86.94409489718161 40.438000254531914 linear +-86.94408933393822 40.437994464184044 linear +-86.94408136871692 40.43798561734682 linear +-86.94407618062849 40.43797961915553 linear +-86.94407161011175 40.43797337655594 linear +-86.94406480496384 40.43796391499347 linear +-86.94406063253307 40.4379574906623 linear +-86.94405526975633 40.43794755970965 linear +-86.94405201176548 40.4379408493101 linear +-86.94404944175928 40.43793394071604 linear +-86.94404818964328 40.437923251559546 linear +-86.94404724328913 40.43791609652186 linear +-86.9440478062777 40.4379055038616 linear +-86.94405086323147 40.437898595683045 linear +-86.94405447745172 40.437891979220886 linear +-86.94406120023949 40.43788256646727 linear +-86.94406802891213 40.437877330286895 linear +-86.94407829550111 40.437869886060724 linear +-86.94408612270051 40.437865829521236 linear +-86.9440949065948 40.437862972109144 linear +-86.94410754741702 40.43785821825041 linear +-86.94411626996612 40.43785592269346 linear +-86.94412998190867 40.43785343074128 linear +-86.94413907073482 40.43785154849707 linear +-86.94414815203099 40.437850195494626 linear +-86.94416213418455 40.437848964591936 linear +-86.94417143420559 40.43784808806333 linear +-86.94418535145441 40.43784811932657 linear +-86.94419476592972 40.43784814601305 linear +-86.94420392966323 40.43784874663863 linear +-86.94421784428407 40.43785023498827 linear +-86.94422688497465 40.437851503432974 linear +-86.94424046281462 40.43785426655621 linear +-86.94424932674535 40.43785646131997 linear +-86.94425809853576 40.43785894637421 linear +-86.94427078407597 40.43786356925765 linear +-86.94427892383032 40.437867187709855 linear +-86.94429106517764 40.43787273047665 linear +-86.9442990797433 40.43787651563331 linear +-86.94430675777872 40.43788072337982 linear +-86.94431773062854 40.43788754210985 linear +-86.94432481940464 40.43789229506859 linear +-86.94433464644287 40.43790006057292 linear +-86.94434057419707 40.437905683503125 linear +-86.9443460726871 40.4379115357612 linear +-86.94435450774056 40.43792019249511 linear +-86.94435988191964 40.43792607909745 linear +-86.94436702121105 40.437935416309095 linear +-86.94437113030953 40.437941858798965 linear +-86.94437494934652 40.43794845434519 linear +-86.94438065188326 40.43795833299983 linear +-86.94438438031436 40.437964940572776 linear +-86.94438880585213 40.43797512653244 linear +-86.94439163531074 40.437981989206904 linear +-86.94439445487869 40.437988847247816 linear +-86.94439719368792 40.4379993423131 linear +-86.9443988306116 40.4380064414546 linear +-86.94440012902372 40.43801706964656 linear +-86.94440050726982 40.43802422080029 linear +-86.94440095782792 40.43803140907484 linear +-86.94440050021397 40.43804209560165 linear +-86.944399939136 40.43804929157135 linear +-86.94439848403357 40.438059947086046 linear +-86.94439708283099 40.43806703763379 linear +-86.94439583003899 40.43807416351723 linear +-86.94439400895122 40.43808486479197 linear +-86.94439280212909 40.43809199965325 linear +-86.94439099286494 40.43810270204011 linear +-86.9443897867726 40.43810983697294 linear +-86.94438858068705 40.43811697190644 linear +-86.94438677155952 40.43812767430675 linear +-86.94438556547463 40.438134809240324 linear +-86.94438375634766 40.43814551164073 linear +-86.94438255026228 40.438152646574245 linear +-86.94438134417662 40.43815978150775 linear +-86.94437953504705 40.43817048390791 linear +-86.94437832896087 40.438177618841365 linear +-86.94437651983145 40.438188321241554 linear +-86.94437531374453 40.438195456174945 linear +-86.94437410765734 40.43820259110833 linear +-86.94437229852656 40.4382132935084 linear +-86.944371092439 40.43822042844175 linear +-86.94436928330632 40.43823113084164 linear +-86.94436807721797 40.43823826577493 linear +-86.94436688070124 40.43824540160548 linear +-86.94436590745828 40.438256103515265 linear +-86.94436619098605 40.438263315776034 linear +-86.94436784591838 40.43827407588749 linear +-86.94436972752705 40.43828115903944 linear +-86.94437219640282 40.438288130862325 linear +-86.94437685530174 40.438298356652915 linear +-86.94438036685797 40.438305055318956 linear +-86.94438627359622 40.43831489342466 linear +-86.94439095201375 40.438321170757774 linear +-86.94439639308466 40.43832708577692 linear +-86.94440579139025 40.438335215292376 linear +-86.94441280159397 40.43834008051773 linear +-86.94442428448029 40.438346461259925 linear +-86.94443248974974 40.43835006753724 linear +-86.94444078267196 40.43835349752836 linear +-86.94445303605464 40.438358854760146 linear +-86.94446150933858 40.43836192313352 linear +-86.9444748647166 40.438365541902 linear +-86.94448373776682 40.43836791929002 linear +-86.94449258644552 40.4383703642373 linear +-86.9445061897152 40.43837304355127 linear +-86.94451554970617 40.438374102997514 linear +-86.94452967126615 40.43837501439393 linear +-86.94453909682797 40.438375407191465 linear +-86.94454851219281 40.43837571819511 linear +-86.94456263838309 40.43837576536707 linear +-86.94457205043942 40.4383752306699 linear +-86.94458606695326 40.43837366553088 linear +-86.94459530768728 40.43837215576956 linear +-86.94460445958663 40.43837060063409 linear +-86.94461825880485 40.43836827269103 linear +-86.94462737632132 40.43836642698134 linear +-86.94464096526077 40.438363471452206 linear +-86.94464990825983 40.438361183076225 linear +-86.94465865790171 40.43835846421514 linear +-86.94467138554667 40.43835370980435 linear +-86.94467956177988 40.43835009261127 linear +-86.94469106342052 40.43834374740689 linear +-86.94469815581583 40.438338946879526 linear +-86.9447046929992 40.438333720713004 linear +-86.94471333478846 40.4383251173752 linear +-86.94471830441347 40.438318966451845 linear +-86.94472422282331 40.4383091106886 linear +-86.94472707940928 40.43830220852231 linear +-86.94472904611611 40.438295126437026 linear +-86.94473029547267 40.43828431468079 linear +-86.9447299821635 40.438277078449865 linear +-86.94472864348211 40.438266331183286 linear +-86.9447277517109 40.43825915979722 linear +-86.94472673253688 40.4382520061121 linear +-86.94472454058393 40.43824137284906 linear +-86.94472262630933 40.43823431265748 linear +-86.94471977057857 40.43822374397322 linear +-86.94471781472036 40.43821670636165 linear +-86.94471581958132 40.438209675351715 linear +-86.94471281899223 40.43819913036147 linear +-86.94471082914926 40.43819209865206 linear +-86.94470784852442 40.43818155040637 linear +-86.94470586179881 40.43817451818371 linear +-86.9447038278495 40.43816752682669 linear +-86.94469940477472 40.43815731219449 linear +-86.94469602496336 40.43815057834019 linear +-86.94469045802515 40.43814063331839 linear +-86.94468674330692 40.43813403015281 linear +-86.94468309103156 40.43812739910583 linear +-86.94467818617693 40.43811728722631 linear +-86.94467561134304 40.43811036109172 linear +-86.94467308615424 40.43809972108682 linear +-86.9446724714085 40.438092516756406 linear +-86.94467269199899 40.43808528989241 linear +-86.9446748404134 40.438074575520865 linear +-86.94467763503275 40.43806765017462 linear +-86.9446837908549 40.43805783319399 linear +-86.94468910629027 40.43805182037496 linear +-86.94469522518824 40.43804626819487 linear +-86.94470459926197 40.438038181879 linear +-86.94471151449304 40.438033262282126 linear +-86.94472274061141 40.43802668014682 linear +-86.94473009817524 40.43802218983146 linear +-86.94473798518085 40.4380182804989 linear +-86.94474975725274 40.43801231931812 linear +-86.94475780481379 40.438008571073595 linear +-86.94476984718703 40.43800293238115 linear +-86.94477787699505 40.43799917552451 linear +-86.94478589659998 40.437995406059194 linear +-86.94479785794033 40.437989665906706 linear +-86.94480566319773 40.43798562062188 linear +-86.94481652014946 40.43797867429195 linear +-86.9448227069749 40.437973181006036 linear +-86.944827309924 40.43796680582012 linear +-86.94483149184617 40.4379563369291 linear +-86.94483339407581 40.43794923116452 linear +-86.9448345734583 40.43793847508553 linear +-86.94483370056948 40.43793129662961 linear +-86.94483164524323 40.43792421658022 linear +-86.94482748934243 40.437913884697466 linear +-86.94482324852149 40.43790738954831 linear +-86.94481510991268 40.43789844307868 linear +-86.94480913421606 40.43789283272303 linear +-86.94480249938326 40.43788766816339 linear +-86.94479254652168 40.43788003379515 linear +-86.94478605836395 40.43787482025408 linear +-86.9447763172158 40.43786701044488 linear +-86.94476946536727 40.43786205275853 linear +-86.94476231608249 40.43785734109081 linear +-86.94475163938566 40.43785027624814 linear +-86.94474451456037 40.43784557186414 linear +-86.94473359143652 40.437838724219304 linear +-86.94472632733374 40.43783414918879 linear +-86.94471883062411 40.437829785281544 linear +-86.94470763097857 40.43782321247245 linear +-86.94470018818356 40.43781880615391 linear +-86.94468887316702 40.437812346866345 linear +-86.9446811358647 40.43780823240185 linear +-86.94467335592077 40.43780417961778 linear +-86.94466172782494 40.43779805466494 linear +-86.94465387739395 40.43779408123209 linear +-86.94464203856664 40.43778819489926 linear +-86.94463401162089 40.4377844288871 linear +-86.94462599158685 40.43778065971467 linear +-86.94461389832686 40.437775084092586 linear +-86.94460578125128 40.43777143848835 linear +-86.94459364053135 40.43776592499826 linear +-86.94458554971904 40.43776224548589 linear +-86.94457745916272 40.43775856564518 linear +-86.94456532339734 40.43775304579642 linear +-86.94455723289396 40.43774936588912 linear +-86.94454502543731 40.43774395249595 linear +-86.94453679530118 40.43774045625704 linear +-86.9445285949514 40.43773692118001 linear +-86.94451632080984 40.437731582910146 linear +-86.94450819280517 40.437727951060005 linear +-86.94449604307216 40.437722448820615 linear +-86.94448794922138 40.43771877315308 linear +-86.94447986086166 40.437715090468934 linear +-86.94446773978417 40.4377095517875 linear +-86.94445966775588 40.437705848253 linear +-86.94444757299358 40.437700276091014 linear +-86.94443951870858 40.437696550073284 linear +-86.94443147155684 40.43769281506863 linear +-86.94441941424206 40.437687195728245 linear +-86.94441138499901 40.437683438290804 linear +-86.94439959501979 40.43767789844733 linear +-86.94439345806718 40.437675002855556 linear From 235bdd96305435c99df75d22fbc39ba2eac5315e Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 12 May 2026 12:04:08 -0400 Subject: [PATCH 61/65] centering robot path --- .../yet_another_gps_publisher_node.hpp | 1 + src/yet_another_gps_publisher_node.cpp | 48 +++++++++++-------- 2 files changed, 29 insertions(+), 20 deletions(-) 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 9182af6..01af524 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 @@ -24,6 +24,7 @@ class yet_another_gps_publisher : public rclcpp::Node { std::string utm_frame_id; std::string odom_frame_id; std::string map_frame_id; + std::string robot_body_frame_id; // parameters for waypoint file loading std::string waypoint_file_path; std::string waypoint_file_topic; diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 5a19df2..e583156 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -46,6 +46,8 @@ yet_another_gps_publisher::yet_another_gps_publisher(const rclcpp::NodeOptions& odom_frame_id = this->declare_parameter("odom_frame_id", "odom"); // this is the MAP frame that we will store the waypoints in over time. map_frame_id = this->declare_parameter("map_frame_id", "map"); + // robot frame to start path in + robot_body_frame_id = this->declare_parameter("robot_body_frame_id", "base_link"); // 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. @@ -251,7 +253,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr return; } - geometry_msgs::msg::Pose robot_postion = robot_pose_map; //msg->pose.pose; + geometry_msgs::msg::Pose robot_postion = robot_pose_map; size_t checked = 0; const size_t N = waypoints.size(); @@ -279,7 +281,7 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr 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 + advance_to_next_waypoint(); checked++; } @@ -354,36 +356,43 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr segment[j].position.y - segment[j - 1].position.y)); } - prev_wp = wp; // shift anchor - ++segment_it; // advance the scanning pointer - processed++; + prev_wp = wp; // shift anchor + ++segment_it; // advance the scanning pointer + processed++; // Only increment once per waypoint processed! + + // Wrap the segment iterator so it can look from the last point back to the first + if (segment_it == waypoints.end()) { + segment_it = waypoints.begin(); + } - // to stop if we’ve walked the whole list without hitting the length. + / to stop if we’ve walked the whole list without hitting the length. // TODO decide is we still wnat to publish? // should probably warn though. I dont see this ever being a problem though. if (processed >= waypoints.size()) break; } - // 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 path from map frame to robot body frame so the robot sits at (0,0) + // and the path extends ahead of it in RViz. + nav_msgs::msg::Path path_body; + path_body.header.stamp = path_map.header.stamp; + path_body.header.frame_id = robot_body_frame_id; /* 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); + try { + // Look up the transform from map to the robot's current body frame at *this* time + auto transform = tf_buffer_.lookupTransform(robot_body_frame_id, map_frame_id, msg->header.stamp); for (const auto& ps : path_map.poses) { geometry_msgs::msg::PoseStamped ps_out; tf2::doTransform(ps, ps_out, transform); - ps_out.header.frame_id = odom_frame_id; + ps_out.header.frame_id = robot_body_frame_id; ps_out.header.stamp = path_map.header.stamp; - path_odom.poses.push_back(ps_out); + path_body.poses.push_back(ps_out); } - path_odom.header.frame_id = odom_frame_id; - path_odom.header.stamp = path_map.header.stamp; } catch (tf2::TransformException& ex) { // TF lookup from map to odom_frame_id (body frame) failed - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF MAP->%s failed: %s", odom_frame_id.c_str(), + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF map->%s failed: %s", robot_body_frame_id.c_str(), ex.what()); return; } @@ -393,18 +402,17 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr tf2::Quaternion pitch_rotation; // M_PI pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 - for (auto& pose_stamped : path_odom.poses) { + for (auto& pose_stamped : path_body.poses) { pose_stamped.pose.position.z = 0.0; tf2::Quaternion original_q, rotated_q; tf2::fromMsg(pose_stamped.pose.orientation, original_q); - rotated_q = pitch_rotation * original_q; // rotate the heading by +90° pitch + rotated_q = pitch_rotation * original_q; pose_stamped.pose.orientation = tf2::toMsg(rotated_q); } - // Now publish the rotated path - path_pub->publish(path_odom); - + // Publish the rotated path + path_pub->publish(path_body); } else { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", (double)cumulative_length); From 16be922100cb4d87a7ee7a2dde8f3029c36fee77 Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 12 May 2026 12:05:50 -0400 Subject: [PATCH 62/65] reformat centering robot --- src/yet_another_gps_publisher_node.cpp | 86 +++++++++++++------------- 1 file changed, 43 insertions(+), 43 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index e583156..b7c101a 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -356,9 +356,9 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr segment[j].position.y - segment[j - 1].position.y)); } - prev_wp = wp; // shift anchor - ++segment_it; // advance the scanning pointer - processed++; // Only increment once per waypoint processed! + prev_wp = wp; // shift anchor + ++segment_it; // advance the scanning pointer + processed++; // Only increment once per waypoint processed! // Wrap the segment iterator so it can look from the last point back to the first if (segment_it == waypoints.end()) { @@ -366,9 +366,9 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr } / to stop if we’ve walked the whole list without hitting the length. - // TODO decide is we still wnat to publish? - // should probably warn though. I dont see this ever being a problem though. - if (processed >= waypoints.size()) break; + // TODO decide is we still wnat to publish? + // should probably warn though. I dont see this ever being a problem though. + if (processed >= waypoints.size()) break; } // Transform path from map frame to robot body frame so the robot sits at (0,0) @@ -380,51 +380,51 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr /* 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 { - try { - // Look up the transform from map to the robot's current body frame at *this* time - auto transform = tf_buffer_.lookupTransform(robot_body_frame_id, map_frame_id, msg->header.stamp); - for (const auto& ps : path_map.poses) { - geometry_msgs::msg::PoseStamped ps_out; - tf2::doTransform(ps, ps_out, transform); - ps_out.header.frame_id = robot_body_frame_id; - ps_out.header.stamp = path_map.header.stamp; - path_body.poses.push_back(ps_out); + try { + // Look up the transform from map to the robot's current body frame at *this* time + auto transform = tf_buffer_.lookupTransform(robot_body_frame_id, map_frame_id, msg->header.stamp); + for (const auto& ps : path_map.poses) { + geometry_msgs::msg::PoseStamped ps_out; + tf2::doTransform(ps, ps_out, transform); + ps_out.header.frame_id = robot_body_frame_id; + ps_out.header.stamp = path_map.header.stamp; + path_body.poses.push_back(ps_out); + } + } catch (tf2::TransformException& ex) { + // TF lookup from map to odom_frame_id (body frame) failed + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF map->%s failed: %s", + robot_body_frame_id.c_str(), ex.what()); + return; } - } catch (tf2::TransformException& ex) { - // TF lookup from map to odom_frame_id (body frame) failed - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF map->%s failed: %s", robot_body_frame_id.c_str(), - ex.what()); - return; - } - if (cumulative_length >= min_spline_length) { - // Apply +90 deg pitch (rotate around Y axis) - tf2::Quaternion pitch_rotation; // M_PI - pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 + if (cumulative_length >= min_spline_length) { + // Apply +90 deg pitch (rotate around Y axis) + tf2::Quaternion pitch_rotation; // M_PI + pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 - for (auto& pose_stamped : path_body.poses) { - pose_stamped.pose.position.z = 0.0; + for (auto& pose_stamped : path_body.poses) { + pose_stamped.pose.position.z = 0.0; - tf2::Quaternion original_q, rotated_q; - tf2::fromMsg(pose_stamped.pose.orientation, original_q); - rotated_q = pitch_rotation * original_q; - pose_stamped.pose.orientation = tf2::toMsg(rotated_q); - } + tf2::Quaternion original_q, rotated_q; + tf2::fromMsg(pose_stamped.pose.orientation, original_q); + rotated_q = pitch_rotation * original_q; + pose_stamped.pose.orientation = tf2::toMsg(rotated_q); + } - // Publish the rotated path - path_pub->publish(path_body); - } else { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", - (double)cumulative_length); + // Publish the rotated path + path_pub->publish(path_body); + } else { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", + (double)cumulative_length); + } } -} -// 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) {} + // 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) {} // Register node as a component // todo chat why are we evening using this here #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) + RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) From 1e454c6c14447a9ff1072140afa9160ced32ea28 Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 12 May 2026 12:12:23 -0400 Subject: [PATCH 63/65] reformat centering robot --- src/yet_another_gps_publisher_node.cpp | 82 +++++++++++++------------- 1 file changed, 40 insertions(+), 42 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index b7c101a..124b4b4 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -365,10 +365,10 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr segment_it = waypoints.begin(); } - / to stop if we’ve walked the whole list without hitting the length. - // TODO decide is we still wnat to publish? - // should probably warn though. I dont see this ever being a problem though. - if (processed >= waypoints.size()) break; + // to stop if we’ve walked the whole list without hitting the length. + // TODO decide is we still wnat to publish? + // should probably warn though. I dont see this ever being a problem though. + if (processed >= waypoints.size()) break; } // Transform path from map frame to robot body frame so the robot sits at (0,0) @@ -380,51 +380,49 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr /* 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 { - try { - // Look up the transform from map to the robot's current body frame at *this* time - auto transform = tf_buffer_.lookupTransform(robot_body_frame_id, map_frame_id, msg->header.stamp); - for (const auto& ps : path_map.poses) { - geometry_msgs::msg::PoseStamped ps_out; - tf2::doTransform(ps, ps_out, transform); - ps_out.header.frame_id = robot_body_frame_id; - ps_out.header.stamp = path_map.header.stamp; - path_body.poses.push_back(ps_out); - } - } catch (tf2::TransformException& ex) { - // TF lookup from map to odom_frame_id (body frame) failed - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF map->%s failed: %s", - robot_body_frame_id.c_str(), ex.what()); - return; + // Look up the transform from map to the robot's current body frame at *this* time + auto transform = tf_buffer_.lookupTransform(robot_body_frame_id, map_frame_id, msg->header.stamp); + for (const auto& ps : path_map.poses) { + geometry_msgs::msg::PoseStamped ps_out; + tf2::doTransform(ps, ps_out, transform); + ps_out.header.frame_id = robot_body_frame_id; + ps_out.header.stamp = path_map.header.stamp; + path_body.poses.push_back(ps_out); } + } catch (tf2::TransformException& ex) { + // TF lookup from map to odom_frame_id (body frame) failed + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 0, "TF map->%s failed: %s", + robot_body_frame_id.c_str(), ex.what()); + return; + } - if (cumulative_length >= min_spline_length) { - // Apply +90 deg pitch (rotate around Y axis) - tf2::Quaternion pitch_rotation; // M_PI - pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 - - for (auto& pose_stamped : path_body.poses) { - pose_stamped.pose.position.z = 0.0; + if (cumulative_length >= min_spline_length) { + // Apply +90 deg pitch (rotate around Y axis) + tf2::Quaternion pitch_rotation; // M_PI + pitch_rotation.setRPY(0.0, 0.0, 0.0); // roll=0, pitch=+90°, yaw=0 - tf2::Quaternion original_q, rotated_q; - tf2::fromMsg(pose_stamped.pose.orientation, original_q); - rotated_q = pitch_rotation * original_q; - pose_stamped.pose.orientation = tf2::toMsg(rotated_q); - } + for (auto& pose_stamped : path_body.poses) { + pose_stamped.pose.position.z = 0.0; - // Publish the rotated path - path_pub->publish(path_body); - } else { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", - (double)cumulative_length); + tf2::Quaternion original_q, rotated_q; + tf2::fromMsg(pose_stamped.pose.orientation, original_q); + rotated_q = pitch_rotation * original_q; + pose_stamped.pose.orientation = tf2::toMsg(rotated_q); } - } - // 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) {} + // Publish the rotated path + path_pub->publish(path_body); + } else { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 0, "GPS path too short (%.2f m)", + (double)cumulative_length); + } +} +// 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) {} // Register node as a component // todo chat why are we evening using this here #include "rclcpp_components/register_node_macro.hpp" - RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) +RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) From 3cd70fdfaee42e453c362f349b3711c92456190c Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 13 May 2026 14:37:13 -0400 Subject: [PATCH 64/65] update --- src/yet_another_gps_publisher_node.cpp | 24 +++++++++--------------- 1 file changed, 9 insertions(+), 15 deletions(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 124b4b4..43b9482 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -146,12 +146,12 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { std::string spline_type; if (!(iss >> lon >> lat >> spline_type)) { - RCLCPP_WARN(this->get_logger(), "Skipping malformed line %d", line_num); + // RCLCPP_WARN(this->get_logger(), "Skipping malformed line %d", line_num); continue; } if (spline_type == "circle") { if (!(iss >> radius)) { - RCLCPP_WARN(this->get_logger(), "Circle method on line %d missing radius, using default 0", line_num); + // 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 */ @@ -164,8 +164,7 @@ bool yet_another_gps_publisher::load_waypoints(const std::string& file_path) { } /* 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); + //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(); @@ -364,11 +363,6 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr if (segment_it == waypoints.end()) { segment_it = waypoints.begin(); } - - // to stop if we’ve walked the whole list without hitting the length. - // TODO decide is we still wnat to publish? - // should probably warn though. I dont see this ever being a problem though. - if (processed >= waypoints.size()) break; } // Transform path from map frame to robot body frame so the robot sits at (0,0) @@ -381,7 +375,11 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr (odom_frame_id) so the robot base link acts as the origin (0,0). */ try { // Look up the transform from map to the robot's current body frame at *this* time - auto transform = tf_buffer_.lookupTransform(robot_body_frame_id, map_frame_id, msg->header.stamp); + // Look up the transform from map to the robot's current body frame using the + // latest available data (TimePointZero) and wait up to 100 ms. + auto transform = tf_buffer_.lookupTransform(robot_body_frame_id, map_frame_id, + tf2::TimePointZero, // use latest transform + std::chrono::milliseconds(100)); // optional but safe timeout for (const auto& ps : path_map.poses) { geometry_msgs::msg::PoseStamped ps_out; tf2::doTransform(ps, ps_out, transform); @@ -417,12 +415,8 @@ void yet_another_gps_publisher::global_ekf_callback(const nav_msgs::msg::Odometr (double)cumulative_length); } } -// gps_waypoint constructor implementation +// gps_waypoint constructor implementation example /* gps_waypoint: */ gps_waypoint::gps_waypoint(double lon, double lat, const std::string& method, double radius) : longitude_(lon), latitude_(lat), method_(method), radius_(radius) {} -// Register node as a component -// todo chat why are we evening using this here -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(yet_another_gps_publisher) From d8bf8a4cbb28a4a63bf41b2199af7e9d4316284c Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 20 May 2026 23:31:14 -0400 Subject: [PATCH 65/65] first of many YEYE ahh comment clean ups --- src/yet_another_gps_publisher_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 43b9482..2cab769 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -51,7 +51,7 @@ 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? (Completed by Elijah May 9 check git logs) + // TODO indentify where this file should be stored? waypoint_file_path = this->declare_parameter("waypoint_file_path", WAYPOINT_FILE); /**********************************************************/