diff --git a/zebROS_ws/src/.gitignore b/zebROS_ws/src/.gitignore
index 329d6153b..9f718b462 100755
--- a/zebROS_ws/src/.gitignore
+++ b/zebROS_ws/src/.gitignore
@@ -37,4 +37,5 @@ teraranger_description
turing_smart_screen
ydlidar
zed-ros-wrapper
+smooth
*.egg-info
diff --git a/zebROS_ws/src/.rosinstall b/zebROS_ws/src/.rosinstall
index 475825e0d..dae75531e 100644
--- a/zebROS_ws/src/.rosinstall
+++ b/zebROS_ws/src/.rosinstall
@@ -68,3 +68,6 @@
local-name: cv_camera
uri: https://github.com/FRC900/cv_camera
version: mono16
+- git:
+ local-name: smooth
+ uri: https://github.com/pettni/smooth/
\ No newline at end of file
diff --git a/zebROS_ws/src/auto_node/paths/2025_4_Note0_blue.csv b/zebROS_ws/src/auto_node/paths/2025_4_Note0_blue.csv
index 03dc31005..666be1aa2 100644
--- a/zebROS_ws/src/auto_node/paths/2025_4_Note0_blue.csv
+++ b/zebROS_ws/src/auto_node/paths/2025_4_Note0_blue.csv
@@ -1,21 +1,23 @@
-0.0,1.3475940227508545,5.588765621185303,3.1415926535897927,0.0,0.0,0.0,1
-0.04024229871318875,1.3621203259005277,5.572484480288233,3.137516245083241,-0.10129661170008848,0.24064733898034257,-0.2697185369239833,1
-0.0804845974263775,1.3863285970291863,5.545350632383985,3.1293639837951717,-0.2025794139247941,0.4812576668079537,-0.5394140976223522,1
-0.12072689613956623,1.420216958721915,5.507365497590794,3.117136616727843,-0.30384365353544857,0.721824555734495,-0.8090784734503345,1
-0.160969194852755,1.463783093898426,5.458531031544731,3.1008350261246895,-0.4050859698597918,0.9623385795414152,-1.0787000566349692,1
-0.20121149356594373,1.5170240028539397,5.398849996691044,3.080460155769061,-0.506304833663133,1.2027852875822915,-1.3482616120622144,1
-0.24145379227913247,1.5799355575159946,5.328326457294214,3.0560129309522104,-0.6075007044643156,1.4431411700785943,-1.6177358283442955,1
-0.28169609099232124,1.6525115907115813,5.24696678901991,3.0274941935109396,-0.7086756560623695,1.6833646100519275,-1.8870753102811255,1
-0.32193838970551,1.7347417145480797,5.154782098226876,2.9949046897173175,-0.8098320631906188,1.9233718133982327,-2.156185889312798,1
-0.3621806884186987,1.8266045487563909,5.0517957397774165,2.9582451586569336,-0.91097010442882,2.162952745559081,-2.424833263329828,1
-0.40242298713188746,1.9280323189046995,4.938082656168727,2.917516299014205,-1.0120907837246809,2.4012688146207157,-2.6920847145297584,1
-0.4426652858450762,2.024623521765578,4.829722428231165,2.8764958194475576,-1.0193373857599208,2.400583398709476,-2.6924914291065063,1
-0.48290758455826494,2.106842124621578,4.737501841160799,2.8395472668181436,-0.9181521387017788,2.162253882738708,-2.4252525980211503,1
-0.5231498832714537,2.1793950754621716,4.656120739336731,2.806682815679028,-0.8166643603910583,1.9226864580014735,-2.1566026418142292,1
-0.5633921819846425,2.2422826445081223,4.585579310371776,2.7779111762571125,-0.7149601375151765,1.682710873571503,-1.887479202513453,1
-0.6036344806978312,2.295500958248499,4.525882441857388,2.75323912225895,-0.6130875915878863,1.4425350661016465,-1.618116966126617,1
-0.64387677941102,2.339046975530622,4.477034089619105,2.732671997989585,-0.5110822424870104,1.2022421369199714,-1.348609597252295,1
-0.6841190781242087,2.3729184830864294,4.439037277982849,2.716213958210034,-0.408973649756458,0.9618735014679554,-1.079003748941,1
-0.7243613768373974,2.397113854378065,4.411894368690093,2.703868105555916,-0.3067879581608221,0.7214526848511779,-0.8093259487934356,1
-0.7646036755505862,2.41163186911327,4.395607262398508,2.6956365803095776,-0.20454908167257763,0.4809942587647568,-0.5395926778034363,1
-0.8048459742637749,2.4164715932383283,4.3901775330574635,2.6915206236546805,-0.10227936242294676,0.24050782235873255,-0.2698148419888686,1
+0.0,1.34759,5.58877,3.14159,0.0,0.0,0.0,1
+0.03674,1.35163,5.58424,3.14159,-0.0928,0.21968,-0.24621,1
+0.07348,1.36374,5.57067,3.13818,-0.1856,0.43933,-0.49239,1
+0.11022,1.38391,5.54806,3.13136,-0.27838,0.65895,-0.73856,1
+0.14696,1.41215,5.51641,3.12114,-0.37114,0.87851,-0.98468,1
+0.1837,1.44846,5.47571,3.1075,-0.46388,1.09802,-1.23076,1
+0.22044,1.49284,5.42597,3.09046,-0.55659,1.31746,-1.47677,1
+0.25718,1.54527,5.3672,3.07001,-0.64927,1.53679,-1.72268,1
+0.29392,1.60576,5.29939,3.04615,-0.74191,1.75599,-1.96845,1
+0.33066,1.67429,5.22256,3.0189,-0.8345,1.97494,-2.21397,1
+0.3674,1.75087,5.13672,2.98824,-0.92703,2.19343,-2.45898,1
+0.40414,1.83544,5.0419,2.95418,-1.0195,2.41054,-2.70246,1
+0.44088,1.92399,4.94261,2.91672,-1.02569,2.40983,-2.70292,1
+0.47762,2.00854,4.84777,2.87904,-0.93324,2.1927,-2.45945,1
+0.51436,2.08509,4.76191,2.84475,-0.84053,1.97422,-2.21444,1
+0.5511,2.1536,4.68507,2.81387,-0.74761,1.75528,-1.96892,1
+0.58784,2.21406,4.61724,2.7864,-0.65451,1.53613,-1.72313,1
+0.62458,2.26647,4.55845,2.76236,-0.56128,1.31684,-1.47718,1
+0.66132,2.31082,4.5087,2.74173,-0.46793,1.09747,-1.23113,1
+0.69806,2.34711,4.46799,2.72454,-0.37447,0.87804,-0.985,1
+0.7348,2.37534,4.43633,2.71078,-0.28093,0.65857,-0.73882,1
+0.77154,2.3955,4.4137,2.70046,-0.18733,0.43907,-0.49258,1
+0.80828,2.4076,4.40013,2.69358,-0.09368,0.21954,-0.24631,1
diff --git a/zebROS_ws/src/auto_node/paths/2025_4_Note0_red.csv b/zebROS_ws/src/auto_node/paths/2025_4_Note0_red.csv
index 8ca87cace..3dcca21d0 100644
--- a/zebROS_ws/src/auto_node/paths/2025_4_Note0_red.csv
+++ b/zebROS_ws/src/auto_node/paths/2025_4_Note0_red.csv
@@ -1,21 +1,23 @@
-0.0,15.192405977249145,5.588765621185303,4.440892098500626e-16,-0.0,-0.0,0.0,1
-0.04024229871318875,15.177879674099472,5.572484480288233,0.004076408506552109,0.10129661170008848,-0.24064733898034257,-0.2697185369239833,1
-0.0804845974263775,15.153671402970813,5.545350632383985,0.01222866979462145,0.2025794139247941,-0.4812576668079537,-0.5394140976223522,1
-0.12072689613956623,15.119783041278085,5.507365497590794,0.024456036861950192,0.30384365353544857,-0.721824555734495,-0.8090784734503345,1
-0.160969194852755,15.076216906101573,5.458531031544731,0.040757627465103585,0.4050859698597918,-0.9623385795414152,-1.0787000566349692,1
-0.20121149356594373,15.022975997146059,5.398849996691044,0.061132497820731935,0.506304833663133,-1.2027852875822915,-1.3482616120622144,1
-0.24145379227913247,14.960064442484004,5.328326457294214,0.08557972263758273,0.6075007044643156,-1.4431411700785943,-1.6177358283442955,1
-0.28169609099232124,14.887488409288418,5.24696678901991,0.11409846007885349,0.7086756560623695,-1.6833646100519275,-1.8870753102811255,1
-0.32193838970551,14.80525828545192,5.154782098226876,0.14668796387247562,0.8098320631906188,-1.9233718133982327,-2.156185889312798,1
-0.3621806884186987,14.713395451243608,5.0517957397774165,0.1833474949328595,0.91097010442882,-2.162952745559081,-2.424833263329828,1
-0.40242298713188746,14.6119676810953,4.938082656168727,0.22407635457558817,1.0120907837246809,-2.4012688146207157,-2.6920847145297584,1
-0.4426652858450762,14.51537647823442,4.829722428231165,0.2650968341422355,1.0193373857599208,-2.400583398709476,-2.6924914291065063,1
-0.48290758455826494,14.433157875378422,4.737501841160799,0.3020453867716495,0.9181521387017788,-2.162253882738708,-2.4252525980211503,1
-0.5231498832714537,14.360604924537828,4.656120739336731,0.3349098379107649,0.8166643603910583,-1.9226864580014735,-2.1566026418142292,1
-0.5633921819846425,14.297717355491876,4.585579310371776,0.36368147733268064,0.7149601375151765,-1.682710873571503,-1.887479202513453,1
-0.6036344806978312,14.2444990417515,4.525882441857388,0.38835353133084327,0.6130875915878863,-1.4425350661016465,-1.618116966126617,1
-0.64387677941102,14.200953024469378,4.477034089619105,0.40892065560020807,0.5110822424870104,-1.2022421369199714,-1.348609597252295,1
-0.6841190781242087,14.16708151691357,4.439037277982849,0.4253786953797589,0.408973649756458,-0.9618735014679554,-1.079003748941,1
-0.7243613768373974,14.142886145621935,4.411894368690093,0.43772454803387717,0.3067879581608221,-0.7214526848511779,-0.8093259487934356,1
-0.7646036755505862,14.128368130886729,4.395607262398508,0.44595607328021547,0.20454908167257763,-0.4809942587647568,-0.5395926778034363,1
-0.8048459742637749,14.123528406761672,4.3901775330574635,0.45007202993511264,0.10227936242294676,-0.24050782235873255,-0.2698148419888686,1
+0.0,15.192409999999999,5.58877,2.6535897932333796e-06,-0.0,-0.0,0.0,1
+0.03674,15.188369999999999,5.58424,2.6535897932333796e-06,0.0928,-0.21968,-0.24621,1
+0.07348,15.17626,5.57067,0.0034126535897929244,0.1856,-0.43933,-0.49239,1
+0.11022,15.156089999999999,5.54806,0.010232653589793195,0.27838,-0.65895,-0.73856,1
+0.14696,15.127849999999999,5.51641,0.02045265358979309,0.37114,-0.87851,-0.98468,1
+0.1837,15.091539999999998,5.47571,0.03409265358979319,0.46388,-1.09802,-1.23076,1
+0.22044,15.04716,5.42597,0.05113265358979291,0.55659,-1.31746,-1.47677,1
+0.25718,14.994729999999999,5.3672,0.07158265358979321,0.64927,-1.53679,-1.72268,1
+0.29392,14.934239999999999,5.29939,0.0954426535897932,0.74191,-1.75599,-1.96845,1
+0.33066,14.86571,5.22256,0.1226926535897932,0.8345,-1.97494,-2.21397,1
+0.3674,14.78913,5.13672,0.15335265358979333,0.92703,-2.19343,-2.45898,1
+0.40414,14.704559999999999,5.0419,0.1874126535897931,1.0195,-2.41054,-2.70246,1
+0.44088,14.61601,4.94261,0.22487265358979291,1.02569,-2.40983,-2.70292,1
+0.47762,14.53146,4.84777,0.2625526535897933,0.93324,-2.1927,-2.45945,1
+0.51436,14.454909999999998,4.76191,0.2968426535897932,0.84053,-1.97422,-2.21444,1
+0.5511,14.386399999999998,4.68507,0.327722653589793,0.74761,-1.75528,-1.96892,1
+0.58784,14.32594,4.61724,0.35519265358979313,0.65451,-1.53613,-1.72313,1
+0.62458,14.27353,4.55845,0.37923265358979297,0.56128,-1.31684,-1.47718,1
+0.66132,14.22918,4.5087,0.3998626535897931,0.46793,-1.09747,-1.23113,1
+0.69806,14.192889999999998,4.46799,0.41705265358979293,0.37447,-0.87804,-0.985,1
+0.7348,14.16466,4.43633,0.4308126535897929,0.28093,-0.65857,-0.73882,1
+0.77154,14.144499999999999,4.4137,0.44113265358979303,0.18733,-0.43907,-0.49258,1
+0.80828,14.132399999999999,4.40013,0.44801265358979325,0.09368,-0.21954,-0.24631,1
diff --git a/zebROS_ws/src/auto_node/paths/2025_4_Note1_blue.csv b/zebROS_ws/src/auto_node/paths/2025_4_Note1_blue.csv
index 44d6442cb..b1ce88d7d 100644
--- a/zebROS_ws/src/auto_node/paths/2025_4_Note1_blue.csv
+++ b/zebROS_ws/src/auto_node/paths/2025_4_Note1_blue.csv
@@ -1,37 +1,35 @@
-0.0,2.411632299423218,4.395606517791748,2.6915206236546743,0.0,0.0,0.0,2
-0.03324619866041034,2.4023270954041607,4.4072848073075415,2.692935154067154,0.04254713229535838,-0.18659183894712625,0.23417794086399177,2
-0.0664923973208208,2.386916231898057,4.42681441474642,2.696063143886176,0.09408563822513046,-0.37122235373420703,0.46967516196397,2
-0.09973859598123114,2.3654925423305664,4.454257636732856,2.701186532446313,0.1541044921624097,-0.5533375591616682,0.7068613410857268,2
-0.13298479464164137,2.338177706320519,4.489694925198005,2.7085866763097264,0.22258616511520804,-0.7321743575782187,0.946223761953224,2
-0.1662309933020517,2.30513648036578,4.533232414814746,2.7185592366056275,0.299960918814996,-0.9066145875229974,1.1884398535424257,2
-0.19947719196246216,2.266602365705525,4.5850144637090375,2.7314310168434233,0.387165473292175,-1.0749070302329613,1.4345018981638546,2
-0.23272339062287284,2.2229283233980173,4.645245557290312,2.7475836389801103,0.485848692168354,-1.2340724879146827,1.6859463539987638,2
-0.26596958928328307,2.1747007016309823,4.7142302166131875,2.7674946738208397,0.5988965850332568,-1.3784379184017863,1.945291330328932,2
-0.2992157879436935,2.1230467456029376,4.792441614448432,2.791829542737564,0.7319594389660657,-1.495266014777542,2.2167580715943354,2
-0.332461986604104,2.0707012673220633,4.880507688493236,2.821742138560954,0.8997298046433111,-1.5480754067189986,2.5048570702445687,2
-0.36570818526451443,2.0256435112956157,4.976674548138118,2.8586378353246973,1.109771891292374,-1.4195420857189591,2.763330407367688,2
-0.39895438392492466,1.993127368688955,5.069815387246817,2.8982536890686372,1.1915904777565152,-1.1252079728789806,2.7888091582667833,2
-0.43220058258533534,1.9697538206742655,5.157644561524588,2.9376563658352537,1.1851784070182405,-0.8437654751599463,2.690790352691309,2
-0.4654467812457458,1.9547954992803749,5.240032200341575,2.9756191635443097,1.141868822291494,-0.5812056689524476,2.549001474824365,2
-0.49869297990615624,1.94796306012166,5.316686870326293,3.0114935358661863,1.0790518545945855,-0.3307421477649113,2.38677862912681,2
-0.5319391785665667,1.9490613225282845,5.387372052704344,3.0448845179003707,1.004354885080604,-0.08822456592717412,2.213001687531107,2
-0.5651853772269771,1.9579463384403,5.4519120081273185,3.0755356275501846,0.9219432863156622,0.14875786281729125,2.031849381793251,2
-0.5984315758873876,1.9745080190905413,5.5101742720356945,3.1032802433880757,0.8345199438243036,0.3816876945735098,1.8455823866467025,2
-0.631677774547798,1.998659954246574,5.562055706520936,3.128021475263331,0.7441822786544319,0.6115339265817427,1.655542330594926,2
-0.6649239732082085,2.030333042144775,5.607472896575928,-3.1334549346578986,0.6529738175594688,0.8389666345706398,1.462571914999187,2
-0.6995988410741018,2.071691855146622,5.647602215013633,-3.113708736623575,0.5694671457927261,1.0748292166894824,1.2590591095056272,2
-0.734273708939995,2.121064147232268,5.680510638528707,-3.0971757612678075,0.47679995262581065,1.3075189958372384,1.05239098733796,2
-0.7689485768058882,2.1782806679099305,5.706024939544298,-3.084067592608194,0.3780308178910271,1.5358971266648729,0.8413405813000495,2
-0.8036234446717815,2.2430992280055717,5.7239051687225695,-3.074538929388032,0.2748002748559455,1.7581816588443486,0.6242160950160296,2
-0.8382983125376747,2.315148208038151,5.733806574377176,-3.068726994874275,0.167612304571673,1.9712901532690643,0.39843864326514267,2
-0.872973180403568,2.393795736130065,5.7352039106709345,-3.0667839138546857,0.056037157143396785,2.1691914601903024,0.15967837723894868,2
-0.9076480482694612,2.4777843260111427,5.7272400130777,-3.068919344741569,-0.06158439868391186,2.3378469958957053,-0.0998895280087525,2
-0.9423229161353545,2.56391911369009,5.708550677042603,-3.075426806854445,-0.18767085539145575,2.4353287805934154,-0.39262181101926935,2
-0.9769977840012475,2.6426806945105965,5.679950427116911,-3.0855506550819887,-0.2919650124350504,2.326063805577348,-0.6807486577273891,2
-1.011672651867141,2.707817424740947,5.651874379601165,-3.0960513564456393,-0.302833204854365,2.027687761084963,-0.766712508672808,2
-1.0463475197330339,2.762036909698944,5.627110530064436,-3.105411722143775,-0.2699466868734228,1.7183317111807628,-0.7316860758939688,2
-1.0810223875989275,2.8060098664589557,5.606105146327541,-3.1131722456716058,-0.2238083085949471,1.4182110140555222,-0.6477495048702906,2
-1.1156972554648208,2.839979546320837,5.589286379486456,-3.119215300450425,-0.1742776584514888,1.1258455710635784,-0.5392778269139594,2
-1.150372123330714,2.864103910202422,5.576983742607744,-3.123564583477376,-0.12543041385844142,0.839102204270713,-0.41629248899762944,2
-1.1850469911966068,2.87850040434493,5.569443019968318,-3.126316419122145,-0.07936109966980372,0.5564909102462919,-0.2837437616447357,2
-1.2197218590625,2.883260537001941,5.5668513121346335,-3.127609942808588,-0.03730435805642609,0.27701633170200374,-0.14441000100583004,2
+0.0,2.41163,4.39561,2.69014,0.0,0.0,0.0,2
+0.033340000000000036,2.40851,4.39951,2.69014,0.0481,-0.18745,0.23444,2
+0.06667000000000001,2.39917,4.41126,2.69174,0.10316,-0.3729,0.47026,2
+0.10001000000000004,2.38369,4.4309,2.69518,0.16526,-0.55582,0.70781,2
+0.13334999999999997,2.36216,4.45849,2.70069,0.23473,-0.73546,0.94757,2
+0.16668000000000005,2.33472,4.49412,2.70852,0.31223,-0.91071,1.19022,2
+0.20001999999999998,2.30155,4.53791,2.71892,0.39885,-1.07982,1.43675,2
+0.2333599999999999,2.26288,4.59001,2.73222,0.4963,-1.23982,1.68869,2
+0.26669,2.21913,4.65063,2.74877,0.6074,-1.38505,1.94859,2
+0.3000299999999999,2.17099,4.72013,2.76901,0.73744,-1.50273,2.22074,2
+0.33337000000000006,2.12001,4.79898,2.7936,0.89957,-1.55606,2.5099,2
+0.3666999999999999,2.07032,4.88697,2.82359,1.10345,-1.4252,2.7686,2
+0.40004000000000006,2.02773,4.97964,2.86037,1.18119,-1.12967,2.7911,2
+0.43338,1.99476,5.07102,2.89975,1.17433,-0.84801,2.69117,2
+0.4667199999999999,1.97087,5.15835,2.9389,1.13159,-0.58524,2.54799,2
+0.50005,1.95554,5.24057,2.97662,1.06955,-0.33452,2.38462,2
+0.5333899999999999,1.94844,5.31715,3.01228,0.99561,-0.09168,2.20982,2
+0.5667300000000001,1.94934,5.38778,3.04547,0.9139,0.14565,2.02774,2
+0.6000599999999999,1.95808,5.45226,3.07594,0.82711,0.37897,1.8406,2
+0.6334000000000001,1.97456,5.51044,3.10351,0.73735,0.60922,1.64972,2
+0.66674,1.99866,5.56221,3.12809,0.64665,0.83706,1.45593,2
+0.7000700000000001,2.03033,5.60747,-3.13354,0.55765,1.06297,1.25974,2
+0.73992,2.078,5.65292,-3.11132,0.45197,1.32989,1.02163,2
+0.7797599999999999,2.13619,5.68877,-3.09331,0.3395,1.59104,0.7776,2
+0.8196100000000001,2.20462,5.71472,-3.07978,0.22216,1.84345,0.52494,2
+0.8594499999999999,2.28281,5.73034,-3.07093,0.10021,2.08142,0.25905,2
+0.89929,2.36993,5.73492,-3.06694,-0.0277,2.29157,-0.02868,2
+0.93914,2.46404,5.72728,-3.06804,-0.16487,2.43242,-0.35491,2
+0.9789800000000001,2.55908,5.70633,-3.07461,-0.28601,2.33798,-0.69668,2
+1.01883,2.64532,5.67689,-3.08601,-0.29236,1.99092,-0.78141,2
+1.0586700000000002,2.7176,5.64691,-3.09766,-0.25101,1.6376,-0.72342,2
+1.09851,2.77606,5.62028,-3.10766,-0.19766,1.29664,-0.61314,2
+1.13836,2.82111,5.59855,-3.11553,-0.1426,0.96473,-0.4775,2
+1.1782,2.85306,5.58252,-3.12122,-0.09002,0.63905,-0.32707,2
+1.2180500000000003,2.87213,5.57268,-3.1248,-0.04209,0.31785,-0.16696,2
diff --git a/zebROS_ws/src/auto_node/paths/2025_4_Note1_red.csv b/zebROS_ws/src/auto_node/paths/2025_4_Note1_red.csv
index cec3dd9d8..89df7de65 100644
--- a/zebROS_ws/src/auto_node/paths/2025_4_Note1_red.csv
+++ b/zebROS_ws/src/auto_node/paths/2025_4_Note1_red.csv
@@ -1,37 +1,35 @@
-0.0,14.128367700576781,4.395606517791748,0.45007202993511886,-0.0,-0.0,0.0,2
-0.03324619866041034,14.137672904595838,4.4072848073075415,0.4486574995226391,-0.04254713229535838,0.18659183894712625,0.23417794086399177,2
-0.0664923973208208,14.153083768101942,4.42681441474642,0.44552950970361715,-0.09408563822513046,0.37122235373420703,0.46967516196397,2
-0.09973859598123114,14.174507457669433,4.454257636732856,0.44040612114348,-0.1541044921624097,0.5533375591616682,0.7068613410857268,2
-0.13298479464164137,14.20182229367948,4.489694925198005,0.43300597728006673,-0.22258616511520804,0.7321743575782187,0.946223761953224,2
-0.1662309933020517,14.234863519634219,4.533232414814746,0.42303341698416563,-0.299960918814996,0.9066145875229974,1.1884398535424257,2
-0.19947719196246216,14.273397634294474,4.5850144637090375,0.41016163674636985,-0.387165473292175,1.0749070302329613,1.4345018981638546,2
-0.23272339062287284,14.317071676601982,4.645245557290312,0.39400901460968285,-0.485848692168354,1.2340724879146827,1.6859463539987638,2
-0.26596958928328307,14.365299298369017,4.7142302166131875,0.3740979797689534,-0.5988965850332568,1.3784379184017863,1.945291330328932,2
-0.2992157879436935,14.416953254397061,4.792441614448432,0.349763110852229,-0.7319594389660657,1.495266014777542,2.2167580715943354,2
-0.332461986604104,14.469298732677936,4.880507688493236,0.3198505150288393,-0.8997298046433111,1.5480754067189986,2.5048570702445687,2
-0.36570818526451443,14.514356488704383,4.976674548138118,0.28295481826509583,-1.109771891292374,1.4195420857189591,2.763330407367688,2
-0.39895438392492466,14.546872631311045,5.069815387246817,0.2433389645211559,-1.1915904777565152,1.1252079728789806,2.7888091582667833,2
-0.43220058258533534,14.570246179325734,5.157644561524588,0.20393628775453942,-1.1851784070182405,0.8437654751599463,2.690790352691309,2
-0.4654467812457458,14.585204500719625,5.240032200341575,0.16597349004548345,-1.141868822291494,0.5812056689524476,2.549001474824365,2
-0.49869297990615624,14.592036939878339,5.316686870326293,0.13009911772360683,-1.0790518545945855,0.3307421477649113,2.38677862912681,2
-0.5319391785665667,14.590938677471716,5.387372052704344,0.09670813568942238,-1.004354885080604,0.08822456592717412,2.213001687531107,2
-0.5651853772269771,14.5820536615597,5.4519120081273185,0.0660570260396085,-0.9219432863156622,-0.14875786281729125,2.031849381793251,2
-0.5984315758873876,14.565491980909458,5.5101742720356945,0.038312410201717384,-0.8345199438243036,-0.3816876945735098,1.8455823866467025,2
-0.631677774547798,14.541340045753426,5.562055706520936,0.013571178326462263,-0.7441822786544319,-0.6115339265817427,1.655542330594926,2
-0.6649239732082085,14.509666957855224,5.607472896575928,6.275047588247691,-0.6529738175594688,-0.8389666345706398,1.462571914999187,2
-0.6995988410741018,14.468308144853378,5.647602215013633,6.2553013902133685,-0.5694671457927261,-1.0748292166894824,1.2590591095056272,2
-0.734273708939995,14.418935852767731,5.680510638528707,6.238768414857601,-0.47679995262581065,-1.3075189958372384,1.05239098733796,2
-0.7689485768058882,14.361719332090068,5.706024939544298,6.225660246197988,-0.3780308178910271,-1.5358971266648729,0.8413405813000495,2
-0.8036234446717815,14.296900771994427,5.7239051687225695,6.216131582977825,-0.2748002748559455,-1.7581816588443486,0.6242160950160296,2
-0.8382983125376747,14.224851791961848,5.733806574377176,6.210319648464068,-0.167612304571673,-1.9712901532690643,0.39843864326514267,2
-0.872973180403568,14.146204263869935,5.7352039106709345,6.208376567444478,-0.056037157143396785,-2.1691914601903024,0.15967837723894868,2
-0.9076480482694612,14.062215673988856,5.7272400130777,6.210511998331362,0.06158439868391186,-2.3378469958957053,-0.0998895280087525,2
-0.9423229161353545,13.976080886309909,5.708550677042603,6.217019460444238,0.18767085539145575,-2.4353287805934154,-0.39262181101926935,2
-0.9769977840012475,13.897319305489402,5.679950427116911,6.227143308671781,0.2919650124350504,-2.326063805577348,-0.6807486577273891,2
-1.011672651867141,13.832182575259052,5.651874379601165,6.2376440100354325,0.302833204854365,-2.027687761084963,-0.766712508672808,2
-1.0463475197330339,13.777963090301055,5.627110530064436,6.247004375733568,0.2699466868734228,-1.7183317111807628,-0.7316860758939688,2
-1.0810223875989275,13.733990133541043,5.606105146327541,6.254764899261399,0.2238083085949471,-1.4182110140555222,-0.6477495048702906,2
-1.1156972554648208,13.700020453679162,5.589286379486456,6.260807954040218,0.1742776584514888,-1.1258455710635784,-0.5392778269139594,2
-1.150372123330714,13.675896089797577,5.576983742607744,6.26515723706717,0.12543041385844142,-0.839102204270713,-0.41629248899762944,2
-1.1850469911966068,13.66149959565507,5.569443019968318,6.267909072711938,0.07936109966980372,-0.5564909102462919,-0.2837437616447357,2
-1.2197218590625,13.656739462998058,5.5668513121346335,6.269202596398381,0.03730435805642609,-0.27701633170200374,-0.14441000100583004,2
+0.0,14.128369999999999,4.39561,0.45145265358979314,-0.0,-0.0,0.0,2
+0.033340000000000036,14.13149,4.39951,0.45145265358979314,-0.0481,0.18745,0.23444,2
+0.06667000000000001,14.14083,4.41126,0.4498526535897933,-0.10316,0.3729,0.47026,2
+0.10001000000000004,14.15631,4.4309,0.446412653589793,-0.16526,0.55582,0.70781,2
+0.13334999999999997,14.17784,4.45849,0.4409026535897933,-0.23473,0.73546,0.94757,2
+0.16668000000000005,14.205279999999998,4.49412,0.4330726535897931,-0.31223,0.91071,1.19022,2
+0.20001999999999998,14.238449999999998,4.53791,0.42267265358979333,-0.39885,1.07982,1.43675,2
+0.2333599999999999,14.27712,4.59001,0.40937265358979325,-0.4963,1.23982,1.68869,2
+0.26669,14.32087,4.65063,0.3928226535897932,-0.6074,1.38505,1.94859,2
+0.3000299999999999,14.36901,4.72013,0.3725826535897929,-0.73744,1.50273,2.22074,2
+0.33337000000000006,14.419989999999999,4.79898,0.34799265358979303,-0.89957,1.55606,2.5099,2
+0.3666999999999999,14.469679999999999,4.88697,0.3180026535897933,-1.10345,1.4252,2.7686,2
+0.40004000000000006,14.51227,4.97964,0.28122265358979304,-1.18119,1.12967,2.7911,2
+0.43338,14.54524,5.07102,0.24184265358979307,-1.17433,0.84801,2.69117,2
+0.4667199999999999,14.56913,5.15835,0.20269265358979327,-1.13159,0.58524,2.54799,2
+0.50005,14.58446,5.24057,0.16497265358979307,-1.06955,0.33452,2.38462,2
+0.5333899999999999,14.59156,5.31715,0.12931265358979305,-0.99561,0.09168,2.20982,2
+0.5667300000000001,14.59066,5.38778,0.09612265358979322,-0.9139,-0.14565,2.02774,2
+0.6000599999999999,14.581919999999998,5.45226,0.065652653589793,-0.82711,-0.37897,1.8406,2
+0.6334000000000001,14.565439999999999,5.51044,0.038082653589793125,-0.73735,-0.60922,1.64972,2
+0.66674,14.541339999999998,5.56221,0.013502653589793301,-0.64665,-0.83706,1.45593,2
+0.7000700000000001,14.50967,5.60747,6.275132653589793,-0.55765,-1.06297,1.25974,2
+0.73992,14.462,5.65292,6.252912653589793,-0.45197,-1.32989,1.02163,2
+0.7797599999999999,14.40381,5.68877,6.234902653589793,-0.3395,-1.59104,0.7776,2
+0.8196100000000001,14.335379999999999,5.71472,6.221372653589793,-0.22216,-1.84345,0.52494,2
+0.8594499999999999,14.25719,5.73034,6.212522653589794,-0.10021,-2.08142,0.25905,2
+0.89929,14.170069999999999,5.73492,6.208532653589794,0.0277,-2.29157,-0.02868,2
+0.93914,14.075959999999998,5.72728,6.209632653589793,0.16487,-2.43242,-0.35491,2
+0.9789800000000001,13.98092,5.70633,6.216202653589793,0.28601,-2.33798,-0.69668,2
+1.01883,13.89468,5.67689,6.227602653589793,0.29236,-1.99092,-0.78141,2
+1.0586700000000002,13.822399999999998,5.64691,6.2392526535897925,0.25101,-1.6376,-0.72342,2
+1.09851,13.763939999999998,5.62028,6.249252653589793,0.19766,-1.29664,-0.61314,2
+1.13836,13.718889999999998,5.59855,6.257122653589793,0.1426,-0.96473,-0.4775,2
+1.1782,13.68694,5.58252,6.262812653589793,0.09002,-0.63905,-0.32707,2
+1.2180500000000003,13.667869999999999,5.57268,6.266392653589794,0.04209,-0.31785,-0.16696,2
diff --git a/zebROS_ws/src/auto_node/paths/2025_4_Note2_blue.csv b/zebROS_ws/src/auto_node/paths/2025_4_Note2_blue.csv
index 76a7f1f6f..6e34a4ce5 100644
--- a/zebROS_ws/src/auto_node/paths/2025_4_Note2_blue.csv
+++ b/zebROS_ws/src/auto_node/paths/2025_4_Note2_blue.csv
@@ -1,34 +1,33 @@
-0.0,2.8784577846527104,5.569355010986328,-3.1276099428085837,0.0,0.0,0.0,3
-0.029308688805128824,2.87033136235436,5.577592410411832,-3.125503648898436,0.07186585262191755,-0.18484671564764116,0.1873710439082446,3
-0.05861737761025765,2.8569129918481844,5.591434338977022,-3.121172038465145,0.1477927061907437,-0.36683496934274934,0.3773108260141282,3
-0.08792606641538647,2.8383155313697723,5.610979013790693,-3.11449863709109,0.2276936173644043,-0.5453032857130032,0.5703409381794171,3
-0.1172347552205153,2.814682214096208,5.6363467151077415,-3.105366591401945,0.311581515983696,-0.7193402529086199,0.7671371011338004,3
-0.14654344402564412,2.786200159739589,5.667687194625604,-3.0936559303756246,0.3995627748650136,-0.8876438070369492,0.9685949238461176,3
-0.17585213283077294,2.753122834084182,5.705190414974579,-3.0792407987584327,0.49183816148815185,-1.0482707936190598,1.1759275959269713,3
-0.20516082163590177,2.715809724734423,5.7491020278878615,-3.061986685069261,0.5887030226520579,-1.1981618815460635,1.3908062395877947,3
-0.2344695104410306,2.6748016539900186,5.799744177622278,-3.041748958129249,0.6905026381406636,-1.3321725853155086,1.6155278238202602,3
-0.2637781992461594,2.6309754621204777,5.857534108113018,-3.0183810806088203,0.7973020450062094,-1.4409448829232012,1.853021178681236,3
-0.29308688805128824,2.585880941081286,5.922943816804184,-2.991797068763848,0.9070351806603422,-1.5060522167763903,2.1055080251515848,3
-0.32239557685641707,2.542396551279928,5.996089871412109,-2.962250640929583,1.008111554596076,-1.4911299992551743,2.3656441961029326,3
-0.3517042656615459,2.5047946689742915,6.075155982976428,-2.930793293517205,1.0733113180777716,-1.3523501570703889,2.5870161371858553,3
-0.3810129544666747,2.4754317222105695,6.156218802002761,-2.899044953102452,1.083239875587212,-1.1186842246745563,2.706224657283879,3
-0.41032164327180354,2.454118326713524,6.2368504893029195,-2.8681763387057244,1.0532239979098992,-0.8576974028384646,2.7361541431963747,3
-0.43963033207693236,2.440486306459488,6.316007068426159,-2.838807770904917,1.002043045874837,-0.5959782907751577,2.712577262646103,3
-0.4689390208820612,2.434314190409637,6.393076908044145,-2.8112592675251493,0.9399432217182389,-0.3390527470863854,2.657252526036485,3
-0.49824770968719,2.435445977580152,6.467637528581923,-2.7856942079742715,0.8722689616345112,-0.08727351768008186,2.5817352884340834,3
-0.5275563984923188,2.4437605433099927,6.539379856369902,-2.7621803683252777,0.8022822107585654,0.16003512567702574,2.492456909041754,3
-0.5568650872974477,2.459160566329956,6.608068943023682,-2.7407116031710914,0.7325051385588603,0.4036398668015253,2.393247256402183,3
-0.5852265836547166,2.480604709426916,6.671741372917682,-2.721557514437811,0.6753553653167714,0.6386136580076626,2.294436215838802,3
-0.6135880800119855,2.5083841424173468,6.73192868726267,-2.704272068660128,0.6094687515763264,0.8658558771384738,2.1795779123849948,3
-0.6419495763692544,2.5420855377550016,6.787964556423057,-2.689097013812934,0.5350583289419562,1.0808052088446234,2.0437077455622945,3
-0.6703110727265233,2.58100824283041,6.838978131954027,-2.676279995916424,0.45191613781839335,1.275187338232881,1.8803634198003536,3
-0.6986725690837927,2.6239275558314015,6.883901438014632,-2.6660603093537274,0.3603366491659622,1.4339258351442017,1.6827571507350183,3
-0.7270340654410612,2.6687857811835074,6.9217045950597536,-2.658544384583666,0.2650045214608014,1.5324148323228457,1.4495218368990526,3
-0.7553955617983301,2.7127491024366095,6.952047330355769,-2.6534867124733053,0.17832881758617003,1.5442088402278438,1.1964117079332068,3
-0.783757058155599,2.7532170917788363,6.975698525511612,-2.6503562234957476,0.11037813160931202,1.4659787399812811,0.9547500078359786,3
-0.812118554512868,2.7886523334618,6.993853814680668,-2.64856819820428,0.06304410983623464,1.3216020368276495,0.7450090833119908,3
-0.8404800508701369,2.81828652907652,7.007491432583313,-2.6476395672491475,0.03274266433029118,1.1371168075186209,0.568902848637,3
-0.8688415472274058,2.841695780020547,7.017318590940954,-2.6472262170117062,0.01457434516967114,0.9292978890142136,0.4206319292149024,3
-0.8972030435846747,2.8586161248494903,7.023858524639382,-2.647095267421329,0.0046171608410562935,0.7074963839904792,0.29393863203349385,3
-0.9255645399419437,2.8688713035603035,7.027512461574448,-2.647092858586911,8.493326253668882e-05,0.47689084087593125,0.18386915738079052,3
-0.9539260362992126,2.872338331378327,7.028596712341747,-2.647120822713449,-0.000985989109592586,0.24045973250303748,0.08677616863440207,3
+0.0,2.87846,5.56936,-3.12648,0.0,0.0,0.0,3
+0.02931999999999979,2.87575,5.57211,-3.12648,0.07216,-0.18459,0.18772,3
+0.05862999999999996,2.86768,5.5804,-3.12437,0.1483,-0.36636,0.37797,3
+0.08794999999999975,2.85432,5.59431,-3.12002,0.22828,-0.54465,0.57126,3
+0.11726999999999999,2.83581,5.61395,-3.11333,0.3121,-0.71858,0.76826,3
+0.14658000000000015,2.81227,5.63943,-3.10418,0.39981,-0.88686,0.96986,3
+0.17589999999999995,2.78392,5.6709,-3.09245,0.49155,-1.04759,1.17723,3
+0.20521999999999974,2.751,5.70856,-3.07804,0.58752,-1.19779,1.39204,3
+0.2345299999999999,2.71392,5.75266,-3.06082,0.68799,-1.33243,1.61655,3
+0.26385000000000014,2.67324,5.80353,-3.04065,0.79298,-1.44239,1.85368,3
+0.29315999999999987,2.62997,5.86156,-3.0174,0.90075,-1.50969,2.10578,3
+0.3224800000000001,2.58587,5.92712,-2.991,1.00086,-1.49858,2.36625,3
+0.3517999999999999,2.54392,5.99977,-2.96166,1.06628,-1.36377,2.59017,3
+0.38111000000000006,2.50735,6.07748,-2.9304,1.07717,-1.13128,2.71179,3
+0.41042999999999985,2.47801,6.15743,-2.89882,1.04817,-0.87028,2.74232,3
+0.4397500000000001,2.45633,6.23748,-2.86809,0.99808,-0.6085,2.71835,3
+0.4690599999999998,2.44226,6.31634,-2.83883,0.93715,-0.35164,2.66222,3
+0.49838000000000005,2.43563,6.39327,-2.81135,0.87074,-0.10005,2.58572,3
+0.5276899999999998,2.43632,6.46775,-2.78583,0.8021,0.14699,2.49538,3
+0.55701,2.4442,6.53943,-2.76231,0.73373,0.39027,2.39508,3
+0.5863299999999998,2.45916,6.60807,-2.7408,0.66804,0.63047,2.28726,3
+0.6170100000000001,2.48227,6.67632,-2.72031,0.59845,0.87563,2.16148,3
+0.6476899999999999,2.51267,6.74034,-2.70194,0.51972,1.10644,2.01163,3
+0.6783700000000001,2.54978,6.79926,-2.686,0.43158,1.31229,1.82962,3
+0.70905,2.59251,6.85199,-2.67276,0.33457,1.47297,1.60729,3
+0.7397399999999998,2.63896,6.89729,-2.66249,0.23589,1.5555,1.34592,3
+0.7704200000000001,2.68631,6.93439,-2.65525,0.15122,1.53086,1.07255,3
+0.8010999999999999,2.73141,6.96352,-2.65062,0.0895,1.40884,0.82586,3
+0.8317799999999997,2.77181,6.98571,-2.64787,0.04965,1.22464,0.6206,3
+0.86246,2.80603,7.00216,-2.64635,0.02577,1.00583,0.45215,3
+0.8931399999999998,2.83323,7.01389,-2.64555,0.0123,0.76769,0.31199,3
+0.9238300000000002,2.85296,7.02163,-2.64518,0.00525,0.51813,0.19307,3
+0.95451,2.86492,7.02598,-2.64502,0.00181,0.26143,0.09027,3
diff --git a/zebROS_ws/src/auto_node/paths/2025_4_Note2_red.csv b/zebROS_ws/src/auto_node/paths/2025_4_Note2_red.csv
index 1bdf79984..f77fe9508 100644
--- a/zebROS_ws/src/auto_node/paths/2025_4_Note2_red.csv
+++ b/zebROS_ws/src/auto_node/paths/2025_4_Note2_red.csv
@@ -1,34 +1,33 @@
-0.0,13.66154221534729,5.569355010986328,6.269202596398377,-0.0,-0.0,0.0,3
-0.029308688805128824,13.669668637645639,5.577592410411832,6.267096302488229,-0.07186585262191755,0.18484671564764116,0.1873710439082446,3
-0.05861737761025765,13.683087008151816,5.591434338977022,6.2627646920549385,-0.1477927061907437,0.36683496934274934,0.3773108260141282,3
-0.08792606641538647,13.701684468630226,5.610979013790693,6.256091290680883,-0.2276936173644043,0.5453032857130032,0.5703409381794171,3
-0.1172347552205153,13.72531778590379,5.6363467151077415,6.2469592449917375,-0.311581515983696,0.7193402529086199,0.7671371011338004,3
-0.14654344402564412,13.75379984026041,5.667687194625604,6.235248583965418,-0.3995627748650136,0.8876438070369492,0.9685949238461176,3
-0.17585213283077294,13.786877165915817,5.705190414974579,6.220833452348225,-0.49183816148815185,1.0482707936190598,1.1759275959269713,3
-0.20516082163590177,13.824190275265575,5.7491020278878615,6.203579338659054,-0.5887030226520579,1.1981618815460635,1.3908062395877947,3
-0.2344695104410306,13.865198346009981,5.799744177622278,6.183341611719042,-0.6905026381406636,1.3321725853155086,1.6155278238202602,3
-0.2637781992461594,13.909024537879521,5.857534108113018,6.159973734198614,-0.7973020450062094,1.4409448829232012,1.853021178681236,3
-0.29308688805128824,13.954119058918714,5.922943816804184,6.133389722353641,-0.9070351806603422,1.5060522167763903,2.1055080251515848,3
-0.32239557685641707,13.99760344872007,5.996089871412109,6.1038432945193755,-1.008111554596076,1.4911299992551743,2.3656441961029326,3
-0.3517042656615459,14.035205331025708,6.075155982976428,6.072385947106998,-1.0733113180777716,1.3523501570703889,2.5870161371858553,3
-0.3810129544666747,14.06456827778943,6.156218802002761,6.040637606692245,-1.083239875587212,1.1186842246745563,2.706224657283879,3
-0.41032164327180354,14.085881673286476,6.2368504893029195,6.009768992295518,-1.0532239979098992,0.8576974028384646,2.7361541431963747,3
-0.43963033207693236,14.099513693540512,6.316007068426159,5.98040042449471,-1.002043045874837,0.5959782907751577,2.712577262646103,3
-0.4689390208820612,14.105685809590362,6.393076908044145,5.952851921114942,-0.9399432217182389,0.3390527470863854,2.657252526036485,3
-0.49824770968719,14.104554022419848,6.467637528581923,5.927286861564065,-0.8722689616345112,0.08727351768008186,2.5817352884340834,3
-0.5275563984923188,14.096239456690007,6.539379856369902,5.903773021915071,-0.8022822107585654,-0.16003512567702574,2.492456909041754,3
-0.5568650872974477,14.080839433670043,6.608068943023682,5.882304256760884,-0.7325051385588603,-0.4036398668015253,2.393247256402183,3
-0.5852265836547166,14.059395290573082,6.671741372917682,5.8631501680276035,-0.6753553653167714,-0.6386136580076626,2.294436215838802,3
-0.6135880800119855,14.031615857582652,6.73192868726267,5.845864722249921,-0.6094687515763264,-0.8658558771384738,2.1795779123849948,3
-0.6419495763692544,13.997914462244998,6.787964556423057,5.830689667402727,-0.5350583289419562,-1.0808052088446234,2.0437077455622945,3
-0.6703110727265233,13.958991757169589,6.838978131954027,5.817872649506217,-0.45191613781839335,-1.275187338232881,1.8803634198003536,3
-0.6986725690837927,13.916072444168599,6.883901438014632,5.8076529629435205,-0.3603366491659622,-1.4339258351442017,1.6827571507350183,3
-0.7270340654410612,13.871214218816492,6.9217045950597536,5.8001370381734585,-0.2650045214608014,-1.5324148323228457,1.4495218368990526,3
-0.7553955617983301,13.82725089756339,6.952047330355769,5.795079366063098,-0.17832881758617003,-1.5442088402278438,1.1964117079332068,3
-0.783757058155599,13.786782908221163,6.975698525511612,5.791948877085541,-0.11037813160931202,-1.4659787399812811,0.9547500078359786,3
-0.812118554512868,13.7513476665382,6.993853814680668,5.790160851794074,-0.06304410983623464,-1.3216020368276495,0.7450090833119908,3
-0.8404800508701369,13.72171347092348,7.007491432583313,5.78923222083894,-0.03274266433029118,-1.1371168075186209,0.568902848637,3
-0.8688415472274058,13.698304219979452,7.017318590940954,5.788818870601499,-0.01457434516967114,-0.9292978890142136,0.4206319292149024,3
-0.8972030435846747,13.681383875150509,7.023858524639382,5.788687921011122,-0.0046171608410562935,-0.7074963839904792,0.29393863203349385,3
-0.9255645399419437,13.671128696439695,7.027512461574448,5.788685512176704,-8.493326253668882e-05,-0.47689084087593125,0.18386915738079052,3
-0.9539260362992126,13.667661668621673,7.028596712341747,5.788713476303242,0.000985989109592586,-0.24045973250303748,0.08677616863440207,3
+0.0,13.661539999999999,5.56936,6.268072653589793,-0.0,-0.0,0.0,3
+0.02931999999999979,13.66425,5.57211,6.268072653589793,-0.07216,0.18459,0.18772,3
+0.05862999999999996,13.67232,5.5804,6.265962653589793,-0.1483,0.36636,0.37797,3
+0.08794999999999975,13.68568,5.59431,6.261612653589793,-0.22828,0.54465,0.57126,3
+0.11726999999999999,13.704189999999999,5.61395,6.254922653589793,-0.3121,0.71858,0.76826,3
+0.14658000000000015,13.72773,5.63943,6.245772653589793,-0.39981,0.88686,0.96986,3
+0.17589999999999995,13.756079999999999,5.6709,6.234042653589793,-0.49155,1.04759,1.17723,3
+0.20521999999999974,13.789,5.70856,6.219632653589793,-0.58752,1.19779,1.39204,3
+0.2345299999999999,13.82608,5.75266,6.202412653589793,-0.68799,1.33243,1.61655,3
+0.26385000000000014,13.86676,5.80353,6.1822426535897925,-0.79298,1.44239,1.85368,3
+0.29315999999999987,13.910029999999999,5.86156,6.158992653589793,-0.90075,1.50969,2.10578,3
+0.3224800000000001,13.95413,5.92712,6.132592653589793,-1.00086,1.49858,2.36625,3
+0.3517999999999999,13.99608,5.99977,6.103252653589793,-1.06628,1.36377,2.59017,3
+0.38111000000000006,14.032649999999999,6.07748,6.071992653589794,-1.07717,1.13128,2.71179,3
+0.41042999999999985,14.06199,6.15743,6.040412653589794,-1.04817,0.87028,2.74232,3
+0.4397500000000001,14.08367,6.23748,6.009682653589794,-0.99808,0.6085,2.71835,3
+0.4690599999999998,14.097739999999998,6.31634,5.980422653589793,-0.93715,0.35164,2.66222,3
+0.49838000000000005,14.10437,6.39327,5.952942653589793,-0.87074,0.10005,2.58572,3
+0.5276899999999998,14.103679999999999,6.46775,5.927422653589793,-0.8021,-0.14699,2.49538,3
+0.55701,14.095799999999999,6.53943,5.9039026535897925,-0.73373,-0.39027,2.39508,3
+0.5863299999999998,14.080839999999998,6.60807,5.882392653589793,-0.66804,-0.63047,2.28726,3
+0.6170100000000001,14.05773,6.67632,5.861902653589793,-0.59845,-0.87563,2.16148,3
+0.6476899999999999,14.02733,6.74034,5.843532653589794,-0.51972,-1.10644,2.01163,3
+0.6783700000000001,13.990219999999999,6.79926,5.827592653589793,-0.43158,-1.31229,1.82962,3
+0.70905,13.947489999999998,6.85199,5.814352653589793,-0.33457,-1.47297,1.60729,3
+0.7397399999999998,13.901039999999998,6.89729,5.804082653589793,-0.23589,-1.5555,1.34592,3
+0.7704200000000001,13.853689999999999,6.93439,5.796842653589794,-0.15122,-1.53086,1.07255,3
+0.8010999999999999,13.808589999999999,6.96352,5.792212653589793,-0.0895,-1.40884,0.82586,3
+0.8317799999999997,13.768189999999999,6.98571,5.789462653589793,-0.04965,-1.22464,0.6206,3
+0.86246,13.73397,7.00216,5.787942653589793,-0.02577,-1.00583,0.45215,3
+0.8931399999999998,13.706769999999999,7.01389,5.787142653589793,-0.0123,-0.76769,0.31199,3
+0.9238300000000002,13.68704,7.02163,5.786772653589793,-0.00525,-0.51813,0.19307,3
+0.95451,13.67508,7.02598,5.786612653589794,-0.00181,-0.26143,0.09027,3
diff --git a/zebROS_ws/src/auto_node/paths/test_drive_rotate.traj b/zebROS_ws/src/auto_node/paths/test_drive_rotate.traj
new file mode 100644
index 000000000..f3a0e6170
--- /dev/null
+++ b/zebROS_ws/src/auto_node/paths/test_drive_rotate.traj
@@ -0,0 +1,94 @@
+{
+ "name":"test_drive_rotate",
+ "version":1,
+ "snapshot":{
+ "waypoints":[
+ {"x":1.7596547603607178, "y":7.133025646209717, "heading":3.141592653589793, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.0, "y":7.133, "heading":0.0, "intervals":69, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true},
+ {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true},
+ {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":2.87}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"1.7596547603607178 m", "val":1.7596547603607178}, "y":{"exp":"7.133025646209717 m", "val":7.133025646209717}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5 m", "val":5.0}, "y":{"exp":"7.133 m", "val":7.133}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":69, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true},
+ {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true},
+ {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"2.87 rad / s", "val":2.87}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.29471],
+ "samples":[
+ {"t":0.0, "x":1.75965, "y":7.13303, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.82837, "ay":-0.01857, "alpha":21.74659, "fx":[143.73206,143.97764,88.86086,88.02413], "fy":[-53.45989,52.79397,124.66152,-125.25895]},
+ {"t":0.02354, "x":1.76155, "y":7.13302, "heading":3.14159, "vx":0.16074, "vy":-0.00044, "omega":0.51192, "ax":7.1022, "ay":-0.0215, "alpha":20.20933, "fx":[144.39443,144.64695,97.54521,96.63888], "fy":[-51.59541,50.88208,117.95139,-118.70089]},
+ {"t":0.04708, "x":1.7673, "y":7.133, "heading":-3.12954, "vx":0.32793, "vy":-0.00094, "omega":0.98765, "ax":7.39577, "ay":-0.00375, "alpha":18.44047, "fx":[145.38897,145.16998,106.77396,105.86671], "fy":[-48.66309,49.30784,109.6153,-110.51552]},
+ {"t":0.07062, "x":1.77707, "y":7.13298, "heading":-3.10629, "vx":0.50203, "vy":-0.00103, "omega":1.42175, "ax":7.70427, "ay":0.0193, "alpha":16.41144, "fx":[146.62794,145.65576,116.67824,115.22797], "fy":[-44.71563,47.7748,98.94103,-100.68688]},
+ {"t":0.09416, "x":1.79102, "y":7.13296, "heading":-3.07282, "vx":0.68339, "vy":-0.00058, "omega":1.80808, "ax":8.01798, "ay":0.03148, "alpha":14.10884, "fx":[148.00869,146.23235,126.96993,124.32338], "fy":[-39.80515,45.87725,85.25513,-89.1852]},
+ {"t":0.1177, "x":1.80933, "y":7.13296, "heading":-3.03026, "vx":0.87213, "vy":0.00016, "omega":2.1402, "ax":8.31951, "ay":0.02207, "alpha":11.56483, "fx":[149.41893,147.04973,136.76435,132.81713], "fy":[-33.98219,43.04789,68.36646,-75.93071]},
+ {"t":0.14124, "x":1.83216, "y":7.13297, "heading":-2.97988, "vx":1.06798, "vy":0.00068, "omega":2.41244, "ax":8.58537, "ay":-0.00636, "alpha":8.86978, "fx":[150.74125,148.26043,144.75653,140.38054], "fy":[-27.2898,38.47621,49.16607,-60.78504]},
+ {"t":0.16478, "x":1.85968, "y":7.13298, "heading":-2.92309, "vx":1.27008, "vy":0.00053, "omega":2.62124, "ax":8.79416, "ay":-0.03757, "alpha":6.12176, "fx":[151.85431,149.9348,149.94382,146.61155], "fy":[-19.75696,31.01077,29.80031,-43.61061]},
+ {"t":0.18832, "x":1.89202, "y":7.13298, "heading":-2.86139, "vx":1.4771, "vy":-0.00035, "omega":2.76535, "ax":8.93208, "ay":-0.05631, "alpha":3.34959, "fx":[152.62463,151.81032,152.34129,150.95199], "fy":[-11.46083,19.26641,12.95761,-24.5942]},
+ {"t":0.21186, "x":1.92926, "y":7.13296, "heading":-2.79629, "vx":1.68736, "vy":-0.00168, "omega":2.8442, "ax":8.98671, "ay":-0.06441, "alpha":0.80638, "fx":[152.90628,152.87505,152.89529,152.76852], "fy":[-3.5642,4.34343,1.55654,-6.71845]},
+ {"t":0.2354, "x":1.97147, "y":7.1329, "heading":-2.72934, "vx":1.89891, "vy":-0.00319, "omega":2.86318, "ax":8.98615, "ay":-0.07192, "alpha":0.05758, "fx":[152.8527,152.85561,152.85154,152.84771], "fy":[-1.37842,-0.81728,-1.06757,-1.63035]},
+ {"t":0.25894, "x":2.01866, "y":7.13281, "heading":-2.66194, "vx":2.11044, "vy":-0.00489, "omega":2.86453, "ax":8.981, "ay":-0.08154, "alpha":0.003, "fx":[152.76432,152.7645,152.76422,152.76404], "fy":[-1.39367,-1.36536,-1.38041,-1.40872]},
+ {"t":0.28248, "x":2.07083, "y":7.13267, "heading":-2.5945, "vx":2.32186, "vy":-0.00681, "omega":2.8646, "ax":8.97433, "ay":-0.09423, "alpha":0.00009, "fx":[152.65072,152.65073,152.65072,152.65071], "fy":[-1.60298,-1.60218,-1.60268,-1.60347]},
+ {"t":0.30602, "x":2.12798, "y":7.13248, "heading":-2.52707, "vx":2.53312, "vy":-0.00902, "omega":2.86461, "ax":8.96538, "ay":-0.11152, "alpha":-0.00007, "fx":[152.49847,152.49846,152.49847,152.49848], "fy":[-1.89691,-1.89755,-1.89709,-1.89645]},
+ {"t":0.32956, "x":2.19009, "y":7.13224, "heading":-2.45964, "vx":2.74416, "vy":-0.01165, "omega":2.8646, "ax":8.95275, "ay":-0.1362, "alpha":-0.00012, "fx":[152.28372,152.28371,152.28373,152.28374], "fy":[-2.31672,-2.31772,-2.31688,-2.31588]},
+ {"t":0.3531, "x":2.25717, "y":7.13193, "heading":-2.3922, "vx":2.95491, "vy":-0.01486, "omega":2.8646, "ax":8.93361, "ay":-0.17383, "alpha":-0.00021, "fx":[151.95815,151.95813,151.95818,151.95819], "fy":[-2.95679,-2.95838,-2.95684,-2.95525]},
+ {"t":0.37664, "x":2.3292, "y":7.13153, "heading":-2.32477, "vx":3.16521, "vy":-0.01895, "omega":2.8646, "ax":8.90119, "ay":-0.23738, "alpha":-0.00042, "fx":[151.40662,151.40658,151.40671,151.40674], "fy":[-4.038,-4.04096,-4.03764,-4.03468]},
+ {"t":0.40018, "x":2.40618, "y":7.13102, "heading":-2.25734, "vx":3.37475, "vy":-0.02454, "omega":2.86459, "ax":8.83447, "ay":-0.36571, "alpha":-0.00133, "fx":[150.27153,150.27141,150.27205,150.27217], "fy":[-6.22199,-6.23043,-6.21923,-6.21079]},
+ {"t":0.42372, "x":2.48807, "y":7.13034, "heading":-2.1899, "vx":3.58272, "vy":-0.03314, "omega":2.86455, "ax":8.62087, "ay":-0.74982, "alpha":-0.02892, "fx":[146.62537,146.62296,146.65166,146.65387], "fy":[-12.80614,-12.95758,-12.70254,-12.5508]},
+ {"t":0.44726, "x":2.5748, "y":7.12935, "heading":-2.12247, "vx":3.78565, "vy":-0.0508, "omega":2.86387, "ax":3.31417, "ay":-4.92463, "alpha":-4.33412, "fx":[35.37294,55.17988,75.53569,59.40394], "fy":[-93.85759,-95.16353,-74.85179,-71.19322]},
+ {"t":0.4708, "x":2.66483, "y":7.12679, "heading":-2.05506, "vx":3.86367, "vy":-0.16672, "omega":2.76185, "ax":2.21176, "ay":-4.20935, "alpha":-3.17967, "fx":[23.3493,39.25845,51.61646,36.26132], "fy":[-77.46132,-80.30251,-66.19312,-62.44234]},
+ {"t":0.49435, "x":2.7564, "y":7.1217, "heading":-1.99004, "vx":3.91573, "vy":-0.26581, "omega":2.687, "ax":1.93327, "ay":-3.13979, "alpha":-2.25279, "fx":[23.83298,35.00432,41.89972,30.80023], "fy":[-57.55278,-60.21745,-49.51722,-46.34053]},
+ {"t":0.51789, "x":2.84911, "y":7.11457, "heading":-1.92679, "vx":3.96124, "vy":-0.33972, "omega":2.63397, "ax":1.96905, "ay":-1.99223, "alpha":-2.15969, "fx":[25.69066,36.20722,41.26538,30.80875], "fy":[-38.13423,-40.88793,-29.88284,-26.64386]},
+ {"t":0.54143, "x":2.9429, "y":7.10602, "heading":-1.86478, "vx":4.0076, "vy":-0.38662, "omega":2.58313, "ax":2.1885, "ay":-0.66404, "alpha":-2.67016, "fx":[28.54401,41.48477,45.803,33.07101], "fy":[-16.82693,-20.36696,-6.10835,-1.87795]},
+ {"t":0.56497, "x":3.03785, "y":7.09674, "heading":-1.80398, "vx":4.05911, "vy":-0.40225, "omega":2.52027, "ax":2.37231, "ay":1.08407, "alpha":-3.30071, "fx":[30.50258,47.3973,49.86838,33.64106], "fy":[12.00735,6.95081,24.54754,30.25335]},
+ {"t":0.58851, "x":3.13406, "y":7.08757, "heading":-1.74465, "vx":4.11496, "vy":-0.37673, "omega":2.44257, "ax":2.13548, "ay":3.30969, "alpha":-3.02814, "fx":[26.96047,45.39505,45.01981,27.92031], "fy":[51.90753,46.43403,60.80114,66.04469]},
+ {"t":0.61205, "x":3.23152, "y":7.07962, "heading":-1.68715, "vx":4.16523, "vy":-0.29882, "omega":2.37129, "ax":1.24603, "ay":5.46247, "alpha":-1.13393, "fx":[16.99191,25.45355,25.19936,17.13346], "fy":[91.75803,90.24301,94.11523,95.54387]},
+ {"t":0.63559, "x":3.32991, "y":7.0741, "heading":-1.63133, "vx":4.19456, "vy":-0.17023, "omega":2.34459, "ax":0.08493, "ay":6.81335, "alpha":1.34407, "fx":[7.19025,-3.72031,-4.69999,7.00874], "fy":[117.29524,117.58694,114.48887,114.20169]},
+ {"t":0.65913, "x":3.42868, "y":7.07198, "heading":-1.57614, "vx":4.19656, "vy":-0.00985, "omega":2.37623, "ax":-1.07501, "ay":7.14665, "alpha":2.3354, "fx":[-7.59597,-26.54781,-30.21774,-8.78084], "fy":[125.03291,122.97335,117.79955,120.4438]},
+ {"t":0.68267, "x":3.52717, "y":7.07373, "heading":-1.5202, "vx":4.17125, "vy":0.15839, "omega":2.43121, "ax":-2.08619, "ay":6.04805, "alpha":3.73843, "fx":[-20.119,-46.25521,-53.0141,-22.55352], "fy":[111.74311,105.78635,93.08915,100.88388]},
+ {"t":0.70621, "x":3.62478, "y":7.07913, "heading":-1.46297, "vx":4.12214, "vy":0.30076, "omega":2.51921, "ax":-2.68919, "ay":4.15987, "alpha":4.51487, "fx":[-30.88265,-57.12642,-62.19304,-32.76696], "fy":[85.10362,76.75289,55.87669,65.29984]},
+ {"t":0.72975, "x":3.72107, "y":7.08736, "heading":-1.40367, "vx":4.05884, "vy":0.39869, "omega":2.6255, "ax":-2.54382, "ay":1.54374, "alpha":3.39888, "fx":[-35.3474,-52.37148,-51.55477,-33.80464], "fy":[38.1797,32.93743,14.69238,19.22508]},
+ {"t":0.75329, "x":3.81592, "y":7.09718, "heading":-1.34186, "vx":3.99896, "vy":0.43503, "omega":2.70551, "ax":-1.92497, "ay":-1.22637, "alpha":1.4379, "fx":[-30.20723,-37.41865,-35.28062,-28.06634], "fy":[-16.21833,-17.6786,-25.40644,-24.13736]},
+ {"t":0.77683, "x":3.90952, "y":7.10708, "heading":-1.27817, "vx":3.95364, "vy":0.40616, "omega":2.73935, "ax":-1.62676, "ay":-3.34262, "alpha":0.8051, "fx":[-26.42466,-30.84577,-28.90069,-24.5116], "fy":[-54.62761,-55.21439,-59.06422,-58.52191]},
+ {"t":0.80037, "x":4.00214, "y":7.11571, "heading":-1.21369, "vx":3.91535, "vy":0.32747, "omega":2.75831, "ax":-1.88703, "ay":-4.8467, "alpha":1.76754, "fx":[-29.96166,-40.34942,-34.16526,-23.91472], "fy":[-78.434,-78.9846,-86.41294,-85.93222]},
+ {"t":0.82391, "x":4.09378, "y":7.12208, "heading":-1.14876, "vx":3.87093, "vy":0.21338, "omega":2.79992, "ax":-3.55083, "ay":-5.48933, "alpha":2.72544, "fx":[-60.91053,-73.58462,-60.52371,-46.57584], "fy":[-87.42248,-85.97186,-99.14082,-100.95224]},
+ {"t":0.84745, "x":4.18392, "y":7.12558, "heading":-1.08285, "vx":3.78734, "vy":0.08416, "omega":2.86407, "ax":-8.62198, "ay":-0.96968, "alpha":0.01542, "fx":[-146.66719,-146.6665,-146.64763,-146.64836], "fy":[-16.39104,-16.44985,-16.5967,-16.53801]},
+ {"t":0.87099, "x":4.27069, "y":7.12729, "heading":-1.01542, "vx":3.58438, "vy":0.06133, "omega":2.86444, "ax":-8.83189, "ay":-0.52861, "alpha":0.00121, "fx":[-150.22833,-150.22819,-150.2274,-150.22754], "fy":[-8.98285,-8.98889,-9.00004,-8.99399]},
+ {"t":0.89453, "x":4.35262, "y":7.12859, "heading":-0.948, "vx":3.37647, "vy":0.04889, "omega":2.86446, "ax":-8.89892, "ay":-0.37467, "alpha":0.00042, "fx":[-151.3682,-151.36815,-151.36796,-151.36801], "fy":[-6.37002,-6.37247,-6.37621,-6.37375]},
+ {"t":0.91807, "x":4.42964, "y":7.12964, "heading":-0.88057, "vx":3.16699, "vy":0.04007, "omega":2.86447, "ax":-8.93171, "ay":-0.29704, "alpha":0.00022, "fx":[-151.92581,-151.92578,-151.9257,-151.92573], "fy":[-5.05104,-5.05244,-5.05425,-5.05285]},
+ {"t":0.94161, "x":4.50171, "y":7.1305, "heading":-0.81313, "vx":2.95673, "vy":0.03308, "omega":2.86448, "ax":-8.95111, "ay":-0.25057, "alpha":0.00013, "fx":[-152.25592,-152.2559,-152.25587,-152.25589], "fy":[-4.2611,-4.262,-4.26302,-4.26211]},
+ {"t":0.96515, "x":4.56883, "y":7.13121, "heading":-0.7457, "vx":2.74602, "vy":0.02718, "omega":2.86448, "ax":-8.96394, "ay":-0.21982, "alpha":0.00007, "fx":[-152.474,-152.47399,-152.47397,-152.47398], "fy":[-3.73851,-3.73906,-3.73959,-3.73904]},
+ {"t":0.98869, "x":4.63099, "y":7.13179, "heading":-0.67827, "vx":2.53501, "vy":0.022, "omega":2.86448, "ax":-8.97303, "ay":-0.19811, "alpha":-0.00021, "fx":[-152.62869,-152.62872,-152.62876,-152.62873], "fy":[-3.37142,-3.36966,-3.36819,-3.36994]},
+ {"t":1.01223, "x":4.68818, "y":7.13225, "heading":-0.61084, "vx":2.32378, "vy":0.01734, "omega":2.86448, "ax":-8.97982, "ay":-0.18207, "alpha":-0.00551, "fx":[-152.74331,-152.7441,-152.74504,-152.74425], "fy":[-3.13809,-3.09059,-3.05588,-3.10338]},
+ {"t":1.03577, "x":4.7404, "y":7.13261, "heading":-0.54341, "vx":2.1124, "vy":0.01305, "omega":2.86435, "ax":-8.98502, "ay":-0.16982, "alpha":-0.0994, "fx":[-152.81724,-152.83338,-152.84642,-152.83363], "fy":[-3.62188,-2.72154,-2.15682,-3.05405]},
+ {"t":1.05931, "x":4.78763, "y":7.13287, "heading":-0.47598, "vx":1.90089, "vy":0.00906, "omega":2.86201, "ax":-8.98166, "ay":-0.16006, "alpha":-1.17409, "fx":[-152.49747,-152.89317,-152.83637,-152.87502], "fy":[-11.35673,-0.02197,5.63276,-5.14425]},
+ {"t":1.08285, "x":4.82989, "y":7.13304, "heading":-0.40861, "vx":1.68946, "vy":0.00529, "omega":2.83437, "ax":-8.91471, "ay":-0.14799, "alpha":-3.76224, "fx":[-149.8809,-152.57593,-151.42512,-152.66491], "fy":[-30.517,9.47651,22.07751,-11.10578]},
+ {"t":1.10639, "x":4.86719, "y":7.13312, "heading":-0.34189, "vx":1.4796, "vy":0.0018, "omega":2.74581, "ax":-8.77548, "ay":-0.1188, "alpha":-6.35318, "fx":[-144.87468,-150.91161,-149.16393,-152.12335], "fy":[-49.15005,24.22588,34.50496,-17.66388]},
+ {"t":1.12993, "x":4.89959, "y":7.13313, "heading":-0.27725, "vx":1.27302, "vy":-0.00099, "omega":2.59625, "ax":-8.57604, "ay":-0.06462, "alpha":-8.89474, "fx":[-138.3118,-146.73006,-147.17504,-151.28719], "fy":[-65.43305,42.75784,42.4166,-24.13786]},
+ {"t":1.15347, "x":4.92718, "y":7.13309, "heading":-0.21614, "vx":1.07114, "vy":-0.00251, "omega":2.38687, "ax":-8.3219, "ay":-0.0007, "alpha":-11.45135, "fx":[-130.7875,-139.42126,-145.79544,-150.20825], "fy":[-79.44921,62.63972,47.10233,-30.34031]},
+ {"t":1.17701, "x":4.95009, "y":7.13303, "heading":-0.15995, "vx":0.87524, "vy":-0.00253, "omega":2.1173, "ax":-8.02823, "ay":0.04567, "alpha":-13.94821, "fx":[-122.68085,-129.62106,-144.97819,-148.95127], "fy":[-91.49798,81.0649,49.68295,-36.14275]},
+ {"t":1.20055, "x":4.96847, "y":7.13298, "heading":-0.11011, "vx":0.68626, "vy":-0.00146, "omega":1.78896, "ax":-7.71992, "ay":0.05771, "alpha":-16.24896, "fx":[-114.24095,-118.85926,-144.55963,-147.59458], "fy":[-101.86183,96.23411,50.98317,-41.42918]},
+ {"t":1.22409, "x":4.98249, "y":7.13296, "heading":-0.068, "vx":0.50453, "vy":-0.0001, "omega":1.40645, "ax":-7.41886, "ay":0.03802, "alpha":-18.26829, "fx":[-105.657,-108.51971,-144.3668,-146.22754], "fy":[-110.75959,107.83518,51.60264,-46.09132]},
+ {"t":1.24763, "x":4.99231, "y":7.13297, "heading":-0.03489, "vx":0.32988, "vy":0.0008, "omega":0.97641, "ax":-7.13672, "ay":0.00137, "alpha":-19.99925, "fx":[-97.11598,-99.2607,-144.25208,-144.94544], "fy":[-118.33941,116.47959,51.98404,-50.03091]},
+ {"t":1.27117, "x":4.99809, "y":7.13299, "heading":-0.0119, "vx":0.16188, "vy":0.00083, "omega":0.50562, "ax":-6.87693, "ay":-0.03527, "alpha":-21.4791, "fx":[-88.84027,-91.11618,-144.10078,-143.84102], "fy":[-124.69396,123.00806,52.45238,-53.16642]},
+ {"t":1.29471, "x":5.0, "y":7.133, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
diff --git a/zebROS_ws/src/auto_node/paths/test_drive_rotate0_blue.csv b/zebROS_ws/src/auto_node/paths/test_drive_rotate0_blue.csv
new file mode 100644
index 000000000..f1c3767e6
--- /dev/null
+++ b/zebROS_ws/src/auto_node/paths/test_drive_rotate0_blue.csv
@@ -0,0 +1,55 @@
+0.0,1.75965,7.13303,3.14159,0.0,0.0,0.0,1
+0.02354,1.76155,7.13302,3.14159,0.51192,0.16074,-0.00044,1
+0.04708,1.7673,7.133,-3.12954,0.98765,0.32793,-0.00094,1
+0.07062,1.77707,7.13298,-3.10629,1.42175,0.50203,-0.00103,1
+0.09416,1.79102,7.13296,-3.07282,1.80808,0.68339,-0.00058,1
+0.1177,1.80933,7.13296,-3.03026,2.1402,0.87213,0.00016,1
+0.14124,1.83216,7.13297,-2.97988,2.41244,1.06798,0.00068,1
+0.16478,1.85968,7.13298,-2.92309,2.62124,1.27008,0.00053,1
+0.18832,1.89202,7.13298,-2.86139,2.76535,1.4771,-0.00035,1
+0.21186,1.92926,7.13296,-2.79629,2.8442,1.68736,-0.00168,1
+0.2354,1.97147,7.1329,-2.72934,2.86318,1.89891,-0.00319,1
+0.25894,2.01866,7.13281,-2.66194,2.86453,2.11044,-0.00489,1
+0.28248,2.07083,7.13267,-2.5945,2.8646,2.32186,-0.00681,1
+0.30602,2.12798,7.13248,-2.52707,2.86461,2.53312,-0.00902,1
+0.32956,2.19009,7.13224,-2.45964,2.8646,2.74416,-0.01165,1
+0.3531,2.25717,7.13193,-2.3922,2.8646,2.95491,-0.01486,1
+0.37664,2.3292,7.13153,-2.32477,2.8646,3.16521,-0.01895,1
+0.40018,2.40618,7.13102,-2.25734,2.86459,3.37475,-0.02454,1
+0.42372,2.48807,7.13034,-2.1899,2.86455,3.58272,-0.03314,1
+0.44726,2.5748,7.12935,-2.12247,2.86387,3.78565,-0.0508,1
+0.4708,2.66483,7.12679,-2.05506,2.76185,3.86367,-0.16672,1
+0.49435,2.7564,7.1217,-1.99004,2.687,3.91573,-0.26581,1
+0.51789,2.84911,7.11457,-1.92679,2.63397,3.96124,-0.33972,1
+0.54143,2.9429,7.10602,-1.86478,2.58313,4.0076,-0.38662,1
+0.56497,3.03785,7.09674,-1.80398,2.52027,4.05911,-0.40225,1
+0.58851,3.13406,7.08757,-1.74465,2.44257,4.11496,-0.37673,1
+0.61205,3.23152,7.07962,-1.68715,2.37129,4.16523,-0.29882,1
+0.63559,3.32991,7.0741,-1.63133,2.34459,4.19456,-0.17023,1
+0.65913,3.42868,7.07198,-1.57614,2.37623,4.19656,-0.00985,1
+0.68267,3.52717,7.07373,-1.5202,2.43121,4.17125,0.15839,1
+0.70621,3.62478,7.07913,-1.46297,2.51921,4.12214,0.30076,1
+0.72975,3.72107,7.08736,-1.40367,2.6255,4.05884,0.39869,1
+0.75329,3.81592,7.09718,-1.34186,2.70551,3.99896,0.43503,1
+0.77683,3.90952,7.10708,-1.27817,2.73935,3.95364,0.40616,1
+0.80037,4.00214,7.11571,-1.21369,2.75831,3.91535,0.32747,1
+0.82391,4.09378,7.12208,-1.14876,2.79992,3.87093,0.21338,1
+0.84745,4.18392,7.12558,-1.08285,2.86407,3.78734,0.08416,1
+0.87099,4.27069,7.12729,-1.01542,2.86444,3.58438,0.06133,1
+0.89453,4.35262,7.12859,-0.948,2.86446,3.37647,0.04889,1
+0.91807,4.42964,7.12964,-0.88057,2.86447,3.16699,0.04007,1
+0.94161,4.50171,7.1305,-0.81313,2.86448,2.95673,0.03308,1
+0.96515,4.56883,7.13121,-0.7457,2.86448,2.74602,0.02718,1
+0.98869,4.63099,7.13179,-0.67827,2.86448,2.53501,0.022,1
+1.01223,4.68818,7.13225,-0.61084,2.86448,2.32378,0.01734,1
+1.03577,4.7404,7.13261,-0.54341,2.86435,2.1124,0.01305,1
+1.05931,4.78763,7.13287,-0.47598,2.86201,1.90089,0.00906,1
+1.08285,4.82989,7.13304,-0.40861,2.83437,1.68946,0.00529,1
+1.10639,4.86719,7.13312,-0.34189,2.74581,1.4796,0.0018,1
+1.12993,4.89959,7.13313,-0.27725,2.59625,1.27302,-0.00099,1
+1.15347,4.92718,7.13309,-0.21614,2.38687,1.07114,-0.00251,1
+1.17701,4.95009,7.13303,-0.15995,2.1173,0.87524,-0.00253,1
+1.20055,4.96847,7.13298,-0.11011,1.78896,0.68626,-0.00146,1
+1.22409,4.98249,7.13296,-0.068,1.40645,0.50453,-0.0001,1
+1.24763,4.99231,7.13297,-0.03489,0.97641,0.32988,0.0008,1
+1.27117,4.99809,7.13299,-0.0119,0.50562,0.16188,0.00083,1
diff --git a/zebROS_ws/src/auto_node/paths/test_drive_rotate0_red.csv b/zebROS_ws/src/auto_node/paths/test_drive_rotate0_red.csv
new file mode 100644
index 000000000..da5488d1d
--- /dev/null
+++ b/zebROS_ws/src/auto_node/paths/test_drive_rotate0_red.csv
@@ -0,0 +1,55 @@
+0.0,14.780349999999999,7.13303,2.6535897932333796e-06,-0.0,-0.0,0.0,1
+0.02354,14.77845,7.13302,2.6535897932333796e-06,-0.51192,-0.16074,-0.00044,1
+0.04708,14.772699999999999,7.133,6.2711326535897935,-0.98765,-0.32793,-0.00094,1
+0.07062,14.762929999999999,7.13298,6.247882653589793,-1.42175,-0.50203,-0.00103,1
+0.09416,14.74898,7.13296,6.214412653589793,-1.80808,-0.68339,-0.00058,1
+0.1177,14.73067,7.13296,6.171852653589793,-2.1402,-0.87213,0.00016,1
+0.14124,14.70784,7.13297,6.121472653589793,-2.41244,-1.06798,0.00068,1
+0.16478,14.680319999999998,7.13298,6.064682653589793,-2.62124,-1.27008,0.00053,1
+0.18832,14.647979999999999,7.13298,6.002982653589793,-2.76535,-1.4771,-0.00035,1
+0.21186,14.61074,7.13296,5.937882653589793,-2.8442,-1.68736,-0.00168,1
+0.2354,14.568529999999999,7.1329,5.870932653589794,-2.86318,-1.89891,-0.00319,1
+0.25894,14.521339999999999,7.13281,5.803532653589793,-2.86453,-2.11044,-0.00489,1
+0.28248,14.469169999999998,7.13267,5.736092653589793,-2.8646,-2.32186,-0.00681,1
+0.30602,14.412019999999998,7.13248,5.668662653589793,-2.86461,-2.53312,-0.00902,1
+0.32956,14.34991,7.13224,5.601232653589793,-2.8646,-2.74416,-0.01165,1
+0.3531,14.282829999999999,7.13193,5.533792653589793,-2.8646,-2.95491,-0.01486,1
+0.37664,14.210799999999999,7.13153,5.466362653589793,-2.8646,-3.16521,-0.01895,1
+0.40018,14.13382,7.13102,5.398932653589793,-2.86459,-3.37475,-0.02454,1
+0.42372,14.051929999999999,7.13034,5.331492653589793,-2.86455,-3.58272,-0.03314,1
+0.44726,13.9652,7.12935,5.264062653589793,-2.86387,-3.78565,-0.0508,1
+0.4708,13.875169999999999,7.12679,5.196652653589793,-2.76185,-3.86367,-0.16672,1
+0.49435,13.7836,7.1217,5.131632653589794,-2.687,-3.91573,-0.26581,1
+0.51789,13.69089,7.11457,5.068382653589794,-2.63397,-3.96124,-0.33972,1
+0.54143,13.5971,7.10602,5.006372653589793,-2.58313,-4.0076,-0.38662,1
+0.56497,13.502149999999999,7.09674,4.945572653589793,-2.52027,-4.05911,-0.40225,1
+0.58851,13.40594,7.08757,4.886242653589793,-2.44257,-4.11496,-0.37673,1
+0.61205,13.30848,7.07962,4.828742653589793,-2.37129,-4.16523,-0.29882,1
+0.63559,13.21009,7.0741,4.772922653589793,-2.34459,-4.19456,-0.17023,1
+0.65913,13.11132,7.07198,4.717732653589794,-2.37623,-4.19656,-0.00985,1
+0.68267,13.01283,7.07373,4.661792653589793,-2.43121,-4.17125,0.15839,1
+0.70621,12.91522,7.07913,4.604562653589793,-2.51921,-4.12214,0.30076,1
+0.72975,12.818929999999998,7.08736,4.545262653589793,-2.6255,-4.05884,0.39869,1
+0.75329,12.724079999999999,7.09718,4.483452653589794,-2.70551,-3.99896,0.43503,1
+0.77683,12.630479999999999,7.10708,4.419762653589793,-2.73935,-3.95364,0.40616,1
+0.80037,12.537859999999998,7.11571,4.355282653589793,-2.75831,-3.91535,0.32747,1
+0.82391,12.44622,7.12208,4.290352653589793,-2.79992,-3.87093,0.21338,1
+0.84745,12.356079999999999,7.12558,4.224442653589794,-2.86407,-3.78734,0.08416,1
+0.87099,12.269309999999999,7.12729,4.157012653589793,-2.86444,-3.58438,0.06133,1
+0.89453,12.18738,7.12859,4.0895926535897935,-2.86446,-3.37647,0.04889,1
+0.91807,12.11036,7.12964,4.022162653589793,-2.86447,-3.16699,0.04007,1
+0.94161,12.03829,7.1305,3.9547226535897932,-2.86448,-2.95673,0.03308,1
+0.96515,11.971169999999999,7.13121,3.8872926535897934,-2.86448,-2.74602,0.02718,1
+0.98869,11.909009999999999,7.13179,3.819862653589793,-2.86448,-2.53501,0.022,1
+1.01223,11.85182,7.13225,3.752432653589793,-2.86448,-2.32378,0.01734,1
+1.03577,11.799599999999998,7.13261,3.685002653589793,-2.86435,-2.1124,0.01305,1
+1.05931,11.752369999999999,7.13287,3.617572653589793,-2.86201,-1.90089,0.00906,1
+1.08285,11.71011,7.13304,3.550202653589793,-2.83437,-1.68946,0.00529,1
+1.10639,11.672809999999998,7.13312,3.4834826535897934,-2.74581,-1.4796,0.0018,1
+1.12993,11.64041,7.13313,3.418842653589793,-2.59625,-1.27302,-0.00099,1
+1.15347,11.61282,7.13309,3.3577326535897933,-2.38687,-1.07114,-0.00251,1
+1.17701,11.58991,7.13303,3.301542653589793,-2.1173,-0.87524,-0.00253,1
+1.20055,11.57153,7.13298,3.2517026535897933,-1.78896,-0.68626,-0.00146,1
+1.22409,11.557509999999999,7.13296,3.209592653589793,-1.40645,-0.50453,-0.0001,1
+1.24763,11.54769,7.13297,3.176482653589793,-0.97641,-0.32988,0.0008,1
+1.27117,11.541909999999998,7.13299,3.153492653589793,-0.50562,-0.16188,0.00083,1
diff --git a/zebROS_ws/src/auto_node/src/TestDriveRotate.py b/zebROS_ws/src/auto_node/src/TestDriveRotate.py
new file mode 100644
index 000000000..bc94ddb70
--- /dev/null
+++ b/zebROS_ws/src/auto_node/src/TestDriveRotate.py
@@ -0,0 +1,21 @@
+from auto_base import AutoBase
+from series_action import SeriesAction
+from wait_action import WaitAction
+from drive_trajectory_iterator import DriveTrajectoryActionIterator
+from parallel_action import ParallelAction
+from intake_action import IntakeAction
+from dynamic_shoot_action import DynamicShootAction
+from geometry_msgs.msg import Twist
+from cmd_vel_action import CmdVelAction
+
+class TestDriveRotate(AutoBase):
+ def __init__(self) -> None:
+ super().__init__(display_name="test_drive_rotate", # must match choreo path name (if expected traj count is != 0)
+ expected_trajectory_count=1) # how many segments of the path there are (split at waypoints)
+
+ def get_action(self) -> SeriesAction:
+ drive_traj_iter = DriveTrajectoryActionIterator(self.get_display_name(), self.expected_trajectory_count)
+
+ return SeriesAction([
+ drive_traj_iter.get_next_trajectory_action()
+ ])
diff --git a/zebROS_ws/src/auto_node/src/auto_node.py b/zebROS_ws/src/auto_node/src/auto_node.py
index 17027367a..1861ab006 100755
--- a/zebROS_ws/src/auto_node/src/auto_node.py
+++ b/zebROS_ws/src/auto_node/src/auto_node.py
@@ -24,7 +24,11 @@
2: AutonomousNames.FourCoralNonProcessor,
3: AutonomousNames.OneCoralMidProcessor, # actually scores non processor side first, then for the 2-coral goes to the processor side
4: AutonomousNames.OneCoralMidNonProcessor, # actually scores processor side first, then for the 2-coral goes to the non processor side
- 5: AutonomousNames.PushFourCoral}
+ 5: AutonomousNames.PushFourCoral,
+ 6: AutonomousNames.TestCmdVel,
+ 7: AutonomousNames.Test4Note,
+ 8: AutonomousNames.TestCmdVelCircle,
+ 9: AutonomousNames.TestDriveRotate}
class AutoNode():
def __init__(self):
diff --git a/zebROS_ws/src/auto_node/src/autos.py b/zebROS_ws/src/auto_node/src/autos.py
index 997af5809..82864c91f 100644
--- a/zebROS_ws/src/auto_node/src/autos.py
+++ b/zebROS_ws/src/auto_node/src/autos.py
@@ -7,6 +7,7 @@
from Test4Note import Test4Note
from TestCmdVel import TestCmdVel
from TestCmdVelCircle import TestCmdVelCircle
+from TestDriveRotate import TestDriveRotate
from FourCoral import FourCoralNonProcessor
from FourCoral import FourCoralProcessor
from FourCoral import PushFourCoral
@@ -19,6 +20,7 @@ class AutonomousNames(str, Enum):
Test4Note = "2025_4_Note"
TestCmdVel = "2025_cmd_vel_test"
TestCmdVelCircle = "2024_cmd_vel_circle_test"
+ TestDriveRotate = "test_drive_rotate"
FourCoralNonProcessor = "2025_4_Coral"
TestDriveForwardBack = "test_drive_forward_back"
FourCoralProcessor = "2025_4_Coral_Processor_Side"
@@ -36,13 +38,15 @@ def init_auto_selection_map() -> dict[AutonomousNames, AutoBase]:
Returns an autonomous selection map, mapping auto names to auto programs.
"""
return {
- AutonomousNames.DriveForward: DriveForward(),
AutonomousNames.Test4Note: Test4Note(),
+ AutonomousNames.TestCmdVel: TestCmdVel(),
AutonomousNames.TestCmdVelCircle: TestCmdVelCircle(),
+ AutonomousNames.TestDriveRotate: TestDriveRotate(),
AutonomousNames.FourCoralNonProcessor: FourCoralNonProcessor(),
AutonomousNames.TestDriveForwardBack: TestDriveForwardBack(),
AutonomousNames.FourCoralProcessor: FourCoralProcessor(),
+ AutonomousNames.DriveForward: DriveForward(),
AutonomousNames.PushFourCoral: PushFourCoral(),
- AutonomousNames.OneCoralMidNonProcessor: OneCoralMidNonProcessor(),
- AutonomousNames.OneCoralMidProcessor: OneCoralMidProcessor()
+ AutonomousNames.OneCoralMidProcessor: OneCoralMidProcessor(),
+ AutonomousNames.OneCoralMidNonProcessor: OneCoralMidNonProcessor()
}
diff --git a/zebROS_ws/src/controller_node/launch/stage_sim.launch b/zebROS_ws/src/controller_node/launch/stage_sim.launch
index 32fc4e95f..7f0757184 100644
--- a/zebROS_ws/src/controller_node/launch/stage_sim.launch
+++ b/zebROS_ws/src/controller_node/launch/stage_sim.launch
@@ -15,7 +15,7 @@
-
+
diff --git a/zebROS_ws/src/controller_node/stage/2024_crescendo.world b/zebROS_ws/src/controller_node/stage/2024_crescendo.world
index 84be53c20..c9b286da3 100644
--- a/zebROS_ws/src/controller_node/stage/2024_crescendo.world
+++ b/zebROS_ws/src/controller_node/stage/2024_crescendo.world
@@ -32,8 +32,8 @@ define floorplan model
)
resolution 0.02
-interval_sim 15 # simulation timestep in milliseconds, was 5
-interval_real 15 # was 5
+interval_sim 5 # simulation timestep in milliseconds, was 5
+interval_real 5 # was 5
threads 4
window
diff --git a/zebROS_ws/src/talon_swerve_drive_controller/CMakeLists.txt b/zebROS_ws/src/talon_swerve_drive_controller/CMakeLists.txt
index f082e526e..85680e9fc 100644
--- a/zebROS_ws/src/talon_swerve_drive_controller/CMakeLists.txt
+++ b/zebROS_ws/src/talon_swerve_drive_controller/CMakeLists.txt
@@ -3,6 +3,9 @@ project(talon_swerve_drive_controller)
include ("../cmake_modules/CMakeOpt.cmake")
+unset(CMAKE_CXX_STANDARD CACHE)
+set(CMAKE_CXX_STANDARD 20)
+
find_package(catkin REQUIRED COMPONENTS
angles
cmake_modules
@@ -21,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
tf
trajectory_msgs
urdf
+ smooth
)
find_package(Eigen3 REQUIRED)
@@ -69,7 +73,7 @@ add_dependencies(${PROJECT_NAME}
${catkin_EXPORTED_TARGETS}
)
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} smooth::smooth)
add_executable(zero_wheel_service src/zero_wheel_angle.cpp)
target_link_libraries(zero_wheel_service ${catkin_LIBRARIES})
@@ -103,14 +107,14 @@ if (NOT (("${ARM_PREFIX}" STREQUAL "arm-frc2020-linux-gnueabi") OR
# )
add_executable(swervebot test/swervebot.cpp src/Swerve.cpp src/SwerveMath.cpp)
- target_link_libraries(swervebot ${catkin_LIBRARIES})
+ target_link_libraries(swervebot ${catkin_LIBRARIES} smooth::smooth)
add_dependencies(swervebot
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_executable(swervebot_from_bag test/swervebot_from_bag.cpp src/Swerve.cpp src/SwerveMath.cpp)
- target_link_libraries(swervebot_from_bag ${catkin_LIBRARIES})
+ target_link_libraries(swervebot_from_bag ${catkin_LIBRARIES} smooth::smooth)
add_dependencies(swervebot_from_bag
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
diff --git a/zebROS_ws/src/talon_swerve_drive_controller/package.xml b/zebROS_ws/src/talon_swerve_drive_controller/package.xml
index 495847f7b..b41eca822 100644
--- a/zebROS_ws/src/talon_swerve_drive_controller/package.xml
+++ b/zebROS_ws/src/talon_swerve_drive_controller/package.xml
@@ -27,6 +27,7 @@
tf
trajectory_msgs
urdf
+ smooth
rostest
std_srvs
diff --git a/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp b/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp
index 417261663..26b1d1054 100644
--- a/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp
+++ b/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp
@@ -57,6 +57,9 @@
#include
#include "talon_swerve_drive_controller/get_wheel_names.h"
+// this seems useful https://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lectures/Lecture2.pdf
+#include "smooth/se2.hpp"
+
namespace talon_swerve_drive_controller
{
@@ -522,6 +525,11 @@ void update(const ros::Time &time, const ros::Duration &period) override
return;
}
+ // Discretize velocity, accounting for the fact that we're not controlling continuously
+ // This finds the pose we would travel in one timestep and converts it to a twist to account for the robot rotating during the timestep (if translation + rotation are both commanded)
+ // Tested in sim, it does help a bit
+ curr_cmd = discretize(curr_cmd, std::max(dt, 1./250.));
+
// We are not stopped, so reset the time which we were
// last not-stopped here. This is used later to give
// a delay between stopping and moving into parking config
@@ -1013,6 +1021,42 @@ struct Commands
Commands() = default;
};
+// This fixes one aspect of skew. The robot's heading changes throughout the timestep, so we can't just drive at the velocity we want for the whole time
+// because our heading is changing (we're rotating), and we'll go in an arc instead of a straight line. We can take the log of the matrix representing the
+// pose we want to drive for this timestep to get the corresponding twist, and drive that instead (divided by the timestep to get velocity).
+// Helpful explanation here: https://www.youtube.com/watch?v=lrjyVhwNNBc
+// This is what 254 is doing here: https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
+// and what WPILib is doing: https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/transformations.html
+// Also, I *think* second-order control of each individual module (from that same cd thread) would correct for this as well,
+// but I'm not sure entirely how it works or how we'd command a velocity and an acceleration to a motor.
+// Anyway, this is basically WPILib's discretize function.
+// (and now we have a cool library)
+Commands discretize(Commands cmd, double dt) {
+ if (dt < 1e-6) {
+ // ROS_WARN_STREAM("swerve_drive_controller: discretization: dt=0? command was " << cmd.lin.x << "," << cmd.lin.y << "," << cmd.ang);
+ return cmd;
+ }
+ // ROS_INFO_STREAM("In: " << cmd.lin.x << "," << cmd.lin.y << "," << cmd.ang << ", dt=" << dt);
+ smooth::SE2d group_element_aka_pose;
+ // set R2 part (translation)
+ group_element_aka_pose.r2()(0,0) = cmd.lin.x * dt;
+ group_element_aka_pose.r2()(1,0) = cmd.lin.y * dt;
+ // set SO2 part (rotation)
+ group_element_aka_pose.so2() = smooth::SO2d(cmd.ang * dt);
+ // ROS_INFO_STREAM("SE2: " << group_element_aka_pose);
+ // logarithmic map to go from pose to twist
+ Eigen::Vector3d tangent_aka_twist = group_element_aka_pose.log(); // {x, y, rotation}
+ // ROS_INFO_STREAM("Twist (vx,vy,omega): " << tangent_aka_twist);
+
+ Commands discretized;
+ discretized.lin = swervemath::Point2d{tangent_aka_twist(0,0) / dt, tangent_aka_twist(1,0) / dt};
+ discretized.ang = tangent_aka_twist(2,0) / dt;
+ discretized.stamp = cmd.stamp;
+ // ROS_INFO_STREAM("Discretized: " << discretized.lin.x << "," << discretized.lin.y << "," << discretized.ang);
+
+ return discretized;
+}
+
realtime_tools::RealtimeBuffer command_;
double brake_last_{ros::Time::now().toSec()};