From e047c10277ab89f80a339b392123557dce8c8aca Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Mon, 8 Jun 2026 16:23:08 +0800 Subject: [PATCH] log --- .vscode/browse.vc.db-shm | Bin 32768 -> 32768 bytes bashes/radar-driver-switch.sh | 122 +++++++---------- bashes/radar-driver-switch.sh.bak | 129 ++++++++++++++++++ src/ground_slam | 1 + .../launch/robot_mode_description.launch.py | 5 +- 调试记录.log | 19 +++ 6 files changed, 200 insertions(+), 76 deletions(-) create mode 100644 bashes/radar-driver-switch.sh.bak create mode 160000 src/ground_slam create mode 100644 调试记录.log diff --git a/.vscode/browse.vc.db-shm b/.vscode/browse.vc.db-shm index 5acc17d8c18cd26b41c998e7637ff5f2588d8423..208cf95692cf16dacb9576a22fe4b1abcc74f19c 100644 GIT binary patch literal 32768 zcmeHQb+i@L+kW;O=|;LcBqS9saOt>oUO*Z_IwU189nx^=6coV%RKzA!FcGm4Q3=IB z5kYK!&pva`zLV$7x!||fx7N&B53}R#{l4!Ef806w&Q^lFzPShppxA~0@XP~1iN)7D zk`OUA9;svX^PX!)cUcUL{%dT5q>G13Qx^B*bqU&9q%58pF#P>VQ0y7!3piHd1~ z%EZImY?B#SO?*Tg%K~)C0c^<$q`4JH$`2$K1@0m~miKa`Ay5IxLu@AoR|Iy^)ho4u z#&v;8^(ZeM7+)XA-U!Il7>FmD5Fa)LP7~h|nOguOh@%NW+LplVwm^;czyac$4)m)d zP%;td&jKA`+0V9sRvG6kqJ6&OV< zC)N@7Ob41K0iP4oX8_lT`7?o4#K~De#<@WLd5pn)>R150ybw4@#4iFS5=lh{(6Uh)>o4-PQun6Ia*MzYV|-M1_rvJ5iMwL9E*Z{Joj_w*vFF z1@k0$R`Q&}XOuBLd5F=(;~5wq;ykgDXD83^y-fNU7R;7%X7aoi)&YKQqQ`nd4={xoolmcuCkg<4ivV#v3wa*bh+B#=_QW8f za&ewVNgz;)H9Tao5Cs}Ahltq?DT8%7pfPK%sa~V9J_j`ewh}vt zUBna2p=XG-Ua#Q=tl`5%){a1H;&EbEC!R-V+M5XU zCSE3*b_Is@(Chq>UbL|{br6O703(U}iL!keL*naxdM}7)KbS#WB5DoL`@&;G*c+JB zmx*!1fTxI4MBCv&XX0bx3i0a*`a6>L5VCiyCS>kkC1ek|#N4ks4j4F|^-f$R5+*QD ziQE%;4#XeC1ooLz#2G^No4Z4`^2cP-uwFVCYDU(|N@5j}OspYfFDft{Xh-ZIv?M+x zh;qbuB5nrIi*>VrXg`ZS5#xz##KcgKs?HjU57p8h-qSBk=~uE(?IqGKW*rf-&SY(k zUZK}f!!>#>$sYA7ahbTdUhh}3uC@?PJxkWsS)%ryV6PUjPiBE~V%-_?*@dV~NA$#J!mdsn`@NaF|vzedWn3rGo zrVY$D*(dujR|?+CJZ65Z2<1s5=EY^^L>lHoHlhg8Ig|rWGY8%zWZ$ej73fa%A>?yN z_Ry)sJi?k2Yq_?8*hoA`JVfjwWPkmFSU;U{CO%?*oF~#VN6HfwiIv0#;xti+Ia7+L zL%d7;%A84>&0HeZ5LKB!?-IAq1J)AdnLDkCE<_(f_U1JU*>{Np#6_ai{j3+_QQ}!5 z2Xjm2*tdkShll(3-cS!;!9KoqctU(Gx}NXWi_q&8*5aZHr0!+u7{o-g}- zQ=&*b^O{)DK=1E~jo4orvzHRriA=1G3#^N9pO^jpIPnwFzXhN134CU@WDjk{*tORC z{12>;ciQT`UH0?~#CJqZ)=O_9iP)J4RPU&_nDE2XE)e%`b%&yPskk9r8% z%g5i%S|eJsmuDGB-NX34LdgCtd$)Y&`-_mhd;cib?htKP7!f4 z_-;dVCb|)_SDz%hu-2B(X0FU(oe=VUas?sZDPJSb5ZUL`XJR+;B$1BwCj0neLe}0+ z;&b8*ae+w7`jfr=1tR=DdW7Smti$4jtj!0Ar-*Yz#zlH<%Kk6kRre9HMh_9M6JTx1 zI+gFQr->^@YsCMOZ z!}yLXYcfbJ|86Bet>T7ALarcH_HxPd)=fwbGFONgw4qj;s&)4inEBD8~Gk z|L?g@$oCJc?+0h-d8pA>`F|Uu&vDGBy!5vqA^i{kUnl(DAag_dZ#`T2?4P%dLGUj> z0)7Pi2>223Bj88CkANQmKLUOP{0R6F@FUk0*0Y3tM1pEm25%446N5GGO9|1oC zegymo_!00U;77oZfFA)r0)7Pi2>223Bj88CkANQmKLUOP{0R6F@FUmQH>j=E4 zIh)_lWnEIAlz&?CA71HQb(ePBsfL=$BL2}3uTLGV`_^$ayjVcVkSn!JvoK9+@9lA3 zn;+1*(-Ss>{)1s~x?xBG5u|Z`1NvWLT+h_Ix{jHTZNhrB@Wq zTz$DjMVXb}tzw|~PQ0m=3N#ALbmie+;Nn$0DC%h4Lp1V_&YZ3jN^*F2Pz0$t&_kV5 z85?JA{D?tnKP)fhUI|6SYVQAKqWQxnCxwH5>87=eV2_c~eSF$|%doRJXh%DnHAihL z1}2ZgcRb426pYMA=)n7E2ft64B4<_pU4-3d)n2@^F6opbx1BDQm2Vu0uQ{$pTKvai+c1wlMUpA^E>mu46Hx=xEczs`LtflR z>(W{*j>lc%pk?hht!-ebiT@t)y?9G&9GGQVz2agkeX)xClRWza@LC>>e8qVNp(N*iu`P(MDjS_AXHIx@wZ87`KNSexVDJ9=3;1ITn z0eBuOL{+VuO?Nva*w(BPqR;DKF~$9(1tmQ514mjjksWMK`2(^`x>*+X+We zKR9?`CXAzw^J=N0O|IJVG{~Y|NVSB?LC$hfE#@Dji>Xny{1sJ>Cx=o^RNbTgajs`< zR{cfLPSV^l*O9tU3B#W9_u|Q&At+H$|`5M}It@b(1n{kmJn(%P( z(-o$xl<24xR<@b843o|Vif6^!Xr`>Zg`;731v^BXRxu#IO6YVwV&IqiRTMwJEaAiu zajl_6nw5Bp$%^sFVbCUNzly>yY5oy+YJ+Wb#VwK~WlQ-_s;*uUxc(06KFfcX*zUyroUs7n8_3M9!IKycmXR#HLXZswJB4a ztghuqu~aKd;x_ooLHbKnirj254nc3Ej|wX&Uit!qRuT%E)W;cPPqx zk{On5y+4jQqOg`Ou+p=XV>TPYGUvPQgejcvZ<)9rw2&M?1w%AO8OC!so)MgP*Xm;h zdWz4*VXZ-6nu8}}O*$+pY*T&&DW>a1SLr*0avq8*?N;p%gJ+WbM}y3<+Fq2f?CT;g z;5X6JamT3g5pBFfOx79rCg3rHJ|l0W@wVXR23#TU7zYiJc*A=wX~nMaBy_*sgDvxv z_1{FL9v_v$Vt|_VQKQrhX4{y0N42E6LmU&tWQSFoaKpk=DmoLWah$8x#|%8-VOj%q zZ|uhckvm51VM(dE^TTZF%CGd6RQ`lFi-!liKZ~WJl2%Aj8@6s}c)vJqhy}lFrldV) zvxnb$HPQ0>be`;45t5X{!mU)rIFMh3Ghdp6t!4`6SvO@-78AiDlf1E>ywnBUgBP(% zRM(0I)`YkU;9iq7lOo$BZ;1Bs(ETnCymLvDV3VXTypJ|P2|sJC0}EsD!$lnSp-jl4 zpD;wbYO-#CXZh1jd~Uk*Zve{O{_Z3869n?DmIC7jyoo1 zPR^lb;JCIDUy9FzTWy%x&uCAeh?d_<>N0Po)x?ugeJLjPshZIR)eg=-h`^a1_u&mZ zAsT7Dl`k)2wYWno9$2SVT+9ML>*?Vtijg6^7Fq0bW_}?e)!jC4%8M za+vhyH2A2?_#5YfvOAeHhULcxCK~u(GjP{N3j@bM!%mpj>wTHHPb+ND;+5}4=rS2> zx5_CPVb4nIbzUeYnHlj8n!7|apCwXVkRVdvS6K(uAz91YdkWxPgiSCF=V!HTV`_+Xmb6{1Vxr*F~ zkhTA+m1q|B*6O-QmYjKTz*SPeR7EDASQ$ZIG;34#H&aSmi)|UToZ@~@(jUfTkyX2- zwF)dqg%{f#bj-(l7Hd*$HI5pS>h%!iEf|TT7$Y7Q@ml3T4^NgH#0F6&u--JlE z*fmsH^O+jycH}jfQ_L_n4=Wg^{Lp)h*L90dH})1bKXl4=22b30Z^n9`8`Lw`gP&zR z(0K7>l=#?WZD-Rgl7uHCZE}wv#E*E@2@G>NCnvTVA%EAH{BEyhP%<*RSl*Ew{E-Le zOYsa=a-0eE)qjJxf~38U@#KA9S(8mk3iFrnBvGfH9s_Mye(d*?4lG|etlhk&0x|h@ z3LR8D??|N)t%H0+lpI|Qf6yU@t7zyf>zJgRw4JanG7%!M*y87A!Jop2>FR3IC0X~_ zR*V8SI9ij&Nb(%KY z`c~6S@?E#BM3QFS`zd5q7iGy5jJ^SRX6ZLND}^ky?*SJvcXEA%!PXKMx5fLt8Y-s7O@un)d|Kd#DU>!yz3~hD|TbHD4+!bodPS=scb??H8u;&W$@Cx6M0t6 zrah|*!=E-v{$`BBnhU$@a#PnG^TsD0J-kEgcA;$TvVK{4F*Ow3fwm~OG8^MCn-}B> zIO=GnG)TsB4%|IV6q2BU*y||EfjP?3dYhc@F~GLwEaWwlU5&9WNOrF!K98HIoF(0i z)wPa#e#Y=9-W&+-Ys%`h7=>flCF*Op2YRYiCkrzu$+tM_mOM`=8l!|>;)vtAk;XpG z62Y3PsNiY?f431647?3-)Is|U0RxR)`^{KsforZ-Ud9}};j;T#s+4EOdmi{cG|=#8 zv(z=h#TG?i7*rD59U{u#~ z)py>DC?gkM4eFbv4Yk>lA~}z;DP_%V^k$GY9Y*7I>=q5QDuG@RB)duBuOXrbvMbKp zTnH0mu*^k->v9|xp=C&Zc}11cq_Hm+n&yhkF!)(a(q7fx)|v!nD}Pe9+hU?Pr_D9( zoQf(lh2K6Ji$}#`?a_$mZs)SEln@^T?HK7v(tb}=b}iwR_sKs%d@J5?lygOE6S$u- In$6w+0k*pO`~Uy| literal 32768 zcmeI5cTg8s5XZkO(xr(sk={f>R76EoP*g-kuwpORN$)+TnckZzn%;Y2OixTDz4x9N z)2r#-yq$T++&JWyV+Q8k3%fHr-@V^G?!NcA9sTR}C2uVT$GIvA$mDv4%Jke##qk*} zO)cvqw>LJ&ZrB#xuz5?}#&yZTdnx-{fYlhZLtyx;L zwdQEe)taX@Uu%KZzFG^F_EXtkAF~#@YJRiKl7AoIs!Lq;KvzA;RS$O6rLKC2t1ffZ zLsjj5e|*njDu=5qS2;qZd|^K&?BS>5NHx>En$ziC_R9Do0%PVaHw z@w*o7njD-y*jc$Jx$mlUI>%$wF()6p`){l|u2wltpJN^GswcSW8dp8hRoA-eNvd{w znD6CWpRdW+;%o5VPF6WZWu3~YD(h7STJK)hpw>=PIb9|Hf8g9tl@dZg2nYcoAOwVf z5D)@FKnMr{As_^VfDjM@LO=)z0U;m+gn$qb0zyCt2mv7=1cZPP5CTF#2nYcoAOwVf z5D)@FKnMr{As_^VfDjM@LO=)z0U;m+gn$qb0zyC$Xi(px&cYnb$097na;!o#cEg_7 z4~O6=9D`$V0!~(Q42JS2u3b8KK7buDm`aVlEmCtPLJ^KgL?H%oh(`jFkb+dCAp=>+ zK^_WFs3s^BqXdIciZV6Tpd1xyc0r~3*1KAL$6bS3^`&*4`dWG>8Zi$Gu>{Mo5^Jz4 z_Q1Y47)Rh}tiy3Q5vSsGoQbn>9*Srn4WSAeO=D;r)zBoWr}+U`@Hf~JUI_4)_jbf^ zYM=!H-g##~MK|n?3$Otf;SyYiI$C4UOWr8LT*EXtug&plL5(`cdR)vXWMVMP%3!G+j} zi*YF~$5d)I=q2z&%?*57&bAWZDMxn12pUP1G?vEGM4D`%^MY{&@~JQNGjQ+ry`cr| zTi%6N^DHkrViZlMMFu-B1XrSf3aP(A58mF?VsFc26?kHu9Z^LyXtBZ03&T~o2G`*R z+=N?j8*axPxC{5-K0JU2@h~1m3!cDJcm~hm1-yh;@EYF0TX+ZW;R9^NHhhB5@CClY zH~0?U<463AU-3Ks#NTSbSU2iUArwXt)PtfamU>bzN~C1!O?@bxGAWyKJwJPs9WT@e zE_p7a9Z^j)X^H0!==f)f==e)^(iK;9(sloTm*wfBcEkjlMN18KKhMUsBUamC(+4hj z#;zSvYq0Z~5Kn@(BX+UFrVm{5WNABMisA0(x#xDo?snMpflHpFZb!^E-2FV|-;UVJ z4x2u3$-4*G5sikspZBz|Blfq$rVm_J`M`rW3v;N+X#XdAv+09X2l&8)9Ts@k3p-*i zEi>BvyoZe)aiASGec+P!kg+4?(Q>2R&%1Nk5eL~}(+4hjmm@o3h0)#zcrQ9T;!rzm z`oJac?PW)-G~E5^c3@I253|OpX`{URogJ~taQA20fl0YM+#08*jq-kucEoDK-Jfj- zCgpOiHBL<%5U_d0+|jK0f%02;jAPXGV_ diff --git a/bashes/radar-driver-switch.sh b/bashes/radar-driver-switch.sh index edf5f15..c64b9ce 100644 --- a/bashes/radar-driver-switch.sh +++ b/bashes/radar-driver-switch.sh @@ -4,38 +4,30 @@ # 0 - 成功 # 1 - 错误：等待TTY设备就绪超时 # 2 - 错误：未找到CH343驱动 -# 3 - 错误：未找到含Qingheng的USB设备 -# 4 - 错误：未找到对应USB地址 -# 5 - 错误：未找到cdc_acm绑定的对应USB设备 -# 6 - 错误：解绑cdc_acm驱动失败 -# 7 - 错误：绑定usb_ch343驱动失败 +# 3 - 错误：雷达端口上未找到CH34x设备 +# 4 - 错误：未从cdc_acm找到雷达接口 +# 5 - 错误：解绑cdc_acm驱动失败 +# 6 - 错误：绑定usb_ch343驱动失败 -# 变量设置 -TTY_DEVICE="/dev/ttyACM*" # 替换为你要等待的tty设备路径 +# ── 配置 ──────────────────────────────────────────── +TTY_DEVICE="/dev/ttyACM*" CDC_ACM_PATH="/sys/bus/usb/drivers/cdc_acm" USB_CH343_PATH="/sys/bus/usb/drivers/usb_ch343" -CHECK_INTERVAL=1 # 检查设备就绪的时间间隔（秒） -MAX_WAIT_SECONDS=30 # 最大等待时间（秒），0表示无限等待 +RADAR_USB_PORT="1-1.4" # 雷达 CH34x 的 USB 物理端口（固定不变） +CHECK_INTERVAL=1 +MAX_WAIT_SECONDS=30 -# 初始化变量 +# ── 初始化 ────────────────────────────────────────── start_time=$(cut -d. -f1 /proc/uptime) -current_time=0 -qinheng_id="" -usb_address="" -vendor_id="" -product_id="" -cdc_acm_sub_addr="" - -# ----------------- 1. 等待TTY设备就绪 ----------------- -# 循环等待tty设备就绪（检查设备节点是否存在+是否可读写） +# ── 1. 等待 TTY 设备就绪 ──────────────────────────── while true; do for dev in /dev/ttyACM*; do [ -e "$dev" ] || continue if [ -c "$dev" ] && [ -r "$dev" ] && [ -w "$dev" ]; then echo "[雷达驱动自动切换程序] 设备 $dev 已就绪！" > /dev/kmsg TTY_DEVICE="$dev" - break 2 # 跳出两层循环 + break 2 fi done @@ -48,82 +40,62 @@ while true; do fi echo "[雷达驱动自动切换程序] 仍在等待${TTY_DEVICE}（已等待${elapsed_seconds}秒）..." > /dev/kmsg fi - sleep ${CHECK_INTERVAL} done -# ------ 2. 查看CH343是否存在 ------ -if ls /sys/bus/usb/drivers/usb_ch343 > /dev/null 2>&1; then - echo "[雷达驱动自动切换程序] 检测到CH343驱动存在，继续执行..." > /dev/kmsg +# ── 2. 确认 CH343 驱动已加载 ──────────────────────── +if [ -d "${USB_CH343_PATH}" ]; then + echo "[雷达驱动自动切换程序] CH343驱动已加载，继续..." > /dev/kmsg else echo "[雷达驱动自动切换程序] 错误：未找到CH343驱动！" > /dev/kmsg exit 2 fi +# ── 3. 通过物理端口定位雷达 CH34x ─────────────────── +radar_sysfs="/sys/bus/usb/devices/${RADAR_USB_PORT}" -# ------------- 3. 查找Qingheng对应的usb接口 ------------- -echo "[雷达驱动自动切换程序] 开始提取QinHeng设备ID..." > /dev/kmsg - -# 执行lsusb并过滤含QinHeng的行，提取ID（核心逻辑） -qinheng_id=$(lsusb | grep "QinHeng" | awk '{print $6}' | head -n 1) - -# 检查是否提取到ID -if [ -z "${qinheng_id}" ]; then - echo "[雷达驱动自动切换程序] 错误：未找到含QinHeng的USB设备！" > /dev/kmsg - exit 3 -else - echo "[雷达驱动自动切换程序] 成功提取QinHeng设备ID：${qinheng_id}" > /dev/kmsg - # 将ID拆分为vendor_id和product_id - vendor_id=$(echo "${qinheng_id}" | cut -d ':' -f 1) - product_id=$(echo "${qinheng_id}" | cut -d ':' -f 2) +if [ ! -d "${radar_sysfs}" ]; then + echo "[雷达驱动自动切换程序] 错误：端口 ${RADAR_USB_PORT} 上无设备！" > /dev/kmsg + exit 3 fi -# -------------------- 4. 提取USB地址 -------------------- -echo "[雷达驱动自动切换程序] 开始查找idVendor=${vendor_id},idProduct=${product_id}的USB地址..." > /dev/kmsg -# 核心命令：过滤dmesg日志，提取USB地址 -usb_address=$(dmesg | grep -E "idVendor=${vendor_id}, idProduct=${product_id}" | grep -oE "usb [0-9]+-[0-9.]+:" | awk '{print $2}' | sed 's/://' | tail -n 1) +vendor_id=$(cat "${radar_sysfs}/idVendor" 2>/dev/null) +product_id=$(cat "${radar_sysfs}/idProduct" 2>/dev/null) -# 检查是否提取到USB地址 -if [ -z "${usb_address}" ]; then - echo "[雷达驱动自动切换程序] 错误：未找到对应USB地址" > /dev/kmsg - exit 4 -else - echo "[雷达驱动自动切换程序] 成功找到USB地址：${usb_address}" > /dev/kmsg +if [ "${vendor_id}" != "1a86" ] || [ "${product_id}" != "55d4" ]; then + echo "[雷达驱动自动切换程序] 错误：端口 ${RADAR_USB_PORT} 上 VID:PID=${vendor_id}:${product_id}，非雷达CH34x！" > /dev/kmsg + exit 3 fi -# ---- 5. 找到cdc_acm驱动对应的usb设备 ---- -echo "[雷达驱动自动切换程序] 开始从${CDC_ACM_PATH}提取${usb_address}对应的子地址..." > /dev/kmsg +echo "[雷达驱动自动切换程序] 雷达确认位于端口 ${RADAR_USB_PORT}，VID:PID=${vendor_id}:${product_id}" > /dev/kmsg + +# ── 4. 查找 cdc_acm 下雷达的接口 ──────────────────── +cdc_acm_sub_addr=$(ls "${CDC_ACM_PATH}" 2>/dev/null | grep "^${RADAR_USB_PORT}:" | head -n 1) -cdc_acm_sub_addr=$(ls "${CDC_ACM_PATH}" 2>/dev/null | grep "^${usb_address}:" | head -n 1) - if [ -z "${cdc_acm_sub_addr}" ]; then - echo "[雷达驱动自动切换程序] 错误：${CDC_ACM_PATH}中未找到${usb_address}绑定的对应USB设备" > /dev/kmsg - exit 5 -else - echo "[雷达驱动自动切换程序] 成功提取cdc_acm子地址：${cdc_acm_sub_addr}" > /dev/kmsg + echo "[雷达驱动自动切换程序] 错误：cdc_acm驱动中未找到端口 ${RADAR_USB_PORT} 的接口" > /dev/kmsg + exit 4 fi -# 6. 切换驱动 -echo "[雷达驱动自动切换程序] 开始切换cdc_acm驱动到USB地址${usb_address}..." > /dev/kmsg +echo "[雷达驱动自动切换程序] 雷达 cdc_acm 接口：${cdc_acm_sub_addr}" > /dev/kmsg -echo "${cdc_acm_sub_addr}" | sudo tee "${CDC_ACM_PATH}/unbind" 2>/dev/null +# ── 5. 解绑 cdc_acm ───────────────────────────────── +echo "${cdc_acm_sub_addr}" | sudo tee "${CDC_ACM_PATH}/unbind" > /dev/null 2>&1 if [ $? -eq 0 ]; then - echo "[雷达驱动自动切换程序] 成功解绑cdc_acm驱动：${cdc_acm_sub_addr}" > /dev/kmsg + echo "[雷达驱动自动切换程序] 已解绑 cdc_acm：${cdc_acm_sub_addr}" > /dev/kmsg else - echo "[雷达驱动自动切换程序] 错误：解绑cdc_acm驱动失败" > /dev/kmsg + echo "[雷达驱动自动切换程序] 错误：解绑 cdc_acm 失败" > /dev/kmsg + exit 5 +fi + +# ── 6. 绑定 ch343 ─────────────────────────────────── +echo "${cdc_acm_sub_addr}" | sudo tee "${USB_CH343_PATH}/bind" > /dev/null 2>&1 +if [ $? -eq 0 ]; then + echo "[雷达驱动自动切换程序] 已绑定 usb_ch343：${cdc_acm_sub_addr}" > /dev/kmsg +else + echo "[雷达驱动自动切换程序] 错误：绑定 usb_ch343 失败" > /dev/kmsg exit 6 fi -echo "${cdc_acm_sub_addr}" | sudo tee "${USB_CH343_PATH}/bind" 2>/dev/null -if [ $? -eq 0 ]; then - echo "[雷达驱动自动切换程序] 成功绑定usb_ch343驱动：${cdc_acm_sub_addr}" > /dev/kmsg -else - echo "[雷达驱动自动切换程序] 错误：绑定usb_ch343驱动失败（检查驱动是否加载）" > /dev/kmsg - exit 7 -fi - - -# 执行完成 -echo "[雷达驱动自动切换程序] 雷达驱动自动切换完毕！" > /dev/kmsg - -exit 0 \ No newline at end of file +echo "[雷达驱动自动切换程序] 雷达驱动切换完毕！" > /dev/kmsg +exit 0 diff --git a/bashes/radar-driver-switch.sh.bak b/bashes/radar-driver-switch.sh.bak new file mode 100644 index 0000000..edf5f15 --- /dev/null +++ b/bashes/radar-driver-switch.sh.bak @@ -0,0 +1,129 @@ +#!/bin/bash + +# 退出码说明 +# 0 - 成功 +# 1 - 错误：等待TTY设备就绪超时 +# 2 - 错误：未找到CH343驱动 +# 3 - 错误：未找到含Qingheng的USB设备 +# 4 - 错误：未找到对应USB地址 +# 5 - 错误：未找到cdc_acm绑定的对应USB设备 +# 6 - 错误：解绑cdc_acm驱动失败 +# 7 - 错误：绑定usb_ch343驱动失败 + +# 变量设置 +TTY_DEVICE="/dev/ttyACM*" # 替换为你要等待的tty设备路径 +CDC_ACM_PATH="/sys/bus/usb/drivers/cdc_acm" +USB_CH343_PATH="/sys/bus/usb/drivers/usb_ch343" +CHECK_INTERVAL=1 # 检查设备就绪的时间间隔（秒） +MAX_WAIT_SECONDS=30 # 最大等待时间（秒），0表示无限等待 + +# 初始化变量 +start_time=$(cut -d. -f1 /proc/uptime) +current_time=0 +qinheng_id="" +usb_address="" +vendor_id="" +product_id="" +cdc_acm_sub_addr="" + + +# ----------------- 1. 等待TTY设备就绪 ----------------- +# 循环等待tty设备就绪（检查设备节点是否存在+是否可读写） +while true; do + for dev in /dev/ttyACM*; do + [ -e "$dev" ] || continue + if [ -c "$dev" ] && [ -r "$dev" ] && [ -w "$dev" ]; then + echo "[雷达驱动自动切换程序] 设备 $dev 已就绪！" > /dev/kmsg + TTY_DEVICE="$dev" + break 2 # 跳出两层循环 + fi + done + + if [ ${MAX_WAIT_SECONDS} -gt 0 ]; then + current_time=$(cut -d. -f1 /proc/uptime) + elapsed_seconds=$((current_time - start_time)) + if [ ${elapsed_seconds} -ge ${MAX_WAIT_SECONDS} ]; then + echo "[雷达驱动自动切换程序] 错误：等待 ${TTY_DEVICE} 超时（${MAX_WAIT_SECONDS}秒）！" > /dev/kmsg + exit 1 + fi + echo "[雷达驱动自动切换程序] 仍在等待${TTY_DEVICE}（已等待${elapsed_seconds}秒）..." > /dev/kmsg + fi + + sleep ${CHECK_INTERVAL} +done + +# ------ 2. 查看CH343是否存在 ------ +if ls /sys/bus/usb/drivers/usb_ch343 > /dev/null 2>&1; then + echo "[雷达驱动自动切换程序] 检测到CH343驱动存在，继续执行..." > /dev/kmsg +else + echo "[雷达驱动自动切换程序] 错误：未找到CH343驱动！" > /dev/kmsg + exit 2 +fi + + +# ------------- 3. 查找Qingheng对应的usb接口 ------------- +echo "[雷达驱动自动切换程序] 开始提取QinHeng设备ID..." > /dev/kmsg + +# 执行lsusb并过滤含QinHeng的行，提取ID（核心逻辑） +qinheng_id=$(lsusb | grep "QinHeng" | awk '{print $6}' | head -n 1) + +# 检查是否提取到ID +if [ -z "${qinheng_id}" ]; then + echo "[雷达驱动自动切换程序] 错误：未找到含QinHeng的USB设备！" > /dev/kmsg + exit 3 +else + echo "[雷达驱动自动切换程序] 成功提取QinHeng设备ID：${qinheng_id}" > /dev/kmsg + # 将ID拆分为vendor_id和product_id + vendor_id=$(echo "${qinheng_id}" | cut -d ':' -f 1) + product_id=$(echo "${qinheng_id}" | cut -d ':' -f 2) +fi + +# -------------------- 4. 提取USB地址 -------------------- +echo "[雷达驱动自动切换程序] 开始查找idVendor=${vendor_id},idProduct=${product_id}的USB地址..." > /dev/kmsg +# 核心命令：过滤dmesg日志，提取USB地址 +usb_address=$(dmesg | grep -E "idVendor=${vendor_id}, idProduct=${product_id}" | grep -oE "usb [0-9]+-[0-9.]+:" | awk '{print $2}' | sed 's/://' | tail -n 1) + +# 检查是否提取到USB地址 +if [ -z "${usb_address}" ]; then + echo "[雷达驱动自动切换程序] 错误：未找到对应USB地址" > /dev/kmsg + exit 4 +else + echo "[雷达驱动自动切换程序] 成功找到USB地址：${usb_address}" > /dev/kmsg +fi + +# ---- 5. 找到cdc_acm驱动对应的usb设备 ---- +echo "[雷达驱动自动切换程序] 开始从${CDC_ACM_PATH}提取${usb_address}对应的子地址..." > /dev/kmsg + +cdc_acm_sub_addr=$(ls "${CDC_ACM_PATH}" 2>/dev/null | grep "^${usb_address}:" | head -n 1) + +if [ -z "${cdc_acm_sub_addr}" ]; then + echo "[雷达驱动自动切换程序] 错误：${CDC_ACM_PATH}中未找到${usb_address}绑定的对应USB设备" > /dev/kmsg + exit 5 +else + echo "[雷达驱动自动切换程序] 成功提取cdc_acm子地址：${cdc_acm_sub_addr}" > /dev/kmsg +fi + +# 6. 切换驱动 +echo "[雷达驱动自动切换程序] 开始切换cdc_acm驱动到USB地址${usb_address}..." > /dev/kmsg + +echo "${cdc_acm_sub_addr}" | sudo tee "${CDC_ACM_PATH}/unbind" 2>/dev/null +if [ $? -eq 0 ]; then + echo "[雷达驱动自动切换程序] 成功解绑cdc_acm驱动：${cdc_acm_sub_addr}" > /dev/kmsg +else + echo "[雷达驱动自动切换程序] 错误：解绑cdc_acm驱动失败" > /dev/kmsg + exit 6 +fi + +echo "${cdc_acm_sub_addr}" | sudo tee "${USB_CH343_PATH}/bind" 2>/dev/null +if [ $? -eq 0 ]; then + echo "[雷达驱动自动切换程序] 成功绑定usb_ch343驱动：${cdc_acm_sub_addr}" > /dev/kmsg +else + echo "[雷达驱动自动切换程序] 错误：绑定usb_ch343驱动失败（检查驱动是否加载）" > /dev/kmsg + exit 7 +fi + + +# 执行完成 +echo "[雷达驱动自动切换程序] 雷达驱动自动切换完毕！" > /dev/kmsg + +exit 0 \ No newline at end of file diff --git a/src/ground_slam b/src/ground_slam new file mode 160000 index 0000000..287e307 --- /dev/null +++ b/src/ground_slam @@ -0,0 +1 @@ +Subproject commit 287e3077d7e3a2d23c15cd3faf113425c2803621 diff --git a/src/origincar_base/launch/robot_mode_description.launch.py b/src/origincar_base/launch/robot_mode_description.launch.py index 4bce9d9..c5ee02a 100644 --- a/src/origincar_base/launch/robot_mode_description.launch.py +++ b/src/origincar_base/launch/robot_mode_description.launch.py @@ -8,13 +8,16 @@ from launch.actions import (DeclareLaunchArgument, GroupAction,LogInfo, IncludeLaunchDescription, SetEnvironmentVariable) from launch.substitutions import LaunchConfiguration +urdf_path = os.path.join(get_package_share_directory('origincar_description'),'urdf','origincar.urdf') + def generate_launch_description(): origincar_description = GroupAction([ launch_ros.actions.Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', - arguments=[os.path.join(get_package_share_directory('origincar_description'),'urdf','origincar.urdf')] + parameters=[{'robot_description': open(urdf_path).read()}], + arguments=[] ) ]) diff --git a/调试记录.log b/调试记录.log new file mode 100644 index 0000000..4518581 --- /dev/null +++ b/调试记录.log @@ -0,0 +1,19 @@ +2026/6/1 +16:12 +关闭了雷达的自启动服务。 + +\TODO lsusb无法识别雷达usb设备，原本属于lslidar的 QinHeng Electronics USB Single Serial被赋予给了下位机的IMU设备。下面是lsusb的输出： + +Bus 002 Device 002: ID 05e3:0626 Genesys Logic, Inc. USB3.1 Hub +Bus 002 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub +Bus 001 Device 004: ID 2ce3:0010 Alcorlink Corp. USB 2.0 Camera +Bus 001 Device 003: ID 1a86:55d4 QinHeng Electronics USB Single Serial +Bus 001 Device 002: ID 05e3:0610 Genesys Logic, Inc. Hub +Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub + +拔插之后lslidar后无改变 +串口设备仅有ttyACM0，无ttyACM1. + + +16:48 +雷达串口问题解决，原因是type-c转接线的芯片问题，换了一条转接线后雷达串口设备正常显示为ttyACM0和ttyACM1了。 \ No newline at end of file