diff --git a/CMakeLists.txt b/CMakeLists.txt index a42f6ce..d9e7643 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,58 +9,70 @@ 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 +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 @@ -68,4 +80,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..162346c 100644 --- a/README.md +++ b/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 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 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 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 \ 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 + +# 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.txt b/data/example_waypoints.txt new file mode 100644 index 0000000..f529760 --- /dev/null +++ b/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 \ No newline at end of file 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/gps_waypoints_parking_lot_mk1.txt b/data/gps_waypoints_parking_lot_mk1.txt new file mode 100644 index 0000000..39ce9e1 --- /dev/null +++ b/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 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 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/include/yet_another_gps_publisher/gps_waypoint.hpp b/include/yet_another_gps_publisher/gps_waypoint.hpp new file mode 100644 index 0000000..b702627 --- /dev/null +++ b/include/yet_another_gps_publisher/gps_waypoint.hpp @@ -0,0 +1,38 @@ +#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 + +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) + // 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; } + + // Store the absolute UTM pose + 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; + double radius_ = 0.0; /* determined later on */ + std::string method_ = "linear"; + 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 new file mode 100644 index 0000000..81a3cc0 --- /dev/null +++ b/include/yet_another_gps_publisher/spline_factory.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include /* for functional paradigme returing of a function */ +#include /* for geoemtry_msg::msg::Pose for points */ +#include /* for map ADT {key: '', value: ''} pair */ +#include /* for method i.e 'linear', 'circular', etc.*/ +#include /* for dynamic array ADT */ + +#include "gps_waypoint.hpp" + +namespace gps_waypoint_spline { + +// '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)>; +/* + 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); /* 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 */ + +private: + 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 1c7d6b0..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 @@ -1,20 +1,84 @@ #pragma once +#include /* for doubly linked lists*/ +#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 for localization and topics. + std::string odom_topic; + 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; + // parameters for spline configs + double min_spline_length; + /// Parameters for confidence + double max_gps_variance; // Threshold in meters squared + bool is_gps_valid = false; + bool do_gps_variance_check = false; - rclcpp::Subscription::SharedPtr sub; - geodesy::UTMPose currentPose; + double arrival_threshold = 2.0; // Meters -public: - yet_another_gps_publisher(const rclcpp::NodeOptions& options); + // Subscribers + rclcpp::Subscription::SharedPtr odom_sub; /* local ekf */ + rclcpp::Subscription::SharedPtr waypoint_file_sub; + + // GPS subscribers + rclcpp::Subscription::SharedPtr gps_odom_sub; /* global ekf */ + rclcpp::Subscription::SharedPtr raw_gps_sub; /* raw gps points*/ + + // Publisher + rclcpp::Publisher::SharedPtr path_pub; /* publishes calculated spline */ + + // Timer unused ircc + 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)*/ + + // Current robot pose (from odometry) + geometry_msgs::msg::Pose current_pose{}; + + // Doubly linked list of waypoints + std::list waypoints; + + // Iterator to next target waypoint + 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); + void timer_callback(); + + // GPS specific Callbacks + 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 + 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 + // unused ircc + 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..f39551b --- /dev/null +++ b/src/spline_methods.cpp @@ -0,0 +1,143 @@ +#include + +#include +#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; +} + +/* 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()) { + throw std::runtime_error("Unknown spline method: " + name); + } + return it->second; +} + +/* 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); +} + +// ------------------------------------------------------------------ +// 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; + + 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_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; + + double dx = b.x - a.x; + double dy = b.y - a.y; + double chord = std::hypot(dx, dy); + + 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; + } + + 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) +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..2cab769 100644 --- a/src/yet_another_gps_publisher_node.cpp +++ b/src/yet_another_gps_publisher_node.cpp @@ -1,27 +1,422 @@ #include "yet_another_gps_publisher/yet_another_gps_publisher_node.hpp" -// For _1 -using namespace std::placeholders; +#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 +#include +#include +#include +#include +#include +#include - // 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); +#include "yet_another_gps_publisher/spline_factory.hpp" - // Log a sample log - RCLCPP_INFO(this->get_logger(), "You passed %f", x); +#ifndef WAYPOINT_FILE +#define WAYPOINT_FILE "/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt" +#endif - // Send a sample message - std_msgs::msg::String msg{}; - msg.data = std::string{"Hello World!"}; - pub->publish(msg); +// 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_) { + /**********************************************************/ + /* 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); + + // 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. + 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/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. + 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. + // TODO indentify where this file should be stored? + waypoint_file_path = this->declare_parameter("waypoint_file_path", WAYPOINT_FILE); + + /**********************************************************/ + /* PUBLISHER */ + /**********************************************************/ + 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)); + + // 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) { + // 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)); + } + } +} + +// 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) { + is_gps_valid = true; + return; + } + + // 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]); + is_gps_valid = false; + return; + } + + is_gps_valid = true; +} + +// 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; /* ignore comments */ + + /* initalize fields*/ + 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); + } + } + /* create waypoint with long, lat, type and radius */ + 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; + } + /* 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; } -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 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; /* 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 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") + 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()); + /* + 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; + } + + geometry_msgs::msg::Pose robot_postion = robot_pose_map; + + 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 = + 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()); + 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 skip + + RCLCPP_INFO(this->get_logger(), "Passed waypoint (distance %.2f < %.2f)", dist, arrival_threshold); + advance_to_next_waypoint(); + checked++; + } + + /* 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++; + + // 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 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 the path + for (const auto& pose : segment) { + geometry_msgs::msg::PoseStamped ps; + ps.header = path_map.header; + ps.pose = pose; + path_map.poses.push_back(ps); + } + + // 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)); + } + + 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(); + } + } + + // 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 { + // Look up the transform from map to the robot's current body frame at *this* time + // 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); + 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; + + 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); + } +} +// 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) {} + diff --git a/tests/unit_spline.cpp b/tests/unit_spline.cpp new file mode 100644 index 0000000..36d83b4 --- /dev/null +++ b/tests/unit_spline.cpp @@ -0,0 +1,107 @@ +#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