Redtoo dev experiements

CMakeLists.txt

@@ -9,63 +9,67 @@ 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
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
-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
             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
             $<INSTALL_INTERFACE:include>)
 endif ()
 
-ament_package()
+ament_package()

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

include/yet_another_gps_publisher/gps_waypoint.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <string>
+
+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; }
+
+    // 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;
+};

include/yet_another_gps_publisher/spline_factory.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <functional>
+#include <geometry_msgs/msg/pose.hpp>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "gps_waypoint.hpp"
+
+namespace gps_waypoint_spline {
+
+    // what does using mean compared to normal spline generator. 
+using SplineGenerator =
+    std::function<std::vector<geometry_msgs::msg::Pose>(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<geometry_msgs::msg::Pose> generate(const std::string& name, const gps_waypoint& start,
+                                                          const gps_waypoint& end);
+
+private:
+    static std::map<std::string, SplineGenerator>& registry();
+};
+
+}  // namespace gps_waypoint_spline

include/yet_another_gps_publisher/yet_another_gps_publisher_node.hpp

@@ -1,20 +1,58 @@
 #pragma once
 
+#include <deque>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#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 "geodesy/utm.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
 
-// yet_another_gps_publisher_node.cpp
 class yet_another_gps_publisher : public rclcpp::Node {
+public:
+    explicit yet_another_gps_publisher(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
 private:
-    rclcpp::Publisher<std_msgs::msg::String>::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;
+    std::string waypoint_file_path;
 
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub;
-    geodesy::UTMPose currentPose;
+    // Subscribers
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub;
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr waypoint_file_sub;
 
-public:
-    yet_another_gps_publisher(const rclcpp::NodeOptions& options);
+    // Publisher
+    rclcpp::Publisher<nav_msgs::msg::Path>::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<gps_waypoint> waypoints;
+
+    // Callbacks
+    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+    void timer_callback();
+
+    // helper to load waypoints from file
+    bool load_waypoints(const std::string& file_path);
 
-    /// 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);
+};

package.xml

@@ -11,14 +11,23 @@
 
     <depend>rclcpp</depend>
 
-    <!--Messages yet_another_gps_publisher-->
+    <!--Messages-->
     <depend>ackermann_msgs</depend>
     <depend>sensor_msgs</depend>
     <depend>std_msgs</depend>
 
     <!--GPS packages-->
     <depend>geodesy</depend>
 
+    <!-- Additional dependencies for spline generation and TF -->
+    <depend>geographic_msgs</depend>         <!-- for GeoPoint -->
+    <depend>geometry_msgs</depend>           <!-- for Pose, PoseStamped -->
+    <depend>nav_msgs</depend>                 <!-- for Path, Odometry -->
+    <depend>tf2</depend>
+    <depend>tf2_ros</depend>
+    <depend>tf2_geometry_msgs</depend>
+    <depend>rclcpp_components</depend>       <!-- for component registration -->
+
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_cmake_flake8</test_depend>
     <test_depend>ament_cmake_xmllint</test_depend>
@@ -32,4 +41,4 @@
     <export>
         <build_type>ament_cmake</build_type>
     </export>
-</package>
+</package>

src/spline_methods.cpp

@@ -0,0 +1,89 @@
+#include <cmath>
+#include <stdexcept>
+
+#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
+std::map<std::string, SplineGenerator>& SplineFactory::registry() {
+    static std::map<std::string, SplineGenerator> 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<geometry_msgs::msg::Pose> SplineFactory::generate(const std::string& name, const gps_waypoint& start,
+                                                              const gps_waypoint& end) {
+    return getGenerator(name)(start, end);
+}
+
+// ------------------------------------------------------------------
+// Concrete generators
+// ------------------------------------------------------------------
+
+static std::vector<geometry_msgs::msg::Pose> 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; // TODO this shouldnt be hardcoded like EVER
+    std::vector<geometry_msgs::msg::Pose> points;
+    points.reserve(num_points + 1);
+
+    for (int i = 0; i <= num_points; ++i) {
+        double t = static_cast<double>(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<geometry_msgs::msg::Pose> 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