From 368d25494cee9a9de7263a8727d347435f048f62 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Thu, 19 Mar 2026 22:00:01 +0800
Subject: [PATCH] =?UTF-8?q?nav=E7=89=B9=E8=B0=83?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 mapping.sh                                    |  16 ++++----
 nav.sh                                        |   2 +-
 src/rm_decision/test/simple_nav_test.py       |  38 +++++++++++++-----
 src/rm_nav/rm_nav_bringup/map/wanzheng.pgm    | Bin 265749 -> 265795 bytes
 .../rm_simple_move/config/simple_move.yaml    |   4 +-
 5 files changed, 39 insertions(+), 21 deletions(-)

diff --git a/mapping.sh b/mapping.sh
index c0272ba..470f9e5 100644
--- a/mapping.sh
+++ b/mapping.sh
@@ -3,14 +3,14 @@ source install/setup.bash
 commands=(
     "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
     "ros2 launch rm_decision decision.launch.py"
-    "ros2 launch rm_simpal_move simple_move.launch.py"
-    "ros2 launch rm_nav_bringup bringup_real.launch.py \
-      world:=wanzheng \
-      mode:=mapping  \
-      lio:=fastlio \
-      lio_rviz:=True \
-      nav_rviz:=True
-    "
+    # "ros2 launch rm_simpal_move simple_move.launch.py"
+    # "ros2 launch rm_nav_bringup bringup_real.launch.py \
+    #   world:=wanzheng \
+    #   mode:=mapping  \
+    #   lio:=fastlio \
+    #   lio_rviz:=True \
+    #   nav_rviz:=True
+    # "
 )
 
 for cmd in "${commands[@]}"; do
diff --git a/nav.sh b/nav.sh
index d6bd858..1fae5ee 100644
--- a/nav.sh
+++ b/nav.sh
@@ -1,7 +1,7 @@
 source install/setup.bash
 
 commands=(
-    # "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
     "ros2 launch rm_decision decision.launch.py"
     "ros2 launch rm_simpal_move simple_move.launch.py"
     "ros2 launch rm_nav_bringup bringup_real.launch.py \
diff --git a/src/rm_decision/test/simple_nav_test.py b/src/rm_decision/test/simple_nav_test.py
index a5ce3a5..dac6bd2 100755
--- a/src/rm_decision/test/simple_nav_test.py
+++ b/src/rm_decision/test/simple_nav_test.py
@@ -23,19 +23,26 @@ class SimpleNavTest(Node):
         # 状态
         self.current_status = None
         self.current_distance = 0.0
+        self.navigation_started = False  # 标记导航是否真正开始
 
         # 定义目标点列表 [x, y, angle, max_speed, tolerance, name]
         self.waypoints = [
-            [0.0, 0.0, 0.0, 1.0, 0.15, "起点"],
-            [1.0, 0.0, 0.0, 1.5, 0.15, "右"],
-            [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"],
-            [0.0, 1.0, math.pi, 1.5, 0.15, "左上"],
-            [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"],
+            [0.0, 0.0, 0.0, 0.8, 1.0, "起点"],
+            [0.12, -6.75, 0.0, 0.8, 1.0, "1"],
+            [1.65, -6.76, 0.0, 0.8, 1.0, "2"],
+            [1.15, 0.5, 0.0, 0.8, 1.0, "3"],
+            [2.70, 0.7, 0.0, 0.8, 1.0, "4"],
+            [2.10, -6.72, 0.0, 0.8, 1.0, "5"],
+            [3.56, -6.82, 0.0, 0.8, 1.0, "6"],
+            [3.2, 0.9, 0.0, 0.8, 1.0, "7"],
+            [5.0, 0.56, 0.0, 0.8, 1.0, "8"],
+            [4.96, -5.96, 0.0, 0.8, 1.0, "9"],
+            # [0.0, 0.0, 0.0, 1.0, 0.25, "回到起点"],
         ]
 
         self.current_index = 0
         self.goal_sent = False
-        self.goal_timeout = 30.0  # 超时时间（秒）
+        self.goal_timeout = 120.0  # 超时时间（秒）
         self.goal_start_time = None
 
         self.get_logger().info('========================================')
@@ -81,6 +88,9 @@ class SimpleNavTest(Node):
         goal_msg.max_speed = float(wp[3])
         goal_msg.tolerance = float(wp[4])
 
+        # 重置状态标志
+        self.navigation_started = False
+
         # 发布目标
         self.nav_goal_pub.publish(goal_msg)
         self.goal_sent = True
@@ -101,14 +111,22 @@ class SimpleNavTest(Node):
         wp = self.waypoints[self.current_index]
         elapsed = time.time() - self.goal_start_time
 
-        # 检查是否到达 (status == 2)
+        # 标记导航已开始（状态为1表示正在导航）
+        if self.current_status == 1:
+            self.navigation_started = True
+
+        # 检查是否到达 (status == 2)，但必须先经过导航中状态
         if self.current_status == 2:
+            # 如果导航还没开始就到达，说明是误判，等待真正开始
+            if not self.navigation_started and elapsed < 2.0:
+                return
+
             self.get_logger().info(
                 f'✓ 到达目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} '
                 f'(用时 {elapsed:.1f}秒)')
             self.current_index += 1
             self.goal_sent = False
-            time.sleep(1.0)  # 等待1秒
+            # time.sleep(0.1)  # 短暂延迟，避免消息丢失
             self.send_next_goal()
             return
 
@@ -118,7 +136,7 @@ class SimpleNavTest(Node):
                 f'✗ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} 导航失败')
             self.current_index += 1
             self.goal_sent = False
-            time.sleep(1.0)
+            # time.sleep(0.1)
             self.send_next_goal()
             return
 
@@ -129,7 +147,7 @@ class SimpleNavTest(Node):
                 f'超时 ({elapsed:.1f}秒)，跳过')
             self.current_index += 1
             self.goal_sent = False
-            time.sleep(1.0)
+            # time.sleep(0.1)
             self.send_next_goal()
             return
 
diff --git a/src/rm_nav/rm_nav_bringup/map/wanzheng.pgm b/src/rm_nav/rm_nav_bringup/map/wanzheng.pgm
index 342acd29b258b0e346af994996373885c6cfa5e5..c2bce1b61e2dbf90ee3f544df8b734a5f4597662 100644
GIT binary patch
delta 14292
zcmcIrdtB7lm7ja(1;fJ`7@it3AmD>H25XGsXcVoA3~tthC<aB1x{9FCsAS_Sv6I#q
z@mF)IHI;0dPeObwwoPyQA)jiSq?)ap74zBMwx4|bjN47ANj^4a^U3xR*gfa|9y7p5
z)EVXv_%Yn`J&$|tIp^Mcen*Zw-#YG`>8o&z_dK|D?W)$bYdouWc;<h-+UNPk+O69*
zY-#e87nhY5Pbu~ImQ;J1H?=)pw4upSQ8vX>Q8vv{UQyvV>v}F-xmthqtS5)<JDw?y
zXR_>)q`t29#?X(yM{)IUht5_WBWf11Miw73q5uvR@dM!f7PAT8Vph%Efgb-8q2!HK
zRRzlI9UeHb&OB4`^iPBC{XACGEMZewW{-c+(8k90V^{hH@Y91N<DHcu|1C3_8c~o|
z@Sb6IQSD=9RuLkIHqu0&CHRpK_=CP*HnVKzG6M>c(=2(Fjq~YYoWY{5y^9#@pGG8w
z-ZXO?)KoG%)GTFxv!b-9OIhCn#kXUO(*SHl73;eClv>EN5E)9@LQ-15`8>Bl%|q-<
z=xk>(%9-n5bA?dRV`Uzcea5Hibgg9OSmkGx^$=LfQ#(8@%K4_0`W{^ylOe8-WBf-|
zRen9rWO4=dZS?o(N(F!ST{%XxJlSMghH_8V`L~0w!2B?5`G7r7jAQ&JQ(0AAd!O=)
zCb!f9n(J7cI=U9}^#_B#70y4!t>Al<^{fBu^=Cl$X_lmq98R)@{J{ZZ|Hfx_cnXz3
z{Vw9Hk1x<4h7&fap0H#kV@w}4!tl*<I0O!J3vBp;xjH<Hpfer+__T`KkDTLsz`L2H
zN+9TcmW_afjLF?=Rx^H^jPuO1l)w%vJhz6ua~q;=B2lek$vS4e9R~jZUOz4g-+E?I
zheKJTc?9_%S1zxyh=m(joHpEb>6Yq>Yx`IO3o*?IW6|%|5Hu{@&;A@FLH`;-nAN0f
ztf+1_q-lA1s5J1>X=b=sz3^^j8O%m2ZNLPpma|E^NmU<#*^{|3dWG<|vP@0SFv!`h
zJQe=6jk(o{Uv)oQW4Le)G-tBcz`4zAQ3kFiF+a@S!irU9y?!?Y-qrjXe)>dEzQ-iJ
zSh$@fG1e0#f!TG;4PRE9W8jl&vqR<A(aw@tKw&pk$3qLQ1XRx|7Vczj#{Br7B6Z}R
zvpg9}SDURQZDKkuOp__NoCgL>1N2P~beOq@wdeW-2wY%O$-pucSw!`dD3q#zzUW~v
zFz+$JV<%Y(DjP()`EL`1^C{*VN{Z5_!9RbSIo%}AAZbjdCzNa587i-q3whAc#>#1>
z$BD(e5q`K3DbTe~w`cZha|%q{V0K&DO{PlKD$>RGhLt~rRLTA@P5JnGlI57ml!t`%
zQhQPTG)o=I6Qy^j1%?QYO328s`$aOlf^)D!T`~K0Poj{SDjhIXX`BR84{EIjFF^bh
z*zPmCSsz0Dy}(797e)rnf??1am20OmEhu0gD&i9ubn@?zp52u6SCIbi#M}dH{7`y1
z`GGZNcUn09l*@Ykm@G}EV+b)%MrtwpAe#`xnG=Rkwc%jX&|hJO(pQp4rODKeJZj1P
zF&rFX<D$<)GF$aZKAfMza#UxWe>l`A(NacZluKSeUSsqfmODxsx%%f2j})1^J3ObA
z&Rh%BonR?8)#T)@QPm8+Nk*v`uJL<1JRgDYIhJND=qPiNPM07S9vemT)qt-8LuCDf
zEFL;<n*Rd3FJMKo{6#G*jDr2DF`!t*;_r`wQdXH7pmrBqXhi-R#d+v0CGZ0m<t$6I
zr^+hJQ_93%)z=sN6Dwe5TG>(aymXlhYbdu`#F8I`M$+gwT)MOhw47zRq(rLX{Ox0h
zw_kvUE~XcwN29cgR8|=57RbU4e0$knIy|XZ_PQ;YMH{JRD2kF+YSEHMHFz57l>jHo
zIeOmfY;v@tQMy{~NM85q@xdY%pF(E{>vF-0f#T(zq>BtvYctD(L#KJB!20I?D7%|h
zxU70bhVV4u?M4=?k;<{?OpM0B)$f&&V9^g*Nq8d#&2q;^QjM;%u!u!JW~HOXVz#j`
z6IqAHfvz0wgkTt~L`?fBn>T8jl+h|lmZ?<nbiZ@BPu&?rFOmfnf#yRzRruZ<wW;58
zJ)bI&7%~0ltcp=38O4ZfERj6gWgw69$U~+A|0o!%h%%QAE>1GX!>&v`KdO5hcTO;C
zGfS5VuP5p}YoUbdM2nd7HuD<pRuI+Y-*PU8jOP$De#Pcve1YD5toTki4BG~36(1hE
z$ki`g7uhxnTrD5o5w6VW!O%lbvZmE>6?Uyyc>l#%hwNrSM%ehIQ6;Ai-60MEr0SuN
z=GaZC$|mYAu_D#zlnuWMd^qbt(!#1|c@g1`5x)1>G?^=+1X~D~zlo(|(~CHXHJ4?Q
zfU+CB_$#b&8v+;7d7A}F*K>#PT|s2EK#77i@Cc`b;zEM*-fHKMIz3KC@mh6~2<1j-
z+*qAvDWc{-*bIHu9F27T5H_1&lc=n)2;Xml*bpH$#*K;6HI#b;Hu3Q9@KjnaRw6~#
zs)q!z_yabFS*q0V8wFJ1y~=8gcpf>R+z`i!B>^@&7{McjZMiUk;-_6K{x3F{89_Qi
zSXi|U%RY34G{D5^q!V1LX#Q`OZOmF>gf86C$Mc}{JZ7}&KN#_?79xZd*?(l{x-u+9
z3<e06FBW{n7Do#a<9Q*lB0NK&BXQ+VFWE%XCn3i?5o!-;)N-1zBspQ=`yad{Sf0ZY
z#I74GEm}&@p7%cDk^-0KB7;t5w!rSsnFE@g7)hW1*c>Yke#&B_#&6FFjy1vo4A75#
zrUz(@Cy^SfWiKzK6NxxD<>hhEnZ;w(n~Lu<vrV8v8Bx;!6Jw!x=nJY2qvV>DYH>^B
zf=|o2T{Qh!PvduE7t^pTUZPHK!~fyE+8Hm}rQrrvg$D*$oH+PbmKZfH@ED*>Jvnap
zlFf`7Z0&SjO8Y&){a!rm{v%Ft!%3Xerv{iOdQ4hak-q(og_D29P)Sh|FiJY$`7HIx
z_Qp|eixx}e=IK-_C>&x5=S!o73Y7JQ&%$eih~W<12L}9jP@Wp*Uxxc=JD)<is>I_q
z{=ip3;yM0Ekm=e+M@#Xs%BP3yVr>ks4C016QO7cxu5?mvEvvqp(SYM<vttvFJ9vl_
zhTkBBJ6+exb}HY4AI&P-DU_C`9kAZX-O9%=#fiP~Ja|t~I2z32p!%0s7;H-9(}VVk
zDkJUgD*<j?#wfJK$)}APBe}(nhYfC?3#D!57@;Kb;Qdx~?Wb+%69jS#?n$G|126k%
zExTx~#KVDHo~0%qye79m-1|KC)z?#l_ukRv?dt!UdS;DL*LvhAAEN{|WeAL3+D$bj
zk*NBKtA9EblkozVnYP~?1s)HGwsGWvxC5vf1^D~rPEl;n;<$GiH6DSDn5f!4{Jy*7
zG5n}va3FH^-!E?`#LgVjL*578>2wc2wiv_%R(+32CRXueF2^e$d9jI1GHF;aLh4T%
zadOJA!;3+%VpcxiCbwf(Xm-B}Y#6V>VBU!u7&R0{3Wlu^+ZdU(G%~3m`yv|)&6}~<
z!sU`d`y-3kU5L&gFSjE_$Nmnl*jLQUnY?a~5KhsOr*R2khgZEE_ljBg1IyAFIG>31
zW9&1068)KS_)I<=vhjE*E91}TjRQrI84P7W(D&IGI9GwkPS2L}Vx~7UL<&tQyYw@b
zuLO2l1l-5<y|1f3LS(f?1%v-I@FtlP0jqashi4KQ#|D9^{I}{RAtFAwl>B<Vm_^Ps
z4hv!nG;n5De$!e3A3Vg}@X0j(UKDs?IKT!RYP&(5D%9xm!gQ{8Vce}AFncTR`|#=s
zAt@++cLvw5_9KB@1SJ<)0xdGTI9SQ``=Gmp#QQ~g=9xv;49SOIcZlzLx!yn$DJZrg
zoo9*A`7Tcq9dpRHWe|%1w$unLHdtD3PExEt9;1BpPBN^R&&DVX+i)MOH|xRg=JG#8
z$Pn9Fh@q2y#6sOJRHnWK>wtf(;&UTZhPv-OuyMJ%_9bqDcxnOR7MM(RG%ples`M-q
z{H-N|<P>@cevMD`g^Ihdif4d#0P8b4qXImmLT_1!f!NxIyPS5)8BA_oR0H+;M?fo4
z8epv0y_i49tX)XmC8_0Z>Nco7;^8HHb1<rnC8ff&nvSfMqSc!@Q2Hy}#UJo-?WCLR
zWJqB=gf|U=h=!$nU2y6Q3gaO>Dcud^ij@!X4R=RdKpvDtKkgAnmT|l=lKS3(@No8N
zb0utgk>Rc5o4ALDM=%fGF$$V%7D<71`<v)_Th5tdp~j7Ebc9LU{usFQC?6OJm%MYh
z*PSh6Gu4Y335*HTMK!ujys?6(hLN+7^dAHx|B&7}=xZ7Wz7|$V=u*xYacpH+-EV;@
zeRdcoTB4FFVk<az;(;c0i~x3w*u~2YTyJ^^ot|5Ow`1qwAZ+yp%+)^=W?e<_Bma&I
zQ5yHx`FH}@y_#ofmGNzW54Vx#HOuJwITq@(P~O+paBQ@b1wBII{R|#SSaDock8Ij>
z4>hv}@d&BxfSyvkB|Wo_=LEwJS;a;zi7pA2EUlD0WU@4r<%DuCDHsEFFPr0D?Ol(%
zKCKh5h$!vQ{Ty>@?MB!y>AnThO7LChW1wLQ8ZFq0dTt|6)@o*UtpqObV=3}Xr4eNF
zXZN>RqUx*K{me9|nSoAsY7^N6V-@yPB3Ws(*6m7Eso40Lpc7@%Vu%-;n=mNI8nsxJ
zIkuT=y}sGae7rh(&-IC-xtXs~tIrbhM;kb{a(5CLBh(2@rWZDHtsjVDPV-j2mKi0Z
ztT4dQs*NC-(jcLU$Lm5(CZP%i_W0utha8N89LW_!ZL}_RSoVjhfa)8XOriu{j8&3N
zy!H*fEKqBZ(pD_Ya25<ock++`EN8P8CK2Njz%~}UIJw=}>bm+%qz}+aQCsw>d0MlI
zMpaH!4)MdCWO&UvN=Z|_SN>h55xDv{P-nbd{QOC-cW=7-t90d!Q7r;pYpek_d;s>p
zlb~<y=K4wZa2RrnlF!*h*u9BQ6z}fk?ppy;_qYs=va!oW2lg)UpHFl9tq5>Z>IE*t
z5~i*;{Ci$d2{~=_S_7IOpQgrR__v+>+b%BqxsxvQ@v8RgK{~A=*-Ioi9s7Brxb!VN
zDN;MMf&=9a=s}vV$pS^qQE%kvrIMf!GS^dAFgDzsW>`Ay$D$d3jDeadc&7ZtL4HG@
zDvb#D3<!)>k3jZqhUNxju3XwwVx+<O3btO=1YefW4~awA=gQ4Fw4CCv5mOA^diSK#
z`AVhIJ=X@Cj_`I>u?4yXa^-`ukvte;HKezL;~Bn4>GW9O@EiOIwUulb?dNPNzCpB>
zxTb=)0+ZdnNBOU1YvBBtWhT>PdVL6s7>~-U_X3+i1}ZRkC8(qg)n+>c^0^(({V$$x
zp*z}O()ai;b)(HKnvWqy`{1iWxBg_~3o<>>@`(AQY6{Pb{E~sfs+6+d7t)JQ52#|0
z+*orZ8uGY}rx<9c#!IS6C-~c{;ikujyc|^pzc_BjwYttM&#T&?dl`!Sr;FInN4XdZ
zUgB@5HY>NQWL5ZflY7D=Jp<aCKB$6x7iy>~sOV~X!Rw%5nRxKNj`QJpAMZh(H}T$1
z7T#?y(~RQbb)khAAbPq*z8U1|?~tTQz>gV$6!i2@kqa0ec!mE&HAXFr&yg={=|<Im
zhlu0v`H`#Nitn0W2!Z+>T-r^q@~2h$UVpmBc-gB83zd+M3L&X87;Haho(SF&yf@p~
z#dnOufK?X4Bji!Om6mM9Z~Q$XI|Vy&VbMw6g_*JJ_Y-Y2=kmq3M``BTP^WpMTq4Xn
z&AZVKvv=GRr}+h00VU64G|~wx`}kz+r$~n8!+c#9Zb3tMhCXU!GuStMXSwj5HNUgs
F{{c{|E;9fC

delta 4972
zcmZV?X?RrC)qCE|WM&e=WFbg^5VimbhDlfisu&OvQ1BHMH^i_AhzipH2?=Cj6IlWo
zMmVx&6s&>>0|BoRgrHzRMW*&sE47do6|EM~3MJ?VJ?Gw;m)PdVeD9upKlkRnO`f%z
zJYe=+;N9;PwIc#%<8t)YgH0BX6jKEqU*w}*mfkBy^Km=+x*2X+Hd^!*7#b{&;><3>
ziRGra5eS`1MDKL4+fLFSiay3^#fudKg&j-Bh+hKU)9o-kPuRpsoNx(F<G|J6#?lAG
zuY_rXzNd|NyfI45Aw3h76<9i4OvJj2#?pZIvKPHGAeC@`3V6yNVQ3{m+oN}k;lOY`
z#53NF!AFEE5IWz92zOycKk)!I+-f)jpPxv?hM<w`H=ic1T&&2393t7tdfeDh0QNx3
zqRCN`8t>(JfzNaiE-V{Qu9=Nj`Q*m(d~pMV8Q{bZkHL>RHuEP6gJW}*k4{-WSqMJ<
zM;1*HZvMK$2!|}4ChUSdZHu9Oa7=|OTu7mwJ6*&Q{HRC6&Bin=S}d*xJiEI_&D@Pl
ziF2RRV3K)87WhQ6U|Sr_Hzi7X4=c0Df&$Shg3UipZfsZq<5YHVPzfZGfF~n#vqegi
z6avm5-+q}Rk^`Pfrxn{rIg+3H&5S^|vmCZtFkB=OwF^bUhF6Qk%><srWDZ$8S0u3C
z!!TSdk}z+VNWrABMmqwH*IJ88$T~U>`Hj9<$4%F7qCfNx5P6pFTsdpLNc~@G@7r-~
zgH0&v!ODB|bbvVY?GQFuRwg{;)-o8(8eBN25Ihv(1R}pQOQezNE}A8~9$H}5Iq2#)
zzo5c+goWDi^F_)#kBXSM9{6!KyEh?{$AuzEo6|&F_jNMn&xV-h<u_bX8kON>1oZ_b
zk2Ts-Sl!K>HXFyO)JjUNCJh7^*svzw=%zl-WkD~}e7n{>`+3oUH8*!EbSepbpNM$O
zpJcQmoXWpl&RQ-y{y%B571imoV5R6BiRyG{O`Jun#4Ts865Sa7rA}30l@_#1r5wYH
zAx+L%qgkoJvY`vp#mYG^h%U@_l8>#ZMJM>ptr*--WxKRmuw@NOAo@BOPx#I4SXL=m
zYZ$BUqrR^kA>!7#pw@2=!G#+|D%QOY6A3*ReJRx6m3LC}h5v0dvcwpCZnJ2OuRlyV
zH5-p}0PV753+cx@%0zF1P0-yCI+egP=6=8V?wUZNu358jsYZG#9oq83mqnJqqo2a(
z=qZO7S-wql61cV$W|5@MR>UZF^^m0<+eM|4%++njz3-@Ys$;AJOBaf3elw*BeY6&K
zvcA^JO<A}@WDC69!%(5?pdzxah=#+wog!0Uc{b!yg~VtJ&BmeHW>2L{&e$ae(8%2f
zyBT>ib2?>VNaP629%DFp0v=P5z}449oUA=>#L-Z>OoWm7_`WW*;(fx)dHUFLHdgpe
zH<A8XPI*h*#kfN(JxRMNc@7b5w<X9!h-69gm!_V^jZ*gFrTIp8nSY2j1)2{VA<f`>
zpW1O+ofyV~YuO)%ocXTE<KqE7#z@~$eSFMPG0Ng~;P*Myi{W$e_{%WpAcK2_Q{bWf
z&_vt^tA;|Vq%k)};I{3mmhXxt`yTzVWr0I3`hYx#>o?(%w_z<?+*#pp#%bY{OFt5~
z(7<?%hS~9B@PbDBPK%bZ>|@biU|lUB13Rg<-0?6qN@<ug(yv4<r80$-)QNXf*muS7
zepq8kiN~^og5p+S55!OY(r@;naJTu*IIdt?Dv}InB}+dOx3LgAr(2nl!U)=~JlV_s
z!dSEMO-bh6BQSlWu?OihUY2|*h6rpw+K40cEArVh!W39B4tmh>dG<A4&TkNdE!cAD
z`v)iMR4uGy^=Zlm%B}~4zY4qD@)fl{K0eiGLjp>z<mi%;#ov&t_@UHsQG3{i^D@94
z_}B0lywr(`diA#=mq<R1l5oSwl7EWf6zLbhPf~jXdc9`HZkvq+47WGz5`Pdq30VwN
z2uT}1QoCKrs6=hl#tWhcyRWj*lS`2~-MH&#nh?PT%HJUexa7f0!lh!fP8UGvRElny
z6+emJSsHSXf|N{qjMiPb1FIa=!hg*J_nL=XN~$EOZ3I>qDSLWzd^*X6SE!gVI6$oD
zGZj%GaaG-F#Ik{{F!(Lmu=@geT60zH)_E`$eNzb?4nj9(VQ-SLE*YHbdVwpDzQm3P
zu8UZC@S31oNBC`6Z#mu(H{VR_YjuWbAxS#TDN5D`B}9)sYWCZS=N;I{z>ca!c*MH#
zoh!8VFI}h2z{x~PrhETENKgv4alypO`2f^1p+ZQffR?a;6h^r5(X$X6a2F+FMH@(x
zZ`q(VhrE`9Bq>OF0;~F|`_lqNA4v1Z$bb_Dk?`}dmtk>owHxlF^D4+7)|H%v7FhO)
zP*JvF&_jjN=H*x!iUYNV_NoHKH&SW5Q}Nj3mU!sLT5GIXK$o$;!YzrluSSrb7Kq8T
zf_xWg8Xidmb*rdh1L7qi)Rft$nkSlIJGLD`LBBCJg){8H`_9t+?_?jk^_)$CM$&go
zQpi<@9aoMZIpPR-iaMz{zIr+pSosOLwH@^5uX<j;4*6Od^ifBeRSvm5J#xajfwY{q
zchV-KmDo&n*DiN-(TEorVUyH8PqL8pvbZ};<F8H4rbOtJhucMN&x8St+afD^!o<i|
zHcaBLy`05#s|=j@T{^^KLmt@IJmkTOjfM*^#?xKoq8BbJa&>p8J4|c3dI&{=@@JPa
z7qVGcjnzA{t`B4|=w+6l6G*RasayQxe$a;D+Zf(~f9Q(OchyxOVsOPbBGGSlM)!k|
zjEBD_y&VR?2}=y$US0C&K<LUMUeem=Y9{M$gG>!lQhE3(&yNh!@E|Knvx3<|tzvm3
z53-mm$O%uf%EpEtoPZR0c!+jttE6=?p6Qk|orja`!h=Jp&GNsclvED`ld;Pf;l`04
ziI!GqY_fVd{QkFCifT5bDs8f6B;2kg6D$W$77Lf$cW)HN^5l}y@GO(4CXU=LXh+)R
z0aIr+(%nStln2JZKqgk(B~`9`0G_gr9raEiMeXH#W1%-ALcE<MZKqLus$q%*D*TqX
z`7kBEVL9CpJ>@hS(x}|8Jqp)}sKSb3mX808X$f2>&OrJWm;Bo}Xvd-!u#>r1w+CqR
zkJm=+Z7NMPt7+Vy$cJ1e-XnKT)G_>kpNk&(*<?^x%G_o*ReoMj3-D9qaeg+~G3=xI
z2q%-EFQ({<$ibj#WHn<l8%>MWfsJKURb)*Zmii6nx~G62YB_9GYZ~cE%v@xV$FYqz
zEIve|>8I%s&xZEmo4EC4Y~AdsU^Zs5$2saU_I)ez;j3q$y~UGC^67EVjJdO{ngM@^
z5cr@iKQ{QS>%bF}jR8?lX~UAIk~~#`@-p0k{R@D15DBCifcXVfyA^#w8PJ}sXvgjB
zmh?zflQM_7Y}kJ`U9%%*@Uu9JNqg$fKR+H?$g{JfQ6xhnPayS9xd3@q(z{FQRy9s@
zqk*G)hJFzAWQi1`db$*UF&A!O?|L^Sos=t5d-O@EEXcuL^`tHRLz*M!N<h5{`e^^X
zr&~t@F_Wd*u_Bi)hZp99dis20<;o!`wH{XYccmRGx)a^y3O(L8>!ocr4wRP{LJE^D
zW=p;Nqh2Ek%;l87F4jUm;bV&Y`8hqRR%s(>>041qwN%GZ#_jypcgh>jLrWIE-y)-S
zrT;U?Ys<AE`&nV8(uJhUC2y>x{Yo5fYIBHKJsMhKX>SpWT~@;dLawyLJz;5e<1_7u
zqP-uk5XB0MVv_!^Ly<?&D&`*P9)KT+LbYQKeLT(DTQz$lRh5WAfipuLxM3~KBZ4Jt
z3R6zAlqnb0I=l~|o=}UT0WD@}8R}wzOR~i<#PzU%2o_id^<Z(7=SaV@s5t4_2rhQJ
zX_RcxW{utq9}zsHnMgK0x-Cwwo~9GO#_q3%<56-p`EHitL7G7>9QP8`5$Lg~JL<8j
z8iO88<+<wih}*V82@#!PKDv<B!wCMN*Hm{3-HcaYk4(=s=)y}tJ63KqGI7(Zu$X}l
z3jE_?qdQ&8o%qUjn9s0>1kQaOc96hjQLW3EH6CZZ0ilQ}wLvPsgG^%8D@TUwVI+sc
T7YMy%U%F@y$VGdMx^e#n#FcjU

diff --git a/src/rm_nav/rm_simple_move/config/simple_move.yaml b/src/rm_nav/rm_simple_move/config/simple_move.yaml
index cdbbe92..fe7bffc 100644
--- a/src/rm_nav/rm_simple_move/config/simple_move.yaml
+++ b/src/rm_nav/rm_simple_move/config/simple_move.yaml
@@ -12,14 +12,14 @@ rm_simple_move:
 
     # PID 线速度 X
     pid_linear_x:
-      kp: 0.8
+      kp: 0.5
       ki: 0.0
       kd: 0.1
       max_output: 3.0
 
     # PID 线速度 Y
     pid_linear_y:
-      kp: 0.8
+      kp: 0.5
       ki: 0.0
       kd: 0.1
       max_output: 3.0