From 287e537c4c8934890ac734ff3042d55515fccd34 Mon Sep 17 00:00:00 2001
From: "Paul Winpenny (plw1g21)" <plw1g21@soton.ac.uk>
Date: Fri, 15 Nov 2024 14:42:19 +0000
Subject: [PATCH] Api node now has a few publisher nodes being created, need to
actaully test on a machine
---
.vs/robobin/v17/.wsuo | Bin 0 -> 14336 bytes
.vs/robobin/v17/DocumentLayout.json | 23 +++++
App/RobobinApp/RobobinApp.csproj.user | 6 +-
.../connection_manager.cpython-312.pyc | Bin 0 -> 4947 bytes
.../message_handler.cpython-312.pyc | Bin 0 -> 2623 bytes
.../{ => api_helpers}/connection_manager.py | 0
.../robobin/api_helpers/message_handler.py | 80 ++++++++++++++++++
ros2/src/robobin/robobin/api_node.py | 44 ++++++++--
ros2/src/robobin/robobin/message_handler.py | 40 ---------
ros2/src/robobin/robobin/motor_controller.py | 0
10 files changed, 144 insertions(+), 49 deletions(-)
create mode 100644 .vs/robobin/v17/.wsuo
create mode 100644 .vs/robobin/v17/DocumentLayout.json
create mode 100644 ros2/src/robobin/robobin/api_helpers/__pycache__/connection_manager.cpython-312.pyc
create mode 100644 ros2/src/robobin/robobin/api_helpers/__pycache__/message_handler.cpython-312.pyc
rename ros2/src/robobin/robobin/{ => api_helpers}/connection_manager.py (100%)
create mode 100644 ros2/src/robobin/robobin/api_helpers/message_handler.py
delete mode 100644 ros2/src/robobin/robobin/message_handler.py
create mode 100644 ros2/src/robobin/robobin/motor_controller.py
diff --git a/.vs/robobin/v17/.wsuo b/.vs/robobin/v17/.wsuo
new file mode 100644
index 0000000000000000000000000000000000000000..ba3598a9956c8e8e60cf96d02f38688a8af569fc
GIT binary patch
literal 14336
zcmeI3+jCPz9LKjsRPc%@Dxy}bh)PPDrdI_ErCeqxx3<*L@{ntZP1{V8Qt&dvyMKj$
zf^R-4I*t#HGx+X{{s}%f<BQbKcTY}xdYYssX%P-&JHMRWJ-hqc-*tbxd-~h$U4MN2
z&E7waFC8`8%$?;O=21_5n<sCcHRgHBs=KqiysTHslWnCCap00!;IC{t%&3{?Su{5!
z%D`!l*}fw4?HlyE^}}Chuk`=k_5|&2KIb&P0LBILsW}7oEV)s0eS=ZfzTRWnSMZDN
zYu{EcouL}_waLXVyakhI21Szr#^FT*$W+?ZdUTrhHm|sAwe!{=p)gyiIByogk~cZ%
z7tDAtpP;7b?N}u*{EtxI4g{rtl(w#2zd-(9F9GCVu8gJXJ>(w;_5x1=PXSK@&j8}#
zE<mE(38=k%f%HJn9hJHlD`mZJrTdEm_saf<@1gylMju=D4--hPHp(B!56Es`A}<@3
z{i-bc-wkYJ|7z<`kI~Ox*DBvX9FWYaf#jxo>41E@*6e!nmwqT7)Ow;dLq1=6q_sqA
z$1A`-;8ox?;B}w_cmvoEya{vyZvh8@w}FGeJHWfZd%z(e0(1d~fhf=o#DE?k4)g+j
zfS0BrU-_u7eB4*|_Er8tPUA#8c^iY|$jzG(`<o^5n4z|4V<6`ZOsbL&5r4>)yF9AW
zWL?!THIkjud6Oji8v|Rhf?F|{BGHn$YzCZ0b$<7QU%KhWA9b3py!w!w;w7aKs84`P
z*m7WAS~m-)U9Df@uNB(GgIevp_0lP~d;*+FI6cFk;?60{@sznrspK}Q^XzB>{|AgG
zTU9!aycdW@=fGR!88q&7ddsHU+qCa<V?DRN-tz=6W!p*<mix2jEZi?4^>HXy3|=-V
ztCbt^K=JQ3#&OpfJLm>KBfQs|rlh!E8dpT;66nH7=8?C&cXtLEW>T)H6+=z%f35r%
zETwbcl-5syf8M0w!IHVaZ;5glkZo4uMHKoKlS`gK_3s#MzW|i{`X~B@Uw%RMpJHD1
z(gE;mKcIa;Q2gTkcQRU1ZTvUT$t1JLI}L06f`3cxZwz|pz^Z+)R~FTHuPNdG8pf|!
zYobDrpMD3YEWz1{9%ENpz&}d<(gf9dQy=^n-?i=r<-cM(?HQH4G>%~bvI55wb)oZ%
z)+QI=$SLqjBMRmmEu^guM~;GDYrNL{p!nr46iX-t#XkXd>4I#b;IRbkuTA_^j1d&S
z@VfZN(eM<OChD#+b{fixL>T^cu?r8i0Oe`x2^Qf~S;H9UZ&>~(>F4^NM(}H$D&w)W
zD$g_PtBf<;1pZU>bLBq{z4H7TO<E=lm#CB7i^hw{TpZEbSX%MY3{Xh};MZAzV!fdJ
z_xitj^1lGqdHynR{iLOORqtH<=zqif7wxY6>*2p_DKFbKD!D?tdZgjZRjY;YzfG~N
zXjgLQRqv_w%3Xi!;hzGBq;MXM*He7(r&ts8EB0~szw+B-$k?&0ap;mZl&z(^E1)q)
zI1SdU-}j@p>xgLS97rbz(%IfvB%R78BJti#Dl!<2^+r<h>|ngNKNZa;dh8Lg4Hq;{
zZdm2xk@1P4D1V=)QkmYtc(N}NP4x^$;_-Mal1#(~BHhtcUp$fO&kPJ^zm(iG6E}Ty
zTajP$sdHTfR5BgJf>Hju@dNs;m*mT~B28V~zw@i_=RRw6thq#P{_n|u9&NV2__-Mx
zHrro%xEcJ?J@sE5qcBV{aN6wr10$-_(A|r8n{1}eJ5JENeE7G(h#4}|mGw}2GOhI;
zo~U>lZ;@kl(0W+{viJ(wi=y{MTIVUri!9=+6tzyXJC6b}a-;Y#C)O^|Up+F^UIynh
zKGs>^OXSBG!Kv+LPu~T|5_{}ZqfOIS>!0?mTCb;>_fMgn`y6}_Ek#Smt<cg@B4U>G
z!lXyi<1%wD@l1t`|NgmO=iDnEavBtS%vl<K4vii7<&eUY$vZikWz3JEr6h;X9YCvO
z)4zn43g>Qh7O51lb;m>93N1C-pXEa?k!x6otfUAfInH>bU%!S{BBG{9B%;_RXHP~%
zPk8*7zH-Pj?<{nzu7qwm{FFHKg;^%+qg@oNlG;2wAGfZNr+W_6NFQ%o?Yqta&c0z1
z|Ekj>d0Bl;vqa=RL+sy8*?XcqZO@nr_yD!kUP33mNgLNoQo)nX{ys5Rf_Lz>)ciM-
zbo$_&rFeIelhkVD(>g}*{zX<w^>f}n%HOqgc<E-*dw$S5|M`E<fBgKvYl5yZu+Czu
zLF@j1Enn>6?gRMKy8nH+?|;Wwi6`xu-P-R!yk~wjT!*c0=Kl3GJ+)eHRq~qUR*l)n
zIAO-`zkhDx{@;ENd=Kyc?fv&Y1SUBIk(>Aa(NF)wcK`3EU$@J;5uPP}U$F6b*>m^5
z{woOXA1(j&y<brN>lV?ywF`oOJ=Rqz9|XVRZ{4Q{#Xm^fk1Ip?@#}ja-F@q<sP<Pk
zmCihx@qhZN&AtDxhTr)DF3l{`_WPuI^U?Po@};|hp!E0G!QYa9!}4$V|3B_0`Ac_n
e_b1P(`{zdYfByYT>;4~RHjw=MYoP0vH2n`9d!CyB
literal 0
HcmV?d00001
diff --git a/.vs/robobin/v17/DocumentLayout.json b/.vs/robobin/v17/DocumentLayout.json
new file mode 100644
index 00000000..218fa7fb
--- /dev/null
+++ b/.vs/robobin/v17/DocumentLayout.json
@@ -0,0 +1,23 @@
+{
+ "Version": 1,
+ "WorkspaceRootPath": "C:\\Users\\plw1g21\\source\\repos\\robobin\\",
+ "Documents": [],
+ "DocumentGroupContainers": [
+ {
+ "Orientation": 0,
+ "VerticalTabListWidth": 256,
+ "DocumentGroups": [
+ {
+ "DockedWidth": 200,
+ "SelectedChildIndex": -1,
+ "Children": [
+ {
+ "$type": "Bookmark",
+ "Name": "ST:0:0:{cce594b6-0c39-4442-ba28-10c64ac7e89f}"
+ }
+ ]
+ }
+ ]
+ }
+ ]
+}
\ No newline at end of file
diff --git a/App/RobobinApp/RobobinApp.csproj.user b/App/RobobinApp/RobobinApp.csproj.user
index 0e19fbac..c541a85b 100644
--- a/App/RobobinApp/RobobinApp.csproj.user
+++ b/App/RobobinApp/RobobinApp.csproj.user
@@ -2,9 +2,9 @@
<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<IsFirstTimeProjectOpen>False</IsFirstTimeProjectOpen>
- <ActiveDebugFramework>net8.0-windows10.0.19041.0</ActiveDebugFramework>
- <ActiveDebugProfile>Windows Machine</ActiveDebugProfile>
- <SelectedPlatformGroup>PhysicalDevice</SelectedPlatformGroup>
+ <ActiveDebugFramework>net8.0-android</ActiveDebugFramework>
+ <ActiveDebugProfile>Android Emulator</ActiveDebugProfile>
+ <SelectedPlatformGroup>Emulator</SelectedPlatformGroup>
<DefaultDevice>pixel_5_-_api_34</DefaultDevice>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(TargetFramework)|$(Platform)'=='Debug|net8.0-android|AnyCPU'">
diff --git a/ros2/src/robobin/robobin/api_helpers/__pycache__/connection_manager.cpython-312.pyc b/ros2/src/robobin/robobin/api_helpers/__pycache__/connection_manager.cpython-312.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..9e8339995626eb47254d11b77a06a152efb18d72
GIT binary patch
literal 4947
zcmds5T}&I<6`rxj_5d*$a0oFNri28XfcaUnNlGB$FWKw{n*|bW5X#i?Oo$z0d+!WM
zv5b_5s7j>l2GmL<YO1hOrA-v1Wu;2nhke*qE45NzJjLnISsqr}zT}}&6jo}Kr=BzR
z*v8OSU8yg<lFz;8-h0m6d(XM&JJ)}*+e;}Zhlh{N{2f}~A!8IK+t})a#xf;P0v(~I
zGBrI#>%3{oWTMVeg6TFTn4g(45>w`R+QYm+#uv<VWoUMs+G+!SnGz|~GG!8M0)3mF
zGNYC$Mlb`l2n<lGU;%2Iw|ZEuawry!ia|LPi;f1Off<nmEd{z!wHba}QC)SCPEves
zIB5coHrS*YIAbYPk^v4S^&*!Tt+0MD2A8RyWm%B08YA<RhxU1F8XJg*_-ITJHS2}p
zG5*|`#$r9@AD`69&Yzo@9PwqC68Rbt141Y|qgh67h*4Q9mE>5Q7qRWJXpAIAt`gL*
zS?7c}ABt-%*6|pTwX#S^lEtXbNVtCv%bTABe>{Cb5{Wb&kKF8<>F%1AVsj)YP7^U6
zlcq`RO6*D~3boWdEs<bBM`i%meoc(TQA03y&iriVpuO>Vjpg}JG$iwU!nMN?USrG#
z3zFJ_+@dyWxjW1~`$kPYRAmNNcjyk2W@|TH2Of7k>{xdlxx;)^u>}HDFlQcM&>Xrk
zDd=6M<lI;|XEv0{lq3~4Jk{uedX;{bmD*<B|D=g6P?2qJ)0A^L_-f7t@-kt2zHPJ&
zr=cm3*~O{;z0ADHV@~u=$N?ftT*mm|Io#D4;qr!(xIk3ku8>$j2nHk>C&Y;;iP50w
z_0XCXq-R7Kyp-VgHS47Afm%)0nLrs|piHx8`!kNJRpk`(hE~my@5|1zNVvwQG{GZ~
z@SHhP36#_UN$lI{H2VB<ocsZhTh!m&hg5gF;%;A?RF94+N5|6c_t9Hio191GIu)+-
zw~Tt+uN?QMxiK7YZ?Jn*)}^p6mE{zcOS4USk6U5gDtka-4?JfNl4?-C&AHHox^uOG
z+iY_#7$`1su46aXD;V9y1&wgHKqu)iUX7wsAiay*6^!oUGy?rB8?(Tz=-pgMfS4D?
z<1H#zO~%F09nw)W3~U&Wtf&}HyhuwYOYn<(izWaI0Fb?2({N1}Obh0uDZFpzJVTej
zTyF*ccrEWY<_MOg`IpqM3RYNf^^Hjdb-@|!5*P|bT?InfX3jcHlKG!nhxx(QVZ!d6
z>jukbPZ{jTgd2CxFwR%Ef@Q&4aK6^0wdj0*WXOk`j8?t|4{FYHMk{>CXyxmq<tn{u
zGtM;Z$&VN<V^u%I>m<`6eeWcAU5BA9U+47}?32x((R|4Ps=yNt=Ov9+GesDhx<tV@
z?#h^%=mmPs*n98fP3ouSOVmx;Q<^w+UWZ>vkG}x#T+5K(=NlQCY~f}_NrJ2-6N)7f
zqV-&yn;aS|4Ab5hXgZAwQ4lUPTUItvn-7j05kH(0C7HWQVzXSLW+)PZtiaGA3YmV7
ziQxUy7>MgqqG<xshd7M=OL*}5JhhrtiUqHWvSu4N!=LkwOlqYQ{-O8y;j`ldqmY8Y
zieWq!hje7Zf1bxNn6qcX&ySB>m>3xt9v;^iNFoG+DH7R(-?xS&9t1U(h`}4UV$CXu
zLCk?HaT0>ercD3D#N_x0e8ZYqpJojNgJN9Pv53^5GCkR;y#^o_5&59L<pMy`5k#}e
zp;<9D2kZAwj1Uqdnk5*CNg{w6B2Fep!Yycw5Re0KHiAGjRu0B_z1N=An>~fHEQ_-w
z49<EY#bq%IqgJ9dZ<cJv>CFOT5n!S%q@RIl0mQ2JFZV6={krDUhEE!PQ=@hbDqVx?
z)kAmK4c4KubqZVe=!{z5uGF`$-cUORm5#x$2GZ;q7`<=5YhNS}depijO5KrEO<S66
z|F)`TxnrqAt!h=OTAx=PerYPV*KD{Cs&231_CAr<-91aCi_D_1QN3@ucd7U1eJkhw
zT;0Cu+@GrNS({utlXCT@ohLUN4?bRaxUhQhv&&B}e|b^u^DBM+^~SNqcQ%~$s<TCL
zwyav!))PwWi7($)dwoi;FE#dl+Bpux%cD!9E8Q!y+Hy>3IhJZVo_3zVZvT>hWgzW5
zw9$3qi^k6z*I}nl@8al6um17wwcfS%l<RcbIk4$;tIlS{+5BYqv(cxcYTLNdHlCWA
zNnHx1onh4(SDf**lf2wV!5(s({R?cii7KhQUw*e7Ge*{)D(`;IzV-ThJLPmO_b>Io
zqG<bm%~88q?ZRfwUmbg2nxTFpA)frcZn*JmH}!|^>UXNme`HytRW#7ubAi)<AO9D*
z02RiYSyaLmMIFEzaZ{Qvf#=(TNdUA)YzSl0R#c3+favsY*#W+&!p<TgV4)Z9cTs^1
zB#YKGRS>h8e9V?HXVgo$-e~1(!IGZ|U-)nLOW&J^UF2{tuh-b+wpCg4-<r1pk~A3e
zbNJAb!v{8Tc+kkfrCbKi!K*416GFj2Br?y%Zis{f<nR*Il(4jRwsm?Ej&c8>fAAc<
z5XVO*MtqYH`E#1KW%DmEdrdSL5kHrOZ=2-vR1!pUasUY;I5~&}aa)h*gX8{z;h}+v
zNj;1cL~G3ouLb?}3eN^XjuDKB1oKjYkf<>jel?2}5yd#^#_qR(c$iG^W%LlsMq<GL
zKF~Frfrfx{dSuN!1@K}fns<iP+%|F#3<P>91q4EB#oqf1cNZRAe0=%g<<*O7d%x1&
zzg}_b4g=Z#{ffI4kF08~N2&F!zO5eVSB~_5MW@-pZILsLk<$i|v#x28-KcMS^1+jL
zQ?6sGt6y>Tue(k!+BO`{<+7!+2WMA=Pp^M+eXZw<zR&wo2M5xQ!A-~BRE;<7=-7lg
zrAws`x_)7QY2OyCFfu!-68rtqyQTN`=&{QCob7o1eF^2PD+*4ST>q|bz&%_^eO*~H
z>^6U0PXpZ+mhgJ$Pn6@(SjKAvb#V!29{`2@c^8I%U`z=&`iaEJ*F+<O0ON4RtKJ7`
zgbw3oz;J>Wt>D=Pe?x#QBp|te0}Kqr{sEj^1>_d>ZN>;0Y<teS^#_*^9<3GKM~`|7
zexZ+G>!FW<z`r^0cS1BUEAqTn%JZ`^VGe+w*Y@!IhjW2QW`v-A9s3EwF+t}cXMjAV
z^lxNb3LT6f!5l)uxN)1>GFdEbugWc!L$AtfEj_P#9G1?lW|O5MBZsrWarGO5Pb+c?
yiLU5fY@vm8<!{)NnWgoU_n;4RH>nl~zzj`)N7=uj9N$o7|6uy)hF262UGkqSz3m$S
literal 0
HcmV?d00001
diff --git a/ros2/src/robobin/robobin/api_helpers/__pycache__/message_handler.cpython-312.pyc b/ros2/src/robobin/robobin/api_helpers/__pycache__/message_handler.cpython-312.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..185a8f39506e9b882bca056c9730a246a471baad
GIT binary patch
literal 2623
zcmb7G&2JM&6rcUFHwFjD2Te?qE=@wZRbr7eP${BFX=xKGVh|rfic+o2&XCwRyJluh
z6w6jQaLA#R+EYMKLDfTxxbP?RUa7)LBfFy>kb2?<$)OUL_RV_NIL09LNq#%?-kY~?
ze(z)bTRa{^Kp&p^cxftv(BEPck{T>_`e3nvWHgH~x`nXxNUm!Z(3)&2c4#l2Qn1-M
z3X2Uy(1L`~f{dkGXhFd;a1|@SHLL=UU=4T_M}X^VQB&ug^MtYV5}8XoI7g@*yvg?}
z;!)tYgMHs6se~+FAy^4zn{5L*NW)t+EPY$JYb#;to&p{l_G`$LQ>MbzbEi|YT)lXD
z{&OCipGjSsIrG`Y^Jh$r>*;*Pa$HRKo@GB?E1z+ecz3YzGAqQQ<a&WHkM{<`m9$ex
z=PcWGJnH7SUKdkl%3Nh6ca`c87ZX=8jF(^8(~FlFp=>dqTb)>%oLFRTf!bt|lDx|n
zse8q}l5yb4CKnmCn*?=*h;NzX^1{PP9at-PVKTqQb<4^)8PBqc@9c&>87c%AY#i84
zRO>U=)o<cYd-|$9!<C-lnpligdd6yr!D?cvl9+lCRr_P>>bLP7IDqU1kgySJKXq4N
zu_2NL&sNF`ixw)$?Nb<*U|E$>Nrh5SLWzXdBvURX&$|VWFvD9WhV8BZ?AY*Jf1A$d
zshg*nv`35(4M;TI3-9L267l%KAXx?Hn&YlIR)C$U(YUaQ+qn#JJd3&ZHL#8b{<(_N
zUYbY4lxkpW2%??hRA=Bca8?{>!F;z`1j9Z6b`w4AI=C_Z{dl!&sM0m`VD^`b)$zH?
z_}pgK>GkNJ`o8btx4kWWnC=H-yBR{U_CY@a3jkfphTRGKkf56bDF6rETpT!0Sl)Fo
zGgdR+vXM5<olVUeLC{GiGASgfhZ2u6;^1^H_fYY1p*^7ElEuVJ!)wzH*c21(?<(5m
z0O*;B9WJW(R`r33K5%<!OF#1Zg&I|K70lmsA+Iq8vLXR3^eEV8qb&;9hPoy|L?sPg
zfLjKIRG<_RZ$L}lAk+{Io=ll4Jp`w@Dz;p+{YQj2HVg*(E@*b+A?9uI{q}e?PmEr1
z@;zWpxZK0{vv;z0bJf9#%HYK2;742f)ax*bYx{Zs9Tpofud$33tYYn+T2@L*yDBKj
z?V2wH<ZVFWNZ5E~6-P_zUG#(eHgL-t)S;+}7Ak44VE6WfO01XE&_C2?hZ!r5&H1C2
z8TCmD&FxRpu*uo9ae2&`88a@78K;t_@=R3sGvSX81A{gfk2)Lj1<(EkdL`XuF(NEf
zE?wr*43{o&>C`T@a8*1!?~nA>KnzN3lDR?fv{5{Kj=DC?UxMEd^>oUN&@S-CHRjPw
zo@<3XOl^7?MD^;@U|=~OScwT#@3d&plstwr@G>vsI*iAu-@c4k{xH(REfZfnXi6Y|
z98rOBakzaQ?*2&igC!F!cN0C2psqu;1Bu!J;D=sxXa{5KvFCeH|Iq!;JDm?!e=h%6
z{^{%L=!weciOtcIzjkc)eYqa5^&Wp*e(XN!{-WAFU+JFT>^{4$*BZ^f{D(gJmmYu8
zIq~?DEq&(of)>T>7xe!cWc^{G1XH|GAxX9m9A2u=_E4Il2@tmF*KQCON801m@_i8D
zF)`1YTK37{(Jg(#zf;QW5T9Hp4Sx$P9<!_!7Z-A3z1Om?!@q*MhKlH@_|N6fPAUe5
zpDijvd5C;p^&ARNXowCI#c~U6%MmTMy<gErwqvq3x~<FFv2A^i_Nla!khRJBX>lfH
qO8!}TOn4C!*mwF;KT(hCBs~r?(LO$2&lO3M{)vo8liLWGf8amDWmx|J
literal 0
HcmV?d00001
diff --git a/ros2/src/robobin/robobin/connection_manager.py b/ros2/src/robobin/robobin/api_helpers/connection_manager.py
similarity index 100%
rename from ros2/src/robobin/robobin/connection_manager.py
rename to ros2/src/robobin/robobin/api_helpers/connection_manager.py
diff --git a/ros2/src/robobin/robobin/api_helpers/message_handler.py b/ros2/src/robobin/robobin/api_helpers/message_handler.py
new file mode 100644
index 00000000..01adeb4d
--- /dev/null
+++ b/ros2/src/robobin/robobin/api_helpers/message_handler.py
@@ -0,0 +1,80 @@
+# robobin/message_handler.py
+import time
+class MessageHandler:
+ def __init__(self, api_node, testing=False):
+ self.api_node = api_node
+ self.testing = testing
+ self.handlers = {
+ "PING": self.handle_ping,
+ "TIME": self.handle_time_request,
+ "MANUALCTRL": self.handle_manual_control
+ }
+
+ def handle_message(self, client_socket, raw_message):
+ """Parses the incoming message and routes the command to the appropriate handler."""
+ command, *args = raw_message.split(" ", 1)
+ data = args[0] if args else None
+ handler = self.handlers.get(command, self.handle_unknown_message)
+ return handler(client_socket, data)
+
+ def handle_ping(self, client_socket, _):
+ """Responds with a PONG message."""
+ response = b"PONG"
+ if self.testing:
+ print(response.decode())
+ else:
+ client_socket.sendall(response)
+
+ return None
+
+ def handle_time_request(self, client_socket, _):
+ """Sends the current server time."""
+ response = time.ctime().encode()
+ if self.testing:
+ print(response.decode())
+ else:
+ client_socket.sendall(response)
+
+ return None
+
+ def handle_manual_control(self, client_socket, message):
+ """Handles manual control commands: W, A, S, D."""
+ # W: linear.x = 0.5, angular.z = 0
+ # A: linear.x = 0, angular.z = 0.5
+ # S: linear.x = -0.5, angular.z = 0
+ # D: linear.x = 0, angular.z = -0.5
+
+ directions = {
+ "W": (0.5, 0), # Move forward
+ "A": (0, 0.5), # Turn left
+ "S": (-0.5, 0), # Move backward
+ "D": (0, -0.5) # Turn right
+ }
+ response_data = directions.get(message.strip().upper(), (0, 0))
+ response = f"Manual control command received: {response_data}".encode()
+ if self.testing:
+ print(response.decode())
+ else:
+ client_socket.sendall(response)
+ print("Processed manual control command:", response_data)
+ return "cmd_vel", response_data
+
+ def handle_unknown_message(self, client_socket, _):
+ """Handles unknown commands."""
+ response = b"Unknown command"
+ if self.testing:
+ print(response.decode())
+ else:
+ client_socket.sendall(response)
+ return None
+
+# Test class without api_node and with testing enabled
+if __name__ == "__main__":
+ message_handler = MessageHandler(None, testing=True)
+ assert message_handler.handle_message(None, "PING") is None
+ assert message_handler.handle_message(None, "TIME") is None
+ assert message_handler.handle_message(None, "MANUALCTRL W") == ("cmd_vel", (0.5, 0))
+ assert message_handler.handle_message(None, "MANUALCTRL A") == ("cmd_vel", (0, 0.5))
+ assert message_handler.handle_message(None, "MANUALCTRL S") == ("cmd_vel", (-0.5, 0))
+ assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.5))
+ assert message_handler.handle_message(None, "UNKNOWN") is None
\ No newline at end of file
diff --git a/ros2/src/robobin/robobin/api_node.py b/ros2/src/robobin/robobin/api_node.py
index df71bb2f..c7fa7e1f 100644
--- a/ros2/src/robobin/robobin/api_node.py
+++ b/ros2/src/robobin/robobin/api_node.py
@@ -1,19 +1,28 @@
+
+from api_helpers.message_handler import MessageHandler
+from api_helpers.connection_manager import ConnectionManager
+
+from geometry_msgs.msg import Twist
# robobin/api_node.py
import rclpy
from rclpy.node import Node
-from .message_handler import MessageHandler
-from .connection_manager import ConnectionManager
class ApiNode(Node):
def __init__(self):
super().__init__('api_node')
self.get_logger().info("ApiNode has been started.")
- # Initialize handlers
+ self.publisher_topics = {
+ "cmd_vel": self.create_publisher(Twist, '/cmd_vel', 10),
+ "nav_send": self.create_publisher(Twist, '/nav_send', 10),
+ "map": self.create_publisher(Twist, '/map', 10)
+ }
+ subscriber_topics = {
+ "location": "/location"
+ }
self.message_handler = MessageHandler(self)
self.connection_manager = ConnectionManager(self)
- # Start connection manager
self.connection_manager.start()
def handle_client_connection(self, client_socket):
@@ -23,11 +32,34 @@ class ApiNode(Node):
data = client_socket.recv(1024).decode()
if not data:
break
- command, *args = data.split(" ", 1)
- self.message_handler.handle_message(client_socket, command, args[0] if args else None)
+ topic,message = self.message_handler.handle_message(client_socket, data)
+ if topic is not None:
+ self.publish_to_topic(topic, message)
finally:
client_socket.close()
self.get_logger().info("Client disconnected.")
+ def publish_to_topic(self, topic, message):
+ """Publishes the message to the specified topic."""
+ if topic in self.publisher_topics:
+ publisher = self.publisher_topics[topic]
+
+ # Check if the topic is 'cmd_vel' and format the message as a Twist message
+ if topic == "cmd_vel" and isinstance(message, tuple):
+ linear_x, angular_z = message
+ twist_msg = Twist()
+ twist_msg.linear.x = linear_x
+ twist_msg.linear.y = 0.0
+ twist_msg.linear.z = 0.0
+ twist_msg.angular.x = 0.0
+ twist_msg.angular.y = 0.0
+ twist_msg.angular.z = angular_z
+
+ publisher.publish(twist_msg)
+ self.get_logger().info(f"Published to {topic}: linear_x={linear_x}, angular_z={angular_z}")
+ else:
+ self.get_logger().warning(f"Unhandled message type for topic: {topic}")
+ else:
+ self.get_logger().warning(f"Unknown topic: {topic}")
def shutdown(self):
"""Stops the connection manager."""
diff --git a/ros2/src/robobin/robobin/message_handler.py b/ros2/src/robobin/robobin/message_handler.py
deleted file mode 100644
index 46f3ffa3..00000000
--- a/ros2/src/robobin/robobin/message_handler.py
+++ /dev/null
@@ -1,40 +0,0 @@
-# robobin/message_handler.py
-
-class MessageHandler:
- def __init__(self, api_node):
- self.api_node = api_node
- self.handlers = {
- "PING": self.handle_ping,
- "TIME": self.handle_time_request,
- "MANUALCTRL": self.handle_manual_control
- }
-
- def handle_message(self, client_socket, command, data):
- """Routes the command to the appropriate handler."""
- handler = self.handlers.get(command, self.handle_unknown_message)
- handler(client_socket, data)
-
- def handle_ping(self, client_socket, _):
- """Responds with a PONG message."""
- client_socket.sendall(b"PONG")
-
- def handle_time_request(self, client_socket, _):
- """Sends the current server time."""
- client_socket.sendall(time.ctime().encode())
-
- def handle_manual_control(self, client_socket, message):
- """Handles manual control commands: W, A, S, D."""
- directions = {
- "W": (1, 0, 0), # Move forward
- "A": (0, 0, 1), # Turn left
- "S": (-1, 0, 0), # Move backward
- "D": (0, 0, -1) # Turn right
- }
- response_data = directions.get(message.strip().upper(), (0, 0, 0))
- response = f"Manual control command received: {response_data}".encode()
- client_socket.sendall(response)
- print("Processed manual control command:", response_data)
-
- def handle_unknown_message(self, client_socket, _):
- """Handles unknown commands."""
- client_socket.sendall(b"Unknown command")
diff --git a/ros2/src/robobin/robobin/motor_controller.py b/ros2/src/robobin/robobin/motor_controller.py
new file mode 100644
index 00000000..e69de29b
--
GitLab