Estickel dev

CMakeLists.txt

@@ -35,6 +35,11 @@ add_executable(yet_another_gps_publisher
 target_include_directories(yet_another_gps_publisher PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
+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)

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

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

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

include/yet_another_gps_publisher/gps_waypoint.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
-#include <geometry_msgs/msg/pose.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp> /* for geometry_msg::msg::Pose for position P(x,y,z) and orientation(quanternion) Q(x,y,z,w) */
+#include <geometry_msgs/msg/pose_stamped.hpp> /* 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 <string>
 
 class gps_waypoint {
@@ -30,9 +30,9 @@ class gps_waypoint {
 private:
     double longitude_ = 0.0;
     double latitude_ = 0.0;
+    double radius_ = 0.0; /* determined later on */
     std::string method_ = "linear";
-    double radius_ = 0.0;
-    geometry_msgs::msg::PoseStamped utm_pose_;
-    geometry_msgs::msg::Pose odom_pose;
-    geometry_msgs::msg::Pose map_pose_;
+    geometry_msgs::msg::PoseStamped utm_pose_; /* Global, stores the absolute global position UTM*/
+    geometry_msgs::msg::Pose map_pose_;        /* Global, for path generation */
+    geometry_msgs::msg::Pose odom_pose;        /* Local, for local robot space position */
 };

include/yet_another_gps_publisher/spline_factory.hpp

@@ -1,10 +1,10 @@
 #pragma once
 
-#include <functional>
-#include <geometry_msgs/msg/pose.hpp>
-#include <map>
-#include <string>
-#include <vector>
+#include <functional>                 /* for functional paradigme returing of a function */
+#include <geometry_msgs/msg/pose.hpp> /* for geoemtry_msg::msg::Pose for points */
+#include <map>                        /* for map ADT {key: '', value: ''} pair */
+#include <string>                     /* for method i.e 'linear', 'circular', etc.*/
+#include <vector>                     /* for dynamic array ADT */
 
 #include "gps_waypoint.hpp"
 
@@ -16,16 +16,22 @@ namespace gps_waypoint_spline {
 // void registerGenerator(std::string name, SplineGenerator gen);
 using SplineGenerator =
     std::function<std::vector<geometry_msgs::msg::Pose>(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 = <P_0, P_1, P_n-1>
+*/
 
 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);
+    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<geometry_msgs::msg::Pose> 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<std::string, SplineGenerator>& registry();
+    static std::map<std::string, SplineGenerator>&
+    registry(); /* {key: 'method', value: 'function()'} meyers singleton returing refrence to the map */
 };
 
 }  // namespace gps_waypoint_spline

include/yet_another_gps_publisher/yet_another_gps_publisher_node.hpp

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <list>  // for doubly linked list
+#include <list> /* for doubly linked lists*/
 #include <string>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
@@ -37,22 +37,22 @@ class yet_another_gps_publisher : public rclcpp::Node {
     double arrival_threshold = 2.0;  // Meters
 
     // Subscribers
-    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub; /* local ekf */
     rclcpp::Subscription<std_msgs::msg::String>::SharedPtr waypoint_file_sub;
 
     // GPS subscribers
-    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr gps_odom_sub;
-    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr raw_gps_sub;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr gps_odom_sub;    /* global ekf */
+    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr raw_gps_sub; /* raw gps points*/
 
     // Publisher
-    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
+    rclcpp::Publisher<nav_msgs::msg::Path>::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{};

src/spline_methods.cpp

@@ -1,5 +1,8 @@
+#include <tf2/LinearMath/Quaternion.h>
+
 #include <cmath>
 #include <stdexcept>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include "yet_another_gps_publisher/spline_factory.hpp"
 
@@ -19,8 +22,10 @@ std::map<std::string, SplineGenerator>& SplineFactory::registry() {
     return reg;
 }
 
+/* registerGenerator: inputs name (classification of spline geometry), and function of spline geometry stores them in regsitry {key: '', value: ''}*/
 void SplineFactory::registerGenerator(const std::string& name, SplineGenerator gen) { registry()[name] = gen; }
 
+/* getGenerator: inputs the name (classification of spline geometry), outputs the actual function maping in regsitry */
 SplineGenerator SplineFactory::getGenerator(const std::string& name) {
     auto it = registry().find(name);
     if (it == registry().end()) {
@@ -29,6 +34,7 @@ SplineGenerator SplineFactory::getGenerator(const std::string& name) {
     return it->second;
 }
 
+/* generate: inputs the classification, waypoint W_0 and waypoint W_n, and ouptus the dynamic array of points*/
 std::vector<geometry_msgs::msg::Pose> SplineFactory::generate(const std::string& name, const gps_waypoint& start,
                                                               const gps_waypoint& end) {
     return getGenerator(name)(start, end);
@@ -39,6 +45,12 @@ std::vector<geometry_msgs::msg::Pose> SplineFactory::generate(const std::string&
 // ------------------------------------------------------------------
 
 static std::vector<geometry_msgs::msg::Pose> linearGenerator(const gps_waypoint& start, const gps_waypoint& end) {
+    /*
+    a <- start position
+    b <- end position 
+    t <- [0, 1]
+    P(t) = a + t(b - a)
+    */
     const auto& a = start.mapPose().position;
     const auto& b = end.mapPose().position;
 
@@ -59,8 +71,9 @@ static std::vector<geometry_msgs::msg::Pose> linearGenerator(const gps_waypoint&
 }
 
 static std::vector<geometry_msgs::msg::Pose> circleGenerator(const gps_waypoint& start, const gps_waypoint& end) {
-    double R = end.radius();
-    if (R <= 0.0) {
+    double R_val = end.radius();
+
+    if (std::abs(R_val) <= 0.001) {
         return linearGenerator(start, end);
     }
 
@@ -71,11 +84,53 @@ static std::vector<geometry_msgs::msg::Pose> circleGenerator(const gps_waypoint&
     double dy = b.y - a.y;
     double chord = std::hypot(dx, dy);
 
-    double theta = 2.0 * std::asin(chord / (2.0 * R));
+    if (std::abs(R_val) < chord / 2.0) {
+        return linearGenerator(start, end);
+    }
+
+    double d = std::sqrt(R_val * R_val - (chord / 2.0) * (chord / 2.0));
+    double mx = (a.x + b.x) / 2.0;
+    double my = (a.y + b.y) / 2.0;
+    double side = (R_val > 0) ? 1.0 : -1.0;
+    double cx = mx - side * d * (dy / chord);
+    double cy = my + side * d * (dx / chord);
+
+    double angle_start = std::atan2(a.y - cy, a.x - cx);
+    double angle_end = std::atan2(b.y - cy, b.x - cx);
+
+    double diff = angle_end - angle_start;
+    if (side > 0) {
+        while (diff <= 0.0) diff += 2.0 * M_PI;
+        while (diff > 2.0 * M_PI) diff -= 2.0 * M_PI;
+    } else {
+        while (diff >= 0.0) diff -= 2.0 * M_PI;
+        while (diff < -2.0 * M_PI) diff += 2.0 * M_PI;
+    }
 
-    // TODO: Replace with actual circular arc interpolation.
-    // For now, return linear as a fallback.
-    return linearGenerator(start, end);
+    double arc_length = std::abs(R_val * diff);
+    int num_points = std::max(2, static_cast<int>(arc_length / 0.1));
+
+    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;
+        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)