From ff8d5ee2813f2807f3c09c1ff563391317f6ea7c Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Thu, 15 May 2025 14:08:11 +0200 Subject: [PATCH] AS: plugdata example added --- .DS_Store | Bin 6148 -> 8196 bytes workspace/.DS_Store | Bin 6148 -> 6148 bytes workspace/src/.DS_Store | Bin 6148 -> 6148 bytes workspace/src/6_DOF_example.pd | 52 ++++ .../__pycache__/osc_ros2.cpython-310.pyc | Bin 26860 -> 28091 bytes workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 259 +++++++++++------- 6 files changed, 206 insertions(+), 105 deletions(-) create mode 100644 workspace/src/6_DOF_example.pd diff --git a/.DS_Store b/.DS_Store index 9fd8c92fd825cd302ac2fe9f1436871dbe9c658f..3844f5cab0072db67fc7b2b45f4240617c3cf4d9 100644 GIT binary patch literal 8196 zcmeHMPiqrF6n~SZ?Z%)KwGcfmcr7(aqfHSZjp;#9gl^S?O3Y@rY3S}w$e#uYg`Cxo z;ZdkZ{Q`ntK=7k@koudMZP;v+>Y>((GhyaUX5R13%ZP40xh-D&O~b-S)VBOj{3V1~dbj0nLDBKr?V27{D`I6l2PBU(M=P zGoTsxFBuT`gNZ6+DU?Gc<<)^krT~yh49kK(@&Ji3Ls<&tP)P}eO_e>c5X!<7gDE)X zO`#513gu8q1t+H9#KM(Xn4vJaI&h{`CstBYx0(UXz;Omd?mkBkk-2ZtgINBKKPC19 z^Y0F2k>D|1yWeSrx&9}+*)pj&GOZd(GHjtiCNPn&_m`(@q>oQ}j=YQjhvsp`VD!60AAEt$Du zzPP_+4%YJ7C9|+{dw)M=EZ(?zXS2HN`2l~U&dkDlgD$G?qxLO&V~6q@Ijhe7)<`2Z z@AFC}Jhbs4|K)Dw+voMT4WB!|)M@RUyw0B04m=(-U|a0{5^?HKovPF(7qW?V4|SPZ z^eXbwML12Ic{$>=%oJ>u0?bs&rXFqMv^=luyn$0(0YtG!T!-qCD2y!74sh^vE4YB`yQyL{6W? zo?LL(9oJ*uW7bD{k+A-F=3$)~42r93h~%~02~qtTdqi2f`+hOaC{haY48I%e&NwA|DVcQ+K6UA zGjKi`V9APA5l`@=|B>U{6uGvE`Vv(Xp*K`gLeR)`98#v^kfR?A(Kn$gFr`oqmBa|j UnZF456N!j^{!3r#=HFr9Cz7PGC;$Ke delta 165 zcmZp1XfcprU|?W$DortDU=RQ@Ie-{MGjUEV6q~50D9R3!2aEYKxH9N5on z&dA6>0W8rt^$^0^oASZzAU;)ug5I2CuHplbKVFmy$Egn4p diff --git a/workspace/.DS_Store b/workspace/.DS_Store index 46e8f7f913bc1403694fc02e823f127a5a1576c2..ddfa767c693bd6cec7ea911472e4e02d4e7a116c 100644 GIT binary patch delta 70 zcmZoMXffDul!Q8j{u3#ug?z3WgTuCbc>W)s{vEItnJnX0^4P9HPql)(9%MSqmwiR3e delta 69 zcmZoMXffDul!Q>Jrt}M&_nE3MPhTwK@vbmPQ6Zwy{}lEhmSlvc7dte0EN5 YUVhi)cxHLV?#=bg9W0yKIR5bi0PLX^9{>OV diff --git a/workspace/src/.DS_Store b/workspace/src/.DS_Store index ddb12ef285c24e56768a2b109ae4177895d91aa9..a3f334bf0abc25cdda8c45f5ed923165e8336e73 100644 GIT binary patch delta 111 zcmZoMXfc=|#>CJzu~2NHo}wrt0|NsP3otMgG2}B8GZ=a1H;Je4{ol%v zBf3eSvEE}!6*~ZqDWMUqXMeA;w<~PkJ2*}TOsf5gJ2hsz!sPxxlB{vQt#D2dc}T|? zC-)J;iyG$_IQN#u*QPa`aF$PuCO^VS66Zy?`%^SFn%AyxI-Aax^WOg?XMPbB)4U&K zZ~5(&lu3A&55m{+Xg28FdM48%h|^J~2yrw*lXq|8G?KHvoTgE(xW2A%T25=w*`3dO zy~92Cpm(^~bLWqHUF07=S}a=5&fWV@UJj0vlT?1iIA8+%va)T1XTaL59owm&rHM?( zsAukN%z?msUsid%teu%J=X|pU$5@^#7m9U7G``T7YuZD;OEoh$oUKdk#Rj(uSOxw^ z1$cjO;f$WaQlr{Bkfm15I>S20scku_!S|F6h!?I}S3*Byy^@p0<-|r)j3O+5ik*8Xg;Xhze z1%NgKb=eIOpu<4C(~tnVI5nTc0@bSKsW#QFI#lNuEkG?)T>#x`KENWq*f2fv7}9l- z-nI+6S1nv5M9@oA_aa#;T}}#1Wy?asVp!Coh@-_G>a6p)-SR`siM2ay!t6VdyN}5ypBzOiSA`A&LXHZD3st{CR zme3dYOjK_|W-+!QnOS~R_CuAt`ZTrEgg4J%~W2YIc7Je4j(?vs%GJR>iY z{8G&W<9faj^&-kaTB2@VCE9jVG95}d!MXNzDK5haN&Y9pg-}-v=OO{Y2iU+II8W78m|JSKD3agjw5=lNkJ>nhQ$P%-lEDWNcV_e$AfO zy=p1Ua!!iZC3*xN3fOEXJYHdC#2=G7y&hx0DnB0?5fk=A0qlLS;sA7s>5rQ*yX5sY+nIe^_ z-&B^P^D5{8Et%{|3y0Rto;dT9>q&9u4Xq%PK8X(SW2Fup;hPP(@gj`(WX(dPd-)hl zz`iD?JD6m+!?37ryfis-N0fdzeGI*o{b4dyIhcCNvk1I(^fn9TA)@~c+mVwk_M;0Dy0iTkfrd_U@h z{Q-~VPLn>A%&w}u@JcTFkzDl8LiAo9y;nVt*xc-bQ4CK*!0=?BgB)co-$ldC`HI%( zOXkFade>4n{B|!3%Y=(~*B3D*px%4n1!9jn3ehy+^RW!><*@hAwuT2oIG&4P4QY+n z8OmN29;=DYD%^y6mql)DuX+h)`*4OYy*`zFt*|kVy5I$u<}53jzVj*3bh=yS@8J-I3v0kNRVT=n?jpp8MH*#or}E>^V=3({v)H2lWv9zUR^X@y>zEA>~qZYAUFyF=aYRb!BQc zrYoZ%MGcRQg=h$z9;4AIMF&PM$hq3X`)UOt?qmDQH!#DS&wju9Yvc&qRnx+*dTMN2 z{eFlp-kKrX^0e8h(GYEC!J1!p#A{E?j!uSSm&4k4CM`M*PJ}M$QM!QXdKa4&-+o$v z*)9aY{-O4E{bWc>+q3`=A(pmGQ^;r964s_?;V-7s@HG1qrLD_xIygBSI!I}hrY$t6 zjfc|m45U3RUy4pI&>f&IZRbh?h_#2nwGhRG(AoxeqqfbFmOxut0`;k1DHlm)3B@Lg zdJr5%a2&xg0PN4|H<3>EFZDjFg{x$d`tCYfiu7^>6$mhb2KdG5p)GR}Sz-eX+nXH3 zDvHEL+{8gd_?GkUCecP@;urnI4>XxcjZH=3TVkC9xbTY-BI|0LONx1>Y$Yk?@;2gy zZw<)Xh?OYtl}Q0{5%G8O`?5=vz#eLRN2wGg)PgiC=aGp^bdySAy9&G?$b}+cMPmI# zv?8Aitjh3jgMZKitfRXqoW|Yv_>4LR*Or-Z?RMkA(&qXj)&*~vtT-F z47nxL;YAntKPzu4kW0lz!djY++G!Q!_ZFcKGNY;7@-R&LQ%^Qkid};+AY+jh2UCel~TprBovK zv2V1-$ZpoMd8v08Rq^u3nZv?VL-S^-1d)!WDMN4s0pD~6a5T>H+Nxb?hgp+aa4N)n zZQF?HK@2!XQnPIriIrDfg1y#WZJ7h?AErKOpBAkvdzl6Ivd^=}wuaqLqh4NsD`W?f zZxA3n!|FQTJ;Pn)PH#Z&757w}a%0@@RgNOza{zo+^NILlL@XibMDP>>)9q;(elB%& z+c(8^eJMm=zQuikL3}SGr#!T$1lm~NH93!AQ zElmaI0faSr9;WmHg7qSDu8L?IGWk)O#M6YP$#hy;2+q-EjtbDnkj}FUfL&|Kv&;hK z>#6NsMUo8@eoLgg*wLO%d_{iDjGoCFz78*-;Ma4~d~?7|{DfKe)L|{{IZODl#05WK zH})J}HUAC!WY3{h^AFj`-u_kdkJyWQcajPA{$8JzFU1j7-`i4x`GFFTgtbtb=xK?< zCU}|!dRwem{4~L?^3RNi}ioUpagXMiof8y2&i zJw9k+d_@<|pm|lKC2+U^n_| zNQ8a6Z&NaD4Z?w^sWc4abl$XisL;PcX8w%fW}UV^gy%tw-ol9#9F5VhBEu;JD@D(9 z!E4)YR>Bg}am8sR061>&_)d${TjAG{&V#aIfrA!!N}sswVe&B# z`K{E?o;cCWYvOq7#S_nxPM!}uB+Xri=ZHTCe}gz4``_Z|B?P?OBRG1KJ$15T=S3X8 z2LSe{NZ`m(ev+l_yg~xO$w`V2&dTEgInx%POn!NCv-K6E3^L_Zs}FNde~5tRo1crX z;uP~te}sUic%12{+Q@&ir%$za@pPtvl+K@~p@#zMES$UHsFs#-N?${ve@E~i2v%zH zH_SdzL-wXx23!Po;yZ)Y?Brl6`7sL*p0HLy?NCJSvc8S=RGs?mpk3@Yish+@@NJHs7cx##tPln)aVRn@I zQ0rN?@7z(*E~OqnS4;dSky9EE!Q07X=%LUgsOPULWqTJOIOL zRx;uzZ?Hon4W}PQ(mVn@duR*+?*4Qd0q#50f&d>@$|J>+42exc6(30wmx z5t;si*#aArKQ){aRdAc@T&h`yp3T53uU1`D8`Q?7TEntTp#N$rgWKFC37Q8PZt3#v z0$e*uTw^ud1mtJ5H^F_S1)3!h?*J3|aLrPN+G26tZr**^g>kqyh8bKWLtkW9TcDfe z10N#b-o>tN;v`XRH4WkwA{tdUb8`cl!?1yaBZg&3F>KgofSLPpDn+%;us|o&&Tsv| zVZ5s1@}|2wxMw$S^FVGJbb!h3umHEL7Pzh1#&2qBfWe<}Vk^I?>BuRA`$%;gcjN}Y z417fQ7;w{LSdD_*=5sY4i?isJBknbfTK>R!dJJ4i~@zan> zSuOai0N1N_OeYa$EP9<`0aNm4ghe^wjCaKIbYZF9aAFofDc;i*KBQCVwgiWg2RdDLwM4+uNHYfOYvbO{C#GeXW+(Gg?pg4VSY+`vn%HO^4fY`-u z2b=02g;CtrzGVx4L*h>hJW|+59iw-Urn0)KQL(KxWf`p{#ZRLSIGF_>F&{a8r1ICF zV^H7pnbfIEo5=95b8M1nwi6K^@B`i>@#TKi8$9|lh_cuv!rKq-%=`_3K8vIOLGU4h z=MeO;SJfj)+~fHcKYrs${CvAN-i*!GSacFvww2ZgU-)7Q^l(U;)zoeWtpg|GRgV3D zg03ru4H~7;9_89bx(AfUn@>!JfKpqnR1;Tr#FgPpXqcdP#1LiYkQdw#=n zyXbxtYdo0ahg~UpDXcG;edS79scei(8@m;{5BU#Z6Xy#myiW>WQuukQKnDt&b}Xi3 zv_X>_2~W4j?*s15?4!yebM9+~ChXGW^g>z=Yhj(90gd!5f^%#*q(ED0gtj-~>Zj!~ zcx5)<)q9ZPZ3O(#58y~;FNZpuX&Garco4C?u})IR_Kh|0cbrxI85Ruv5}O~}=uPKm zau?uP;l;zA8Y{_bg5`|a2)jKt0Bx3gytd3rYM}SP77H65Y<=X9t$8-~Qsc%>u^5}8 zY9yg9!AGMLuT0j zj&C`C0ErgJExeY!34W)Yv1vFIgOgK1{W5(YF@-cb8r8#>VtO<>IU1d(aJ4E>+^*?u p1id)jgdmPn2ZDM8!|V?(w~}s_y4+k=AS745Hm8uZi4f-R{vRHa@?ro0 delta 6239 zcma)A3ve69dA_|n93CV=5PV2{i2xrWA&H_y$)rT_A<3d;%MvY%dW#YTav=&NcqA7` zQG^bL8hScz9u3`1CTSAagx$JP)q1R=aojqI$MMwD&eToZb{yx@#!V`B?AVjInI_{V zcHRHq13^%sC*=+P+yAxu?f?J#I6nUa^6sm|J5^NVk>Kxx?>AodKX=P}P&u_8SVN*@ zfe^_cCK;C(h$#&#R2q~*@<5{X&{R}60(y8d5)ZJ*Xe4?iLIaoB)MP+t292;034k&l z9*cB^RLe2Vz=7*Zho(Z`E|U4G^0y>a0c{*xfnWSRHx9ZWEfGr6FtA%n)EP2W7RZv4 zmSA|*l*U!6PROj1I*>ml=`AQ*a!@TzSP=Mtp;0HohIXYY8P54WA|)L}N)&;{Wjekr zJqD9O{TV4mLG_SBci8VaGAGSAO=-zxxW!QJv;xC;KvG~7Qh1LSQFj_(410yFm==J{ zM+Kub-uEitEX&KNen0XVR zEf?Xhh8R>VKq``&sfpQtj;>2;pkFWaGOYxi({u{mle8-7)K#O#geWAP5QQR$Lhbz5 zh=dP#RioD#Y337iQtCGbsu2UZOqb~F?_eNT-axg`rQ381-S_B*q+1N+Mrm%Kw|`@x zXXh}L<+%yTv<^HxriZ35m%pZgq(^k*K{0gXMZ}9_-ka+xn8o~)ImuKO74`);&cu$7 zOzZ6mS=x}6k_Bb~ZJ3Z3WmXM?-lmPT>0t>XxDjmX($WD*6mozzqdWzQmQ{)%4P_lR zt&-Op4Up-;0->8HltmIB5j?hru_1h0D3W(T)<;gzx#=*C_I#9LPo z1J{Neytg5!c7&0~x8nfIJe8A$W+8-Mf!OYV7{byOi77IRL}CbgPcMb`PDoOU zun(ZayKo>{mMnxS&}av8&;^PASajrqity*ELY`ljtBOvvJR|9_Y$3=^dNt%8yOr5C z3bP%(<`tl?2|^L$h05V#R0HB-<>RW!#lAN zW?eqsw6qb5&IjG?&PJ;Xz zVV(A5Q_IGAm=v}TAl-WhBxR;TK!V_|_?SM-<}hy6_vIz%w`F}L=_y=ZqHv(TePY*B z<{!@0_aOw4t#5he0f@`}dLfi1hE8FNt0LB^&m)9IHSQC8|7-o2FYEkBo{%_ktbO`teasmhjA|f$el(?6PPtR zVuv)#jhZfq1~uKVsj90qtL-ADChVNQ#qN@^YiT$9y3$e_<4jGD?E(9ru?7UV+w6f| zyPJNDBW5sXvkL4g%_?|VO%KJJRcAFGbiL_yvgoLXjse@322NZsFZ0vBMsl3Lv;GUJ zB_($9LzO%EbCsu@tO)LjtdKAGPRR+%f8@K!FOI6bxat#x@{;u@`EPZ&gkAcNlJwH??h_&cWYhznHmS*SMn5jl)qJS!?y#w+lizD$u6GumwNU<^AC{i z=HK$qe7p@`w%L^V-O^WyoM;CK?fg^aw=**pmx%l;${!p($=~yNeTl-n`*K&}BtLw4 zz|}I}ric2v0{nromwDCYr zV*KChTDK~(h;BJpSRad6>P##;X{n=A)AOtod)30z(-ECoE`4|$uln)KQs5$~s$k_f za4C}GNKPU-0fZa%UA(G(6Y1jp^`Udl(J2Ps1u&{`AI2{QhPQo&pBAhV3BqrLUtE(V zsgTIBOuWQHG@_D1{K;NYMO@eAkK|vfJ{cr3(LPi){+;@rDOr(;3(Q;~c8NzLE*TwB zBLDCIPmi9qpEv8LiGF0Sfq)-SOU#HBXU6zN4klVE}(5Gt^R{kR~4A9M^h4kQn06v{rDXeH(Azaw2>YM0OSA z#>PUVk*jt^TtYqKU}BXFV(S!=b4U;n`y7yv+lJ!|Q_A3Dk$oODp9R8ywq*yg@y!5x zG_$AmBGE*(UE}GkwT@YkUeCO=bz0Us&`GRa#~x^Ynm@Qb>Pw?g)Y^)=7}dW|K=A}L z5`+BF9qV99uUrt55Jg>sZaLtVJEG4*;ljsk-n`bDh-NPgf&X#rqYKF&A+cwnL-Uc$ z-W|`%YeEyd&P@T0H*}1wX(Ql2or8aa|GKl1f45_a^zfO^_fsM`VkWC6!NM@KrA&se z0*UHu1VN1=S!*H%c@wz{joQ(hBrL)#GHogI;W@S>M8oVlyzLl6p!r|;osKw2p3409 zt`fzCVZR}>-MpxKlgP_!yt_NLL1g0=I{szOTkHmyo7efv-9ebnTis{KN;^JQcqGpL3=MMogS{^AT!>7R?h@DfgRuBo`dj2* zk<79FgT#+}fDIOp;E;!%=Qr-LLzGSJht8d zVrj9c9*&J!sy@XgEk~HK@H~s4`e&$iL??z19}x$Y_x zV_P&4V#@#H*ya!hn!ScZ1X-MW-$NFI%U(w!;yS=P2V2Qcd1P?w?j@9dABa^n%_3KZ z=`3s_(J9?hk!2RDe}Lp)kz7Zz9?38GKMZal?U{E53kmGU?;qbmUgKL%9M!6@tWZ$P zA3ss0Sy(|enHNsD<*t=w{BPJrC0bS^sYP-PWgN*$wq<*f+dI=kp!;E_>hv6OH$u}Q z9=4Z%>CEBUA>?EhvEVSDoQ}pKmJ**GVJ+C}{aiVFME+3D96MV_ww*ysWh?^MkXYnO zB*sKy(4Q5f^*>|lUy#%yY2)d0?T%y6Ea9)8+eUuK-9rs2JkZz`BnA@P_8IQ-47W}e zN8&(&%aMthVruW4F3yIv;h-9}a1a(mBEl<31+rpg*Fy`H@hy^6B)KHKbVw;F<*MY* z4Ye0zsk`8ZH5vbNng7pF1Np~H?fI*uP~UG#v=%;Z2rJ-EUI@BBFbip5LS^spS1vT4 zyKNSXS0^220e%MIM-ufWHTbF2`+Jh^Fx<2O@AWedqt0}gnyEe_EvU(YM0>(%NJ{~; z0I$p0|9J%AKVhxeZ$dY11ZACr;)`pgos@wE0^y55NGy8<`;AmIrm>VGetnK&r?5) z12>=(%StsEcFsxdhDxLFB-gwMC{iWu=U5h zdc2*V9^XZZ_@mu<6ow6W4 z4LeadHW@Z9v!CNw#cXP1%7~7}jj5^F$kbKVk1*h$@e+F;NiVV?Bv+Be|JO5IIdBs_ aHo2K}@P{ThmKNi>u^(5`HRK`cNB;-#7vy;W diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 2c89247..e3c6712 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py @@ -27,7 +27,6 @@ class JointNameListener(Node): self.joint_names = None def joint_state_callback(self, msg: JointState): - print("Joint names received from JointState message:") self.joint_names = list(msg.name) class OSC_ROS2_interface(Node): @@ -36,27 +35,6 @@ class OSC_ROS2_interface(Node): def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask): super().__init__('scaled_joint_trajectory_publisher') - while True: - try: - self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip() - if self.trajectory_topic_name == "": - self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory' - break - elif self.trajectory_topic_name.startswith("/"): - break - else: - print("Invalid topic name. A valid topic name should start with '/'.") - except Exception as e: - print(f"An error occurred: {e}") - - - - # ROS2 Publisher - self.publisher = self.create_publisher( - JointTrajectory, - self.trajectory_topic_name, - 1 - ) self.subscription = self.create_subscription( JointState, @@ -89,52 +67,8 @@ class OSC_ROS2_interface(Node): } self.speed_scaling = 0.2 self.new = False + self.n_joints = len(joint_names) - - while True: - try: - print('+-' * 50) - log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if log_ip == "": - log_ip = "127.0.0.1" - print('--' * 50) - log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ") - if log_port == "": - log_port = 5005 - else: - log_port = int(log_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if state_ip == "": - state_ip = "127.0.0.1" - print('--' * 50) - state_port = input("Enter the target port for the log messages (or press Enter for default: 7000): ") - if state_port == "": - state_port = 7000 - else: - state_port = int(state_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ") - if commands_port == "": - commands_port = 8000 - else: - commands_port = int(commands_port) - break - except ValueError: - print("Invalid input. Please enter a valid port.") - continue if robot: while True: print('+-' * 50) @@ -240,39 +174,105 @@ class OSC_ROS2_interface(Node): try: ''' else: - if not(self.joint_names): - - while True: - print('-+'*50) - print("Joint names:") - print(self.joint_names) - print('-'*50) - correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: (y/n)?: ").strip() - if correct.lower() == 'y': - break - elif correct.lower() == 'n': + while True: + print('+-' * 50) + update_limits = input("Do you want to set joint limits? (y/n): ").strip().lower() + if update_limits == 'y': + self.joint_lim = [[None]*self.n_joints,[None]*self.n_joints] + for i, joint in enumerate(self.joint_names): while True: - joint_names = [] - print('-+'*50) - print("Enter the joint names manually one by one. Type 'done' when you are finished:") - print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.") - while True: - print('-'*50) - joint_name = input("Enter joint name (or 'done' to finish): ").strip() - if joint_name.lower() == 'done': - break - if joint_name: - joint_names.append(joint_name) - print('-+'*50) - correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip() - if correct.lower() == 'y': + try: + print("-" * 50) + lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter to keep current): ").strip() + upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter to keep current): ").strip() + + if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit): + print('--' * 50) + print(" !!! Invalid input. Lower limit must be less than upper limit. !!! ") + continue + self.joint_lim[0][i] = float(lower_limit) if lower_limit!="" else None + self.joint_lim[1][i] = float(upper_limit) if upper_limit!="" else None break - else: - print("Please re-enter the joint names.") - print('invalid input. Please enter "y" or "n".') + except ValueError: + print("Invalid input. Please enter numeric values or leave blank to keep current limits.") + break + elif update_limits == 'n': + self.joint_lim = None + print("Invalid input. Please enter 'y' or 'n'.") + + print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}') + + + while True: + try: + print('+-' * 50) + self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip() + if self.trajectory_topic_name == "": + self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory' + break + elif self.trajectory_topic_name.startswith("/"): + break + else: + print("Invalid topic name. A valid topic name should start with '/'.") + except Exception as e: + print(f"An error occurred: {e}") + + # ROS2 Publisher + self.publisher = self.create_publisher( + JointTrajectory, + self.trajectory_topic_name, + 1 + ) + + while True: + try: + print('+-' * 50) + log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): ")) + if log_ip == "": + log_ip = "127.0.0.1" + print('--' * 50) + log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ") + if log_port == "": + log_port = 5005 + else: + log_port = int(log_port) + break + except ValueError: + print("Invalid input. Please enter a valid IP address.") + continue + while True: + try: + print('+-' * 50) + state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): ")) + if state_ip == "": + state_ip = "127.0.0.1" + print('--' * 50) + state_port = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ") + if state_port == "": + state_port = 7000 + else: + state_port = int(state_port) + break + except ValueError: + print("Invalid input. Please enter a valid IP address.") + continue + while True: + try: + print('+-' * 50) + commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ") + if commands_port == state_port: + print("The commands port must be different from the state port.") + continue + if commands_port == "": + commands_port = 8000 + else: + commands_port = int(commands_port) + break + except ValueError: + print("Invalid input. Please enter a valid port.") + continue + - self.n_joints = len(joint_names) - osc_startup() osc_udp_client(state_ip, state_port, "osc_client") @@ -289,7 +289,18 @@ class OSC_ROS2_interface(Node): osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK) osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK) print('--' * 50) - self.hz = float(input("Enter the desired refresh frequency (Hz): ")) + while True: + try: + print('+-' * 50) + self.hz = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ") + if self.hz == "": + self.hz = 100 + else: + self.hz = float(self.hz) + break + except ValueError: + print("Invalid input. Please enter a valid number.") + continue print() print('=-=' * 50) print() @@ -306,6 +317,7 @@ class OSC_ROS2_interface(Node): print('=-=' * 50) print() + self.get_logger().info(f"Publishing joint trajectory to {self.trajectory_topic_name}") self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}') self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}') self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}') @@ -854,16 +866,53 @@ def main(): rclpy.spin_once(node) joint_names = node.joint_names node.destroy_node() - ''' if joint_names: + correct_names = input("The following joint names were found:\n" + "\n".join(joint_names) + "\nAre these correct? (y/n): ").strip().lower() while True: - try: - joint_velocity_limits = {name: float(input(f"Enter the velocity limit for joint '{name}' (or press Enter to skip): ").strip())} for name in joint_names} + if correct_names == 'y': break - except ValueError: - print("Invalid input. Please enter numeric values or leave blank to skip.") - ''' - joint_velocity_limits = None + elif correct_names == 'n': + joint_names = None + break + correct_names = input("Invalid input. Please enter 'y' or 'n'.") + if not(joint_names): + print("Please enter the joint names manually.") + while True: + joint_names = [] + print('-+'*50) + print("Enter the joint names manually one by one. Type 'done' when you are finished:") + print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.") + while True: + print('-'*50) + joint_name = input("Enter joint name (or 'done' to finish): ").strip() + if joint_name.lower() == 'done': + break + if joint_name: + joint_names.append(joint_name) + print('-+'*50) + correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip() + if correct_names.lower() == 'y': + break + else: + print("Please re-enter the joint names.") + while True: + try: + joint_velocity_limits = {} + for name in joint_names: + while True: + try: + print('--'*50) + limit = input(f"Enter the velocity limit for joint '{name}': ").strip() + if limit == "": + continue + else: + joint_velocity_limits[name] = float(limit) + break + except ValueError: + print("Invalid input. Please enter a numeric value or press Enter to skip.") + break + except ValueError: + print("Invalid input. Please enter numeric values or leave blank to skip.") robot = None cost_mask = None break