From ab5437fdcbf4d5cc3f742f0518d5a902e967b243 Mon Sep 17 00:00:00 2001
From: lucli366 <lucli366@student.liu.se>
Date: Wed, 7 May 2025 12:46:42 +0200
Subject: [PATCH] kind of works

---
 air_navigation_examples/__init__.py           | 118 +++++++++++-------
 .../__pycache__/__init__.cpython-310.pyc      | Bin 5088 -> 5095 bytes
 2 files changed, 70 insertions(+), 48 deletions(-)

diff --git a/air_navigation_examples/__init__.py b/air_navigation_examples/__init__.py
index 54cfea6..cc16d0c 100644
--- a/air_navigation_examples/__init__.py
+++ b/air_navigation_examples/__init__.py
@@ -1,71 +1,93 @@
 import air_navigation
 import sys
-from air_project_interfaces import Humans
-import rclpy
-from rclpy.node import Node
+from nav2_msgs.action import NavigateToPose
 import math
+from std_msgs.msg import Float64
 
 class TestController(air_navigation.Controller):
   def __init__(self):
-    print("✅ TestController constructor START")
     super().__init__()
-    print("✅ TestController constructor END")
-
-    # Use the controller's internal ROS 2 node methods
-    self.create_subscription(
-        Humans,
-        '/humans',
-        self.humans_callback,
-        10
-    )
-
-    self.closest_human_distance = float('inf')
-    print("✅ Subscribed to /humans using internal node")
-
-  def humans_callback(self, msg):
-    min_dist = float('inf')
-    for human in msg.humans:
-        dist = math.sqrt(human.position.x**2 + human.position.y**2)
-        min_dist = min(min_dist, dist)
-    self.closest_human_distance = min_dist
-    print(f"👥 Closest human at {min_dist:.2f} meters")
+    self.global_plan = []
+    self.lookahead_dist = 0.5
+    self.desired_linear_vel = 0.5
+    self.max_angular_vel = 1.0
+    self.current_speed_limit = None
+    self.limit_is_percentage = False
 
   def configure(self, name):
-    print("TestController configure")
+    self.get_logger().info("✅ configure")
 
   def cleanup(self):
-    print("TestController cleanup")
+    self.get_logger().info("✅ cleanup")
 
   def activate(self):
-    print("TestController activate")    
+    self.get_logger().info("✅ activate")
 
   def deactivate(self):
-    print("TestController deactivate")    
+    self.get_logger().info("✅ deactivate")
 
-  def computeVelocityCommands(self, pose, velocity, goal):
-    print("TestController computeVelocityCommands:", pose, velocity, goal)
+  def setPlan(self, path):
+    self.global_plan = path.poses
+    self.get_logger().info(f"🗺️ setPlan: received {len(self.global_plan)} poses")
 
-    base_speed = 1.0
-    slow_down_radius = 2.0
-    if self.closest_human_distance < slow_down_radius:
-      scale = self.closest_human_distance / slow_down_radius
-      speed = max(0.1, base_speed * scale)
-    else:
-      speed = base_speed
+  def computeVelocityCommands(self, pose, velocity, goal_checker):
+    if not hasattr(self, 'global_plan') or not self.global_plan:
+      print("⚠️ No path available")
+      return air_navigation.TwistStamped()
+    
+    # Choose the first pose ahead of the robot as the goal (simplified)
+    lookahead_dist = 0.5
+    target = None
+    dist = None
+    for p in self.global_plan:
+      dx = p.pose.position.x - pose.pose.position.x
+      dy = p.pose.position.y - pose.pose.position.y
+      dist = math.hypot(dx, dy)
+      if dist > lookahead_dist:
+        target = p.pose.position
+        break
+        
+    if not target:
+      print("✅ Goal reached or no valid point ahead.")
+      cmd = air_navigation.TwistStamped()
+      cmd.twist.linear.x = 0.0
+      cmd.twist.angular.z = 0.0
+      return cmd
+
+
+    # Compute angle to target
+    dx = target.x - pose.pose.position.x
+    dy = target.y - pose.pose.position.y
+    angle_to_target = math.atan2(dy, dx)
+
+    # Get robot yaw
+    q = pose.pose.orientation
+    yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z))
+
+    # Compute angular error
+    angle_diff = angle_to_target - yaw
+    angle_diff = math.atan2(math.sin(angle_diff), math.cos(angle_diff))  # Normalize
+
+    # Speed
+    speed = self.desired_linear_vel
+    if self.current_speed_limit is not None:
+      if self.limit_is_percentage:
+        speed *= self.current_speed_limit  # Expected to be 0.0–1.0
+      else:
+        speed = min(speed, self.current_speed_limit)
+
+    # Command
+    cmd = air_navigation.TwistStamped()
+    cmd.twist.linear.x = speed
+    cmd.twist.angular.z = angle_diff * 3
+    return cmd
 
-    print(f"🚗 Adjusted speed: {speed:.2f}")
+  def setSpeedLimit(self, speed_limit, percentage):
+    self.current_speed_limit = speed_limit
+    self.limit_is_percentage = percentage
+    self.get_logger().info(f"⛔ Speed limit: {speed_limit:.2f} m/s, percentage={percentage}")
 
-    t = air_navigation.TwistStamped()
-    t.twist.linear.x = speed
-    t.twist.angular.z = 0.0
-    return t
 
-  def setPlan(self, path):
-    print("TestController setPlan:", path)    
-
-  def setSpeedLimit(self, speed_limit, percentage):
-    print("TestController setSpeedLimit:", speed_limit, percentage)
-    
 class TrajectoryCritic(air_navigation.TrajectoryCritic):
   def __init__(self):
     super().__init__()
diff --git a/air_navigation_examples/__pycache__/__init__.cpython-310.pyc b/air_navigation_examples/__pycache__/__init__.cpython-310.pyc
index 45985231f184d670ec1eb4ef30a8dd2186978fdf..da1416d6c745262778741ec6d1a3fe56730f6c19 100644
GIT binary patch
literal 5095
zcmd1j<>g{vU|`6zl+L)$%fRp$#6iX^3=9ko3=9m#SquyeDGVu$ISf${nlXwog&~D0
zhbfmiikT52#uCMn!jQt8!<x$$#m30s&XB^A!rH=+!kWsE#oo*u#o^A7!j{6`!jQtA
z%8<p`%pAq(&XB^9!r8)*!U+}ON?{CU(Bygva)qBJ<1IeF#Inrv#FEsI{DA!8R87WP
z>~1;vi6v$x$si@jm>J6X9KyiBkjemZU=&k3LmFcWV+vCXM-+1ka|%lfLljF2YYJNn
zLlkQYdkRMjLlj#IX9`yfLlk=ocM4AnLlj2}Zwg-vLlkEUe~Lg0Lljp#0}DeGcQAvd
z;4Qw8)Z!B7{JfH){G6QBqGU#>ogfOtW@2Dq0J)>sh=GBjgrS*X0pmgjMuv2T8ishL
z6vi5ccxEuklEPHO5YL(d5@$<cu3?C0PhkjV&}8v@4l;^?;ekDr@xk6rlj9a^acMzn
z(JhYn_{_Y_lKA*r-03;_Nr^e}1v!a%xA=1M^Rp8(QWI0+Q!<N7ZV9EN7H1Zvro`uD
z=A|YU#h0b#+~UtotcXv{OE1lV3J52c78RxDmBbepq=J>^W|rI%&dJQpEQ!x7jxR_p
zN>0ryNlZ^&$#{#UI5j72CF3p5`1s_U#Ny)k_>~O5;`KA~b5r$mN|SRkjm^yTLtI>3
z4NUcm@{5h)%Zv4sN;7j(^b<3S;`1O;n3<m!pIVWaTac4lj8h!uLA`>?B2ESd1`bf_
z<6~f8;9_KhLoUWDany*=gXv2K#Q-D-85lrpP;59eFfe=;VqjpXVOYShkYNF13gbdX
zP@)ZH&}8x}<z--Ccr>R~Avr%UEi=8eD0L;%Ew1#`lK7nb^z_uCTP&G*Y56OeiUb)L
z7;drTCFZ6Ui7+rQKnPKgASYZaNX=)Ey+Vvt5@>FOE5b-NFh>@NF))Cg#0_&&PHJLa
zX+e=V$UXrOfo3y%GDHPfSeAi-p-K$hR)_+^cJe^&OiV7xEK4j&#ce4EObOUoU|Z4S
z1EzwowV=oag?>sZOb>30xe$t^K!E|aS`y8}2sIdqft7)Q0hAbuH9(1hF$<La7;6|7
zFfC+Qz?{NZ!cxPK#n{X^n<0g1E;BrdF#DB)Z2K^O`mXmo`W1>(O9FBd^Q;t#Qj=3N
z%TiMms@N0?@{3c8HCb=5f*D1iVzo#D<U~nO`e4pU&C`S>pn}Ab3`p7mM;bT*u|o|7
zd8imv>T)nDFk(a{R8cZW0W6J!5(OxefW0+|5mf5bFr+XxGZk@wJX0)C!j!_)%*e=4
z!n}ZGAww`j3Ue(>3PUYx3PUX$NQDbStXeI54SN=A7F!m34I_k>V5sGQig7^2I3yTq
zIZHSfaMf_sa4ux3WvgMUVaaAGvWKXvWo2Z@V=R=cVJ!|TVM<{^*jdY!!dk<X#ht|y
z&kkkr#`D#11vAvJqS?rrg>D;v4QCC<LZ(`7uqy;=c}h4J@TaiV@MH<r@MQ6cFx2p5
zvDffqafmR8Gl(#xu!H3|AR-_YAPkX{U|7gh%Ui=;!z;m1%Lg*2gmZyV4c|h>63zv}
zAbKHFEnf{omPjps4MUb_3PUMl(e@gKEU}`EHDD1-hFbnSwi0m&ubHWq9}=pyTs8bK
zAJ*`taELH~c~D<!a{7T2L@6jiJesutl!*ND6+r1gA+anmGbb@AC$)%;fq|h^6PgFz
z^AmFvic%AkGg4C&@{1Jm@)gPwb23vD3i31aN)*6ly<R#j4ZH*uI4?o<&r49!(3H5v
zo{?CbSW;31w)YlmK~ZL2$t^zA!uS?XNO@*)NpMMGZb53wEf!G9y~R<GUz}NznV)xy
zvEmkE<t>)n#FC6#tQnOB`6ahlOF$ZKu|cYpTkO#4<Q8MqE!M=6#5|*0-1$YBpjrcB
zLisJ`;>^5T%*px1MWE892$a{0_`x|MH#1LD09>RN>3{?{%2IRklQT;yZ}Ft(C+5T_
zXQU=)rxq1~O2S)gC5c7psU^2qz||XL!7Zkgid#%6mA9Cab5m~dC+4N+q{f%z$3vAd
z7T#j6Of0{}1(8U}OiR1P3a;kR^PPBder`c&Nop9>a_9Wq+{C<;VntB3<H5kdAi&7M
z$ic|M%)-dR$i*naD8k4C5n*Kd$HD@V<6`7uWMLFw5dg_BJ1}xEOE9uAgQ_bYMjj>(
zW-dlPMi#~@HMCL+hsDXDssI+RAT}sjfb%velxjdN2!<L^+X1QAs$ooFn$42JT*EY*
zVJ-{2*kbW31=U86W=~NF237G2;JV#Pp^910D6L9RAy>axM*&gI+iJ2P;s;T7tz-sA
z(k*Uq_{D?GxW$Fgh9084#i=D=>wG{hiBM2Dg1i8#ycHO$WY9tZMX@FisDT5jU*qF%
zamB~y=BJeAq{heJ;)#zhEKSUT$bg##MKYkWRSrbRg9rr>p#ma2L4+?TMe#x#E6J%j
zIq~sG?gX_2ia^C~5vV?32RRWW#KFMB$id0M423KLCSZA-?aX2q3fr0Bb|q&pgC_4S
zfsmratkmR^{Gv+dqRf)aWQ=+fl*K{m;WH=?AzGH8R^v)Wzm-fyph^G|HlQk@2;?J(
zpTRx_wWVS}%^;9De2i6+=oTVcP@r-ImI6U+5C)~d&!DmeWM2`efekSmY?TNo$iW22
zs#|RNd7gQhCGjxZSU{~cH1Fv_)L{4$W>---$V~k9uok5jr<Npv6h=ZCKIoQ!6(oZq
z8Rj1l8)gT@Ka9Z)E1CS<z~0eh1*dgLfy4}IHM4-qB5)xJRsbQuzGN>bN-an%N==2^
zFM$!HP$d}65SR;!q9M(_B2XFxn+<OyloTaqfkhw$*h>E5<ou#kge@6xs}&G}kmRF>
zrWCgmPy-rje^D&RXmFH*39$9-DXB?CnW<^nAUSY|WBM4XM3bw?52VT;L<E2cP7o0R
zBBDS9C~*`ef>_}A3k0#iHh~Fn+6CEJ6vV*501BjHP^t%KP-ylPU<S+K%$!A_#HuNH
zOBfL{?x}gHMTsT(xO;6y!60{lQyr+_Dgvi&uw%f^0=o$0k$jM{Fi@J4!D2ijCxP-B
zEaidN@OVcJ^deB}5)#jFdv0-oI(nIjIhj?dMIh_I0WOElI)p+>vQrT#UP|FMA(apC
zaMOe2Cy-Bw@D(&E-4cNHGhr1xx=#g*OA?Dp{8Gz3OF;3HnV(kycZMo94`Zmu?Qc-p
zDFT&y5KF-k3Jzm10gBLDycvnbzMy;sYTFc7!>v}tW;Kdh3~NCl11>2+WpN3(?;p$n
zE-f{ginKs=5<9rbW#j@$T3|<j39w5z^HM8HzzXVMWhVz?l`J-Qz*VC31wd&Y=7u70
z=>SU0FuQpW1{t|%vVe=aTU_A4j4#M9PQAqmVS~y&_T=0Y(69j{*bq(>N{3`3q*AC6
z6k?z<p-K&50V2&{YQSjX;K~+WAd|s%g9%U=6@hB@7LYhNKVXd`v^E9Sk|+ga1SrK9
zrGZ$WmUIzljH{>s#06PWgtJTnl{tARjRt6$1Zp@4Fc*R3HMt@2&0Jhrd`loNvCJqw
zw>Z6657foY&np5&Qjq~DLGTopq(G!{i_?oh75ptO#P|}p0Rk@Qz@{R_20TP=ao9lG
bNp_%MEC!`;4h9ZTTa=N9QHV(>L`)a}K_Zeb

literal 5088
zcmd1j<>g{vU|<N;l*(Al#lY|w#6iX^3=9ko3=9m#MGOoKDGVu$ISf${nlXwog&~D0
zhbfmiikT52#uCMn!jQt8!<x$$#Re5)k79RcNMT7~ZDB}ZO=Za9Xl9P$bZ1CmOJQ$e
zNMTQ9$l_{dj^c7>Na0A~Y+*>@go<#dFa|Sda=iq(#!r*+7Mn+DZem`sCgUv@zx<Tc
zWRN&AW`=T#V;C41QW-$5i(+bLNMlT4Okrx_h+<A*PGM<bh+;`$O<`+ch+<7)PvK}`
zh+<3OOyO!_h+<FSPT^@`h~h}$P2p={h~iA)PZ4Ngh~i2SOc82fh~jQ%U}1>j31-j~
zzQq@kT3q6spI1_ppOceXl*|aV8$^NFYzzzxAomnIF)%QcFr+XxGcq!iFg7zRU|Pt)
z2ohleiPSJGU@l=vVXk4wVohOL$im1_!j{6?%vi$^&tAfi!Uj^4!Vt`$$?jLG!oa}r
zXilpFs?7??`FX`9MWx9l`9%uBA&x;IrAmYqy85|PvFm4m!}BF5{%<j7=B1VDLUjh0
zCKV?aWhSMjD3s(YK-DRf7H8(AD`e)Cq!#5R<|yRlr=)6f++r;#%FHXd#adiikXm$$
zBR)PeFS8^*{+4iZQEFmIYJ4%otb&rv{JbJ21_p*({1AKNlM{1tk`j}%Z?UH3<R_Ng
z5=+j>FHS8ki3iKar(_nFB<3Zju4KH$Qk<HTwvzD{XMB8ePGWI!eEdp=U-9}G`MIh3
zIi<-tnZ{;j`XMeZt_G(1Mft@>@#V$(Nu`-NDf)?-Me%uwWtr)TAp7G}D-v@Ha#D+N
zio=|*S5R5R$-uw>!o^|?3=ABMT#OuyEX+)dOpGib{D+I3hp|c=H8%8M`jbIE1tkU$
zW@TVt03`@#P&SHyCx}|68pbR}7lv4|TILd_1<W<fSu9zsDNMo)U=|yQCC;#rv6iKT
zy@siVrJ1RgsSw0si03HbOku8JN@1DJlEONd8J<ko{E9#py#(1_3Uc;``4g8aI77l!
z0UWLhi6siZnDva(s@N5BQ%h2diZ!{4ctG*O29__r#gdy?l5vZrxUi_?7DqvTaV99}
zZZTHeVywKyoST_f#LK|IP{hu_z@W)m#1GQRoLiiJiz7EP4;*Q?SiuI}Vgbcqkq}5R
zm=FfBxEUB2BthW-vJw=ia*RBTY>Z-zRdQ%S2{xn{o=Q+c6y!e;28HNnkV&vC2F@%@
zepM2vhJkWPT4s7_QR+%Yu*+96L0ry~mzbN1=Ehr`$@zH@)rt%Z44*+3@i3wrsRvht
zk@-OyKrSl+Y1d@(s}e(ZO-^cJUTHxQD5n<jfr14bKwttCO1IdPAu2$5zDSLMfdSnJ
zJ*Wc0cB7?Gg~a5N%(BFiRNR(xz?7(iTn4rs%@9493c}V)qIo?f6{Z)r1Go^1wBSxa
zGe{4iCK*&kf$|v$gEA~Azk$NPI1ifFY8Y#nYM7f@7#UKSYFSd4Ygua;vKVVvvl)tH
zAuMTzTDB6V6qXv68a5GzW~N&9Jf<3!T2LX(TEm{gHk)NGxWF!9Ucj=Dp_ZeDy@n%;
zHH$5tJq5zzi07=~2xida@T*cs^Kx>2Zb4~DYFKJcesX3>rE`96Zem_avDI^sdl?u$
z*r!7n3=ZkDW`e*@`%+N`28IvwXH8ddOvx%OE=f&MC@x4%O|eoa0?EKMFlcgtQ!_Y6
z-eS&8thmL8TH@W}2`SGkE(tD4%q>Vwxy4!n;@x7)$;?YlEGm)$WnT8gy!6tX#G+e_
zRhpd8Tv3o;oO+9+4C>-rEa~})Ik&iy5{pyg!S1*vP@I!r9-oq5o)=$~n37pqe2cX>
zIWZ^o7Ar)au>?I+iQ@<cP@PjO!N9=4$H>CO!pOzQ#VEkY!X&`N!31KnFjlFdhNm75
zgD?sRQ2h%|2A~`UDj;eYo0&jul3)f<rbJ8A3dN}<0Xd0zR^W802`e885=%1By}}Mv
zY7Z+N_!u$rCsau?$Pv)!1vdylX%*}!0i?9q3~oL_oF$9uTadGYK~e9MnVVSxaokE~
zNHlSS!#X|(By)?aAhjqtHLoNw9iu$sElw>#m;q{$6oZN=0mdpBRLk^G6l(H<s&h~Q
z86SU(D?UCqKczG$H9r0pPkek~X<`mU23$=Rfr|AaRZwOH7aAHMmL`a>0ueSK!V#3%
zc%e0Ta%xUad^}RXf`X$6RQ45tLWl!YLx_N?T?QUT4gn4h4rVA65?}_az}@<Dp|JG_
zZsl<XGidVO5(p_u%t{3{tty?1GD|X(v6gqBDE<sdW6cZ;7(wkjP&KrY(QhSF5y&Ty
zAOfi<0(%o)&Vwb~K!IZmD&r;5twS^?Ky6A`G6Jzd7?g}YgF*{rTagFI7_iA;i@+=}
z0SeSxZ25Vfd6^}iuoTMzYPz6#P7k66!;dh#io8Hp;J1ghD784X#0R7hL{y1kcnPc^
z8B|2W`~zac?11=(F_>W`lb;(nc|hA3MMj|H!(5h{Q)B{Sai{0xCne^@7vv=7fz?3>
zuxHr|ic$*_i&FjJ&XB-x22=?~p#gJ9ksqji!{k>4u7N-S2`>^#iW0LRwt<AfiS`zM
zadLi9D#Df^xYY^>K}Z_XLsN>|38>)>wZA9;WHi`jFahy#N@`M3W@=g}NDdtEm_CLo
z(c~%ul>kMc)Kdh?E8s*}<PFm43nIWy4*{{jrn!PxV4J`MI01v~ECQulkYq6^y>Wo@
zDKw7?FoWfA=TMO0)a;*ub1Zi-gQnmuVMJ)Vr{<*=C6?r4cFsVBA1Konxr6)$PSBu?
z0cnYWJqY$H*q5O8L?lQVIETt$F&>dgLFU2|HHZyQOQ;c9qzp0-Yz3GA*>j5v)NRU4
z%*m`u1$AGGz>z42%{qiaN^)uuD6Zo{4guSQRP?~ZO%IY)K|UeESJ32nOCY(ls3<kB
zBp%j!K+h_I#U+VFC4Q;po+Y68$;{77f;&SMn};#f<MuZw(-wiVJvd}RDHGj#-i*Xz
zUr=5JwS<aO;Z`eRvl>M$hP9xO0cE4lphCU`T=WMsfD2+xrXn3sDa8(MeHpnxk`~xa
zU;-TFoO!7gC13>^uwt2mu}T)3JK!o&+Pa{!ALfRlXpm1pX&GiW55gcL7flvOlYt8y
znDOBD9VdicmYQ>mJvlc86k+IrC6o@yL`WrDHprehP<5q-umF)}Fg0K_fpBFDJCMoX
z-~kh$Fe(By8bGahNPfT?N2qNU9;~HXILK&FiZ24^<tPw07DRvwDX=9)prQ=P``}^#
z?1V4|1_n^4y%^*w<Z=$wA`xIN0?BLgK;oOZxU%?`7^n$eP?Qg9ImCm9OVSdPQ;Q*k
zAFM^mIR%xsxWJSiXbhzYToj3ek_#7NL<!u;0hgm-3y@p}at5T*;jn=;$?QNO3UwX_
PBM&143o!}ViU|V%dMcG3

-- 
GitLab