Redtoo dev experiements

CMakeLists.txt

@@ -9,63 +9,75 @@ 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
+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)
 
-# 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}/
-#)
-
+install(DIRECTORY data
+        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,47 +1,48 @@
-An opinionated ROS2 C++ node template, optimised for ISC.
+# Functionality
 
-# Instructions
+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.  
 
-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
+# Parameters
 
-# Dependencies
-Some common extra dependencies are included. Review them and remove what you don't need.
-These are marked with yet_another_gps_publisher.
+- min_spline_length <double> 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 <std::string> "odom_topic" 
+- utm_frame_id <std::string>"utm_frame_id". Keep in mind this changes between Dearborn and Indina
+- odom_frame_id <std::string> "odom_frame_id"
+- waypoint_file_path <std::string> "waypoint_file" path
+- max_gps_variance <double> 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. 
 
-# Features
+# Subscribed Topics
 
-- 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
+# Published Topics
 
 # File structure
 
 ```
 .
 ├── 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 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 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   // Placeholders for future unit testing 
+    └── unit.cpp  
 ```
 
 yet_another_gps_publisher_NODE_NAME_node: Source files for the ROS2 node object itself, and only itself
 
 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
+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.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 

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/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