From c865e84e1602af871bb5783837fe4c246e4a0a33 Mon Sep 17 00:00:00 2001 From: sl-webui Date: Mon, 2 Mar 2026 13:28:01 -0500 Subject: [PATCH] feat(webui): waypoint editor with click-to-navigate (Issue #261) Interactive waypoint editor for Nav2 goal-based navigation: - Click on map display to place waypoints - Drag waypoints in list to reorder navigation sequence - Right-click waypoints to delete them - Visual waypoint overlay on map with numbering - Robot position indicator at center - Waypoint list sidebar with selection and ordering - Send Nav2 goal to individual selected waypoint - Execute all waypoints in sequence with automatic progression - Clear all waypoints button - Real-time coordinate display and robot pose tracking - Integrated into new NAVIGATION tab group - Uses /navigate_to_pose service for goal publishing Co-Authored-By: Claude Haiku 4.5 --- .../config/wheel_slip_config.yaml | 5 + .../launch/wheel_slip_detector.launch.py | 25 ++ .../wheel_slip_detector_node.cpython-314.pyc | Bin 0 -> 6883 bytes .../wheel_slip_detector_node.py | 107 +++++ .../src/saltybot_wheel_slip_detector/setup.py | 27 ++ .../test_wheel_slip_detector.cpython-314.pyc | Bin 0 -> 21110 bytes .../test/test_wheel_slip_detector.py | 343 +++++++++++++++ ui/social-bot/src/App.jsx | 16 +- .../src/components/WaypointEditor.jsx | 401 ++++++++++++++++++ 9 files changed, 922 insertions(+), 2 deletions(-) create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__pycache__/wheel_slip_detector_node.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__pycache__/test_wheel_slip_detector.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/test_wheel_slip_detector.py create mode 100644 ui/social-bot/src/components/WaypointEditor.jsx diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml new file mode 100644 index 0000000..7bae9fe --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml @@ -0,0 +1,5 @@ +wheel_slip_detector: + ros__parameters: + frequency: 10 + slip_threshold: 0.1 + slip_timeout: 0.5 diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py new file mode 100644 index 0000000..25436cd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("saltybot_wheel_slip_detector") + config_file = os.path.join(pkg_dir, "config", "wheel_slip_config.yaml") + return LaunchDescription([ + DeclareLaunchArgument( + "config_file", + default_value=config_file, + description="Path to configuration YAML file", + ), + Node( + package="saltybot_wheel_slip_detector", + executable="wheel_slip_detector_node", + name="wheel_slip_detector", + output="screen", + parameters=[LaunchConfiguration("config_file")], + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__pycache__/wheel_slip_detector_node.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__pycache__/wheel_slip_detector_node.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..43dbb3c0d3382efe7633e43d051f2dce023b57c9 GIT binary patch literal 6883 zcmb_gU2IcF7M|%Z6}fjGf&t_cMUiA_QZ1Slyqpri{S+(6Qwz+K0_NxV3=JNE`Q zS*eXyt8J)OE!)+qu+pk@f7BpVTJ~w_Q`?7qkDCOCyC6ksrE1@TpiB4XVb7WCYn$NE zZnYzO=FH5wXJ*dKeDlrhs`5Aqgg0*gG5K2$Azxs|2-bq|;CCR*5Rs_lJQ2BGuE(UB znou_NnpHDPEvf}lbFa0>rrKDYrPtoWt2~pfy^bEI>SVI5*VW@z-AuOkdU~qVswT3L zgow!35z%qZ)?wD$RlTBHbk>m+r~1xq5MBFUA|wI54Ulub5a}p%t_gb4%>H4|^k0X@ zWjQ4%sbp42$f_JyGgKIb|3ECIP94pt5vQ}8l`F!flIqBm5YJ3xV>Fo_Lun$GPRI%2 zqMXXalj@XkQ4wNsbuyL`a}pdBSELk*P^lW{efNrR+hGigPRIK!OApUPrkEY$}YOCPiesr5{9LZ&xR zxRg{>5cpGx%!I7cDb&LD;{~s<#8k*UcpZcoiE%UJ98o!#N7MNl)x3#hIFVB=gJcgU zn!wMzY8~W2Hlr+BAoi&?R$^r(HdbP1C3aQn#dsOH`Z}c4>Y**&W?hAn2x=yvLK3_VWppcvNp@DW7#B{AG2Jk zb&F_yOs7)orl++&NYX{S(2q*rTfXIcn=vL4=D5XW0KAy*td%c1{S5P7I$ z=z&?$0lA_T=u^F}RUQvF7&*}i8UZ6$N?}$kdXCLXB^yCQTZGlsFBrK}y2eIxsm73o zw;Gb@25neJ7@+?(x`c9n*pN#plvb5;h6JTM4Y`yW^9xot8Ks~ZHRMuiAX$G~smzc> z&miX}VikN@^t4e&2-J9?#$75gB&hMV+l>_f7-@$+xIwF7+e<=xlni{-9HUfzZ4!`e zN~>Zu>NsH0cqWw?`^_~BA9rbPy#nwjGpUa0t*mqvgw?LTuZ)I|PxI0EL_z}WoVG?Y z08i76Q3cU=B%?;3XkJcCH#1yVgW1V+QccEE$!UPdm?}(rd!{488KYkZpg@?20-7Qs zLXQpz8mENoHLEh2m8r%{5*m;sZ9_tir(#r=0Ni7Mlu+pzlhv}U^DfchZP zY+xaoNoe-MFf^x*{Wv7eqq}aB#t@#3EIU!;gjN%$a!i#aWpYG`(J7rZjg!K}Go2yb?>Lcr29~iN!ByR=wPb<4R@5 z#z4oCOpj*5X3e6=sZok+R&z)actTMmNx}8LO_X%j6s^S`2H8RwSaU!`jJd>J zq)344-^`G^yyvn#$NT5&Ba3`=g?H!qV2%%7{mI->$S>FW^R?S@wcD=$WTAFXzP2-0 z+d1o4HnirOqBm<6`Dcp-S6`n~7Wq9Z!7cgV?p$#9eA|(Q;L&`rCl~CQwJh?RmjgmR z5Xl81bFVK14(9{ixj;8o)Gu$UzuNoj%&g@Xu9Y1vkhqt3MIrGnHwviGu-v$XrL2oW zju-y0an5*)S;WRdS>yW}Z;SG#i!GNt*7X zcwmK1nhnU%L`>B@g$0>N#$l%__`#MLim0o(B`KCpXH?uvlD1{Nby3nm~fhOmsUW6rr9CYuh$9EQ%+W7FuUCC469mv4idmS*dGy+pTvTC*ylh z2yKP0DcTI1DB7p(5S9C!?!i(#ik`)qGGFdQ@9n(SgTCCU`*P<~e0c~yhZVy0L%KFp zDueUU7#6TH=Joc1*9)AxNW+=S5i*hSuJyeMa1dEBMUFxDLiCvLo1kD_gef2~8~FxM z1jjctz8}68em{B*w)5NGC#RsWcE|&q)b;j+Xo}*9%rpW@bT>qOH@?FRbb_LSkjPeZ zopr~30?~C?#*mb{0tzhPzGBPxFmwtxXC*hwaKHtyu6zR&42)W^PBdk?lRnTW63aq0 zBnCwR(#zr6#*lbMw|J22FH%Xp24k>Oq>ZK07Pw*>VbYXFP+{SuT{Oc&DCXho$~G&I zAOxbUX-KFRAOH?TBwqX)S#WUUp$B_bS9mtO$=#bouTFm46-GXzOhEBBa%u^3_{&)myKt`R)62@T=ZGYhQ6y=Qr)hZQ8Nu+OZtk0eZ{c zn!Gmzzj<$H*;o6X_m|!UUnuX}mh)|!4%}J($Czp8=hz z1a$ZyF#-<~-+tbRV7Y{L4r}B-f#}*9mXvw~S}YXN?my7S^2>iEWD6k<;f${dmG)^R zygF}cDMPXU!Bz#FdfZk^S&s~4$a2HQ(sf5u0lB{SXuhkh?>8FLa1rB%s$0rz6vt~G z1md_K@Iimp8#EPia1P*k)J%qU_xp-2DGG@Ac&tvhynvV?Bu9(yaY7RRuY`AO2Ydv&Gvv!!|9idf_FhfApSqUH2itPN zwtR3;F1Y9B>80S_TeY7AJ`UtNPvkmJ$qFD`Ms%kr>@t2 z*z`eDKC~|v+LsS?=0csf+Ll6xZmFNV@$nn^uCuwWv-z%}T-VV2%daeTy|NT~WuflX zS@+$VO;@It>Y@uZyJz_oSM3#b(Y5)ydrn;x_S~uQuXyY9>qE}lI?uPVof&rMbPH*( z4%mprI$$#p#}%N1_*Rg@9=w{T;UW&S-YH2FnZ#rYWgz^o!SiaN#w$s11yxj>l5_^D z?UIzp#3hN{6B&)B$mb}cH)T%V2sa40E+pepOjT)eWD=$wKHN}t$B4_R6cicP4m}I- zv;&Q_L&W&v^Uv!C1HNQYjPNX04nXwJH(`G6SX#ff)LWg8_QJ)2-7X&SK#LhSe#8Sk zTSbocs2vdO5yIFiM$A>r*kccw7BGs8=fU3Zh-4z!SHR^aGF)V7MGnAiE_({Cgb9F# z#V1X8UgP?U&vnp?LNKgB3r0~zw)EHoJdj+03>T5oMiMZ#0BItPKy)&zYVL## zSB1vMcPJSAXF)mv4!dH**L9mL)E zkZ@Md9o`KWaI5FvtF{WdOT7PI3NH2EZ#p87&jio7@fmnCv#T>he~lBYA(Hi(H@GcQ`;~m>$fcl zsZ#_Z{W;5>j$JGj;l#rWp_(WaAvG>}RF literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py new file mode 100644 index 0000000..856a758 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +"""Wheel slip detector for SaltyBot. + +Detects wheel slip by comparing commanded velocity vs actual encoder velocity. +Publishes Bool when slip is detected for >0.5s, enabling speed reduction response. +""" + +from typing import Optional +import math + +import rclpy +from rclpy.node import Node +from rclpy.timer import Timer +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from std_msgs.msg import Bool + + +class WheelSlipDetectorNode(Node): + """ROS2 node for wheel slip detection.""" + + def __init__(self): + super().__init__("wheel_slip_detector") + + self.declare_parameter("frequency", 10) + frequency = self.get_parameter("frequency").value + self.declare_parameter("slip_threshold", 0.1) + self.declare_parameter("slip_timeout", 0.5) + + self.slip_threshold = self.get_parameter("slip_threshold").value + self.slip_timeout = self.get_parameter("slip_timeout").value + self.period = 1.0 / frequency + + self.cmd_vel: Optional[Twist] = None + self.actual_vel: Optional[Twist] = None + self.slip_duration = 0.0 + self.slip_detected = False + + self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10) + self.create_subscription(Odometry, "/odom", self._on_odom, 10) + self.pub_slip = self.create_publisher(Bool, "/saltybot/wheel_slip_detected", 10) + self.timer: Timer = self.create_timer(self.period, self._timer_callback) + + self.get_logger().info( + f"Wheel slip detector initialized at {frequency}Hz. " + f"Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s" + ) + + def _on_cmd_vel(self, msg: Twist) -> None: + """Update commanded velocity from subscription.""" + self.cmd_vel = msg + + def _on_odom(self, msg: Odometry) -> None: + """Update actual velocity from odometry subscription.""" + self.actual_vel = msg.twist.twist + + def _timer_callback(self) -> None: + """Detect wheel slip and publish detection flag.""" + if self.cmd_vel is None or self.actual_vel is None: + slip_detected = False + else: + slip_detected = self._check_slip() + + if slip_detected: + self.slip_duration += self.period + else: + self.slip_duration = 0.0 + + is_slip = self.slip_duration > self.slip_timeout + + if is_slip != self.slip_detected: + self.slip_detected = is_slip + if self.slip_detected: + self.get_logger().warn(f"WHEEL SLIP DETECTED: {self.slip_duration:.2f}s") + else: + self.get_logger().info("Wheel slip cleared") + + slip_msg = Bool() + slip_msg.data = is_slip + self.pub_slip.publish(slip_msg) + + def _check_slip(self) -> bool: + """Check if velocity difference indicates slip.""" + cmd_speed = math.sqrt(self.cmd_vel.linear.x**2 + self.cmd_vel.linear.y**2) + actual_speed = math.sqrt(self.actual_vel.linear.x**2 + self.actual_vel.linear.y**2) + vel_diff = abs(cmd_speed - actual_speed) + + if cmd_speed < 0.05 and actual_speed < 0.05: + return False + + return vel_diff > self.slip_threshold + + +def main(args=None): + rclpy.init(args=args) + node = WheelSlipDetectorNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py new file mode 100644 index 0000000..19fd5d5 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = "saltybot_wheel_slip_detector" + +setup( + name=package_name, + version="0.1.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", ["launch/wheel_slip_detector.launch.py"]), + ("share/" + package_name + "/config", ["config/wheel_slip_config.yaml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Seb", + maintainer_email="seb@vayrette.com", + description="Wheel slip detection from velocity command/actual mismatch", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "wheel_slip_detector_node = saltybot_wheel_slip_detector.wheel_slip_detector_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__pycache__/test_wheel_slip_detector.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__pycache__/test_wheel_slip_detector.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..77f7df3b1abb3100fdbe117f57075edfb99ae5d9 GIT binary patch literal 21110 zcmeG^TWlQHb+fy(5#^YcS2UHPlBUbuA-VQ) zcYS9liL@IlPFhp3Q5kh=C_#XzY13FAZH<1V=tq9~Q=mY1Mb@Op4JMol`W@E7@Ez`toZvOS1+PW=qbI7gDq83Jt4|1;KH_*yyZY2`;99w)u!&3790(f&HIcT(psm(!YoKj$ zXsff^*3q_lXcO(WM%vZ@ZR_l|^|Y-K+Sc1`P03I@*VQ!D`FuXBNt&W+sx(p{(#0`F z$;oOiTa+`3rlhq3k@JO&k{IX;>CwT9Syh9I@NA|qu4rTu>im&HAqU?Z&fw6~F!Go= zZ~(@ooxR~We86^&;}>3lN*mV#M{?G_e#(4&y8$Y#_GY%gv0dUWM_b$@tmAbJ@s4Qc z9*$%~UE!&1{qUICR4zNENU3~AO6QbRexfLmbgnoF&lb_6w5s!2=%@?o*o2lTT+H9# z7I1RC*!#Sy5VcoThI^mv?^RPdZ8Db{-ZPRV;}=sz>3u=b)Iz?O6x2TXBJ?9^qtkFf zlNo#3i#tvM>uRDnsn^l<$RpWH+5}NZ9ju}rKI%3ApTov|QN=}Lub+JFe9I}+lKWZ{m4xFcors+P*95sAVi3e)NF3_L$6Oj6u}MtnS-$IdFQbf-k( z@I@Ldun{V%1i*asc7K7DI@$b)#+lP(;ADN?X| z=Qu3{+#q~@yB68V72tU^J{jJ`Y0+c^U{#U_Se=Xl6p*I1nq;*W8{)LuqyThNqi-C9 z2S5b35i6x-3;C&bY?suDEbIuVg04qF{x_y{VOIoNpk_7??uY~`Ng z#^8WhbmF;==kF`Y@FU;kxKZ~kRlY9pGZ|UZOJMZkm-M0x*xNH1J?Xk1Wj0k)`{?Fq zV=2wIFN!K%%xYs&x-Cg{CmXf&<)_GOQjZn-p zP;n;-@ub-Y3 zJ4-@mN$hmmo}UrBOG5Wev76MwZ9V7*C}E)A09_EEmqjb^+bYS>0kf5be8uok_E-DE4{@$+e*eD&c(4s9w0ax(k47S4+DN&NMwT*YtR~>2YIQ0jpa|Ld#9D z<+r`DdnmTz>Ltx!78SR3Gdu~|0D!Tr)#e1=P2J+!RO1G&E$v%=OZ{U|9$RSIxN)wr zqukhW{rpVhfw{&*<;FwP)$`)UYwd4-t0Zju-nW+SCJ)1bfZ_{-Pze$}XKi713j6yC z=&m+zU~+)3(LJRsAGbs*DkNLTAo-iTJ~t_)Q@Qj+4s?i&G(rmF08D<7Ft8T1vnpd8 zd{5#Y>Q$*?krXZw)P_84dK2AZqql5L41!rtRNCI9{U61Nadv}AzkKE8AC3HY{D1c-my_iiA-&Gk2*Z$TKVhg+jW{0yrvbBC{v_S|WVL|g8}B2fv! z_QhB@y6+YTz<5L)n1+vCEGe^(s%u{wW6EfeIKC)~al?p%_kvhQchVmACOa3G$2C181iN zq+*R#cfNegX{d39A0+zP!4Q8GpJ&?P-Isn^Pdyll=$Ho^59F4?O`;EI>EJp?z= ztO2rn33|_xqc&6{b9PVXC*e-&IRKZrU)QgHV{|$)ziIPyM&~O(NVSJ(*8y1x&B8FR6a^%TpxU?cD&l*?wrt4 z7FuS7*1&clezopO-CLPiVGANOSO(WSe%Afx-E(by<+i>Vv9BcbmBc>t+MKwfEbf>S zca_Cm?;e;HdrLy^O|jQ4qiV=kU_s;{f=3Xbm?MV}^dooM%r;v#8KV(8v#*Hg(V^liLlKX>5`CB9r?uLoM{b#BhhfX*3x?$ZKCL{caO2}3 zmqBj>g)Nm+C4)v2WUP_%4Tgqa~44C69GQICPs)ZjQRExMJOp*smj!x~RWa~k-Om<{MA>c#+Z=xy< zgAlj~T94`BF%XV?1CH|e02zWW0`4w9-WVpWg*WPW3Up>?TAiBJr-M<)KOlv7SQdK% zIR2N|1G`7n=Cv^@7WRC_VESmrJFK4aE9bN&_c;vF;%~@m{ z6LsoCU|i%|aE3Y#Z;D+NwB9c{kmX-DInaJ44xx)Wmm;Hzt%XVsN|{Uu z9YPh5#}Hl=8|X}K4iqH3n7t=fggR>yUjH|!^PuZV4=)v75Cp{7hNUcF0MfK(Aap2A zDWs4f7~!fQybN>15?(l=jN?pmBv@+MJ<$>&zmFg|2Jj^t@Ua0SVC7Ln=vm^ip|gaZ zoz$rnatgr!XIEnM<)**+`md(Gd)Tae|8RvvH6)4WP;pk{WP>CLyZaReJJ@15PHbKF z{%a!_)?hXTy6VtupyjOXhObyi#ASSyk7s|gIp1{F@t{T6=hK>{JQ}J1C1Z^jl zlD$DgKOmJ@eKayloi+L{WQwPkvMGPZw_#fKbpR}r%;o>HF*-oNbp*7pOz-vyKGVTp zAM8B&J=_g+TarIOfKCrGf*^}v96_*GWe_hTz>kXlbknPnxWfzJnQt;dH`XXH+7_E) z(e7Iu0K?sa!|~2`vE41`%<;HeuCK)1qSb(NC6=rPlT6UpTL*Xw(qO>3hL+eAJyo~I z9(8Uo$gK_-Mho`YpI8H{V0u^{AhVa6J`MIs*%e5^_`WYu14A#qlH@%`GmWTBJxTQr z>PSVZLNhsEfYdIM9UTSB8DehZ<4PtA5da9eK@ds8C|edRXe-$1QeaJOq$}p<2gu=k ztUwoKjO-DNw&jLX=@-cs?6egD!wPSA7z8X&wwEWnmZu+NV4LB{p$sE%;9tN^)o%k} zv5)0=Mlf%{D_hIL)>&aY_*&;gnrXqxzF3jF=lJh5-k-I_+W9KW-`Kz%Uk^S|9>ih= zk=DXJ4aQRq1Sp{&3+q`T&u|raupW{Q!L$N|PmN%vi5ZDEBhM}*2YI(5{EmWZ2NXms ztiBOtIVts!tYT^#`YBCEB~7a|WTCYtnXZWv4CW%yGzt`pF0SgrbM~meMI?XuUL*NO z>o&gi(mT!5PtM01=HgO0F1;UbyB*;gH$#$=iSjY&ZM)M~i{6ZyaKf~lLR-~WAQ8mg z4{x9vX2W=!uP{ZJVLCg+Y$fS|sDflM$PqMx4*3k^-$D2WlX!{hqG(fW-KblE*diBT zMFvq*dDVwM_ai{b1wnBq(+mR!>u zNp$L*45s*?asL3fQvU@2Q?NBOza5*7&hO})j$W<3QadL+R2CkZ6Qr^r%?fSvOu?qi z3R{C|{@2S+Nh6au37y_A9shFZXXk!$?!EmpI}Xl@2TQ`il6cS&ot4C-%|}jt$w%P4 zopQjeE+;1MC!O94?su2m#pYit;x1Jl1(}*KQ-Rzil}=BL(*!h(U_uH%B&V6Wted-- zv4NSx$3HWo1fOD*;1xI)auI-E`WqQBR9`ix&t6cdZxY!$)leU>ex_ z?(i`%u$#ezzSAs5dluV7w5S0X7B!AZ!^bY5xzn;V>+O>ui`tX*)`WJ=K;c+QOC2qk zVoaSfQed1cg<)kZbs=lKjFX2n>AVVcNaISOO*fHGT}Wl&-IJUWkhw4qON4iw;8lUV zxm=Dr3#yq>EH(m}ahUgp?ABM9ZH$DTyge33digXYhC|F+3sp(#eanEKJ||ZQJQ2Jv zIBcoNrfy)o4L}WF5Ty!GTqZF<^W2Vi2{ez*<;^+@w;;z5U?PH};FD+Jr#BmDD;4dQ z>lNQs0QKEUchjkx*ng~s9v?8|S9pg(0FYj~Y0EVKW_)?;)X4>6+M|_(DxN-k;tTK5 zTi#usB%HFH*8RrJK3hUOHwUxHaZK@E(|C#p2>V6>al&zzC74 zSot$bOkPo+>_a~q*?>KY!aCk zC52&F^xEyz^D)b=0xJ+)1jPs=M!{q9S2DaC{R3v%ZB^iVh-bzsSTT*evHX^gI`Gyr zEN>e=t*zQcv+uNd5{IOL0)ey}X}mz~H~PxgXaQaapl?Wms}iE3VAv%n!yf&g8@F#h zGkgjHXg8MlX{hmrT+Qc6M5&M=_fy0zvb;i)5qpJd)|HNy@&sL@?Se2oj!K;`S-$9)+*mCXkY`hhIU47=tGuJw1<1HR{O)b~Btw__rjiRx$;n2w7a;0V|eZ^746 z&my`Tm{{9t_!=w}%%&B?(dgXCOlUKE_;HpoAOZ|g{lk@G?7c)(p9R47EFM||$4+ob z{@GA5G86$m4YI4qv%GL;p1Nl~xVut_hiogCa$?(+4u@hEM}2x)v5~XEy6Sv0Qh1|N zE>3(Jl29fgbVo)~4w_~`u#)kt2GKou5eHG!eEs%wV8Y-MLsTU{K=7Jfv)h&ik1O-% zB_6gawTLNsdX#<}SYHMAal6>|c+pEo^(^)>X6Z*gf%&owvQVKo`MmJMv@~HR~iaSIHD(6fHWm(EDJvK!Z5t`1)g%6^Fj42c=OtpKBLC#_5XyP zER;ax&j9#6S*Bmxk~Qi?CZoWcH0HzxrO`N?wE(}cVU5^|DcW#8E`4|NK2Wq8?B7W*J?vW*e_eMg5I8tiC`+{A4Fly z4*MiSo9*CYgw*C$lc(;zN)uiBhuod!; z!S!VuJ(ihLgnv~6Y>mU(e1+KtveI81I$85fXphhWxX9w^UPl=tMZ%}Rt+(cLzOCnm zAT79D&0MA?=ekw%dAW_(-duQpESnlF 0.0 + assert node.slip_detected is False + + def test_slip_declared_after_timeout(self, node): + """Test that slip is declared after timeout period.""" + cmd = Twist() + cmd.linear.x = 1.0 + odom = Odometry() + odom.twist.twist.linear.x = 0.5 + node._on_cmd_vel(cmd) + node._on_odom(odom) + for _ in range(6): + node._timer_callback() + assert node.slip_detected is True + + def test_slip_recovery_resets_duration(self, node): + """Test that slip duration resets when condition clears.""" + cmd = Twist() + cmd.linear.x = 1.0 + odom1 = Odometry() + odom1.twist.twist.linear.x = 0.5 + node._on_cmd_vel(cmd) + node._on_odom(odom1) + for _ in range(3): + node._timer_callback() + odom2 = Odometry() + odom2.twist.twist.linear.x = 1.0 + node._on_odom(odom2) + node._timer_callback() + assert node.slip_duration == pytest.approx(0.0) + assert node.slip_detected is False + + def test_slip_cumulative_time(self, node): + """Test that slip duration accumulates across callbacks.""" + cmd = Twist() + cmd.linear.x = 1.0 + odom = Odometry() + odom.twist.twist.linear.x = 0.5 + node._on_cmd_vel(cmd) + node._on_odom(odom) + for _ in range(3): + node._timer_callback() + assert node.slip_duration == pytest.approx(0.3) + assert node.slip_detected is False + for _ in range(3): + node._timer_callback() + assert node.slip_duration == pytest.approx(0.6) + assert node.slip_detected is True + + +class TestNoDataConditions: + """Test suite for behavior when sensor data is unavailable.""" + + def test_no_slip_without_cmd_vel(self, node): + """Test no slip declared when cmd_vel not received.""" + node.cmd_vel = None + odom = Odometry() + odom.twist.twist.linear.x = 0.5 + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is False + + def test_no_slip_without_odometry(self, node): + """Test no slip declared when odometry not received.""" + cmd = Twist() + cmd.linear.x = 1.0 + node._on_cmd_vel(cmd) + node.actual_vel = None + node._timer_callback() + assert node.slip_detected is False + + +class TestScenarios: + """Integration-style tests for realistic scenarios.""" + + def test_scenario_normal_motion_no_slip(self, node): + """Scenario: Normal motion with good wheel traction.""" + cmd = Twist() + cmd.linear.x = 0.5 + for i in range(10): + odom = Odometry() + odom.twist.twist.linear.x = 0.5 + (i * 0.001) + node._on_cmd_vel(cmd) + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is False + + def test_scenario_ice_slip_persistent(self, node): + """Scenario: Ice causes persistent wheel slip.""" + cmd = Twist() + cmd.linear.x = 1.0 + for _ in range(10): + odom = Odometry() + odom.twist.twist.linear.x = 0.7 + node._on_cmd_vel(cmd) + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is True + + def test_scenario_sandy_surface_intermittent_slip(self, node): + """Scenario: Sandy surface causes intermittent slip.""" + cmd = Twist() + cmd.linear.x = 0.8 + speeds = [0.7, 0.8, 0.6, 0.8, 0.7, 0.8] + for speed in speeds: + odom = Odometry() + odom.twist.twist.linear.x = speed + node._on_cmd_vel(cmd) + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is False + + def test_scenario_sudden_obstacle_slip(self, node): + """Scenario: Robot hits obstacle and wheels slip.""" + cmd = Twist() + cmd.linear.x = 1.0 + for _ in range(3): + odom = Odometry() + odom.twist.twist.linear.x = 1.0 + node._on_cmd_vel(cmd) + node._on_odom(odom) + node._timer_callback() + for _ in range(8): + odom = Odometry() + odom.twist.twist.linear.x = 0.2 + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is True + + def test_scenario_wet_surface_recovery(self, node): + """Scenario: Wet surface slip, then wheel regains traction.""" + cmd = Twist() + cmd.linear.x = 1.0 + for _ in range(6): + odom = Odometry() + odom.twist.twist.linear.x = 0.8 + node._on_cmd_vel(cmd) + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is True + for _ in range(3): + odom = Odometry() + odom.twist.twist.linear.x = 1.0 + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is False + + def test_scenario_backward_motion(self, node): + """Scenario: Backward motion with slip.""" + cmd = Twist() + cmd.linear.x = -0.8 + for _ in range(6): + odom = Odometry() + odom.twist.twist.linear.x = -0.4 + node._on_cmd_vel(cmd) + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is True + + def test_scenario_diagonal_motion_slip(self, node): + """Scenario: Diagonal motion with slip.""" + cmd = Twist() + cmd.linear.x = 0.7 + cmd.linear.y = 0.7 + for _ in range(6): + odom = Odometry() + odom.twist.twist.linear.x = 0.5 + odom.twist.twist.linear.y = 0.5 + node._on_cmd_vel(cmd) + node._on_odom(odom) + node._timer_callback() + assert node.slip_detected is True diff --git a/ui/social-bot/src/App.jsx b/ui/social-bot/src/App.jsx index 8e4be5f..73e1876 100644 --- a/ui/social-bot/src/App.jsx +++ b/ui/social-bot/src/App.jsx @@ -58,6 +58,9 @@ import JoystickTeleop from './components/JoystickTeleop.jsx'; // Network diagnostics (issue #222) import { NetworkPanel } from './components/NetworkPanel.jsx'; +// Waypoint editor (issue #261) +import { WaypointEditor } from './components/WaypointEditor.jsx'; + const TAB_GROUPS = [ { label: 'SOCIAL', @@ -85,6 +88,13 @@ const TAB_GROUPS = [ { id: 'cameras', label: 'Cameras', }, ], }, + { + label: 'NAVIGATION', + color: 'text-teal-600', + tabs: [ + { id: 'waypoints', label: 'Waypoints' }, + ], + }, { label: 'FLEET', color: 'text-green-600', @@ -244,8 +254,10 @@ export default function App() { )} - {activeTab === 'health' && } - {activeTab === 'cameras' && } + {activeTab === 'health' && } + {activeTab === 'cameras' && } + + {activeTab === 'waypoints' && } {activeTab === 'fleet' && } {activeTab === 'missions' && } diff --git a/ui/social-bot/src/components/WaypointEditor.jsx b/ui/social-bot/src/components/WaypointEditor.jsx new file mode 100644 index 0000000..ffbf571 --- /dev/null +++ b/ui/social-bot/src/components/WaypointEditor.jsx @@ -0,0 +1,401 @@ +/** + * WaypointEditor.jsx — Interactive waypoint navigation editor with click-to-place and drag-to-reorder + * + * Features: + * - Click on map canvas to place waypoints + * - Drag waypoints to reorder navigation sequence + * - Right-click to delete waypoints + * - Real-time waypoint list with labels and coordinates + * - Send Nav2 goal to /navigate_to_pose action + * - Execute waypoint sequence with automatic progression + * - Clear all waypoints button + * - Visual feedback for active waypoint (executing) + */ + +import { useEffect, useRef, useState } from 'react'; + +function WaypointEditor({ subscribe, publish, callService }) { + const [waypoints, setWaypoints] = useState([]); + const [selectedWaypoint, setSelectedWaypoint] = useState(null); + const [isDragging, setIsDragging] = useState(false); + const [dragIndex, setDragIndex] = useState(null); + const [activeWaypoint, setActiveWaypoint] = useState(null); + const [executing, setExecuting] = useState(false); + + const [mapData, setMapData] = useState(null); + const [robotPose, setRobotPose] = useState({ x: 0, y: 0, theta: 0 }); + + const containerRef = useRef(null); + + const mapDataRef = useRef(null); + const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 }); + const waypointsRef = useRef([]); + + // Subscribe to map data + useEffect(() => { + const unsubMap = subscribe( + '/map', + 'nav_msgs/OccupancyGrid', + (msg) => { + try { + const mapInfo = { + width: msg.info.width, + height: msg.info.height, + resolution: msg.info.resolution, + origin: msg.info.origin, + }; + setMapData(mapInfo); + mapDataRef.current = mapInfo; + } catch (e) { + console.error('Error parsing map data:', e); + } + } + ); + return unsubMap; + }, [subscribe]); + + // Subscribe to robot odometry + useEffect(() => { + const unsubOdom = subscribe( + '/odom', + 'nav_msgs/Odometry', + (msg) => { + try { + const pos = msg.pose.pose.position; + const ori = msg.pose.pose.orientation; + + const siny_cosp = 2 * (ori.w * ori.z + ori.x * ori.y); + const cosy_cosp = 1 - 2 * (ori.y * ori.y + ori.z * ori.z); + const theta = Math.atan2(siny_cosp, cosy_cosp); + + const newPose = { x: pos.x, y: pos.y, theta }; + setRobotPose(newPose); + robotPoseRef.current = newPose; + } catch (e) { + console.error('Error parsing odometry data:', e); + } + } + ); + return unsubOdom; + }, [subscribe]); + + const handleCanvasClick = (e) => { + if (!mapDataRef.current || !containerRef.current) return; + + const rect = containerRef.current.getBoundingClientRect(); + const clickX = e.clientX - rect.left; + const clickY = e.clientY - rect.top; + + const robot = robotPoseRef.current; + const zoom = 1; + + const centerX = containerRef.current.clientWidth / 2; + const centerY = containerRef.current.clientHeight / 2; + + const worldX = robot.x + (clickX - centerX) / zoom; + const worldY = robot.y - (clickY - centerY) / zoom; + + const newWaypoint = { + id: Date.now(), + x: parseFloat(worldX.toFixed(2)), + y: parseFloat(worldY.toFixed(2)), + label: `WP-${waypoints.length + 1}`, + }; + + setWaypoints((prev) => [...prev, newWaypoint]); + waypointsRef.current = [...waypointsRef.current, newWaypoint]; + }; + + const handleDeleteWaypoint = (id) => { + setWaypoints((prev) => prev.filter((wp) => wp.id !== id)); + waypointsRef.current = waypointsRef.current.filter((wp) => wp.id !== id); + if (selectedWaypoint === id) setSelectedWaypoint(null); + }; + + const handleWaypointSelect = (id) => { + setSelectedWaypoint(selectedWaypoint === id ? null : id); + }; + + const handleWaypointDragStart = (e, index) => { + setIsDragging(true); + setDragIndex(index); + }; + + const handleWaypointDragOver = (e, targetIndex) => { + if (!isDragging || dragIndex === null || dragIndex === targetIndex) return; + + const newWaypoints = [...waypoints]; + const draggedWaypoint = newWaypoints[dragIndex]; + newWaypoints.splice(dragIndex, 1); + newWaypoints.splice(targetIndex, 0, draggedWaypoint); + + setWaypoints(newWaypoints); + waypointsRef.current = newWaypoints; + setDragIndex(targetIndex); + }; + + const handleWaypointDragEnd = () => { + setIsDragging(false); + setDragIndex(null); + }; + + const sendNavGoal = async (waypoint) => { + if (!callService) return; + + try { + const heading = waypoint.theta || 0; + const halfHeading = heading / 2; + + const goal = { + pose: { + position: { + x: waypoint.x, + y: waypoint.y, + z: 0, + }, + orientation: { + x: 0, + y: 0, + z: Math.sin(halfHeading), + w: Math.cos(halfHeading), + }, + }, + }; + + await callService( + '/navigate_to_pose', + 'nav2_msgs/NavigateToPose', + { pose: goal.pose } + ); + + setActiveWaypoint(waypoint.id); + return true; + } catch (e) { + console.error('Error sending nav goal:', e); + return false; + } + }; + + const executeWaypoints = async () => { + if (waypoints.length === 0) return; + + setExecuting(true); + for (const waypoint of waypoints) { + const success = await sendNavGoal(waypoint); + if (!success) break; + await new Promise((resolve) => setTimeout(resolve, 500)); + } + setExecuting(false); + setActiveWaypoint(null); + }; + + const clearWaypoints = () => { + setWaypoints([]); + waypointsRef.current = []; + setSelectedWaypoint(null); + setActiveWaypoint(null); + }; + + const sendSingleGoal = async () => { + if (selectedWaypoint === null) return; + + const wp = waypoints.find((w) => w.id === selectedWaypoint); + if (wp) { + await sendNavGoal(wp); + } + }; + + return ( +
+ {/* Map area */} +
+
+
e.preventDefault()} + > + + {waypoints.map((wp, idx) => { + if (!mapDataRef.current) return null; + + const robot = robotPoseRef.current; + const zoom = 1; + const centerX = containerRef.current?.clientWidth / 2 || 400; + const centerY = containerRef.current?.clientHeight / 2 || 300; + + const canvasX = centerX + (wp.x - robot.x) * zoom; + const canvasY = centerY - (wp.y - robot.y) * zoom; + + const isActive = wp.id === activeWaypoint; + const isSelected = wp.id === selectedWaypoint; + + return ( + + + + {idx + 1} + + {idx < waypoints.length - 1 && ( + + )} + + ); + })} + + + +
+ {waypoints.length === 0 && ( +
+
Click to place waypoints
+
Right-click to delete
+
+ )} +
+
+
+ + {/* Info panel */} +
+
+ Waypoints: + {waypoints.length} +
+
+ Robot Position: + + ({robotPose.x.toFixed(2)}, {robotPose.y.toFixed(2)}) + +
+
+
+ + {/* Waypoint list sidebar */} +
+
+
WAYPOINTS
+
{waypoints.length}
+
+ + {/* Waypoint list */} +
+ {waypoints.length === 0 ? ( +
Click map to add waypoints
+ ) : ( + waypoints.map((wp, idx) => ( +
handleWaypointDragStart(e, idx)} + onDragOver={(e) => { + e.preventDefault(); + handleWaypointDragOver(e, idx); + }} + onDragEnd={handleWaypointDragEnd} + onClick={() => handleWaypointSelect(wp.id)} + onContextMenu={(e) => { + e.preventDefault(); + handleDeleteWaypoint(wp.id); + }} + className={`p-2 rounded border text-xs cursor-move transition-colors ${ + wp.id === activeWaypoint + ? 'bg-red-950 border-red-700 text-red-300' + : wp.id === selectedWaypoint + ? 'bg-amber-950 border-amber-700 text-amber-300' + : 'bg-gray-900 border-gray-700 text-gray-400 hover:border-gray-600' + }`} + > +
+
#{idx + 1}
+
+
{wp.label}
+
+ {wp.x.toFixed(2)}, {wp.y.toFixed(2)} +
+
+
+
+ )) + )} +
+ + {/* Control buttons */} +
+ + + + + +
+ + {/* Instructions */} +
+
CONTROLS:
+
• Click: Place waypoint
+
• Right-click: Delete waypoint
+
• Drag: Reorder waypoints
+
• Click list: Select waypoint
+
+ + {/* Topic info */} +
+
+ Service: + /navigate_to_pose +
+
+
+
+ ); +} + +export { WaypointEditor }; -- 2.47.2