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/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 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..1db783f --- /dev/null +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -0,0 +1,27 @@ +#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; } + + // 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 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..6903830 --- /dev/null +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "gps_waypoint.hpp" + +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)>; + +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 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..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 @@ -1,20 +1,58 @@ #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 "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::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::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 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); +}; \ 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 diff --git a/src/spline_methods.cpp b/src/spline_methods.cpp new file mode 100644 index 0000000..f5e4fab --- /dev/null +++ b/src/spline_methods.cpp @@ -0,0 +1,89 @@ +#include +#include + +#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& 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; // TODO this shouldnt be hardcoded like EVER + 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 diff --git a/src/yet_another_gps_publisher_node.cpp b/src/yet_another_gps_publisher_node.cpp index 16919da..b88d0f1 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" -// For _1 -using namespace std::placeholders; +#include +#include +#include +#include +#include -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); +#include "yet_another_gps_publisher/spline_factory.hpp" - // 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); +// 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"); + 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"); - // Log a sample log - RCLCPP_INFO(this->get_logger(), "You passed %f", x); + // Subscribers + odom_sub_ = this->create_subscription( + odom_topic_, 10, std::bind(&yet_another_gps_publisher::odom_callback, this, std::placeholders::_1)); - // Send a sample message - std_msgs::msg::String msg{}; - msg.data = std::string{"Hello World!"}; - pub->publish(msg); + // 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)); + + 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); +} + +// Odom callback +void yet_another_gps_publisher::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + current_pose_ = msg->pose.pose; +} + +// 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()) { + RCLCPP_ERROR(this->get_logger(), "Could not open file: %s", file_path.c_str()); + return false; + } + + 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 spline_type; + + if (!(iss >> lon >> lat >> spline_type)) { + 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); + } + } + + gps_waypoint wp(lon, lat, spline_type, 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: spline_type=%s at (%.6f, %.6f)", waypoints_.size(), + spline_type.c_str(), lon, lat); + } + file.close(); + return true; } -void yet_another_gps_publisher::sub_cb(const std_msgs::msg::String::SharedPtr msg) { - // Echo message - this->pub->publish(*msg); +// 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 + + // 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; + } } + +// 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: + // 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_); + } +} + +// 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 +// 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