From 481ff219cfdd37e5d8304c030c4c55a14ec1e6ff Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Mon, 15 Jun 2020 10:30:40 +0200 Subject: [PATCH 01/48] Add diagnostics per ipd --- march_rqt_robot_monitor/config/analyzers.yaml | 2 +- .../check_input_device.py | 27 +++++++++++++++++++ .../diagnostic_analyzers/topic_frequency.py | 19 ------------- .../src/march_rqt_robot_monitor/updater.py | 7 ++--- 4 files changed, 32 insertions(+), 23 deletions(-) create mode 100644 march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py delete mode 100644 march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/topic_frequency.py diff --git a/march_rqt_robot_monitor/config/analyzers.yaml b/march_rqt_robot_monitor/config/analyzers.yaml index d772e39..2fa7e0c 100644 --- a/march_rqt_robot_monitor/config/analyzers.yaml +++ b/march_rqt_robot_monitor/config/analyzers.yaml @@ -10,7 +10,7 @@ analyzers: type: diagnostic_aggregator/GenericAnalyzer path: Inputs contains: 'input_device' - remove_prefix: 'march_rqt_robot_monitor_node: ' + remove_prefix: 'march_rqt_robot_monitor_node: input_device ' control: type: diagnostic_aggregator/GenericAnalyzer path: Control diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py new file mode 100644 index 0000000..1640feb --- /dev/null +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py @@ -0,0 +1,27 @@ +from diagnostic_updater import FrequencyStatusParam, HeaderlessTopicDiagnostic +import rospy + + +class CheckInputDevice(object): + """Base class to diagnose whether the input devices are connected properly.""" + + def __init__(self, topic, message_type, updater, frequency): + self._frequency_params = FrequencyStatusParam({'min': frequency, 'max': frequency}, 0.1, 10) + self._updater = updater + self._diagnostics = {} + + rospy.Subscriber(topic, message_type, self._cb) + + def _cb(self, msg): + """ + Update the frequency diagnostics for given input device. + + :type msg: march_shared_resources.msg.Alive + :param msg: Alive message + """ + if msg.id in self._diagnostics: + self._diagnostics[msg.id].tick() + else: + self._diagnostics[msg.id] = HeaderlessTopicDiagnostic('input_device {0}'.format(msg.id), + self._updater, self._frequency_params) + self._updater.force_update() diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/topic_frequency.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/topic_frequency.py deleted file mode 100644 index 7a305a4..0000000 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/topic_frequency.py +++ /dev/null @@ -1,19 +0,0 @@ - -import diagnostic_updater -import rospy - - -class CheckTopicFrequency(object): - """Base class to diagnose whether the IPD is connected properly.""" - - def __init__(self, name, topic, message_type, general_updater, frequency): - self._name = name - self._frequency_bounds = {'min': frequency, 'max': frequency} - - self._topic_frequency = diagnostic_updater.HeaderlessTopicDiagnostic( - topic, general_updater, diagnostic_updater.FrequencyStatusParam(self._frequency_bounds, 0.1, 10)) - rospy.Subscriber(topic, message_type, self.cb) - - def cb(self, msg): - """Update the frequency checker.""" - self._topic_frequency.tick() diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py index ebe11e2..d710ab9 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -1,12 +1,13 @@ import diagnostic_updater import rospy from sensor_msgs.msg import JointState, Temperature -from std_msgs.msg import Time +from march_shared_resources.msg import Alive + +from .diagnostic_analyzers.check_input_device import CheckInputDevice from .diagnostic_analyzers.control import CheckJointValues from .diagnostic_analyzers.imc_state import CheckImcStatus from .diagnostic_analyzers.temperature import CheckJointTemperature -from .diagnostic_analyzers.topic_frequency import CheckTopicFrequency def main(): @@ -16,7 +17,7 @@ def main(): updater.setHardwareID('MARCH IVc') # Frequency checks - CheckTopicFrequency('Input_Device', '/march/input_device/alive', Time, updater, 5) + CheckInputDevice('/march/input_device/alive', Alive, updater, 5) # Temperature checks check_temp_joint_left_ankle = \ From 77c44d68c30fb4d65a748e1120ca743d59a8f84c Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 16 Jun 2020 11:33:15 +0200 Subject: [PATCH 02/48] Added the active gait and subgait to the rqt_robot_monitor. --- march_rqt_robot_monitor/config/analyzers.yaml | 5 +++ .../diagnostic_analyzers/gait_state.py | 41 +++++++++++++++++++ .../src/march_rqt_robot_monitor/updater.py | 7 +++- 3 files changed, 52 insertions(+), 1 deletion(-) create mode 100644 march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py diff --git a/march_rqt_robot_monitor/config/analyzers.yaml b/march_rqt_robot_monitor/config/analyzers.yaml index d772e39..11f5c47 100644 --- a/march_rqt_robot_monitor/config/analyzers.yaml +++ b/march_rqt_robot_monitor/config/analyzers.yaml @@ -21,3 +21,8 @@ analyzers: path: iMotioncubes contains: 'IMC' remove_prefix: 'march_rqt_robot_monitor_node: IMC ' + Gait: + type: diagnostic_aggregator/GenericAnalyzer + path: gait + contains: 'gait' + remove_prefix: 'march_rqt_robot_monitor_node: ' diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py new file mode 100644 index 0000000..4fb8b74 --- /dev/null +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py @@ -0,0 +1,41 @@ + + +from diagnostic_msgs.msg import DiagnosticStatus +import rospy + +from march_shared_resources.msg import GaitActionGoal + + +class CheckGaitStatus(object): + def __init__(self, updater): + """Initializes an gait diagnostic which analyzes gait and subgait states. + + :type updater: diagnostic_updater.Updater + """ + self._updater = updater + + self._goal_sub = rospy.Subscriber('/march/gait/schedule/goal', GaitActionGoal, self._cb_goal) + + def _cb_goal(self, msg): + """Callback for the gait scheduler goal. + + :param msg: GaitGoal + """ + diagnostic = self._diagnostics(msg.goal.name, msg.goal.current_subgait) + self._updater.add('(Sub)gait status', diagnostic) + + @staticmethod + def _diagnostics(name, subgait): + """Create a diagnostic function corresponding to gait and subgait data. + + :param name: the gait name + :param subgait: the subgait object from the GaitGoal message + """ + def d(stat): + stat.add('Gait type', subgait.gait_type) + stat.add('Subgait version', subgait.version) + stat.summary(DiagnosticStatus.OK, '{gait}, {subgait}'.format(gait=name, subgait=subgait.name)) + + return stat + + return d diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py index ebe11e2..1a9e3f4 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -4,6 +4,7 @@ from std_msgs.msg import Time from .diagnostic_analyzers.control import CheckJointValues +from .diagnostic_analyzers.gait_state import CheckGaitStatus from .diagnostic_analyzers.imc_state import CheckImcStatus from .diagnostic_analyzers.temperature import CheckJointTemperature from .diagnostic_analyzers.topic_frequency import CheckTopicFrequency @@ -16,7 +17,7 @@ def main(): updater.setHardwareID('MARCH IVc') # Frequency checks - CheckTopicFrequency('Input_Device', '/march/input_device/alive', Time, updater, 5) + # CheckTopicFrequency('Input_Device', '/march/input_device/alive', Time, updater, 5) # Temperature checks check_temp_joint_left_ankle = \ @@ -57,8 +58,12 @@ def main(): updater.add('Control velocity values', check_current_movement_values.velocity_diagnostics) updater.add('Control effort values', check_current_movement_values.effort_diagnostics) + # IMC state check CheckImcStatus(updater) + # Gait information + CheckGaitStatus(updater) + updater.force_update() while not rospy.is_shutdown(): From a1263b7f18cbb674fb06c80ec30f03d7c0276d30 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 16 Jun 2020 11:42:54 +0200 Subject: [PATCH 03/48] Added the frequency check back in the code. --- march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py index 1a9e3f4..3eb36e0 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -17,7 +17,7 @@ def main(): updater.setHardwareID('MARCH IVc') # Frequency checks - # CheckTopicFrequency('Input_Device', '/march/input_device/alive', Time, updater, 5) + CheckTopicFrequency('Input_Device', '/march/input_device/alive', Time, updater, 5) # Temperature checks check_temp_joint_left_ankle = \ From 78059c670d7e946e0838a57c2ba99257a2197468 Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Tue, 16 Jun 2020 12:21:46 +0200 Subject: [PATCH 04/48] Update plugin manifests --- march_rqt_gait_selection/plugin.xml | 7 ++++++- march_rqt_input_device/plugin.xml | 7 ++++++- .../resource/img/march-walking.png | Bin 2640 -> 8287 bytes march_rqt_note_taker/plugin.xml | 9 +++++++-- march_rqt_software_check/plugin.xml | 5 +++++ 5 files changed, 24 insertions(+), 4 deletions(-) diff --git a/march_rqt_gait_selection/plugin.xml b/march_rqt_gait_selection/plugin.xml index fd61927..5031dac 100644 --- a/march_rqt_gait_selection/plugin.xml +++ b/march_rqt_gait_selection/plugin.xml @@ -4,9 +4,14 @@ RQT plugin to configure which version of gaits will be performed at runtime. + + + resource/img/march-walking.png + Plugins related to the March exoskeleton. + resource/img/march-walking.png - March IV Gait Selection + GUI to select gait versions for the March exoskeleton. diff --git a/march_rqt_input_device/plugin.xml b/march_rqt_input_device/plugin.xml index 51f9f89..dc2d374 100644 --- a/march_rqt_input_device/plugin.xml +++ b/march_rqt_input_device/plugin.xml @@ -4,9 +4,14 @@ An example Python GUI plugin to create a great user interface. + + + resource/img/march-walking.png + Plugins related to the March exoskeleton. + resource/img/march-walking.png - Great user interface to provide real value. + GUI to control the March exoskeleton. diff --git a/march_rqt_input_device/resource/img/march-walking.png b/march_rqt_input_device/resource/img/march-walking.png index 5c5dbeedb8a0e406fcacccd472a35cb8e1bc7e30..15f9c92cab4f0056efc65e5c7129c8a88accdabf 100644 GIT binary patch literal 8287 zcmb7pXEa=2)c)v((TN^ykVG&_7`?YdZ&AZ2qcfsSL&!i zS4&5YWR08z_<*&jN$r2g{s<+fJHc`Q0RI3mt8S+P0NfweQdKfW{@Amg53hIxVzWZn z{JVA2dlycMr=%7a71+8t23@j1zcLaAjI~6^G`a-}x7Hc*!R@5D-#q`|l9LL-?H8-NJR!_O`%}2B zYc-MpjzcW<5=vn)xR>hh{@U4=j5;wD$Dw3<%*~c%KanuUA;DkkeC)9gDLMJq`UN=MbkdvYo9I0;1dR{!}}Li?s)(8C-j-zbv(* z$>Nhmpem&ACEYokebq*&+AB`HF+27l0*#(9Nm#y}&G#-zCy@fo2`uQx76a^!rKU^HEecM<;SJVNnHjx# zUi3|#?BBEBmL>DELrFnhpGSP-r@3h>3MQG**$)z(^`HZ-v}UT|T!mNtE(>dpQ*f1n zIrpRw!^7{T;t3--zbyMMzR+43l2`o&-C(Q6Sc=kE^%q$`dK8Ac5$2i)xOcYTTD&AM ztVRMn80nW`xH`7D_kZy>kz~u<7(VEq9@kYQ#eYjAUBye;Xb#3iTfiv|I`mZ@SodI6 z@GFMXf>Hk63vD<2IzWoABX|zwp116J1~iSI{+vLFW!gQFV4#-PZ2Eh$$D?Rp&}>v9 z6!(+wWd{jQrz43vaiYB*H9>c)xy{auZ()pU4Uztstl@P9FBpi6&kK1gSumaBRXi(g z5@9M8a`kW-#OoyFvvk>HUgXoY0rjuRti=UP@K;LggHDFnG&I~*xuuLc55E6us6(!Q z_-SS?5xzMRxSn^jXX3%Z=abXybUCB;Pmk&e2+@@|N$@R%^We(?0H{?U76?bJ~gx&k#$$jUPU| zHRXCbv0yw91uqi~f`8iXZrIHE7;y4-v>^fank8CvB?h(}%FkuBC8(>rG}CgU0tvdQ zzp6eVt2{~F-24Eu`Zt*eZ@E?PhG)GKA_-JGH^O3VauIBlQ9Ht!EuNP!au|I=u)1C?`igq^fl@T}-7A$sh zJlZUwScY&fO}th-YYe@5WSXl5ox*ox5@8hRUH5;~2$&>OpQZzq5KTes7XGXYTubNE z#pY^4LkM~H=(l-~Pt$^P^hwqAAlj1>WI!r!)iXckHqo!I?f))V`g91^p2Y6%p!}^Z zLZG<80b&m(nl7P?%Gje+jupH-`xkP7hFdSg8X|TuC>|B-EXoxQZr-76Sa<11jU|MY zjlo4@xTeG~0rnxX+ep7*%jJ&0~}IY>Y7^Kv6@k$o@7>=1@Y7 zdm_>q-u2HH7eHm0)S&|nOslSZ295naDX#6{MBe?{HvUCf#MqF^+Itmk)-pw5Zj9Ke zBQ2hizSiW&B!qZ7PcS!=FD$yaOV$@E3jhhu(L9$e@nM8bY4Xu>AVDh8g_Yh{NcEOFxUD$ZUl71j-iqA>j2U|YiAA}{;_22 zK-(wO=P`pQ+n;q|eF3)CqSm)GpdZM%ayAxew&FTa{`P*SwmHY-6Rvwy|F*(&a{*5bH{)#MhVn1qVq~0akTa zhct+NL-U0{JLoH+5G_iFMq*J7^S+|J6PG{q1oV7^=8^ojEcEQ)*-sy_Pew=0?*)x- z`7WzV=+{1c>Rm|mB2^Am*A$4R2Qdh`8e4uc2m%v-xwM)mIH&HsvA>69{F-@UKm?gR zg%3#!P6|q;ZFn7yyXezxy$g--xs~mPD%zFGGwM7|*o}#84E}J{rWjLLM1P<-JT_Tngcg*4#A!Mz4 zPv)o-ih4abwHKRGCE#?!G*-BnCOPQQouFCQC!7O`q6yWCsq-0yc+hG2RES;DlPaJ| zwuF19H1;TwVGxcmyS2nz^NkB8+K8|Zm2cDQoSO1dttxa)2Iar%+id>Guub08&#@l6 z1w~^X9nVk*fUbEfleW38QW&1mOO0#Slo8b27t!?*!|MC$S9l_!7z8&(VC|Lwg513- z3+rf9VaT~hu}hJk*vl)2fh1<@!>+>meAeHS(+t++o^{22kQhPi=X*x&93RfG1On$Z zj*ER7Wtsyor7NP@snIH!*AI7}Q*9moMfxp7+-q|#ROD>t_L%S#ox|wOXc_0i(zkcW zeJ!oj`0P(VVcQArN3xeMs(Pbw>cZ;;#)rjrKb7&rk^F{aQP?8--ZId&>`rM~2HqZ8 zL!db6d|ZRmIOra|Ei`*jJJ+`5g)n0k;G*s8MHcgAbX83)KBc1NJZs7TGhac|d&;Zh zA1vzH0TBj=<^YAGVq^*RF5Shm;SSYheCq)+u3~c`!NV$(8~YPHJ&8eB`PUkaxs+-S zo+gLzs%M{Jkm6o~H5X5@SV!@?vRvErw+S?L2?2C=uLB7NG?Wwz?6%Ts%IH98+*b^B z3108447p8$?omV7__8J|M0a74ZYKf`95yj&+zOC({3u)Ac32!NfCybNDV!GEVH|g9 z`u>qTxhP^{GbofZn5AJ9`9SMP0qI`Wa)<`gb1gMrM<@CbJm7P1N|qOW!I{la0vf6;lw=0U5r zDa1#glidH2i0ryX?FhN`q`js|jZ0_9BUJ9L{oZqqM*2E)Lu$xHjGQzx9hvYYtL_Q z$HPH(E%UmUQOqv__!uj*#%iDMe8}l$vMrx#`M~6dW{;G&0;>vjaOD1dPI$eD5{DO2%*C=2 zbae^#NST-(O-pk>+OVkNw0ltVH7gHfbA;NU!`}z1r*(f`S^^SNe_8qPXh(y& zc-Tbw%$w|Jj-}KK)kF6v2WOn+2ten6ng5{sbUB53EH?jC8*oln8qA9Mh3CMpQZ`wY z&u~(xKINGDN*=r~HC0XQ4(X5i?s|@^BBL3udrE=#o`<86gCY}n7uZh_JhPyA1rpJ9 z$SD_7BKSNU6M2RLV1*tED#$~oZwJA2^=XGRcLkfV&J}6}fZCBy{k%Aa4p^rpjO&{J z5wnak#pKqoJD6Fo^R@<&u^t{Yx_AZ#j^>{QN?mYbhc&7@0aMjqQo=8z#~;X(pA6Pd z^RtBEYKJZC&eMkM>XgqCm#wH6cHta{%zaL;9>|#$WO9HDXd1-!1I8~I>&BbjX-eAt zs#Ykth~}SB)?1>mWP&@RD;hx_T-PUAUX!schC!#2it4BQF@MnKOuv3DM(kG9CESRz z3T`__7L3zN5LyVbd7k2S`Va2gMkPbItkwC zUhPp8#qq-PkL>a0-8x6ZFIjk-65SsO}FYD#d;3aq<}) z_UTzZGFd(rGuL^Symyr{iE<=UHQ;B;RXTnPU5{BY41!{}PxJsDa>rnLbrnew>jIm@ zP8Ux*Gxjv*_ionMgV;BrToW5df_+YJ?p16k>iY3w?Rv^*f}f-HXe-3509?0?poG5J zDG7m-J(@SsCwA!5;8ulLi^e$?vUDLQr29WsF+Nx5qKe%1tk}(Mx`;urGRZ?>E~bAr zBW6a}2Vl?y58+g7zS!+yD6BtY+YjTxAI$98X3CdftOB$-=SZ*JM$;k6-g983dM%eW z6>0y2z9Pu)L4%pbM*HFm_sC9<#~2rZV3~)-q+oP zA&V-6dIL67veRS*WK-iBbgWEm&pLp*0`t4?wBO&7`zFB$vdJ6Ov+jyC0z?H6ZBc za`Qf1H>90=J%Yh$EIvQJ{@DRb_z4_8ZLy~oA$OUy?GxYYyS6RF9v;0MIKTVrZ^MM$ z;MKmvcVar-b2Dl>hi)I7dtcx3=GKGIe?Q5`um_~=KQpsx`UZq-nRPn@KFoh~F%9qsXJ?xK`a_jA; zS3hDP3_hyELa071ISa7KdvnPC)4hhpbiZS(HP}C>m7VwOtqr?O=|8k1xTmGBH9id5 z0k>cej?7*svXEpkj2*xx{c|Arx(~S3012O$XuEs-$0^n`|9+FTY3cB^aeG2A5w>cJOqAC-D(>=A`5I2tODlCFrRU+o zNi*d`Nm0qk`RNGWo&Y#9Fc7QgC2h|647gPvYp6BnBdt}~B2aav($f2l_vO_nF+6L4TUR1@W1ABk0n;VZ+P4{u+J0L@(owfS2i`r7r z=|f&5R}a$96R>JsZM#pkH67aF`dcLUMVpZ$Hont89tEY z8Cy@Q&6yW+U`j~ilwjD{6#X^X^&23hKBA2bVQk9SyM?9y@hh#DICBih{ibxXQ3OxO z(7Ap(n;4GQt5y8;l}PYPo7vl(t5J6rD;qGee)n{F08f)!uXog2&U-XnXW#LTSxNmL zlzf~weta&>=!*yZ_0IJyS^9CE>PrW51)q2RA>YIMc9zL4CU#$aSlAXZw}+$5=ND*1Cr-QD_$MbX-pg zB`fkzZ?ej;(=A~AF1QaG0Etd+qw885L~7SbE%qcJjm}~@BM#KIQh&tS2^#tj^ zq>?4vJ?h$86jNQ7U5`B+g~tABQb*E+KI?FMAhw`$v9={UD^Hhh`_QdJnL%^Jk0m0n zKJ%cj4OS9btaXe@eynGE<>qpk17v*eHK!l0t4F4Q#E)PQr>M_T{GOY=)SHG|tS zr2)saMA#4(Iu}23r_2`&igk_jh=u5K8Ot#~^`j_Zc|xSAf6Dw9eJ6EzPtLrS+1=6f zA5o5B{u(is*5az;umdRTgQz z0>_LeK~Ud?Hc}`K4^vZ1&O4!9AOoCuh7*IGIp~MOca?$1vD4&&jd?*BlqG?SIvr(0 z99F#CgCO&!YnGKh2ez1{4Ku+-_CwOM;0Z z5s-8QwU&o^St+~zDd~8;-`C_V<%}T|c3!_ydj8kO5}Q?W$A2HK|17SNpQo%WDF= z2|9QLki^=2#*-OU=u*gc^Za+O+~nI?j~wb64hB;?bkjGIo%T1)XG@;;fJoQ7F4v<1 zuL`Bqu-@ZccuuNK=&Zv8aPE0S#kSq7`>EraVuh?_m=95H-{npFMUc~y+mp!b7PLp0 z6_*SMY<(|C(~OrQsA8$#=}mk3R}#VhQC|?L;M*u-sF-x|9(a5Dg_@$Pd!ADUs3`R} z33#htEq|Pe~qEd&0-ekrE(#1Zpk%SR*s}(BoK0#|&**C6;<9vTOWl)+6 z_~%(ScT(N%8T-?osPJVUJ$SwhX*Dzc;d@9Q;BE?*iCLH7%Z3jxFROY7jnYbwG_2H! z*L=uP{l`G3`?Bl!Nlmt^?Kd$E?*SL{oOLk3gQWIx z$X{P#l3=2D@A$!sF9`l@)IAw)2u=+;og~&qDie{jDpQE-R)zwf)B%?U{fK_Gnv=Wn z+rtvg9Gx!W8>AR?m6O5Lk7@G63YDKkn*Yi0&G|8Qz`nWLxc0X^pCz1ktr1q?R~~7` z?!9&U;oo3uk*ny+wN5%afmEsmgY#rd2G1W4(2D3JDZ#|pkC4pn;1Rm07x16{S%bF( ztEn-3G^0$erDq{@W1#bga7l>-bJ;_|lAN=SJk84cohztKneW&6Ni$0#JQ7ziU&_hu zt#`ctL>yl&i6fMK<9-c320Teo_IiXs@aLml_a$+5MTiKUWpPX787Jlmhr?&XzkzhQ ze;9&y4PrW@;5MVT- zD;oKyRzgnGBBrKt&~vvIr>jiGVxx0daSj-+ckL|Zx+Iu_n?Lh3u^c=S&39bud*_#j zftY-=opeSXXdwqF`o;E((u?R7`z={NziifVQ)iOE1q<}`QoA?)s&n!w*_nZ92flso zZ!H=_MvAIr8Q+Jsot2gKaT})LW`AEq^kChhR<~R{C%JsrzqbdzObvt0O$6tyt})Z; z29TL2bRy}B3z?$9=fIFb)2U92-O@B~4kB{K6Gdd+YRQdtmmXvSbjFsh_Pcgg2(w|I zFaf2O?JBKMFh>dP{Gq+E(occb$gHf%y!;*WmF=c%X67QMb{x-@1Q?7P_*aPZi z)<~-H$v>XyKQZEgQ?DC#ouXC;rg=FeA6xaQczQe%bQ8mj5y)0sUADsF|Imz^0^S@@ zV#fIkmOYV5$6w}#-<0}Ctup=&|92;G##~K;50k-Emx_JAT()>HOxp3`XWKV||Ddw* zh5@ZEk)i7%77I>y{)^nyh6(UWnkLm%dNb`!(1sCDtIR!<#r5q+g4DW|gx#vehVM^W zC5M|&e%BkT)M~w6S`CgAepATO!PYPL$=2etjRXJ16N@IiMv~!kWP8uM)y=oe@|?_m zb{DB(l|}@@0+{j5p)t-S&$yG+)09d^%%DH|nWfBdKyS8RS+9#}xD8{W6lWufNwSpA z1*ddS-xn)(LG*HUJplWB^R4+0(vYTfZx+R9Z*0}z2NF#6*JP?sRT5n4Nbed`xDYue zfl*8-f_{+bJkX_{w?%?KXMZG_O5uXd4rPPTX!f@77(?X>!>!^uhB3+0BN?lV^kR@t zOZ_hJaPRe@eU-JR2X1ef=k(pz#Kka!N9*UDD;K6bn0*1btx~s$4i5%-yk26^PII$n z*FT)-St>?*b7t57ZnBm7CP(%%Ln@LvhD z7x_u(AK=PtpBqG0siNZX7|s^;4sU>S55z_qZ*lMt`Z&865j2ri%b~7HCm;1F6JcBU zI$u>BS3md(NTN=w0~J5I`)?U&&{0yb=s+Qw;Tg7UgEWF`7sgqCCqR$UBZTpb0=DB zspvNkHb38qUBsj7Uf_3{k+OCsEeELhw{UCitZH~vL79*CwvYw`GbM<3C)(xsbgp>pj&uJ%j>La4{EzXVD0KE+vJ_$Ax7OI# zH-vcO(nl@k{+u=6Uj6==x!`j_!n*R%;;8Ym=WhJX>#Jtw>j3k0l(v8Ac;^735D};# zL_`qs#8^aBT1-?L@Zv%6WcxsN3zNCJr%94LwubX2U02erjf zaXQmd$6h+t4%3c0ZHLY{)_V%o@ocMuR-~MYs0anQau^Ls2uWabF1e4r-~O?%BpexO z_T9wM?_ck}@ALfL@AG?q&-46_XN4K}ua(1*2;W9%R1&IiBtj%2Pd1lHK{z5KPBw|j zhHA12oPj~B#;n((RjcS79HD>2ul!48riKOR)GDql%tEJ85eS8tm6Mv77YkFw0*Z4| z`M9c)h#XyuG91=~y?aVnKtz^fg9ni11TD&wIRz{REqSRn07OB+pj89luox1j048w? ztXy2is%z)+=B^5KY87SfbgG-$@dra3YVahC$D~<+Ai&Ez4zh7?4S(72F|FPG%+7Nq zbnq|<7GN{!$#PoRez1itl^lw?X=R=(arJpZUmq-Kb^zJ6y6bp1X{qbKR}XEM@v^9 zTD645po={l@&!;yB4<8!sXLv9Hm~xe66pyJ`-7PETH1RC(42OBt}f0cHQ9_otERcL zmrO?zPOAx{Rvp_O6-6S0q0wuARwYqBBa`wOnGE>?)O)=6f*}Tm{d5luv-v=6qG%;> z5ex(w^hLj_Rvp!QWn(Lkz3~B`A3ecK+diix*TqO6NJ(xg&Lm?(cqPmNWWXN`BZ*>c zZ{KpDmOtHhJt|3L;jCQ3kqFQH?B-aXs^e|!t8F3Kr02-$T&+$M)72vQKFzGcI zbsE;(^?l5G4Yxk=I@=G|#oDxX_oG#-STHl2%oHmvUA_GEz0as==}?4ILc`%J3Nkrz ztd->pW?{D&czbsh&7D2xwVRpcq^7xpMa6lf+mkqYvP&^e2~|KK6h;yS?p<;@C%t_< zvvJP_+ZnYQtVSJk3o=;0Z9k!~a)q0ODL@ni>O5WCbmdI!79)TB=T3%4`39pg;PbQM z>e)1R_VE6`uaqN|NKa6fWkf-wDAUQ-1GVg~X}VxLqfUb_5Mo(*5$m?>N9nSi;)|dw z*^J9>A=PSP?fo}Ub-bOQJiXzf?M*rj8tpq4~t=ejuD;9>nQZ%40H^Wcjc*;m^#Zolj2&LC%+ zjjN0E*j3rcNZ{hiiU$<4fWk~C^{uCP=~uVov>JJS(_VH}H8K(ijoWY0>^x*y=Em|O z-v7frES{aO6h#08_kDks<85kovjLmgz_Y7wMwVqBTEF>{92r-#ITkHy)hZtP`{uav zI|H>O#a{se;7)V!!UMO^*w)P>FK?lDNZGvJ{|mJw#di&up5|cfs_V&eCiB*(l|1>* zr;6wFCW^SO0X>5wle zU7rj2oOTyN>#2S|syued9505rD4@Ejo$99cc!)|kzV%b!Td#l#O#wlG+ZGh#3xx3Y z53#Sdg&7$sxLtMtPIUEBb$V)KIFcyINks;}Z0Vr6vo|Iu-(}}m+bM3mqL5%XdK~79 zrgmDp2S~G7D0QbH0CgTOk%&y5%Z5>>VQ?hC2S@5BGK2{#fN0Hc^MP9OT{h+yWn;G( z*t9>I$Gd7)E^e0{K@do@Ti9~&C;$s)W@FT8__C!FfTGM4jwBS8ceXQ`?_HJyz(NzJ7$mf1wuHjCOUfu&*wfBEI^jQ z7YvOFdjp|p%4^6U#2*Y{)@#^$@F+LUD+lZ4<6+Q^NNs(Dps#iv0={_0QiIBr`EnM6iJDFQ=pcl@BD!86c8^#smbPX ze><(F*tJ=&r6kuyNv?}@yCrsC0NgG+^NX@E=`{cpq&v{6qbEv|3_3DXlHx@fh@%1? zy>n4)Z+9h|dHn9Bv1eAJjyE3uA^Ow*37C~LjptV1%ujEa$1A_Nle-qpK_!YjcK1@A z`}r-bTwKOGYwpIP*Rpoi4dhIV$}M%L@yH#E;zb#VV?v}LBZYgH&gJ*7?_lNPQi^lV zhR1g-EWvEhvV8t!ytylS%%$GrW%cv#an-C`?p=C09X$ig$xr9j$6sS45X7j{FyxDF zo6_k$Pl+6BToho|Ymh{dGItt<87V9*&ZBo|gw$j+eM3H$FPO!HFKpzAd#>YOpHu=- zA7b^5SF&tg5zoHAm%zfLtJ~s1_blR-7N4GOr>Ucx4WCsLibQyOcNIVxwNvNua_7Plc2+d--`W;7Keme39$LYMH7m!QCsqn^afBQe z10dk-A0o|Wp|!i8WRo6)R*gxo9dpK~DAUP_u3qdG1I?YiSdBU?23_pveD1VlQmiIw znmZWs1;}&R@Ob<12g4Y38f<0*C%Sqs!sODRNNw<&1hwirPY{(xB@sSj0beIAI~oGx yr+G)?j68vh%NEXQ_jP2BC+}R@qx%3VQRIKq`lghECe}Ux0000 - + + + resource/img/march-walking.png + Plugins related to the March exoskeleton. + + resource/img/march-walking.png - March note taker + GUI to take notes while using the March exoskeleton. diff --git a/march_rqt_software_check/plugin.xml b/march_rqt_software_check/plugin.xml index d101d0a..2bdd21d 100644 --- a/march_rqt_software_check/plugin.xml +++ b/march_rqt_software_check/plugin.xml @@ -4,6 +4,11 @@ RQT plugin to perform all software checks. + + + resource/img/march-walking.png + Plugins related to the March exoskeleton. + resource/img/march-walking.png March Software Check From d5fb6ff278aa00f15e3907237e5ba84f4c49772e Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Tue, 16 Jun 2020 12:59:20 +0200 Subject: [PATCH 05/48] Add march_monitor package --- march_monitor/CMakeLists.txt | 10 ++ march_monitor/config/full_monitor.perspective | 138 ++++++++++++++++++ march_monitor/launch/monitor.launch | 11 ++ march_monitor/package.xml | 18 +++ .../launch/march_rqt_robot_monitor.launch | 7 +- 5 files changed, 181 insertions(+), 3 deletions(-) create mode 100644 march_monitor/CMakeLists.txt create mode 100644 march_monitor/config/full_monitor.perspective create mode 100644 march_monitor/launch/monitor.launch create mode 100644 march_monitor/package.xml diff --git a/march_monitor/CMakeLists.txt b/march_monitor/CMakeLists.txt new file mode 100644 index 0000000..ac605d6 --- /dev/null +++ b/march_monitor/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.0.2) +project(march_monitor) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/march_monitor/config/full_monitor.perspective b/march_monitor/config/full_monitor.perspective new file mode 100644 index 0000000..5e46b78 --- /dev/null +++ b/march_monitor/config/full_monitor.perspective @@ -0,0 +1,138 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'march_rqt_input_device/InputDevicePlugin': [1], u'rqt_robot_monitor/RobotMonitor': [1], u'march_rqt_gait_selection/GaitSelectionPlugin': [1], u'march_rqt_note_taker/NotesPlugin': [1]}" + } + }, + "groups": { + "plugin__march_rqt_note_taker__NotesPlugin__1": { + "keys": {}, + "groups": { + "dock_widget__notes_widget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Note taker'" + } + }, + "groups": {} + } + } + }, + "plugin__march_rqt_gait_selection__GaitSelectionPlugin__1": { + "keys": {}, + "groups": { + "dock_widget__GaitSelection": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'March Gait Selection'" + } + }, + "groups": {} + } + } + }, + "plugin__march_rqt_input_device__InputDevicePlugin__1": { + "keys": {}, + "groups": { + "dock_widget__main_widget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'March Input Device'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_robot_monitor__RobotMonitor__1": { + "keys": {}, + "groups": { + "dock_widget__Robot Monitor": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Robot Monitor'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000001000000030000006400000064000000c80100000009010000000200')", + "pretty-print": " d d " + } + }, + "groups": {} + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb000200000000000000000438000009ff000009d70000000000000456000009ff000009d700000001000000000a00')", + "pretty-print": " 8 V " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('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')", + "pretty-print": " rmarch_rqt_input_device__InputDevicePlugin__2__main_widget Lrqt_topic__TopicPlugin__1__TopicWidget r r 8 x V 0__DockWidgetContainer__1 brqt_robot_monitor__RobotMonitor__1__Robot Monitor 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} diff --git a/march_monitor/launch/monitor.launch b/march_monitor/launch/monitor.launch new file mode 100644 index 0000000..8d7a5ae --- /dev/null +++ b/march_monitor/launch/monitor.launch @@ -0,0 +1,11 @@ + + + + + + + diff --git a/march_monitor/package.xml b/march_monitor/package.xml new file mode 100644 index 0000000..0aef607 --- /dev/null +++ b/march_monitor/package.xml @@ -0,0 +1,18 @@ + + + + march_monitor + 0.0.0 + Configuration package for march monitor packages. + + Project March + TODO + + catkin + + march_rqt_gait_selection + march_rqt_input_device + march_rqt_note_taker + march_rqt_robot_monitor + rqt_gui + diff --git a/march_rqt_robot_monitor/launch/march_rqt_robot_monitor.launch b/march_rqt_robot_monitor/launch/march_rqt_robot_monitor.launch index 08f5706..af9b475 100644 --- a/march_rqt_robot_monitor/launch/march_rqt_robot_monitor.launch +++ b/march_rqt_robot_monitor/launch/march_rqt_robot_monitor.launch @@ -1,13 +1,14 @@ + + - + output="screen"/> - + From 534a40d629714ee529d7f34b39e3bd88f6e22ca5 Mon Sep 17 00:00:00 2001 From: JorisWeeda <45165354+JorisWeeda@users.noreply.github.com> Date: Wed, 17 Jun 2020 14:06:29 +0200 Subject: [PATCH 06/48] Apply suggestions from code review Co-authored-by: Olav de Haas --- march_rqt_robot_monitor/config/analyzers.yaml | 4 ++-- .../diagnostic_analyzers/gait_state.py | 2 -- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/march_rqt_robot_monitor/config/analyzers.yaml b/march_rqt_robot_monitor/config/analyzers.yaml index 11f5c47..a5bc5cb 100644 --- a/march_rqt_robot_monitor/config/analyzers.yaml +++ b/march_rqt_robot_monitor/config/analyzers.yaml @@ -21,8 +21,8 @@ analyzers: path: iMotioncubes contains: 'IMC' remove_prefix: 'march_rqt_robot_monitor_node: IMC ' - Gait: + gait: type: diagnostic_aggregator/GenericAnalyzer - path: gait + path: Gait contains: 'gait' remove_prefix: 'march_rqt_robot_monitor_node: ' diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py index 4fb8b74..a443c23 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py @@ -1,5 +1,3 @@ - - from diagnostic_msgs.msg import DiagnosticStatus import rospy From 0ed4619f6ed83e9dc0c89b550008be76cf6f8025 Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Wed, 17 Jun 2020 14:27:58 +0200 Subject: [PATCH 07/48] Fix joint names --- .../src/march_rqt_robot_monitor/updater.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py index d710ab9..c0ed884 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -21,35 +21,35 @@ def main(): # Temperature checks check_temp_joint_left_ankle = \ - CheckJointTemperature('Temperature left ankle', '/march/temperature/left_ankle', Temperature) + CheckJointTemperature('left_ankle', '/march/temperature/left_ankle', Temperature) updater.add('Temperature left ankle', check_temp_joint_left_ankle.diagnostics) check_temp_joint_left_knee = \ - CheckJointTemperature('Temperature left knee', '/march/temperature/left_knee', Temperature) + CheckJointTemperature('left_knee', '/march/temperature/left_knee', Temperature) updater.add('Temperature left knee', check_temp_joint_left_knee.diagnostics) check_temp_joint_left_hip_fe = \ - CheckJointTemperature('Temperature left hip FE', '/march/temperature/left_hip_fe', Temperature) + CheckJointTemperature('left_hip_fe', '/march/temperature/left_hip_fe', Temperature) updater.add('Temperature left hip FE', check_temp_joint_left_hip_fe.diagnostics) check_temp_joint_left_hip_aa = \ - CheckJointTemperature('Temperature left hip AA', '/march/temperature/left_hip_aa', Temperature) + CheckJointTemperature('left_hip_aa', '/march/temperature/left_hip_aa', Temperature) updater.add('Temperature left hip AA', check_temp_joint_left_hip_aa.diagnostics) check_temp_joint_right_ankle = \ - CheckJointTemperature('Temperature right ankle', '/march/temperature/right_ankle', Temperature) + CheckJointTemperature('right_ankle', '/march/temperature/right_ankle', Temperature) updater.add('Temperature right ankle', check_temp_joint_right_ankle.diagnostics) check_temp_joint_right_knee = \ - CheckJointTemperature('Temperature right knee', '/march/temperature/right_knee', Temperature) + CheckJointTemperature('right_knee', '/march/temperature/right_knee', Temperature) updater.add('Temperature right knee', check_temp_joint_right_knee.diagnostics) check_temp_joint_right_hip_fe = \ - CheckJointTemperature('Temperature right hip FE', '/march/temperature/right_hip_fe', Temperature) + CheckJointTemperature('right_hip_fe', '/march/temperature/right_hip_fe', Temperature) updater.add('Temperature right hip FE', check_temp_joint_right_hip_fe.diagnostics) check_temp_joint_right_hip_aa = \ - CheckJointTemperature('Temperature right hip AA', '/march/temperature/right_hip_aa', Temperature) + CheckJointTemperature('right_hip_aa', '/march/temperature/right_hip_aa', Temperature) updater.add('Temperature right hip AA', check_temp_joint_right_hip_aa.diagnostics) # control checks From 4ee86104704a84ee0bc02967b6252de35507780a Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Wed, 17 Jun 2020 14:29:17 +0200 Subject: [PATCH 08/48] Fix reading thresholds from parameter server --- .../diagnostic_analyzers/temperature.py | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py index 2de6882..f5a5652 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py @@ -1,8 +1,6 @@ - from diagnostic_msgs.msg import DiagnosticStatus import rospy - LOWER_THRESHOLD_VALID_TEMPERATURE_VALUE = 0 UPPER_THRESHOLD_VALID_TEMPERATURE_VALUE = 100 @@ -11,21 +9,24 @@ class CheckJointTemperature(object): """Base class to diagnose the joint temperatures.""" def __init__(self, joint_name, topic, msg_type): - rospy.Subscriber(topic, msg_type, self.cb) + rospy.Subscriber(topic, msg_type, self._cb) - self._default_thresholds = \ - rospy.get_param('safety_node/default_temperature_threshold', None) + self._default_threshold = \ + rospy.get_param('/march/safety_node/default_temperature_threshold', None) self._thresholds_warning = \ - rospy.get_param('safety_node/temperature_thresholds_warning/{jn}'.format(jn=joint_name), None) + rospy.get_param('/march/safety_node/temperature_thresholds_warning/{jn}'.format(jn=joint_name), + self._default_threshold) self._thresholds_non_fatal = \ - rospy.get_param('safety_node/temperature_thresholds_non_fatal/{jn}'.format(jn=joint_name), None) + rospy.get_param('/march/safety_node/temperature_thresholds_non_fatal/{jn}'.format(jn=joint_name), + self._default_threshold) self._thresholds_fatal = \ - rospy.get_param('safety_node/temperature_thresholds_fatal/{jn}'.format(jn=joint_name), None) + rospy.get_param('/march/safety_node/temperature_thresholds_fatal/{jn}'.format(jn=joint_name), + self._default_threshold) self._timestamp = None self._temperature = None - def cb(self, msg): + def _cb(self, msg): """Save the latest published temperature with corresponding timestamp.""" self._temperature = float(msg.temperature) self._timestamp = msg.header.stamp @@ -42,7 +43,7 @@ def diagnostics(self, stat): stat.summary(DiagnosticStatus.WARN, 'No valid temperature value ({tp}).'.format(tp=self._temperature)) return stat - stat.add('Default threshold', self._default_thresholds) + stat.add('Default threshold', self._default_threshold) stat.add('Warning threshold', self._thresholds_warning) stat.add('Non fatal threshold', self._thresholds_non_fatal) stat.add('Fatal threshold', self._thresholds_fatal) From ef6cd3467bfddcb3340ae013209b426dfa053c7d Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Wed, 17 Jun 2020 14:29:44 +0200 Subject: [PATCH 09/48] Fix checking for temp thresholds Add warning when no thresholds have been found --- .../diagnostic_analyzers/temperature.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py index f5a5652..038f0cc 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py @@ -49,11 +49,13 @@ def diagnostics(self, stat): stat.add('Fatal threshold', self._thresholds_fatal) stat.add('Current temperature', self._temperature) - if self._temperature < self._thresholds_warning: + if self._default_threshold is None: + stat.summary(DiagnosticStatus.WARN, 'No thresholds found') + elif self._temperature < self._thresholds_warning: stat.summary(DiagnosticStatus.OK, 'OK ({tp}).'.format(tp=self._temperature)) - elif self._thresholds_warning < self._temperature < self._thresholds_non_fatal: + elif self._temperature < self._thresholds_non_fatal: stat.summary(DiagnosticStatus.WARN, 'Temperature almost too high ({tp}).'.format(tp=self._temperature)) else: - stat.summary(DiagnosticStatus.ERROR, 'Temperature to high ({tp}).'.format(tp=self._temperature)) + stat.summary(DiagnosticStatus.ERROR, 'Temperature too high ({tp}).'.format(tp=self._temperature)) return stat From 7f5c2a3a7b77d2f1dcacae5be8351854c5bd6987 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Wed, 17 Jun 2020 16:58:16 +0200 Subject: [PATCH 10/48] Fixed the gait summary pop up menu and replaced the add updater to the constructor. --- march_rqt_robot_monitor/config/analyzers.yaml | 2 +- .../diagnostic_analyzers/gait_state.py | 34 ++++++++++--------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/march_rqt_robot_monitor/config/analyzers.yaml b/march_rqt_robot_monitor/config/analyzers.yaml index 5211a52..460eebf 100644 --- a/march_rqt_robot_monitor/config/analyzers.yaml +++ b/march_rqt_robot_monitor/config/analyzers.yaml @@ -24,5 +24,5 @@ analyzers: gait: type: diagnostic_aggregator/GenericAnalyzer path: Gait - contains: 'gait' + contains: 'Gait' remove_prefix: 'march_rqt_robot_monitor_node: ' diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py index a443c23..464baa3 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py @@ -10,30 +10,32 @@ def __init__(self, updater): :type updater: diagnostic_updater.Updater """ - self._updater = updater - self._goal_sub = rospy.Subscriber('/march/gait/schedule/goal', GaitActionGoal, self._cb_goal) + self._goal_msg = None + + self._updater = updater + self._updater.add('Gait', self._diagnostics) def _cb_goal(self, msg): """Callback for the gait scheduler goal. :param msg: GaitGoal """ - diagnostic = self._diagnostics(msg.goal.name, msg.goal.current_subgait) - self._updater.add('(Sub)gait status', diagnostic) + self._goal_msg = msg - @staticmethod - def _diagnostics(name, subgait): - """Create a diagnostic function corresponding to gait and subgait data. + def _diagnostics(self, stat): + """Create a diagnostic function corresponding to gait and subgait data.""" + if self._goal_msg is None: + stat.add('Topic error', 'No events recorded') + stat.summary(DiagnosticStatus.STALE, 'No gait activity recorded') + return stat - :param name: the gait name - :param subgait: the subgait object from the GaitGoal message - """ - def d(stat): - stat.add('Gait type', subgait.gait_type) - stat.add('Subgait version', subgait.version) - stat.summary(DiagnosticStatus.OK, '{gait}, {subgait}'.format(gait=name, subgait=subgait.name)) + stat.add('Gait', str(self._goal_msg.goal.name)) + stat.add('Subgait', str(self._goal_msg.goal.current_subgait.name)) + stat.add('Gait type', str(self._goal_msg.goal.current_subgait.gait_type)) + stat.add('Subgait version', str(self._goal_msg.goal.current_subgait.version)) - return stat + stat.summary(DiagnosticStatus.OK, 'Gait: {gait}, {subgait}' + .format(gait=str(self._goal_msg.goal.name), subgait=str(self._goal_msg.goal.current_subgait.name))) - return d + return stat From a4d8dc62b511f4804d28a9e21039e4b861c73037 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Thu, 18 Jun 2020 13:13:57 +0200 Subject: [PATCH 11/48] Added an extra subgait dropdown menu since the ramp and door has six subgaits instead of five. --- .../resource/gait_selection.ui | 84 ++++++++++++++----- .../gait_selection_view.py | 2 +- 2 files changed, 66 insertions(+), 20 deletions(-) diff --git a/march_rqt_gait_selection/resource/gait_selection.ui b/march_rqt_gait_selection/resource/gait_selection.ui index 2f38753..955b428 100644 --- a/march_rqt_gait_selection/resource/gait_selection.ui +++ b/march_rqt_gait_selection/resource/gait_selection.ui @@ -7,7 +7,7 @@ 0 0 1138 - 288 + 338 @@ -73,7 +73,7 @@ QGroupBox::title { 1100 - 250 + 300 @@ -94,7 +94,7 @@ QGroupBox::title { 0 0 1098 - 248 + 298 @@ -133,7 +133,7 @@ QGroupBox::title { 280 90 341 - 141 + 181 @@ -155,16 +155,16 @@ QPushButton{color: #FFFFFF} 0 0 339 - 139 + 179 160 - 100 + 120 161 - 21 + 31 @@ -187,9 +187,9 @@ QPushButton{color: #FFFFFF} 160 - 60 + 70 161 - 21 + 31 @@ -214,7 +214,7 @@ QPushButton{color: #FFFFFF} 160 20 161 - 21 + 31 @@ -238,8 +238,8 @@ QPushButton{color: #FFFFFF} 20 20 - 81 - 41 + 91 + 51 @@ -251,9 +251,9 @@ Logger 20 - 80 - 81 - 41 + 100 + 91 + 51 @@ -267,10 +267,10 @@ versions - 120 - 10 + 130 + 20 20 - 121 + 141 @@ -315,7 +315,7 @@ versions 10 50 241 - 181 + 221 @@ -701,6 +701,52 @@ QPushButton{color: #FFFFFF} ? + + + + 810 + 250 + 271 + 25 + + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + true + + + + 630 + 250 + 181 + 21 + + + + + 0 + 0 + + + + Unused + + + Qt::AlignCenter + + diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 0bd0967..33c7afe 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -6,7 +6,7 @@ from .gait_selection_pop_up import PopUpWindow -AMOUNT_OF_AVAILABLE_SUBGAITS = 5 +AMOUNT_OF_AVAILABLE_SUBGAITS = 6 class GaitSelectionView(QWidget): From c05874a082010ec0bdd98ea52b2eb3d0e1491c6c Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Fri, 19 Jun 2020 16:59:31 +0200 Subject: [PATCH 12/48] Created a new UI which the user can scale to a preffered size. This also includes dynamically scaling the dropdown menus of the versions. --- .../resource/gait_selection.ui | 1156 ++++++++--------- 1 file changed, 518 insertions(+), 638 deletions(-) diff --git a/march_rqt_gait_selection/resource/gait_selection.ui b/march_rqt_gait_selection/resource/gait_selection.ui index 955b428..693b701 100644 --- a/march_rqt_gait_selection/resource/gait_selection.ui +++ b/march_rqt_gait_selection/resource/gait_selection.ui @@ -6,16 +6,10 @@ 0 0 - 1138 - 338 + 1437 + 515 - - - 1 - 1 - - 16777215 @@ -37,11 +31,11 @@ QGroupBox::title { padding: 0 3px 0 3px; } - - + + - + 0 0 @@ -65,15 +59,15 @@ QGroupBox::title { true - + 0 0 - 1100 - 300 + 0 + 0 @@ -93,660 +87,546 @@ QGroupBox::title { 0 0 - 1098 - 298 + 1397 + 475 + + + 0 + 0 + + true - - - true - - - - 350 - 50 - 271 - 31 - - - - - 0 - 0 - - - - - Ubuntu Condensed - - - - - - true - - - - 280 - 90 - 341 - 181 - - - - - 0 - 0 - - - - QPushButton{background: #287cbc} -QPushButton{color: #FFFFFF} - - - true - - - - - 0 - 0 - 339 - 179 - - - - - - 160 - 120 - 161 - 31 - - + + + - + 0 0 - - <html><head/><body><p>Save the current selected versions as default</p></body></html> - - - - - - Save as default - + + + + + + 0 + 0 + + + + + true + + + + LOGGER + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + false + + + + 0 + 0 + + + + false + + + #Log { +background-color: #F2F1F0 +} + + + + + + + - - - - 160 - 70 - 161 - 31 - - + + + - + 0 0 - - <html><head/><body><p>Apply the current changes, if another gait is selected the changes will be lost</p></body></html> - - - - - Apply + QPushButton{background: #287cbc} +QPushButton{color: #FFFFFF} + + + + + + 0 + 0 + + + + Apply + + + + + + + + 0 + 0 + + + + Refresh + + + + + + + + 0 + 0 + + + + + true + + + + Actions + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + Save as defaults + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Qt::Horizontal + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + - - - - 160 - 20 - 161 - 31 - - - - - 0 - 0 - - - - <html><head/><body><p>Refresh the gait version map</p></body></html> - - - - - - Refresh + + + + + + Ubuntu Condensed + + + + + + + true + + + + SUBGAITS + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + 200 + 0 + + + + + Ubuntu Light + + + + QFrame::NoFrame + + + QFrame::Plain + + + Unused + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + Ubuntu Condensed + + + + false + + + + + + + + 0 + 0 + + + + + 200 + 0 + + + + + Ubuntu Light + + + + QFrame::NoFrame + + + QFrame::Plain + + + Unused + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + Ubuntu Condensed + + + + + + + + + 0 + 0 + + + + + 200 + 0 + + + + + Ubuntu Light + + + + QFrame::NoFrame + + + QFrame::Plain + + + Unused + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + Ubuntu Condensed + + + + + - - - - 20 - 20 - 91 - 51 - - - - Clear -Logger + + + + + Qt::Vertical - - - - 20 - 100 - 91 - 51 - - - - <html><head/><body><p>Create pop up to display all gaits and subgait versions</p></body></html> - - - See all -versions + + + + + Qt::Vertical - - - - 130 - 20 - 20 - 141 - - + + + Qt::Vertical - - - - - - 630 - 10 - 181 - 20 - - - - - 0 - 0 - - - - - true - - - - SUBGAITS - - - Qt::AlignCenter - - - - - false - - - - 10 - 50 - 241 - 221 - - - - - 0 - 0 - - - - false - - - #Log { -background-color: #F2F1F0 -} - - - - - - - - - 280 - 10 - 341 - 20 - - - - - 0 - 0 - - - - - true - - - - GAIT - - - Qt::AlignCenter - - - - - - 630 - 50 - 181 - 20 - - - - - 0 - 0 - - - - QFrame::NoFrame - - - QFrame::Plain - - - Unused - - - Qt::AlignCenter - - - - - - 630 - 90 - 181 - 20 - - - - - 0 - 0 - - - - Unused - - - Qt::AlignCenter - - - - - - 630 - 130 - 181 - 20 - - - - - 0 - 0 - - - - Unused - - - Qt::AlignCenter - - - - - - 630 - 170 - 181 - 20 - - - - - 0 - 0 - - - - Unused - - - Qt::AlignCenter - - - - - true - - - - 630 - 210 - 181 - 21 - - - - - 0 - 0 - - - - Unused - - - Qt::AlignCenter - - - - - - 820 - 10 - 271 - 20 - - - - - 0 - 0 - - - - - true - - - - VERSIONS - - - Qt::AlignCenter - - - - - - 810 - 50 - 271 - 25 - - - - - 0 - 0 - - - - - Ubuntu Condensed - - - - - - - 810 - 90 - 271 - 25 - - - - - 0 - 0 - - - - - Ubuntu Condensed - - - - - - - 810 - 130 - 271 - 25 - - - - - 0 - 0 - - - - - Ubuntu Condensed - - - - - - - 810 - 170 - 271 - 25 - - - - - 0 - 0 - - - - - Ubuntu Condensed - - - - - - - 810 - 210 - 271 - 25 - - - - - 0 - 0 - - - - - Ubuntu Condensed - - - - - - - 10 - 10 - 261 - 20 - - - - - 0 - 0 - - - - - true - - - - LOGGER - - - Qt::AlignCenter - - - - - - 10 - 30 - 1071 - 20 - - - - Qt::Horizontal - - - - - - 280 - 50 - 61 - 31 - - - - - 0 - 0 - - - - - true - - - - Select : - - - Qt::AlignCenter - - - - - - 10 - 10 - 31 - 25 - - - - <html><head/><body><p>This tool is used to set subgait-versions of a gait when the exoskeleton is running. If a change is made it has to be applied before selecting a new gait, otherwise the changes will be lost.</p></body></html> - - - QPushButton{background: #287cbc} + + + + + + 1 + 0 + + + + QPushButton{background: #287cbc} QPushButton{color: #FFFFFF} - - - ? - - - - - - 810 - 250 - 271 - 25 - - - - - 0 - 0 - - - - - Ubuntu Condensed - - - - - - true - - - - 630 - 250 - 181 - 21 - - - - - 0 - 0 - - - - Unused - - - Qt::AlignCenter - - + + + + QLayout::SetDefaultConstraint + + + + + + 0 + 0 + + + + + true + + + + GAIT + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + All versions + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Qt::Horizontal + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + Clear logger + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + true + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + + + From c3ba86251ae6931acc7cf87ea25d5a9b137dfc44 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Fri, 19 Jun 2020 17:01:53 +0200 Subject: [PATCH 13/48] Created a function which adds more subgait menu's and labels in case necessary. The default amount of subgaits is set to three as the UI has a starting amount of three menu's --- .../gait_selection_view.py | 44 +++++++++++++------ 1 file changed, 31 insertions(+), 13 deletions(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 33c7afe..cfcf894 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -1,12 +1,11 @@ from PyQt5.QtCore import Qt -from PyQt5.QtWidgets import QWidget +from PyQt5.QtWidgets import QComboBox, QLabel, QWidget from python_qt_binding import loadUi -import rospy from .gait_selection_pop_up import PopUpWindow -AMOUNT_OF_AVAILABLE_SUBGAITS = 6 +DEFAULT_AMOUNT_OF_AVAILABLE_SUBGAITS = 3 class GaitSelectionView(QWidget): @@ -31,14 +30,14 @@ def __init__(self, ui_file, controller): self._is_update_active = True # Search for children in GUI - self._gait_content = self.GaitSelectionInterface + self._subgaits = self.SubgaitsFrame self._gait_menu = self.GaitMenu self._logger = self.Log self._subgait_labels = [] self._subgait_menus = [] - for index in range(AMOUNT_OF_AVAILABLE_SUBGAITS): + for index in range(DEFAULT_AMOUNT_OF_AVAILABLE_SUBGAITS): self._subgait_labels.append(getattr(self, 'SubgaitLabel_{nr}'.format(nr=index))) self._subgait_menus.append(getattr(self, 'SubgaitMenu_{nr}'.format(nr=index))) @@ -65,6 +64,26 @@ def __init__(self, ui_file, controller): self._refresh() # gait and subgait related layout functions + def add_subgait_menus(self, amount_of_new_subgait_menus): + """Add subgait labels and dropdown menu's in case a gait has more subgaits then available menu's. + + :param amount_of_new_subgait_menus: the amount of new subgait labels and menu's + """ + for new_subgait in range(amount_of_new_subgait_menus): + + new_subgait_label = QLabel(self) + new_subgait_label.setFont(self._subgait_labels[0].font()) + new_subgait_label.setAlignment(self._subgait_labels[0].alignment()) + + new_subgait_menu = QComboBox(self) + new_subgait_menu.setFont(self._subgait_menus[0].font()) + new_subgait_menu.currentIndexChanged.connect(lambda: self.update_version_colors()) + + self._subgait_labels.append(new_subgait_label) + self._subgait_menus.append(new_subgait_menu) + + self._subgaits.layout().addRow(new_subgait_label, new_subgait_menu) + def update_version_menus(self): """When a gait is selected set the subgait labels and populate the subgait menus with the available versions.""" self._is_update_active = True @@ -76,15 +95,14 @@ def update_version_menus(self): gait_name = self._gait_menu.currentText() subgaits = self.available_gaits[gait_name]['subgaits'] + if len(subgaits) > len(self._subgait_labels): + amount_of_new_subgait_menus = len(subgaits) - len(self._subgait_labels) + self.add_subgait_menus(amount_of_new_subgait_menus) + latest_used_index = 0 for index, (subgait_name, versions) in enumerate(subgaits.items()): - - try: - subgait_menu = self._subgait_menus[index] - subgait_label = self._subgait_labels[index] - except index: - rospy.logfatal('Not enough labels and menus available in GUI, change layout using Qt tool.') - return + subgait_label = self._subgait_labels[index] + subgait_menu = self._subgait_menus[index] subgait_menu.setDisabled(0) subgait_label.setDisabled(0) @@ -106,7 +124,7 @@ def update_version_menus(self): latest_used_index = index + 1 - for unused_index in range(latest_used_index, AMOUNT_OF_AVAILABLE_SUBGAITS): + for unused_index in range(latest_used_index, len(self._subgait_labels)): self._subgait_labels[unused_index].setDisabled(1) self._subgait_menus[unused_index].setDisabled(1) From 22d673605cfbe5221e98fe0794d1e2e7546ea6bc Mon Sep 17 00:00:00 2001 From: gaiavdh Date: Mon, 22 Jun 2020 12:14:39 +0200 Subject: [PATCH 14/48] Add ramp down single step gait to input device. --- .../src/march_rqt_input_device/input_device_view.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py index 9d653eb..94b2be9 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py @@ -159,6 +159,10 @@ def _create_buttons(self): self.create_button('gait_ramp_door_slope_down', image_path='/gait_ramp_door_slope_down.png', callback=lambda: self._controller.publish_gait('gait_ramp_door_slope_down')) + gait_ramp_door_slope_down_single = \ + self.create_button('gait_ramp_door_slope_down_single', + callback=lambda: self._controller.publish_gait('gait_ramp_door_slope_down_single')) + gait_ramp_door_last_step = \ self.create_button('gait_ramp_door_last_step', image_path='/gait_ramp_door_last_step.png', callback=lambda: self._controller.publish_gait('gait_ramp_door_last_step')) @@ -265,7 +269,8 @@ def _create_buttons(self): [gait_rough_terrain_high_step, gait_rough_terrain_middle_steps, gait_rough_terrain_first_middle_step, gait_rough_terrain_second_middle_step, gait_rough_terrain_third_middle_step], - [gait_ramp_door_slope_up, gait_ramp_door_slope_down, gait_ramp_door_last_step], + [gait_ramp_door_slope_up, gait_ramp_door_slope_down, gait_ramp_door_last_step, + gait_ramp_door_slope_down_single], [gait_tilted_path_left_straight_start, gait_tilted_path_left_single_step, gait_tilted_path_left_straight_end, gait_tilted_path_left_flexed_knee_step, From ceeee50b2dbffe438eb435f3eb42dc0c25eecd6f Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Tue, 23 Jun 2020 11:01:26 +0200 Subject: [PATCH 15/48] Implement new gait version service --- .../gait_selection_controller.py | 48 +++++++++---------- .../gait_selection_view.py | 26 +++++----- 2 files changed, 34 insertions(+), 40 deletions(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py index fd13a57..4b07826 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py @@ -1,55 +1,53 @@ - import ast -import sys import rospy +from std_srvs.srv import Trigger -from march_shared_resources.srv import StringTrigger, Trigger +from march_shared_resources.srv import SetGaitVersion class GaitSelectionController(object): def __init__(self): """Base class to communicate with the gait selection node.""" - try: - rospy.wait_for_service('/march/gait_selection/get_version_map', 3) - rospy.wait_for_service('/march/gait_selection/get_directory_structure', 3) - rospy.wait_for_service('/march/gait_selection/set_version_map', 3) - rospy.wait_for_service('/march/gait_selection/update_default_versions', 3) - except rospy.ROSException: - rospy.logerr('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.') - rospy.signal_shutdown('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.') - sys.exit(0) - self._get_version_map = rospy.ServiceProxy('/march/gait_selection/get_version_map', Trigger) self._get_directory_structure = rospy.ServiceProxy('/march/gait_selection/get_directory_structure', Trigger) - self._set_version_map = rospy.ServiceProxy('/march/gait_selection/set_version_map', StringTrigger) + self._set_gait_version = rospy.ServiceProxy('/march/gait_selection/set_gait_version', SetGaitVersion) self._set_default_versions = rospy.ServiceProxy('/march/gait_selection/update_default_versions', Trigger) def get_version_map(self): """Get the gait version map used in the gait selection node.""" try: return dict(ast.literal_eval(self._get_version_map().message)) - except ValueError: - return None + except (ValueError, rospy.ServiceException): + return dict() def get_directory_structure(self): """Get the gait directory of the selected gait_directory in the gait selection node.""" try: return dict(ast.literal_eval(self._get_directory_structure().message)) - except ValueError: - return None + except (ValueError, rospy.ServiceException): + return dict() - def set_version_map(self, gait_version_map): + def set_gait_version(self, gait_name, subgait_names, versions): """Set a new gait version map to use in the gait selection node. - :param gait_version_map: - The new gait version map as dictionary to parse to the gait selection node + :param str gait_name: The name of the gait + :param list(str) subgait_names: Names of subgaits of which to change the version + :param list(str) versions: Names of the versions """ - result = self._set_version_map(str(gait_version_map)) - return result.success + try: + result = self._set_gait_version(gait_name, subgait_names, versions) + if result.message: + rospy.logwarn(result.message) + return result.success + except rospy.ServiceException: + return False def set_default_versions(self): """Save the current gait version map in the gait selection node as a default.""" - result = self._set_default_versions() - return result.success + try: + result = self._set_default_versions() + return result.success + except rospy.ServiceException: + return False diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 33c7afe..c13e7c6 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -74,7 +74,7 @@ def update_version_menus(self): self._clear_gui() gait_name = self._gait_menu.currentText() - subgaits = self.available_gaits[gait_name]['subgaits'] + subgaits = self.available_gaits[gait_name] latest_used_index = 0 for index, (subgait_name, versions) in enumerate(subgaits.items()): @@ -112,18 +112,6 @@ def update_version_menus(self): self._is_update_active = False - def update_selected_versions(self): - """Read the subgait version menus and save the newly selected versions to the local version map.""" - gait_name = self._gait_menu.currentText() - - for subgait_label, subgait_menu in zip(self._subgait_labels, self._subgait_menus): - subgait_name = subgait_label.text() - if subgait_name != 'Unused': - try: - self.version_map[gait_name][subgait_name] = subgait_menu.currentText() - except KeyError: - pass - def update_version_colors(self): """Update the subgait labels with a specific color to represent a change in version.""" if self._is_update_active or self._is_refresh_active: @@ -198,8 +186,16 @@ def _save_default(self): def _apply(self): """Apply newly selected subgait versions to the gait selection node.""" - self.update_selected_versions() - if self._controller.set_version_map(self.version_map): + gait_name = self._gait_menu.currentText() + subgait_names = [] + versions = [] + + for subgait_label, subgait_menu in zip(self._subgait_labels, self._subgait_menus): + if subgait_label.text() != 'Unused': + subgait_names.append(subgait_label.text()) + versions.append(subgait_menu.currentText()) + + if self._controller.set_gait_version(gait_name, subgait_names, versions): self._log('Version change applied', 'success') else: self._log('Version change applied failed', 'failed') From d7f9dd3efe7504cf42b23a9fb4913072f2260f23 Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Tue, 23 Jun 2020 11:41:33 +0200 Subject: [PATCH 16/48] Fix service error handling --- .../gait_selection_controller.py | 24 ++++++---- .../gait_selection_errors.py | 13 +++++ .../gait_selection_view.py | 47 +++++++++++++------ 3 files changed, 59 insertions(+), 25 deletions(-) create mode 100644 march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_errors.py diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py index 4b07826..6181571 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py @@ -5,6 +5,8 @@ from march_shared_resources.srv import SetGaitVersion +from .gait_selection_errors import GaitServiceError, InvalidResponseError + class GaitSelectionController(object): def __init__(self): @@ -19,15 +21,19 @@ def get_version_map(self): """Get the gait version map used in the gait selection node.""" try: return dict(ast.literal_eval(self._get_version_map().message)) - except (ValueError, rospy.ServiceException): - return dict() + except ValueError: + raise InvalidResponseError + except rospy.ServiceException: + raise GaitServiceError def get_directory_structure(self): """Get the gait directory of the selected gait_directory in the gait selection node.""" try: return dict(ast.literal_eval(self._get_directory_structure().message)) - except (ValueError, rospy.ServiceException): - return dict() + except ValueError: + raise InvalidResponseError + except rospy.ServiceException: + raise GaitServiceError def set_gait_version(self, gait_name, subgait_names, versions): """Set a new gait version map to use in the gait selection node. @@ -38,16 +44,14 @@ def set_gait_version(self, gait_name, subgait_names, versions): """ try: result = self._set_gait_version(gait_name, subgait_names, versions) - if result.message: - rospy.logwarn(result.message) - return result.success + return result.success, result.message except rospy.ServiceException: - return False + raise GaitServiceError def set_default_versions(self): """Save the current gait version map in the gait selection node as a default.""" try: result = self._set_default_versions() - return result.success + return result.success, result.message except rospy.ServiceException: - return False + raise GaitServiceError diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_errors.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_errors.py new file mode 100644 index 0000000..5355f79 --- /dev/null +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_errors.py @@ -0,0 +1,13 @@ +class GaitSelectionError(Exception): + def __init__(self, msg): + super(GaitSelectionError, self).__init__(msg) + + +class InvalidResponseError(GaitSelectionError): + def __init__(self): + super(InvalidResponseError, self).__init__('Failed to parse response') + + +class GaitServiceError(GaitSelectionError): + def __init__(self): + super(GaitServiceError, self).__init__('Failed to contact gait selection') diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index c13e7c6..ad7eba7 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -3,6 +3,7 @@ from python_qt_binding import loadUi import rospy +from .gait_selection_errors import GaitSelectionError from .gait_selection_pop_up import PopUpWindow @@ -134,13 +135,17 @@ def _refresh(self): self._is_refresh_active = True self._clear_gui(clear_gait_menu=True) - self.available_gaits = self._controller.get_directory_structure() - self.version_map = self._controller.get_version_map() + try: + self.available_gaits = self._controller.get_directory_structure() + self.version_map = self._controller.get_version_map() - self._gait_menu.addItems(sorted(self.available_gaits.keys())) + self._gait_menu.addItems(sorted(self.available_gaits.keys())) - self._log('Directory data refreshed', 'success') - self._is_refresh_active = False + self._log('Directory data refreshed', 'success') + except GaitSelectionError as e: + self._log(str(e), 'error') + finally: + self._is_refresh_active = False # logger def _log(self, msg, color_tag='info'): @@ -179,10 +184,14 @@ def _clear_gui(self, clear_gait_menu=False): def _save_default(self): """Save the currently selected subgait versions as default values.""" - if self._controller.set_default_versions(): - self._log('Set new default versions', 'success') - else: - self._log('Set new default versions failed', 'error') + try: + success, msg = self._controller.set_default_versions() + if success: + self._log(msg if msg else 'Set new default versions', 'success') + else: + self._log(msg if msg else 'Failed to set default versions', 'error') + except GaitSelectionError as e: + self._log(str(e), 'error') def _apply(self): """Apply newly selected subgait versions to the gait selection node.""" @@ -195,16 +204,24 @@ def _apply(self): subgait_names.append(subgait_label.text()) versions.append(subgait_menu.currentText()) - if self._controller.set_gait_version(gait_name, subgait_names, versions): - self._log('Version change applied', 'success') - else: - self._log('Version change applied failed', 'failed') + try: + success, msg = self._controller.set_gait_version(gait_name, subgait_names, versions) + if success: + self._log(msg if msg else 'Version change applied', 'success') + else: + self._log(msg if msg else 'Version change applied failed', 'failed') + except GaitSelectionError as e: + self._log(str(e), 'error') def _show_version_map_pop_up(self): """Use a pop up window to display all the gait, subgaits and currently used versions.""" - version_map = self._controller.get_version_map() - version_map_string = '' + try: + version_map = self._controller.get_version_map() + except GaitSelectionError as e: + self._log(str(e), 'error') + return + version_map_string = '' for gait_name in sorted(version_map.keys()): version_map_string += '{gait} \n'.format(gait=gait_name) for subgait_name, version in version_map[gait_name].items(): From 46b25895a3c39ec870bad014c80799f879013209 Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Tue, 23 Jun 2020 12:18:37 +0200 Subject: [PATCH 17/48] Disable save button when there is nothing to save --- .../src/march_rqt_note_taker/notes_widget.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py b/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py index 05ebf77..b21fc57 100644 --- a/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py +++ b/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py @@ -12,11 +12,12 @@ def __init__(self, model, ui_file): super(NotesWidget, self).__init__() self._model = model + self._can_save = True loadUi(ui_file, self) - self._model.rowsInserted.connect(self.update_status) - self._model.rowsRemoved.connect(self.update_status) + self._model.rowsInserted.connect(lambda: [self.update_status(), self._set_saved(False)]) + self._model.rowsRemoved.connect(lambda: [self.update_status(), self._set_saved(False)]) self.table_view.setModel(self._model) self.table_view.verticalScrollBar().rangeChanged.connect(self._handle_change_scroll) @@ -66,6 +67,10 @@ def _delete_selected(self): if indices and indices[0].isValid(): self._model.remove_rows(indices[0].row(), len(indices)) + def _set_saved(self, saved): + self._can_save = not saved + self.save_button.setEnabled(not saved) + def _handle_load(self): rospy.logwarn('Loading notes from a file is not yet implemented') @@ -83,3 +88,4 @@ def _handle_save(self): return else: rospy.loginfo('Successfully written to file {0}'.format(file_name)) + self._set_saved(True) From 718096c3f3cdc7426db98a10e9b97d14e0615ee6 Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Tue, 23 Jun 2020 15:06:36 +0200 Subject: [PATCH 18/48] Add autosave checkbox --- march_rqt_note_taker/resource/note_taker.ui | 10 ++++++++++ .../src/march_rqt_note_taker/notes_widget.py | 8 +++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/march_rqt_note_taker/resource/note_taker.ui b/march_rqt_note_taker/resource/note_taker.ui index 644e0b8..8d00366 100644 --- a/march_rqt_note_taker/resource/note_taker.ui +++ b/march_rqt_note_taker/resource/note_taker.ui @@ -55,6 +55,16 @@ + + + + Autosave + + + false + + + diff --git a/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py b/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py index b21fc57..6d27b8c 100644 --- a/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py +++ b/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py @@ -1,4 +1,4 @@ -from python_qt_binding import loadUi +from python_qt_binding import loadUi, QtCore from python_qt_binding.QtGui import QKeySequence from python_qt_binding.QtWidgets import QFileDialog, QShortcut, QWidget import rospy @@ -13,6 +13,7 @@ def __init__(self, model, ui_file): self._model = model self._can_save = True + self._has_autosave = True loadUi(ui_file, self) @@ -29,6 +30,8 @@ def __init__(self, model, ui_file): self.load_button.clicked.connect(self._handle_load) self.save_button.clicked.connect(self._handle_save) + self.autosave_check_box.stateChanged.connect(self._handle_autosave) + self.autosave_check_box.setChecked(self._has_autosave) self._delete_shortcut = QShortcut(QKeySequence('Delete'), self) self._delete_shortcut.activated.connect(self._delete_selected) @@ -89,3 +92,6 @@ def _handle_save(self): else: rospy.loginfo('Successfully written to file {0}'.format(file_name)) self._set_saved(True) + + def _handle_autosave(self, state): + self._has_autosave = state == QtCore.Qt.Checked From 6999ccb596f34c00af86929449ebf342058d028a Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 23 Jun 2020 23:11:51 +0200 Subject: [PATCH 19/48] Added the right alignments for all the subgait labels. --- march_rqt_gait_selection/resource/gait_selection.ui | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/march_rqt_gait_selection/resource/gait_selection.ui b/march_rqt_gait_selection/resource/gait_selection.ui index 693b701..bdf7faa 100644 --- a/march_rqt_gait_selection/resource/gait_selection.ui +++ b/march_rqt_gait_selection/resource/gait_selection.ui @@ -359,7 +359,7 @@ QPushButton{color: #FFFFFF} Unused - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop @@ -404,7 +404,7 @@ QPushButton{color: #FFFFFF} Unused - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop @@ -446,7 +446,7 @@ QPushButton{color: #FFFFFF} Unused - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop From 2f738dd95543c4713736fc4d34d06d5b3889d562 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 23 Jun 2020 23:23:32 +0200 Subject: [PATCH 20/48] Updated the perspective file to better fit the new rqt gait selection format. --- march_monitor/config/full_monitor.perspective | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/march_monitor/config/full_monitor.perspective b/march_monitor/config/full_monitor.perspective index 5e46b78..1939efd 100644 --- a/march_monitor/config/full_monitor.perspective +++ b/march_monitor/config/full_monitor.perspective @@ -9,10 +9,10 @@ } }, "groups": { - "plugin__march_rqt_note_taker__NotesPlugin__1": { + "plugin__march_rqt_gait_selection__GaitSelectionPlugin__1": { "keys": {}, "groups": { - "dock_widget__notes_widget": { + "dock_widget__GaitSelection": { "keys": { "dockable": { "type": "repr", @@ -24,17 +24,17 @@ }, "dock_widget_title": { "type": "repr", - "repr": "u'Note taker'" + "repr": "u'March Gait Selection'" } }, "groups": {} } } }, - "plugin__march_rqt_gait_selection__GaitSelectionPlugin__1": { + "plugin__march_rqt_note_taker__NotesPlugin__1": { "keys": {}, "groups": { - "dock_widget__GaitSelection": { + "dock_widget__notes_widget": { "keys": { "dockable": { "type": "repr", @@ -46,7 +46,7 @@ }, "dock_widget_title": { "type": "repr", - "repr": "u'March Gait Selection'" + "repr": "u'Note taker'" } }, "groups": {} @@ -113,13 +113,13 @@ "keys": { "geometry": { "type": "repr(QByteArray.hex)", - "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb000200000000000000000438000009ff000009d70000000000000456000009ff000009d700000001000000000a00')", - "pretty-print": " 8 V " + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000000000000001b0000077f0000043700000000000000370000077f0000043700000000000000000780')", + "pretty-print": " 7 7 7 " }, "state": { "type": "repr(QByteArray.hex)", - "repr(QByteArray.hex)": "QtCore.QByteArray('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')", - "pretty-print": " rmarch_rqt_input_device__InputDevicePlugin__2__main_widget Lrqt_topic__TopicPlugin__1__TopicWidget r r 8 x V 0__DockWidgetContainer__1 brqt_robot_monitor__RobotMonitor__1__Robot Monitor 6MinimizedDockWidgetsToolbar " + "repr(QByteArray.hex)": "QtCore.QByteArray('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')", + "pretty-print": " rmarch_rqt_input_device__InputDevicePlugin__2__main_widget Lrqt_topic__TopicPlugin__1__TopicWidget x S rmarch_rqt_input_device__InputDevicePlugin__1__main_widget dmarch_rqt_note_taker__NotesPlugin__1__notes_widget ~march_rqt_gait_selection__GaitSelectionPlugin__1__GaitSelection r ~ " } }, "groups": { @@ -135,4 +135,4 @@ } } } -} +} \ No newline at end of file From d8f52dcc1bf8f8faa21c4d29600d8c7c686233c3 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Wed, 24 Jun 2020 12:03:06 +0200 Subject: [PATCH 21/48] Add manual load/reset subgait with version button --- .../resource/gait_selection.ui | 119 +++++++++--------- .../gait_selection_view.py | 1 + 2 files changed, 62 insertions(+), 58 deletions(-) diff --git a/march_rqt_gait_selection/resource/gait_selection.ui b/march_rqt_gait_selection/resource/gait_selection.ui index bdf7faa..7e73272 100644 --- a/march_rqt_gait_selection/resource/gait_selection.ui +++ b/march_rqt_gait_selection/resource/gait_selection.ui @@ -7,7 +7,7 @@ 0 0 1437 - 515 + 634 @@ -88,7 +88,7 @@ QGroupBox::title { 0 0 1397 - 475 + 594 @@ -200,7 +200,7 @@ QPushButton{color: #FFFFFF} - Apply + apply @@ -213,7 +213,7 @@ QPushButton{color: #FFFFFF} - Refresh + refresh @@ -247,7 +247,7 @@ QPushButton{color: #FFFFFF} - Save as defaults + save as defaults @@ -499,7 +499,7 @@ QPushButton{color: #FFFFFF} QLayout::SetDefaultConstraint - + @@ -520,20 +520,7 @@ QPushButton{color: #FFFFFF} - - - - - 0 - 0 - - - - All versions - - - - + Qt::Horizontal @@ -546,27 +533,14 @@ QPushButton{color: #FFFFFF} - - - - Qt::Vertical - - - - 20 - 40 - - - - - + Qt::Horizontal - + Qt::Horizontal @@ -579,8 +553,26 @@ QPushButton{color: #FFFFFF} - - + + + + true + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + 0 @@ -588,41 +580,52 @@ QPushButton{color: #FFFFFF} - Clear logger + all versions - - - - Qt::Horizontal + + + + + 0 + 0 + - - - 40 - 20 - + + clear logger - + - - - - true - + + - + 0 0 - - - Ubuntu Condensed - + + load/reset subgaits + + + + Qt::Vertical + + + QSizePolicy::Expanding + + + + 20 + 40 + + + + diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index cfcf894..f69105f 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -48,6 +48,7 @@ def __init__(self, ui_file, controller): self.ClearLogger.clicked.connect(lambda: self._logger.clear()) self.SeeAllVersions.clicked.connect(lambda: self._show_version_map_pop_up()) + self.LoadSubgaits.clicked.connect(lambda: self.update_version_menus()) self._gait_menu.currentIndexChanged.connect(lambda: self.update_version_menus()) for subgait_menu in self._subgait_menus: From 5cad593add7f8cd9fcc110824bb00043a5f64d41 Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Wed, 24 Jun 2020 17:58:11 +0200 Subject: [PATCH 22/48] Add icons and tooltips --- march_rqt_note_taker/resource/note_taker.ui | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/march_rqt_note_taker/resource/note_taker.ui b/march_rqt_note_taker/resource/note_taker.ui index 8d00366..35744aa 100644 --- a/march_rqt_note_taker/resource/note_taker.ui +++ b/march_rqt_note_taker/resource/note_taker.ui @@ -31,6 +31,10 @@ Load + + + .. + 16 @@ -47,6 +51,10 @@ Save + + + .. + 16 @@ -57,6 +65,9 @@ + + <html><head/><body><p>Automatically saves all actions to the most recently saved file when enabled.</p><p><span style=" font-weight:600;">Can only be enabled once the minutes have been saved to a file.</span></p></body></html> + Autosave @@ -171,6 +182,10 @@ Start take + + + .. + From c4d0582874b01fbca13b6d2b8437cc1648205c6b Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Wed, 24 Jun 2020 18:09:39 +0200 Subject: [PATCH 23/48] Add incremental autosaving --- .../src/march_rqt_note_taker/entry_model.py | 6 ++ .../src/march_rqt_note_taker/notes_widget.py | 76 ++++++++++++++++--- 2 files changed, 70 insertions(+), 12 deletions(-) diff --git a/march_rqt_note_taker/src/march_rqt_note_taker/entry_model.py b/march_rqt_note_taker/src/march_rqt_note_taker/entry_model.py index 790f837..283f4ac 100644 --- a/march_rqt_note_taker/src/march_rqt_note_taker/entry_model.py +++ b/march_rqt_note_taker/src/march_rqt_note_taker/entry_model.py @@ -77,3 +77,9 @@ def insert_log_msg(self, log_msg): def __str__(self): """Returns a string representation of the model.""" return '\n'.join(str(x) for x in self._entries) + + def __len__(self): + return len(self._entries) + + def __getitem__(self, index): + return self._entries[index] diff --git a/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py b/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py index 6d27b8c..cfd80a2 100644 --- a/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py +++ b/march_rqt_note_taker/src/march_rqt_note_taker/notes_widget.py @@ -1,3 +1,5 @@ +import os + from python_qt_binding import loadUi, QtCore from python_qt_binding.QtGui import QKeySequence from python_qt_binding.QtWidgets import QFileDialog, QShortcut, QWidget @@ -7,6 +9,8 @@ class NotesWidget(QWidget): + INSERT = 0 + REMOVE = 1 def __init__(self, model, ui_file): super(NotesWidget, self).__init__() @@ -14,11 +18,17 @@ def __init__(self, model, ui_file): self._model = model self._can_save = True self._has_autosave = True + self._autosave_file = None + self._last_save_file = None loadUi(ui_file, self) - self._model.rowsInserted.connect(lambda: [self.update_status(), self._set_saved(False)]) - self._model.rowsRemoved.connect(lambda: [self.update_status(), self._set_saved(False)]) + self._model.rowsInserted.connect( + lambda parent, first, last: [self.update_status(), self._set_saved(False), + self._autosave(first, last, self.INSERT)]) + self._model.rowsRemoved.connect( + lambda parent, first, last: [self.update_status(), self._set_saved(False), + self._autosave(first, last, self.REMOVE)]) self.table_view.setModel(self._model) self.table_view.verticalScrollBar().rangeChanged.connect(self._handle_change_scroll) @@ -32,6 +42,7 @@ def __init__(self, model, ui_file): self.save_button.clicked.connect(self._handle_save) self.autosave_check_box.stateChanged.connect(self._handle_autosave) self.autosave_check_box.setChecked(self._has_autosave) + self.autosave_check_box.setEnabled(False) self._delete_shortcut = QShortcut(QKeySequence('Delete'), self) self._delete_shortcut.activated.connect(self._delete_selected) @@ -50,7 +61,7 @@ def _handle_insert_entry(self): self.table_view.verticalScrollBar().setSliderPosition(self._last_scroll_max) def update_status(self): - self.messages_label.setText('Displaying {0} messages'.format(self._model.rowCount())) + self.messages_label.setText('Displaying {0} messages'.format(len(self._model))) def _handle_start_take(self): take = self.camera_spin_box.value() @@ -81,17 +92,58 @@ def _handle_save(self): result = QFileDialog.getSaveFileName(self, 'Save File', '.', 'Minute files (*.txt)') file_name = result[0] if file_name: - if file_name[-4:] != '.txt': - file_name += '.txt' + self._save(file_name) + + def _handle_autosave(self, state): + self._has_autosave = state == QtCore.Qt.Checked + if not self._has_autosave and self._autosave_file is not None: + self._autosave_file.close() + elif self._has_autosave and self._can_save and self._last_save_file: + self._save(self._last_save_file) + + def _autosave(self, first, last, action): + """Writes the last changes incrementally to the autosave file. + + :param int first: The first index of affected entries in the model + :param int last: the last index of affected entries in the model (inclusive) + :param int action: Either INSERT or REMOVE + """ + if self._has_autosave and self._last_save_file is not None: + if self._autosave_file is None or self._autosave_file.closed: + try: + self._autosave_file = open(self._last_save_file, 'r+') + self._autosave_file.seek(0, os.SEEK_END) + except IOError as e: + rospy.logerr('Failed to open file {f} for autosaving: {e}'.format(f=self._last_save_file, e=e)) + return + try: - with open(file_name, 'w') as f: - f.write(str(self._model)) + if action == self.INSERT: + if first != 0: + self._autosave_file.write('\n') + self._autosave_file.write('\n'.join(str(entry) for entry in self._model[first:last + 1])) + elif action == self.REMOVE: + self._autosave_file.seek(0, os.SEEK_SET) + self._autosave_file.write(str(self._model)) + self._autosave_file.truncate() + else: + rospy.logwarn('Unknown autosave action: {a}'.format(a=action)) + return + self._autosave_file.flush() except IOError as e: - rospy.logwarn('Failed to open file: {0}'.format(e)) - return + rospy.logerr('Failed to write to file {f} for autosaving: {e}'.format(f=self._last_save_file, e=e)) else: - rospy.loginfo('Successfully written to file {0}'.format(file_name)) self._set_saved(True) - def _handle_autosave(self, state): - self._has_autosave = state == QtCore.Qt.Checked + def _save(self, file_name): + if file_name[-4:] != '.txt': + file_name += '.txt' + try: + with open(file_name, 'w') as f: + f.write(str(self._model)) + except IOError as e: + rospy.logerr('Failed to open file: {e}'.format(e=e)) + else: + self._set_saved(True) + self._last_save_file = file_name + self.autosave_check_box.setEnabled(True) From 84b564ae837c08a654265a373f7833d9ffdb6c0b Mon Sep 17 00:00:00 2001 From: Olav de Haas Date: Thu, 25 Jun 2020 10:48:39 +0200 Subject: [PATCH 24/48] Use std_srvs package --- march_rqt_note_taker/src/march_rqt_note_taker/notes_plugin.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/march_rqt_note_taker/src/march_rqt_note_taker/notes_plugin.py b/march_rqt_note_taker/src/march_rqt_note_taker/notes_plugin.py index 496d40e..88c821a 100644 --- a/march_rqt_note_taker/src/march_rqt_note_taker/notes_plugin.py +++ b/march_rqt_note_taker/src/march_rqt_note_taker/notes_plugin.py @@ -6,8 +6,7 @@ import rospkg import rospy from std_msgs.msg import String - -from march_shared_resources.srv import Trigger +from std_srvs.srv import Trigger from .entry import Entry from .entry_model import EntryModel From 599062c2f98e3f56aab634c553d412f3c20e3a7a Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Thu, 25 Jun 2020 11:04:02 +0200 Subject: [PATCH 25/48] Hide unused menu's and labels instead of disabling --- .../src/march_rqt_gait_selection/gait_selection_view.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 9cea055..654c8e7 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -102,6 +102,7 @@ def update_version_menus(self): self.add_subgait_menus(amount_of_new_subgait_menus) latest_used_index = 0 + for index, (subgait_name, versions) in enumerate(subgaits.items()): subgait_label = self._subgait_labels[index] subgait_menu = self._subgait_menus[index] @@ -109,6 +110,9 @@ def update_version_menus(self): subgait_menu.setDisabled(0) subgait_label.setDisabled(0) + subgait_menu.show() + subgait_label.show() + subgait_label.setText(subgait_name) subgait_menu.addItems(versions) @@ -130,6 +134,9 @@ def update_version_menus(self): self._subgait_labels[unused_index].setDisabled(1) self._subgait_menus[unused_index].setDisabled(1) + self._subgait_labels[unused_index].hide() + self._subgait_menus[unused_index].hide() + self._is_update_active = False def update_version_colors(self): From 6c34ff975f70de20b4a71e8ac184f0b88a511812 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Thu, 25 Jun 2020 11:06:06 +0200 Subject: [PATCH 26/48] Remove the setDisable because these are redundant when using the hide/show. --- .../src/march_rqt_gait_selection/gait_selection_view.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 654c8e7..dad6220 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -107,9 +107,6 @@ def update_version_menus(self): subgait_label = self._subgait_labels[index] subgait_menu = self._subgait_menus[index] - subgait_menu.setDisabled(0) - subgait_label.setDisabled(0) - subgait_menu.show() subgait_label.show() @@ -131,9 +128,6 @@ def update_version_menus(self): latest_used_index = index + 1 for unused_index in range(latest_used_index, len(self._subgait_labels)): - self._subgait_labels[unused_index].setDisabled(1) - self._subgait_menus[unused_index].setDisabled(1) - self._subgait_labels[unused_index].hide() self._subgait_menus[unused_index].hide() From 3c64873e4313eace68d8885345bf3b35a1dced17 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Thu, 25 Jun 2020 11:13:29 +0200 Subject: [PATCH 27/48] Reduced used space of the subgait name labels. --- march_rqt_gait_selection/resource/gait_selection.ui | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/march_rqt_gait_selection/resource/gait_selection.ui b/march_rqt_gait_selection/resource/gait_selection.ui index 7e73272..d14c0ee 100644 --- a/march_rqt_gait_selection/resource/gait_selection.ui +++ b/march_rqt_gait_selection/resource/gait_selection.ui @@ -340,7 +340,7 @@ QPushButton{color: #FFFFFF} - 200 + 150 0 @@ -385,7 +385,7 @@ QPushButton{color: #FFFFFF} - 200 + 150 0 @@ -427,7 +427,7 @@ QPushButton{color: #FFFFFF} - 200 + 150 0 From 8c6d427f1cab85a5ad3e8c71a8e771648e19dc10 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Thu, 25 Jun 2020 11:44:34 +0200 Subject: [PATCH 28/48] Comment temperature monitoring --- march_rqt_robot_monitor/config/analyzers.yaml | 10 +-- .../src/march_rqt_robot_monitor/updater.py | 67 ++++++++++--------- 2 files changed, 39 insertions(+), 38 deletions(-) diff --git a/march_rqt_robot_monitor/config/analyzers.yaml b/march_rqt_robot_monitor/config/analyzers.yaml index 460eebf..38723b6 100644 --- a/march_rqt_robot_monitor/config/analyzers.yaml +++ b/march_rqt_robot_monitor/config/analyzers.yaml @@ -1,11 +1,11 @@ pub_rate: 1.0 base_path: '' analyzers: - temperatures: - type: diagnostic_aggregator/GenericAnalyzer - path: Temperatures - contains: 'Temperature' - remove_prefix: 'march_rqt_robot_monitor_node: ' +# temperatures: +# type: diagnostic_aggregator/GenericAnalyzer +# path: Temperatures +# contains: 'Temperature' +# remove_prefix: 'march_rqt_robot_monitor_node: ' inputs: type: diagnostic_aggregator/GenericAnalyzer path: Inputs diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py index 22d84bc..922ea7f 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -1,6 +1,7 @@ import diagnostic_updater import rospy -from sensor_msgs.msg import JointState, Temperature +from sensor_msgs.msg import JointState +# from sensor_msgs.msg import Temperature from march_shared_resources.msg import Alive @@ -8,7 +9,7 @@ from .diagnostic_analyzers.control import CheckJointValues from .diagnostic_analyzers.gait_state import CheckGaitStatus from .diagnostic_analyzers.imc_state import CheckImcStatus -from .diagnostic_analyzers.temperature import CheckJointTemperature +# from .diagnostic_analyzers.temperature import CheckJointTemperature def main(): @@ -21,37 +22,37 @@ def main(): CheckInputDevice('/march/input_device/alive', Alive, updater, 5) # Temperature checks - check_temp_joint_left_ankle = \ - CheckJointTemperature('left_ankle', '/march/temperature/left_ankle', Temperature) - updater.add('Temperature left ankle', check_temp_joint_left_ankle.diagnostics) - - check_temp_joint_left_knee = \ - CheckJointTemperature('left_knee', '/march/temperature/left_knee', Temperature) - updater.add('Temperature left knee', check_temp_joint_left_knee.diagnostics) - - check_temp_joint_left_hip_fe = \ - CheckJointTemperature('left_hip_fe', '/march/temperature/left_hip_fe', Temperature) - updater.add('Temperature left hip FE', check_temp_joint_left_hip_fe.diagnostics) - - check_temp_joint_left_hip_aa = \ - CheckJointTemperature('left_hip_aa', '/march/temperature/left_hip_aa', Temperature) - updater.add('Temperature left hip AA', check_temp_joint_left_hip_aa.diagnostics) - - check_temp_joint_right_ankle = \ - CheckJointTemperature('right_ankle', '/march/temperature/right_ankle', Temperature) - updater.add('Temperature right ankle', check_temp_joint_right_ankle.diagnostics) - - check_temp_joint_right_knee = \ - CheckJointTemperature('right_knee', '/march/temperature/right_knee', Temperature) - updater.add('Temperature right knee', check_temp_joint_right_knee.diagnostics) - - check_temp_joint_right_hip_fe = \ - CheckJointTemperature('right_hip_fe', '/march/temperature/right_hip_fe', Temperature) - updater.add('Temperature right hip FE', check_temp_joint_right_hip_fe.diagnostics) - - check_temp_joint_right_hip_aa = \ - CheckJointTemperature('right_hip_aa', '/march/temperature/right_hip_aa', Temperature) - updater.add('Temperature right hip AA', check_temp_joint_right_hip_aa.diagnostics) + # check_temp_joint_left_ankle = \ + # CheckJointTemperature('left_ankle', '/march/temperature/left_ankle', Temperature) + # updater.add('Temperature left ankle', check_temp_joint_left_ankle.diagnostics) + # + # check_temp_joint_left_knee = \ + # CheckJointTemperature('left_knee', '/march/temperature/left_knee', Temperature) + # updater.add('Temperature left knee', check_temp_joint_left_knee.diagnostics) + # + # check_temp_joint_left_hip_fe = \ + # CheckJointTemperature('left_hip_fe', '/march/temperature/left_hip_fe', Temperature) + # updater.add('Temperature left hip FE', check_temp_joint_left_hip_fe.diagnostics) + # + # check_temp_joint_left_hip_aa = \ + # CheckJointTemperature('left_hip_aa', '/march/temperature/left_hip_aa', Temperature) + # updater.add('Temperature left hip AA', check_temp_joint_left_hip_aa.diagnostics) + # + # check_temp_joint_right_ankle = \ + # CheckJointTemperature('right_ankle', '/march/temperature/right_ankle', Temperature) + # updater.add('Temperature right ankle', check_temp_joint_right_ankle.diagnostics) + # + # check_temp_joint_right_knee = \ + # CheckJointTemperature('right_knee', '/march/temperature/right_knee', Temperature) + # updater.add('Temperature right knee', check_temp_joint_right_knee.diagnostics) + # + # check_temp_joint_right_hip_fe = \ + # CheckJointTemperature('right_hip_fe', '/march/temperature/right_hip_fe', Temperature) + # updater.add('Temperature right hip FE', check_temp_joint_right_hip_fe.diagnostics) + # + # check_temp_joint_right_hip_aa = \ + # CheckJointTemperature('right_hip_aa', '/march/temperature/right_hip_aa', Temperature) + # updater.add('Temperature right hip AA', check_temp_joint_right_hip_aa.diagnostics) # control checks check_current_movement_values = CheckJointValues('march/joint_states', JointState) From 0629815d849ec8d43a560105406f30a95e18ad3f Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Thu, 25 Jun 2020 12:42:24 +0200 Subject: [PATCH 29/48] Removed the temperature monitoring stuff instead of commenting. --- march_rqt_robot_monitor/config/analyzers.yaml | 5 --- .../src/march_rqt_robot_monitor/updater.py | 37 +------------------ 2 files changed, 2 insertions(+), 40 deletions(-) diff --git a/march_rqt_robot_monitor/config/analyzers.yaml b/march_rqt_robot_monitor/config/analyzers.yaml index 38723b6..3e82019 100644 --- a/march_rqt_robot_monitor/config/analyzers.yaml +++ b/march_rqt_robot_monitor/config/analyzers.yaml @@ -1,11 +1,6 @@ pub_rate: 1.0 base_path: '' analyzers: -# temperatures: -# type: diagnostic_aggregator/GenericAnalyzer -# path: Temperatures -# contains: 'Temperature' -# remove_prefix: 'march_rqt_robot_monitor_node: ' inputs: type: diagnostic_aggregator/GenericAnalyzer path: Inputs diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py index 922ea7f..a38de0d 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -1,7 +1,7 @@ import diagnostic_updater import rospy from sensor_msgs.msg import JointState -# from sensor_msgs.msg import Temperature +from sensor_msgs.msg import Temperature from march_shared_resources.msg import Alive @@ -9,7 +9,7 @@ from .diagnostic_analyzers.control import CheckJointValues from .diagnostic_analyzers.gait_state import CheckGaitStatus from .diagnostic_analyzers.imc_state import CheckImcStatus -# from .diagnostic_analyzers.temperature import CheckJointTemperature +from .diagnostic_analyzers.temperature import CheckJointTemperature def main(): @@ -21,39 +21,6 @@ def main(): # Frequency checks CheckInputDevice('/march/input_device/alive', Alive, updater, 5) - # Temperature checks - # check_temp_joint_left_ankle = \ - # CheckJointTemperature('left_ankle', '/march/temperature/left_ankle', Temperature) - # updater.add('Temperature left ankle', check_temp_joint_left_ankle.diagnostics) - # - # check_temp_joint_left_knee = \ - # CheckJointTemperature('left_knee', '/march/temperature/left_knee', Temperature) - # updater.add('Temperature left knee', check_temp_joint_left_knee.diagnostics) - # - # check_temp_joint_left_hip_fe = \ - # CheckJointTemperature('left_hip_fe', '/march/temperature/left_hip_fe', Temperature) - # updater.add('Temperature left hip FE', check_temp_joint_left_hip_fe.diagnostics) - # - # check_temp_joint_left_hip_aa = \ - # CheckJointTemperature('left_hip_aa', '/march/temperature/left_hip_aa', Temperature) - # updater.add('Temperature left hip AA', check_temp_joint_left_hip_aa.diagnostics) - # - # check_temp_joint_right_ankle = \ - # CheckJointTemperature('right_ankle', '/march/temperature/right_ankle', Temperature) - # updater.add('Temperature right ankle', check_temp_joint_right_ankle.diagnostics) - # - # check_temp_joint_right_knee = \ - # CheckJointTemperature('right_knee', '/march/temperature/right_knee', Temperature) - # updater.add('Temperature right knee', check_temp_joint_right_knee.diagnostics) - # - # check_temp_joint_right_hip_fe = \ - # CheckJointTemperature('right_hip_fe', '/march/temperature/right_hip_fe', Temperature) - # updater.add('Temperature right hip FE', check_temp_joint_right_hip_fe.diagnostics) - # - # check_temp_joint_right_hip_aa = \ - # CheckJointTemperature('right_hip_aa', '/march/temperature/right_hip_aa', Temperature) - # updater.add('Temperature right hip AA', check_temp_joint_right_hip_aa.diagnostics) - # control checks check_current_movement_values = CheckJointValues('march/joint_states', JointState) updater.add('Control position values', check_current_movement_values.position_diagnostics) From 805ab801b1844e9cfbde62d0f747f7812233ce52 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Thu, 25 Jun 2020 12:43:18 +0200 Subject: [PATCH 30/48] Removed unused imports in updater. --- march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py index a38de0d..a196ce0 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -1,7 +1,6 @@ import diagnostic_updater import rospy from sensor_msgs.msg import JointState -from sensor_msgs.msg import Temperature from march_shared_resources.msg import Alive @@ -9,7 +8,6 @@ from .diagnostic_analyzers.control import CheckJointValues from .diagnostic_analyzers.gait_state import CheckGaitStatus from .diagnostic_analyzers.imc_state import CheckImcStatus -from .diagnostic_analyzers.temperature import CheckJointTemperature def main(): From 2b32488f8a485fffeb3955d15620dcae62661273 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 30 Jun 2020 15:23:58 +0200 Subject: [PATCH 31/48] Created a generator to extract the version number of the .subgait file is present. This was necessary because for some weird reason numbers like 10, 11, 12 etc. were scaled after the 1. --- .../gait_selection_view.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index dad6220..1337fdc 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -88,6 +88,19 @@ def add_subgait_menus(self, amount_of_new_subgait_menus): def update_version_menus(self): """When a gait is selected set the subgait labels and populate the subgait menus with the available versions.""" + + def version_sorter(version): + """Used in the sort function to sort numbers which are 9< + + :param version: str of the version + """ + try: + length = len(version) + version_number = ''.join(letter for letter in version[length-2: length] if letter.isdigit()) + return int(version_number) + except ValueError: + return version + self._is_update_active = True if self._is_refresh_active: return @@ -111,7 +124,7 @@ def update_version_menus(self): subgait_label.show() subgait_label.setText(subgait_name) - subgait_menu.addItems(versions) + subgait_menu.addItems(sorted(versions, key=version_sorter)) try: current_version = self.version_map[gait_name][subgait_name] From 5d5039829e881352e8d0b36f3572c36e0e87010c Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 30 Jun 2020 15:25:18 +0200 Subject: [PATCH 32/48] Fixed indenting of the nested function. --- .../src/march_rqt_gait_selection/gait_selection_view.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 1337fdc..5665d37 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -88,15 +88,14 @@ def add_subgait_menus(self, amount_of_new_subgait_menus): def update_version_menus(self): """When a gait is selected set the subgait labels and populate the subgait menus with the available versions.""" - def version_sorter(version): - """Used in the sort function to sort numbers which are 9< + """Used in the sort function to sort numbers which are 9<. :param version: str of the version """ try: length = len(version) - version_number = ''.join(letter for letter in version[length-2: length] if letter.isdigit()) + version_number = ''.join(letter for letter in version[length - 2: length] if letter.isdigit()) return int(version_number) except ValueError: return version From 9157ae68a3684bd1bce7280bc81a4642fe06aef1 Mon Sep 17 00:00:00 2001 From: JorisWeeda <45165354+JorisWeeda@users.noreply.github.com> Date: Tue, 30 Jun 2020 15:46:52 +0200 Subject: [PATCH 33/48] Changed letter to char (from character) letter.isdigit() is equally confusing so it seems way better to use char --- .../src/march_rqt_gait_selection/gait_selection_view.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 5665d37..c007f7b 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -95,7 +95,7 @@ def version_sorter(version): """ try: length = len(version) - version_number = ''.join(letter for letter in version[length - 2: length] if letter.isdigit()) + version_number = ''.join(char for char in version[length - 2: length] if char.isdigit()) return int(version_number) except ValueError: return version From 09f78ab2fe4529c286954c9e5ee642d8ad18b2a1 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Fri, 3 Jul 2020 14:19:38 +0200 Subject: [PATCH 34/48] The current subgait version index was derived from the old version list, this is now fixed. --- .../src/march_rqt_gait_selection/gait_selection_view.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index c007f7b..d2bea84 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -122,8 +122,10 @@ def version_sorter(version): subgait_menu.show() subgait_label.show() + versions = sorted(versions, key=version_sorter) + subgait_label.setText(subgait_name) - subgait_menu.addItems(sorted(versions, key=version_sorter)) + subgait_menu.addItems(versions) try: current_version = self.version_map[gait_name][subgait_name] From 2ccbf4e3b81b4a9ccb058ece618d611e2be94511 Mon Sep 17 00:00:00 2001 From: Olav de Haas <23523462+Olavhaasie@users.noreply.github.com> Date: Tue, 14 Jul 2020 15:47:24 +0200 Subject: [PATCH 35/48] Fix gait goal variables --- .../diagnostic_analyzers/gait_state.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py index 464baa3..0c5f619 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/gait_state.py @@ -30,12 +30,12 @@ def _diagnostics(self, stat): stat.summary(DiagnosticStatus.STALE, 'No gait activity recorded') return stat - stat.add('Gait', str(self._goal_msg.goal.name)) - stat.add('Subgait', str(self._goal_msg.goal.current_subgait.name)) - stat.add('Gait type', str(self._goal_msg.goal.current_subgait.gait_type)) - stat.add('Subgait version', str(self._goal_msg.goal.current_subgait.version)) + stat.add('Gait', str(self._goal_msg.goal.gait_name)) + stat.add('Subgait', str(self._goal_msg.goal.subgait_name)) + stat.add('Gait type', str(self._goal_msg.goal.gait_type)) + stat.add('Subgait version', str(self._goal_msg.goal.version)) stat.summary(DiagnosticStatus.OK, 'Gait: {gait}, {subgait}' - .format(gait=str(self._goal_msg.goal.name), subgait=str(self._goal_msg.goal.current_subgait.name))) + .format(gait=str(self._goal_msg.goal.gait_name), subgait=str(self._goal_msg.goal.subgait_name))) return stat From ba5a74fb6dd3049207a8eb5c756ba002c97735ef Mon Sep 17 00:00:00 2001 From: marnixbrands Date: Wed, 15 Jul 2020 13:51:47 +0200 Subject: [PATCH 36/48] Add fixed steps ramp down button to ipd --- .../src/march_rqt_input_device/input_device_view.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py index 94b2be9..18dbee8 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py @@ -159,6 +159,10 @@ def _create_buttons(self): self.create_button('gait_ramp_door_slope_down', image_path='/gait_ramp_door_slope_down.png', callback=lambda: self._controller.publish_gait('gait_ramp_door_slope_down')) + gait_ramp_door_slope_down_fixed = \ + self.create_button('gait_ramp_door_slope_down_fixed', + callback=lambda: self._controller.publish_gait('gait_ramp_door_slope_down_fixed')) + gait_ramp_door_slope_down_single = \ self.create_button('gait_ramp_door_slope_down_single', callback=lambda: self._controller.publish_gait('gait_ramp_door_slope_down_single')) @@ -269,8 +273,8 @@ def _create_buttons(self): [gait_rough_terrain_high_step, gait_rough_terrain_middle_steps, gait_rough_terrain_first_middle_step, gait_rough_terrain_second_middle_step, gait_rough_terrain_third_middle_step], - [gait_ramp_door_slope_up, gait_ramp_door_slope_down, gait_ramp_door_last_step, - gait_ramp_door_slope_down_single], + [gait_ramp_door_slope_up, gait_ramp_door_slope_down, gait_ramp_door_slope_down_fixed, + gait_ramp_door_last_step, gait_ramp_door_slope_down_single], [gait_tilted_path_left_straight_start, gait_tilted_path_left_single_step, gait_tilted_path_left_straight_end, gait_tilted_path_left_flexed_knee_step, From 4c56332a28fab4c403beec371026f5435081ef2a Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Tue, 21 Jul 2020 09:36:02 +0200 Subject: [PATCH 37/48] add parametric to version list --- .../march_rqt_gait_selection/gait_selection_view.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index d2bea84..b69159d 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -53,7 +53,7 @@ def __init__(self, ui_file, controller): self._gait_menu.currentIndexChanged.connect(lambda: self.update_version_menus()) for subgait_menu in self._subgait_menus: - subgait_menu.currentIndexChanged.connect(lambda: self.update_version_colors()) + subgait_menu.currentIndexChanged.connect(lambda: self.update_version()) # loaded gaits from gait selection node self.available_gaits = {} @@ -79,7 +79,7 @@ def add_subgait_menus(self, amount_of_new_subgait_menus): new_subgait_menu = QComboBox(self) new_subgait_menu.setFont(self._subgait_menus[0].font()) - new_subgait_menu.currentIndexChanged.connect(lambda: self.update_version_colors()) + new_subgait_menu.currentIndexChanged.connect(lambda: self.update_version()) self._subgait_labels.append(new_subgait_label) self._subgait_menus.append(new_subgait_menu) @@ -123,6 +123,7 @@ def version_sorter(version): subgait_label.show() versions = sorted(versions, key=version_sorter) + versions.append('parametric') subgait_label.setText(subgait_name) subgait_menu.addItems(versions) @@ -147,7 +148,7 @@ def version_sorter(version): self._is_update_active = False - def update_version_colors(self): + def update_version(self): """Update the subgait labels with a specific color to represent a change in version.""" if self._is_update_active or self._is_refresh_active: return @@ -157,6 +158,9 @@ def update_version_colors(self): subgait_name = subgait_label.text() if subgait_name != 'Unused': try: + if 'parametric' == str(subgait_menu.currentText()): + # make a pop up menu to select the parametric gait _version1_version2_param + print('here') if str(self.version_map[gait_name][subgait_name]) != str(subgait_menu.currentText()): subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) else: From 3cca8cbb86fe1ca82c7f35f5c126342e4ba137da Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Tue, 21 Jul 2020 12:30:27 +0200 Subject: [PATCH 38/48] create parametric subgaits pop up menu --- .../gait_selection_view.py | 21 +++- .../parametric_pop_up.py | 110 ++++++++++++++++++ 2 files changed, 128 insertions(+), 3 deletions(-) create mode 100644 march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index b69159d..06e94c0 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -4,6 +4,7 @@ from .gait_selection_errors import GaitSelectionError from .gait_selection_pop_up import PopUpWindow +from .parametric_pop_up import ParametricPopUpWindow DEFAULT_AMOUNT_OF_AVAILABLE_SUBGAITS = 3 @@ -62,6 +63,8 @@ def __init__(self, ui_file, controller): # pop up window self._version_map_pop_up = PopUpWindow(self) + self._parametric_pop_up = ParametricPopUpWindow(self) + # populate gait menu for the first time self._refresh() @@ -123,10 +126,10 @@ def version_sorter(version): subgait_label.show() versions = sorted(versions, key=version_sorter) - versions.append('parametric') subgait_label.setText(subgait_name) subgait_menu.addItems(versions) + subgait_menu.addItem('parametric') try: current_version = self.version_map[gait_name][subgait_name] @@ -159,8 +162,12 @@ def update_version(self): if subgait_name != 'Unused': try: if 'parametric' == str(subgait_menu.currentText()): - # make a pop up menu to select the parametric gait _version1_version2_param - print('here') + versions = self.available_gaits[gait_name][subgait_name] + self._parametric_pop_up.show_pop_up(versions) + new_version = self.get_parametric_version() + subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) + subgait_menu.addItem(new_version) + subgait_menu.setCurrentIndex(subgait_menu.count() - 1) if str(self.version_map[gait_name][subgait_name]) != str(subgait_menu.currentText()): subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) else: @@ -267,3 +274,11 @@ def _show_version_map_pop_up(self): version_map_string += '\n' self._version_map_pop_up.show_message(version_map_string) + + def _show_parametric_pop_up(self): + """Use a pop up window to get the base version, other version and parameter for a parametric subgait.""" + self._parametric_pop_up.show_pop_up() + + def get_parametric_version(self): + return '%{0}_({1})_({2})'.format(self._parametric_pop_up.parameter, self._parametric_pop_up.base_version, + self._parametric_pop_up.other_version) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py new file mode 100644 index 0000000..8a5bbd0 --- /dev/null +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py @@ -0,0 +1,110 @@ + +from PyQt5.QtCore import Qt +from PyQt5.QtWidgets import QDialog, QComboBox, QDialogButtonBox, QGridLayout, QFormLayout, QLabel, QLineEdit,QPushButton, QScrollArea, QSizePolicy, QWidget + + +class ParametricPopUpWindow(QDialog): + def __init__(self, parent, width=500, height=600): + """Base class for creating a pop up window over an existing widget. + + :param parent: + The parent widget to connect to the pop up + :param width: + Starting width of the the pop up widget + :param height: + Starting height of the the pop up widget + """ + super(ParametricPopUpWindow, self).__init__(parent=parent, flags=Qt.Window) + self.resize(width, height) + # self.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) + + self._layout = QGridLayout(self) + + # self._scroll_area = QScrollArea() + # self._scroll_area.setVerticalScrollBarPolicy(Qt.ScrollBarAsNeeded) + # self._scroll_area.setHorizontalScrollBarPolicy(Qt.ScrollBarAsNeeded) + # self._scroll_area.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) + # self._scroll_area.setWidgetResizable(True) + # + # self._content_frame = QWidget(self._scroll_area, flags=Qt.Window) + # self._content_frame.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) + # self._content = QGridLayout(self._content_frame) + # + # self.msg_label = QLabel(self) + # self._content.addWidget(self.msg_label) + # + # self._scroll_area.setWidget(self._content_frame) + # self._layout.addWidget(self._scroll_area) + self.setWindowTitle('test') + self._form_layout = QFormLayout() + self._parameter_line_edit = QLineEdit('parameter here please', self) + self._layout.addWidget(self._parameter_line_edit) + self._layout.addItem(self._form_layout) + self._apply_button = QDialogButtonBox.Ok + self._cancel_button = QDialogButtonBox.Close + self._button_box = QDialogButtonBox(self) + self._layout.addWidget(self._button_box) + self._button_box.addButton(self._apply_button) + self._button_box.addButton(self._cancel_button) + self._button_box.accepted.connect(self.apply) + self._button_box.rejected.connect(self.cancel) + + self._button_box.centerButtons() + versions = ['1', '2', '3'] + base_subgait_label = QLabel('base subgait', self) + + self._base_subgait_menu = QComboBox(self) + self._base_subgait_menu.currentIndexChanged.connect(self.base_subgait_changed) + self._base_subgait_menu.addItems(versions) + + other_subgait_label = QLabel('other subgait', self) + + self._other_subgait_menu = QComboBox(self) + self._other_subgait_menu.currentIndexChanged.connect(self.base_subgait_changed) + self._other_subgait_menu.addItems(versions) + + # self._layout.addItem(base_subgait_label, 1, 1) + self._form_layout.addRow(base_subgait_label, self._base_subgait_menu) + + self._form_layout.addRow(other_subgait_label, self._other_subgait_menu) + + self.base_version = '' + self.other_version = '' + self.parameter = 0.0 + + + def show_pop_up(self, versions): + """Add message to the pop up and show the window.""" + self._base_subgait_menu.clear() + self._base_subgait_menu.addItems(versions) + self._other_subgait_menu.clear() + self._other_subgait_menu.addItems(versions) + return super(ParametricPopUpWindow, self).exec_() + + def cancel(self): + print('cancel') + self.accept() + + def apply(self): + print('accepted') + base_version = self._base_subgait_menu.currentText() + if base_version == '': + self.reject() + else: + self.base_version = base_version + other_version = self._other_subgait_menu.currentText() + if other_version == '': + self.reject() + else: + self.other_version = other_version + parameter = float(self._parameter_line_edit.text()) + if 0.0 <= parameter <= 1.0: + self.parameter = parameter + else: + self.reject() + + print('leaving with, base version {0}, other version {1} and parameter {2}'.format(base_version, other_version, parameter)) + self.accept() + + def base_subgait_changed(self): + pass From 58b9f3ee8904286d6a95ca7e2a34a2279355ebb6 Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Tue, 21 Jul 2020 16:40:52 +0200 Subject: [PATCH 39/48] clean code and improve pop up --- .../gait_selection_view.py | 27 ++++-- .../parametric_pop_up.py | 90 +++++++------------ 2 files changed, 48 insertions(+), 69 deletions(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 06e94c0..de80901 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -8,6 +8,7 @@ DEFAULT_AMOUNT_OF_AVAILABLE_SUBGAITS = 3 +PARAMETRIC_GAIT_CHARACTER = '_' class GaitSelectionView(QWidget): @@ -137,8 +138,12 @@ def version_sorter(version): subgait_menu.setCurrentIndex(current_version_index) except ValueError: - self._log('Default version of {gn} {sgn} does not exist in loaded gaits.' - .format(gn=gait_name, sgn=subgait_name), 'error') + if current_version[0] == PARAMETRIC_GAIT_CHARACTER: + subgait_menu.addItem(current_version) + subgait_menu.setCurrentIndex(subgait_menu.count() - 1) + else: + self._log('Default version of {gn} {sgn} does not exist in loaded gaits.' + .format(gn=gait_name, sgn=subgait_name), 'error') except KeyError: self._log('{gn} has no default version for {sgn}.' .format(gn=gait_name, sgn=subgait_name), 'error') @@ -163,11 +168,15 @@ def update_version(self): try: if 'parametric' == str(subgait_menu.currentText()): versions = self.available_gaits[gait_name][subgait_name] - self._parametric_pop_up.show_pop_up(versions) - new_version = self.get_parametric_version() - subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) - subgait_menu.addItem(new_version) - subgait_menu.setCurrentIndex(subgait_menu.count() - 1) + if self._parametric_pop_up.show_pop_up(versions): + new_version = self.get_parametric_version() + subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) + subgait_menu.addItem(new_version) + subgait_menu.setCurrentIndex(subgait_menu.count() - 1) + else: + subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['error'])) + current_version_index = versions.index(self.version_map[gait_name][subgait_name]) + subgait_menu.setCurrentIndex(current_version_index - 1) if str(self.version_map[gait_name][subgait_name]) != str(subgait_menu.currentText()): subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) else: @@ -280,5 +289,5 @@ def _show_parametric_pop_up(self): self._parametric_pop_up.show_pop_up() def get_parametric_version(self): - return '%{0}_({1})_({2})'.format(self._parametric_pop_up.parameter, self._parametric_pop_up.base_version, - self._parametric_pop_up.other_version) + return '{0}{1}_({2})_({3})'.format(PARAMETRIC_GAIT_CHARACTER, self._parametric_pop_up.parameter, + self._parametric_pop_up.base_version, self._parametric_pop_up.other_version) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py index 8a5bbd0..53436bb 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py @@ -1,10 +1,11 @@ from PyQt5.QtCore import Qt -from PyQt5.QtWidgets import QDialog, QComboBox, QDialogButtonBox, QGridLayout, QFormLayout, QLabel, QLineEdit,QPushButton, QScrollArea, QSizePolicy, QWidget +from PyQt5.QtWidgets import QComboBox, QDialog, QDialogButtonBox, QFormLayout, QGridLayout, QLabel, QLineEdit,\ + QMessageBox class ParametricPopUpWindow(QDialog): - def __init__(self, parent, width=500, height=600): + def __init__(self, parent, width=500, height=200): """Base class for creating a pop up window over an existing widget. :param parent: @@ -16,62 +17,32 @@ def __init__(self, parent, width=500, height=600): """ super(ParametricPopUpWindow, self).__init__(parent=parent, flags=Qt.Window) self.resize(width, height) - # self.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) self._layout = QGridLayout(self) - - # self._scroll_area = QScrollArea() - # self._scroll_area.setVerticalScrollBarPolicy(Qt.ScrollBarAsNeeded) - # self._scroll_area.setHorizontalScrollBarPolicy(Qt.ScrollBarAsNeeded) - # self._scroll_area.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) - # self._scroll_area.setWidgetResizable(True) - # - # self._content_frame = QWidget(self._scroll_area, flags=Qt.Window) - # self._content_frame.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) - # self._content = QGridLayout(self._content_frame) - # - # self.msg_label = QLabel(self) - # self._content.addWidget(self.msg_label) - # - # self._scroll_area.setWidget(self._content_frame) - # self._layout.addWidget(self._scroll_area) - self.setWindowTitle('test') + self.setWindowTitle('parametric gait selector') self._form_layout = QFormLayout() - self._parameter_line_edit = QLineEdit('parameter here please', self) + + self._parameter_line_edit = QLineEdit(self) self._layout.addWidget(self._parameter_line_edit) + self._parameter_label = QLabel('parameter', self) + self._form_layout.addRow(self._parameter_label, self._parameter_line_edit) self._layout.addItem(self._form_layout) - self._apply_button = QDialogButtonBox.Ok - self._cancel_button = QDialogButtonBox.Close - self._button_box = QDialogButtonBox(self) + self._button_box = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Close, self) self._layout.addWidget(self._button_box) - self._button_box.addButton(self._apply_button) - self._button_box.addButton(self._cancel_button) - self._button_box.accepted.connect(self.apply) + self._button_box.accepted.connect(self.save) self._button_box.rejected.connect(self.cancel) self._button_box.centerButtons() - versions = ['1', '2', '3'] base_subgait_label = QLabel('base subgait', self) - self._base_subgait_menu = QComboBox(self) self._base_subgait_menu.currentIndexChanged.connect(self.base_subgait_changed) - self._base_subgait_menu.addItems(versions) + self._form_layout.addRow(base_subgait_label, self._base_subgait_menu) other_subgait_label = QLabel('other subgait', self) - self._other_subgait_menu = QComboBox(self) self._other_subgait_menu.currentIndexChanged.connect(self.base_subgait_changed) - self._other_subgait_menu.addItems(versions) - - # self._layout.addItem(base_subgait_label, 1, 1) - self._form_layout.addRow(base_subgait_label, self._base_subgait_menu) - self._form_layout.addRow(other_subgait_label, self._other_subgait_menu) - - self.base_version = '' - self.other_version = '' - self.parameter = 0.0 - + self._form_layout.setHorizontalSpacing(80) def show_pop_up(self, versions): """Add message to the pop up and show the window.""" @@ -79,31 +50,30 @@ def show_pop_up(self, versions): self._base_subgait_menu.addItems(versions) self._other_subgait_menu.clear() self._other_subgait_menu.addItems(versions) + + self._parameter_line_edit.setText('') + + self.base_version = '' + self.other_version = '' + self.parameter = 0.0 return super(ParametricPopUpWindow, self).exec_() def cancel(self): - print('cancel') - self.accept() - - def apply(self): - print('accepted') - base_version = self._base_subgait_menu.currentText() - if base_version == '': - self.reject() - else: - self.base_version = base_version - other_version = self._other_subgait_menu.currentText() - if other_version == '': - self.reject() - else: - self.other_version = other_version - parameter = float(self._parameter_line_edit.text()) + self.reject() + + def save(self): + self.base_version = self._base_subgait_menu.currentText() + self.other_version = self._other_subgait_menu.currentText() + try: + parameter = float(self._parameter_line_edit.text()) + except ValueError: + QMessageBox.warning(self, '', 'Enter a string that can be converted to a float') + return if 0.0 <= parameter <= 1.0: self.parameter = parameter else: - self.reject() - - print('leaving with, base version {0}, other version {1} and parameter {2}'.format(base_version, other_version, parameter)) + QMessageBox.warning(self, '', 'Enter a float greater or equal than zero and smaller or equal to one.') + return self.accept() def base_subgait_changed(self): From cbd8aacfd9417a52caf28ffb9154c753cfaccd58 Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Wed, 22 Jul 2020 12:39:27 +0200 Subject: [PATCH 40/48] small fix and change pop up size --- .../gait_selection_view.py | 5 ++--- .../parametric_pop_up.py | 16 +++++++--------- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index de80901..1140bc9 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -61,9 +61,8 @@ def __init__(self, ui_file, controller): self.available_gaits = {} self.version_map = {} - # pop up window + # pop up windows self._version_map_pop_up = PopUpWindow(self) - self._parametric_pop_up = ParametricPopUpWindow(self) # populate gait menu for the first time @@ -174,7 +173,7 @@ def update_version(self): subgait_menu.addItem(new_version) subgait_menu.setCurrentIndex(subgait_menu.count() - 1) else: - subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['error'])) + # parametric pop up window unsuccessful stopped, reset version to default current_version_index = versions.index(self.version_map[gait_name][subgait_name]) subgait_menu.setCurrentIndex(current_version_index - 1) if str(self.version_map[gait_name][subgait_name]) != str(subgait_menu.currentText()): diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py index 53436bb..64e9aa8 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py @@ -5,8 +5,8 @@ class ParametricPopUpWindow(QDialog): - def __init__(self, parent, width=500, height=200): - """Base class for creating a pop up window over an existing widget. + def __init__(self, parent, width=500, height=160): + """A pop up window for retrieving a base version, other version and parameter for a parametric subgait. :param parent: The parent widget to connect to the pop up @@ -35,22 +35,21 @@ def __init__(self, parent, width=500, height=200): self._button_box.centerButtons() base_subgait_label = QLabel('base subgait', self) self._base_subgait_menu = QComboBox(self) - self._base_subgait_menu.currentIndexChanged.connect(self.base_subgait_changed) + self._base_subgait_menu.currentIndexChanged.connect(lambda: None) self._form_layout.addRow(base_subgait_label, self._base_subgait_menu) other_subgait_label = QLabel('other subgait', self) self._other_subgait_menu = QComboBox(self) - self._other_subgait_menu.currentIndexChanged.connect(self.base_subgait_changed) + self._other_subgait_menu.currentIndexChanged.connect(lambda: None) self._form_layout.addRow(other_subgait_label, self._other_subgait_menu) self._form_layout.setHorizontalSpacing(80) def show_pop_up(self, versions): - """Add message to the pop up and show the window.""" + """Reset and show pop up.""" self._base_subgait_menu.clear() self._base_subgait_menu.addItems(versions) self._other_subgait_menu.clear() self._other_subgait_menu.addItems(versions) - self._parameter_line_edit.setText('') self.base_version = '' @@ -59,9 +58,11 @@ def show_pop_up(self, versions): return super(ParametricPopUpWindow, self).exec_() def cancel(self): + """Close without applying the values.""" self.reject() def save(self): + """Check and save value while closing, close if successful.""" self.base_version = self._base_subgait_menu.currentText() self.other_version = self._other_subgait_menu.currentText() try: @@ -75,6 +76,3 @@ def save(self): QMessageBox.warning(self, '', 'Enter a float greater or equal than zero and smaller or equal to one.') return self.accept() - - def base_subgait_changed(self): - pass From c7a0a100f5f0cbed7c2c84aed7724d265b15736e Mon Sep 17 00:00:00 2001 From: marnixbrands Date: Tue, 28 Jul 2020 12:17:04 +0200 Subject: [PATCH 41/48] Add slalom walk button to monitor IPD --- .../src/march_rqt_input_device/input_device_view.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py index 18dbee8..39efcd3 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py @@ -72,6 +72,11 @@ def _create_buttons(self): gait_walk_large = \ self.create_button('gait_walk_large', image_path='/gait_walk_large.png', callback=lambda: self._controller.publish_gait('gait_walk_large')) + + gait_slalom_walk = \ + self.create_button('gait_slalom_walk', + callback=lambda: self._controller.publish_gait('gait_slalom_walk')) + gait_balanced_walk = \ self.create_button('gait_balanced_walk', callback=lambda: self._controller.publish_gait('gait_balanced_walk')) @@ -258,10 +263,10 @@ def _create_buttons(self): # Position in the array determines position on screen. march_button_layout = [ - [home_sit, home_stand, gait_walk, gait_walk_small, gait_walk_large, gait_balanced_walk, - sm_to_unknown_button], + [home_sit, home_stand, gait_walk, gait_walk_small, gait_walk_large, gait_balanced_walk, gait_slalom_walk], - [gait_sit, gait_stand, rocker_switch_increment, rocker_switch_decrement, stop_button, error_button], + [gait_sit, gait_stand, rocker_switch_increment, rocker_switch_decrement, stop_button, error_button, + sm_to_unknown_button], [gait_sofa_sit, gait_sofa_stand, gait_single_step_normal, gait_single_step_small, continue_button, pause_button], From bc364a90341e5945d97eae4f40b83b44496d5d9a Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Tue, 28 Jul 2020 14:33:37 +0200 Subject: [PATCH 42/48] use .ui file --- .../resource/gait_selection.ui | 9 ++ .../resource/parametric_pop_up.ui | 141 ++++++++++++++++++ .../gait_selection_view.py | 8 +- .../parametric_pop_up.py | 62 +++----- 4 files changed, 173 insertions(+), 47 deletions(-) create mode 100644 march_rqt_gait_selection/resource/parametric_pop_up.ui diff --git a/march_rqt_gait_selection/resource/gait_selection.ui b/march_rqt_gait_selection/resource/gait_selection.ui index d14c0ee..064c702 100644 --- a/march_rqt_gait_selection/resource/gait_selection.ui +++ b/march_rqt_gait_selection/resource/gait_selection.ui @@ -370,6 +370,9 @@ QPushButton{color: #FFFFFF} Ubuntu Condensed + + Qt::StrongFocus + false @@ -415,6 +418,9 @@ QPushButton{color: #FFFFFF} Ubuntu Condensed + + Qt::ClickFocus + @@ -457,6 +463,9 @@ QPushButton{color: #FFFFFF} Ubuntu Condensed + + Qt::TabFocus + diff --git a/march_rqt_gait_selection/resource/parametric_pop_up.ui b/march_rqt_gait_selection/resource/parametric_pop_up.ui new file mode 100644 index 0000000..5dc83f2 --- /dev/null +++ b/march_rqt_gait_selection/resource/parametric_pop_up.ui @@ -0,0 +1,141 @@ + + + ParametricPopUp + + + Qt::WindowModal + + + + 0 + 0 + 500 + 160 + + + + + 500 + 160 + + + + parametric gait selector + + + + + + + + + true + + + + + 145 + 130 + 341 + 25 + + + + QDialogButtonBox::Abort|QDialogButtonBox::Ok + + + false + + + + + + 9 + 10 + 481 + 111 + + + + + 2 + + + 3 + + + + + The parameter uses for linear interpolation in %. + + + parameter = 0.00 + + + + + + + + + + base version + + + + + + + + + + + + + + + + + other version + + + + + + + + + + + + + + + + + ehllo + + + 100 + + + 1 + + + Qt::Horizontal + + + QSlider::TicksBelow + + + 10 + + + + + + + + + + diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 1140bc9..51d03ce 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -63,7 +63,7 @@ def __init__(self, ui_file, controller): # pop up windows self._version_map_pop_up = PopUpWindow(self) - self._parametric_pop_up = ParametricPopUpWindow(self) + self._parametric_pop_up = ParametricPopUpWindow(self, ui_file.replace('gait_selection.ui', 'parametric_pop_up.ui')) # populate gait menu for the first time self._refresh() @@ -167,7 +167,7 @@ def update_version(self): try: if 'parametric' == str(subgait_menu.currentText()): versions = self.available_gaits[gait_name][subgait_name] - if self._parametric_pop_up.show_pop_up(versions): + if self._show_parametric_pop_up(versions): new_version = self.get_parametric_version() subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) subgait_menu.addItem(new_version) @@ -283,9 +283,9 @@ def _show_version_map_pop_up(self): self._version_map_pop_up.show_message(version_map_string) - def _show_parametric_pop_up(self): + def _show_parametric_pop_up(self, versions): """Use a pop up window to get the base version, other version and parameter for a parametric subgait.""" - self._parametric_pop_up.show_pop_up() + return self._parametric_pop_up.show_pop_up(versions) def get_parametric_version(self): return '{0}{1}_({2})_({3})'.format(PARAMETRIC_GAIT_CHARACTER, self._parametric_pop_up.parameter, diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py index 64e9aa8..5cb7df5 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py @@ -2,10 +2,11 @@ from PyQt5.QtCore import Qt from PyQt5.QtWidgets import QComboBox, QDialog, QDialogButtonBox, QFormLayout, QGridLayout, QLabel, QLineEdit,\ QMessageBox +from python_qt_binding import loadUi class ParametricPopUpWindow(QDialog): - def __init__(self, parent, width=500, height=160): + def __init__(self, parent, ui_file): """A pop up window for retrieving a base version, other version and parameter for a parametric subgait. :param parent: @@ -16,63 +17,38 @@ def __init__(self, parent, width=500, height=160): Starting height of the the pop up widget """ super(ParametricPopUpWindow, self).__init__(parent=parent, flags=Qt.Window) - self.resize(width, height) + loadUi(ui_file, self) - self._layout = QGridLayout(self) - self.setWindowTitle('parametric gait selector') - self._form_layout = QFormLayout() - self._parameter_line_edit = QLineEdit(self) - self._layout.addWidget(self._parameter_line_edit) - self._parameter_label = QLabel('parameter', self) - self._form_layout.addRow(self._parameter_label, self._parameter_line_edit) - self._layout.addItem(self._form_layout) - self._button_box = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Close, self) - self._layout.addWidget(self._button_box) - self._button_box.accepted.connect(self.save) - self._button_box.rejected.connect(self.cancel) - - self._button_box.centerButtons() - base_subgait_label = QLabel('base subgait', self) - self._base_subgait_menu = QComboBox(self) - self._base_subgait_menu.currentIndexChanged.connect(lambda: None) - self._form_layout.addRow(base_subgait_label, self._base_subgait_menu) - - other_subgait_label = QLabel('other subgait', self) - self._other_subgait_menu = QComboBox(self) - self._other_subgait_menu.currentIndexChanged.connect(lambda: None) - self._form_layout.addRow(other_subgait_label, self._other_subgait_menu) - self._form_layout.setHorizontalSpacing(80) + self.buttonBox.accepted.connect(self.save) + self.buttonBox.rejected.connect(self.cancel) + self.parameterSlider.valueChanged.connect(self.value_changed) def show_pop_up(self, versions): """Reset and show pop up.""" - self._base_subgait_menu.clear() - self._base_subgait_menu.addItems(versions) - self._other_subgait_menu.clear() - self._other_subgait_menu.addItems(versions) - self._parameter_line_edit.setText('') + self.baseVersionComboBox.clear() + self.baseVersionComboBox.addItems(versions) + self.otherVersionComboBox.clear() + self.otherVersionComboBox.addItems(versions) + self.parameterSlider.setTickPosition(50) + self.parameterLabel.setText('parameter = 0.0') self.base_version = '' self.other_version = '' self.parameter = 0.0 return super(ParametricPopUpWindow, self).exec_() + def value_changed(self): + """Puts the new slider value in the label next to it.""" + self.parameterLabel.setText('parameter = {0}'.format(self.parameterSlider.value() / 100.0)) + def cancel(self): """Close without applying the values.""" self.reject() def save(self): """Check and save value while closing, close if successful.""" - self.base_version = self._base_subgait_menu.currentText() - self.other_version = self._other_subgait_menu.currentText() - try: - parameter = float(self._parameter_line_edit.text()) - except ValueError: - QMessageBox.warning(self, '', 'Enter a string that can be converted to a float') - return - if 0.0 <= parameter <= 1.0: - self.parameter = parameter - else: - QMessageBox.warning(self, '', 'Enter a float greater or equal than zero and smaller or equal to one.') - return + self.base_version = self.baseVersionComboBox.currentText() + self.other_version = self.otherVersionComboBox.currentText() + self.parameter = self.parameterSlider.value() / 100.0 self.accept() From a6781809b82fa74d079533638f526af1944ccfc2 Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Tue, 28 Jul 2020 14:58:43 +0200 Subject: [PATCH 43/48] fix bug and tooltips --- .../resource/parametric_pop_up.ui | 48 ++++++++----------- .../gait_selection_view.py | 5 +- .../parametric_pop_up.py | 10 ++-- 3 files changed, 28 insertions(+), 35 deletions(-) diff --git a/march_rqt_gait_selection/resource/parametric_pop_up.ui b/march_rqt_gait_selection/resource/parametric_pop_up.ui index 5dc83f2..31c949f 100644 --- a/march_rqt_gait_selection/resource/parametric_pop_up.ui +++ b/march_rqt_gait_selection/resource/parametric_pop_up.ui @@ -10,13 +10,13 @@ 0 0 500 - 160 + 120 500 - 160 + 120 @@ -31,29 +31,13 @@ true - - - - 145 - 130 - 341 - 25 - - - - QDialogButtonBox::Abort|QDialogButtonBox::Ok - - - false - - - 9 + 10 10 481 - 111 + 106 @@ -66,7 +50,7 @@ - The parameter uses for linear interpolation in %. + The parameter decides 'how much' of the base version is done. parameter = 0.00 @@ -76,7 +60,7 @@ - + The parametric subgait would result to this version if the parameter is 0%. base version @@ -86,14 +70,14 @@ - + The parametric subgait would result to this version if the parameter is 0%. - + The parametric subgait would result to this version if the parameter is 100%. other version @@ -103,17 +87,17 @@ - + The parametric subgait would result to this version if the parameter is 100%. - + The parameter decides 'how much' of the base version is done. - ehllo + ello 100 @@ -132,6 +116,16 @@ + + + + QDialogButtonBox::Abort|QDialogButtonBox::Ok + + + false + + + diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 51d03ce..8466567 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -63,7 +63,8 @@ def __init__(self, ui_file, controller): # pop up windows self._version_map_pop_up = PopUpWindow(self) - self._parametric_pop_up = ParametricPopUpWindow(self, ui_file.replace('gait_selection.ui', 'parametric_pop_up.ui')) + self._parametric_pop_up = ParametricPopUpWindow(self, ui_file.replace('gait_selection.ui', + 'parametric_pop_up.ui')) # populate gait menu for the first time self._refresh() @@ -175,7 +176,7 @@ def update_version(self): else: # parametric pop up window unsuccessful stopped, reset version to default current_version_index = versions.index(self.version_map[gait_name][subgait_name]) - subgait_menu.setCurrentIndex(current_version_index - 1) + subgait_menu.setCurrentIndex(max(current_version_index - 1, 0)) if str(self.version_map[gait_name][subgait_name]) != str(subgait_menu.currentText()): subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) else: diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py index 5cb7df5..cd65c7a 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/parametric_pop_up.py @@ -1,7 +1,6 @@ from PyQt5.QtCore import Qt -from PyQt5.QtWidgets import QComboBox, QDialog, QDialogButtonBox, QFormLayout, QGridLayout, QLabel, QLineEdit,\ - QMessageBox +from PyQt5.QtWidgets import QDialog from python_qt_binding import loadUi @@ -19,7 +18,6 @@ def __init__(self, parent, ui_file): super(ParametricPopUpWindow, self).__init__(parent=parent, flags=Qt.Window) loadUi(ui_file, self) - self.buttonBox.accepted.connect(self.save) self.buttonBox.rejected.connect(self.cancel) self.parameterSlider.valueChanged.connect(self.value_changed) @@ -30,8 +28,8 @@ def show_pop_up(self, versions): self.baseVersionComboBox.addItems(versions) self.otherVersionComboBox.clear() self.otherVersionComboBox.addItems(versions) - self.parameterSlider.setTickPosition(50) - self.parameterLabel.setText('parameter = 0.0') + self.parameterSlider.setValue(50) + self.parameterLabel.setText('parameter = 0.50') self.base_version = '' self.other_version = '' @@ -40,7 +38,7 @@ def show_pop_up(self, versions): def value_changed(self): """Puts the new slider value in the label next to it.""" - self.parameterLabel.setText('parameter = {0}'.format(self.parameterSlider.value() / 100.0)) + self.parameterLabel.setText('parameter = {val:.2f}'.format(val=self.parameterSlider.value() / 100.0)) def cancel(self): """Close without applying the values.""" From 204b433ca6baecb8ea6d69ce6418912d7a0f52e4 Mon Sep 17 00:00:00 2001 From: Olav de Haas <23523462+Olavhaasie@users.noreply.github.com> Date: Wed, 29 Jul 2020 15:32:49 +0200 Subject: [PATCH 44/48] Removed the march_rqt_software_check package --- march_rqt_software_check/CMakeLists.txt | 19 --- .../launch/checks/git_branch.launch | 5 - .../launch/checks/slave_count.launch | 5 - .../launch/march_rqt_software_check.launch | 3 - march_rqt_software_check/package.xml | 21 --- march_rqt_software_check/plugin.xml | 17 --- .../resource/img/march-walking.png | Bin 8287 -> 0 bytes .../resource/software_check.ui | 143 ------------------ .../scripts/march_git_branch_check | 22 --- .../scripts/march_rqt_software_check | 9 -- march_rqt_software_check/setup.py | 12 -- .../src/march_rqt_software_check/__init__.py | 0 .../march_rqt_software_check/check_runner.py | 82 ---------- .../checks/__init__.py | 0 .../checks/gait_file_directory_check.py | 33 ---- .../checks/git_branch_check.py | 38 ----- .../checks/launch_check.py | 22 --- .../checks/slave_count_check.py | 22 --- .../checks/software_check.py | 50 ------ .../checks/urdf_check.py | 43 ------ .../src/march_rqt_software_check/color.py | 12 -- .../software_check.py | 68 --------- .../software_check_thread.py | 11 -- 23 files changed, 637 deletions(-) delete mode 100644 march_rqt_software_check/CMakeLists.txt delete mode 100644 march_rqt_software_check/launch/checks/git_branch.launch delete mode 100644 march_rqt_software_check/launch/checks/slave_count.launch delete mode 100644 march_rqt_software_check/launch/march_rqt_software_check.launch delete mode 100644 march_rqt_software_check/package.xml delete mode 100644 march_rqt_software_check/plugin.xml delete mode 100644 march_rqt_software_check/resource/img/march-walking.png delete mode 100644 march_rqt_software_check/resource/software_check.ui delete mode 100755 march_rqt_software_check/scripts/march_git_branch_check delete mode 100755 march_rqt_software_check/scripts/march_rqt_software_check delete mode 100644 march_rqt_software_check/setup.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/__init__.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/check_runner.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/checks/__init__.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/checks/gait_file_directory_check.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/checks/git_branch_check.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/checks/launch_check.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/checks/slave_count_check.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/checks/software_check.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/checks/urdf_check.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/color.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/software_check.py delete mode 100644 march_rqt_software_check/src/march_rqt_software_check/software_check_thread.py diff --git a/march_rqt_software_check/CMakeLists.txt b/march_rqt_software_check/CMakeLists.txt deleted file mode 100644 index d410fec..0000000 --- a/march_rqt_software_check/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(march_rqt_software_check) - -find_package(catkin REQUIRED) - -catkin_python_setup() -catkin_package() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY launch resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/march_git_branch_check scripts/${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/march_rqt_software_check/launch/checks/git_branch.launch b/march_rqt_software_check/launch/checks/git_branch.launch deleted file mode 100644 index c895499..0000000 --- a/march_rqt_software_check/launch/checks/git_branch.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/march_rqt_software_check/launch/checks/slave_count.launch b/march_rqt_software_check/launch/checks/slave_count.launch deleted file mode 100644 index b7dc188..0000000 --- a/march_rqt_software_check/launch/checks/slave_count.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/march_rqt_software_check/launch/march_rqt_software_check.launch b/march_rqt_software_check/launch/march_rqt_software_check.launch deleted file mode 100644 index 9859e7c..0000000 --- a/march_rqt_software_check/launch/march_rqt_software_check.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/march_rqt_software_check/package.xml b/march_rqt_software_check/package.xml deleted file mode 100644 index e47638d..0000000 --- a/march_rqt_software_check/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - march_rqt_software_check - 0.0.0 - rqt automated software checks for the March exoskeleton - - Project March - - TODO - - catkin - - python-rospkg - rospy - rqt_gui - rqt_gui_py - - - - - diff --git a/march_rqt_software_check/plugin.xml b/march_rqt_software_check/plugin.xml deleted file mode 100644 index 2bdd21d..0000000 --- a/march_rqt_software_check/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - RQT plugin to perform all software checks. - - - - - resource/img/march-walking.png - Plugins related to the March exoskeleton. - - - resource/img/march-walking.png - March Software Check - - - diff --git a/march_rqt_software_check/resource/img/march-walking.png b/march_rqt_software_check/resource/img/march-walking.png deleted file mode 100644 index 15f9c92cab4f0056efc65e5c7129c8a88accdabf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8287 zcmb7pXEa=2)c)v((TN^ykVG&_7`?YdZ&AZ2qcfsSL&!i zS4&5YWR08z_<*&jN$r2g{s<+fJHc`Q0RI3mt8S+P0NfweQdKfW{@Amg53hIxVzWZn z{JVA2dlycMr=%7a71+8t23@j1zcLaAjI~6^G`a-}x7Hc*!R@5D-#q`|l9LL-?H8-NJR!_O`%}2B zYc-MpjzcW<5=vn)xR>hh{@U4=j5;wD$Dw3<%*~c%KanuUA;DkkeC)9gDLMJq`UN=MbkdvYo9I0;1dR{!}}Li?s)(8C-j-zbv(* z$>Nhmpem&ACEYokebq*&+AB`HF+27l0*#(9Nm#y}&G#-zCy@fo2`uQx76a^!rKU^HEecM<;SJVNnHjx# zUi3|#?BBEBmL>DELrFnhpGSP-r@3h>3MQG**$)z(^`HZ-v}UT|T!mNtE(>dpQ*f1n zIrpRw!^7{T;t3--zbyMMzR+43l2`o&-C(Q6Sc=kE^%q$`dK8Ac5$2i)xOcYTTD&AM ztVRMn80nW`xH`7D_kZy>kz~u<7(VEq9@kYQ#eYjAUBye;Xb#3iTfiv|I`mZ@SodI6 z@GFMXf>Hk63vD<2IzWoABX|zwp116J1~iSI{+vLFW!gQFV4#-PZ2Eh$$D?Rp&}>v9 z6!(+wWd{jQrz43vaiYB*H9>c)xy{auZ()pU4Uztstl@P9FBpi6&kK1gSumaBRXi(g z5@9M8a`kW-#OoyFvvk>HUgXoY0rjuRti=UP@K;LggHDFnG&I~*xuuLc55E6us6(!Q z_-SS?5xzMRxSn^jXX3%Z=abXybUCB;Pmk&e2+@@|N$@R%^We(?0H{?U76?bJ~gx&k#$$jUPU| zHRXCbv0yw91uqi~f`8iXZrIHE7;y4-v>^fank8CvB?h(}%FkuBC8(>rG}CgU0tvdQ zzp6eVt2{~F-24Eu`Zt*eZ@E?PhG)GKA_-JGH^O3VauIBlQ9Ht!EuNP!au|I=u)1C?`igq^fl@T}-7A$sh zJlZUwScY&fO}th-YYe@5WSXl5ox*ox5@8hRUH5;~2$&>OpQZzq5KTes7XGXYTubNE z#pY^4LkM~H=(l-~Pt$^P^hwqAAlj1>WI!r!)iXckHqo!I?f))V`g91^p2Y6%p!}^Z zLZG<80b&m(nl7P?%Gje+jupH-`xkP7hFdSg8X|TuC>|B-EXoxQZr-76Sa<11jU|MY zjlo4@xTeG~0rnxX+ep7*%jJ&0~}IY>Y7^Kv6@k$o@7>=1@Y7 zdm_>q-u2HH7eHm0)S&|nOslSZ295naDX#6{MBe?{HvUCf#MqF^+Itmk)-pw5Zj9Ke zBQ2hizSiW&B!qZ7PcS!=FD$yaOV$@E3jhhu(L9$e@nM8bY4Xu>AVDh8g_Yh{NcEOFxUD$ZUl71j-iqA>j2U|YiAA}{;_22 zK-(wO=P`pQ+n;q|eF3)CqSm)GpdZM%ayAxew&FTa{`P*SwmHY-6Rvwy|F*(&a{*5bH{)#MhVn1qVq~0akTa zhct+NL-U0{JLoH+5G_iFMq*J7^S+|J6PG{q1oV7^=8^ojEcEQ)*-sy_Pew=0?*)x- z`7WzV=+{1c>Rm|mB2^Am*A$4R2Qdh`8e4uc2m%v-xwM)mIH&HsvA>69{F-@UKm?gR zg%3#!P6|q;ZFn7yyXezxy$g--xs~mPD%zFGGwM7|*o}#84E}J{rWjLLM1P<-JT_Tngcg*4#A!Mz4 zPv)o-ih4abwHKRGCE#?!G*-BnCOPQQouFCQC!7O`q6yWCsq-0yc+hG2RES;DlPaJ| zwuF19H1;TwVGxcmyS2nz^NkB8+K8|Zm2cDQoSO1dttxa)2Iar%+id>Guub08&#@l6 z1w~^X9nVk*fUbEfleW38QW&1mOO0#Slo8b27t!?*!|MC$S9l_!7z8&(VC|Lwg513- z3+rf9VaT~hu}hJk*vl)2fh1<@!>+>meAeHS(+t++o^{22kQhPi=X*x&93RfG1On$Z zj*ER7Wtsyor7NP@snIH!*AI7}Q*9moMfxp7+-q|#ROD>t_L%S#ox|wOXc_0i(zkcW zeJ!oj`0P(VVcQArN3xeMs(Pbw>cZ;;#)rjrKb7&rk^F{aQP?8--ZId&>`rM~2HqZ8 zL!db6d|ZRmIOra|Ei`*jJJ+`5g)n0k;G*s8MHcgAbX83)KBc1NJZs7TGhac|d&;Zh zA1vzH0TBj=<^YAGVq^*RF5Shm;SSYheCq)+u3~c`!NV$(8~YPHJ&8eB`PUkaxs+-S zo+gLzs%M{Jkm6o~H5X5@SV!@?vRvErw+S?L2?2C=uLB7NG?Wwz?6%Ts%IH98+*b^B z3108447p8$?omV7__8J|M0a74ZYKf`95yj&+zOC({3u)Ac32!NfCybNDV!GEVH|g9 z`u>qTxhP^{GbofZn5AJ9`9SMP0qI`Wa)<`gb1gMrM<@CbJm7P1N|qOW!I{la0vf6;lw=0U5r zDa1#glidH2i0ryX?FhN`q`js|jZ0_9BUJ9L{oZqqM*2E)Lu$xHjGQzx9hvYYtL_Q z$HPH(E%UmUQOqv__!uj*#%iDMe8}l$vMrx#`M~6dW{;G&0;>vjaOD1dPI$eD5{DO2%*C=2 zbae^#NST-(O-pk>+OVkNw0ltVH7gHfbA;NU!`}z1r*(f`S^^SNe_8qPXh(y& zc-Tbw%$w|Jj-}KK)kF6v2WOn+2ten6ng5{sbUB53EH?jC8*oln8qA9Mh3CMpQZ`wY z&u~(xKINGDN*=r~HC0XQ4(X5i?s|@^BBL3udrE=#o`<86gCY}n7uZh_JhPyA1rpJ9 z$SD_7BKSNU6M2RLV1*tED#$~oZwJA2^=XGRcLkfV&J}6}fZCBy{k%Aa4p^rpjO&{J z5wnak#pKqoJD6Fo^R@<&u^t{Yx_AZ#j^>{QN?mYbhc&7@0aMjqQo=8z#~;X(pA6Pd z^RtBEYKJZC&eMkM>XgqCm#wH6cHta{%zaL;9>|#$WO9HDXd1-!1I8~I>&BbjX-eAt zs#Ykth~}SB)?1>mWP&@RD;hx_T-PUAUX!schC!#2it4BQF@MnKOuv3DM(kG9CESRz z3T`__7L3zN5LyVbd7k2S`Va2gMkPbItkwC zUhPp8#qq-PkL>a0-8x6ZFIjk-65SsO}FYD#d;3aq<}) z_UTzZGFd(rGuL^Symyr{iE<=UHQ;B;RXTnPU5{BY41!{}PxJsDa>rnLbrnew>jIm@ zP8Ux*Gxjv*_ionMgV;BrToW5df_+YJ?p16k>iY3w?Rv^*f}f-HXe-3509?0?poG5J zDG7m-J(@SsCwA!5;8ulLi^e$?vUDLQr29WsF+Nx5qKe%1tk}(Mx`;urGRZ?>E~bAr zBW6a}2Vl?y58+g7zS!+yD6BtY+YjTxAI$98X3CdftOB$-=SZ*JM$;k6-g983dM%eW z6>0y2z9Pu)L4%pbM*HFm_sC9<#~2rZV3~)-q+oP zA&V-6dIL67veRS*WK-iBbgWEm&pLp*0`t4?wBO&7`zFB$vdJ6Ov+jyC0z?H6ZBc za`Qf1H>90=J%Yh$EIvQJ{@DRb_z4_8ZLy~oA$OUy?GxYYyS6RF9v;0MIKTVrZ^MM$ z;MKmvcVar-b2Dl>hi)I7dtcx3=GKGIe?Q5`um_~=KQpsx`UZq-nRPn@KFoh~F%9qsXJ?xK`a_jA; zS3hDP3_hyELa071ISa7KdvnPC)4hhpbiZS(HP}C>m7VwOtqr?O=|8k1xTmGBH9id5 z0k>cej?7*svXEpkj2*xx{c|Arx(~S3012O$XuEs-$0^n`|9+FTY3cB^aeG2A5w>cJOqAC-D(>=A`5I2tODlCFrRU+o zNi*d`Nm0qk`RNGWo&Y#9Fc7QgC2h|647gPvYp6BnBdt}~B2aav($f2l_vO_nF+6L4TUR1@W1ABk0n;VZ+P4{u+J0L@(owfS2i`r7r z=|f&5R}a$96R>JsZM#pkH67aF`dcLUMVpZ$Hont89tEY z8Cy@Q&6yW+U`j~ilwjD{6#X^X^&23hKBA2bVQk9SyM?9y@hh#DICBih{ibxXQ3OxO z(7Ap(n;4GQt5y8;l}PYPo7vl(t5J6rD;qGee)n{F08f)!uXog2&U-XnXW#LTSxNmL zlzf~weta&>=!*yZ_0IJyS^9CE>PrW51)q2RA>YIMc9zL4CU#$aSlAXZw}+$5=ND*1Cr-QD_$MbX-pg zB`fkzZ?ej;(=A~AF1QaG0Etd+qw885L~7SbE%qcJjm}~@BM#KIQh&tS2^#tj^ zq>?4vJ?h$86jNQ7U5`B+g~tABQb*E+KI?FMAhw`$v9={UD^Hhh`_QdJnL%^Jk0m0n zKJ%cj4OS9btaXe@eynGE<>qpk17v*eHK!l0t4F4Q#E)PQr>M_T{GOY=)SHG|tS zr2)saMA#4(Iu}23r_2`&igk_jh=u5K8Ot#~^`j_Zc|xSAf6Dw9eJ6EzPtLrS+1=6f zA5o5B{u(is*5az;umdRTgQz z0>_LeK~Ud?Hc}`K4^vZ1&O4!9AOoCuh7*IGIp~MOca?$1vD4&&jd?*BlqG?SIvr(0 z99F#CgCO&!YnGKh2ez1{4Ku+-_CwOM;0Z z5s-8QwU&o^St+~zDd~8;-`C_V<%}T|c3!_ydj8kO5}Q?W$A2HK|17SNpQo%WDF= z2|9QLki^=2#*-OU=u*gc^Za+O+~nI?j~wb64hB;?bkjGIo%T1)XG@;;fJoQ7F4v<1 zuL`Bqu-@ZccuuNK=&Zv8aPE0S#kSq7`>EraVuh?_m=95H-{npFMUc~y+mp!b7PLp0 z6_*SMY<(|C(~OrQsA8$#=}mk3R}#VhQC|?L;M*u-sF-x|9(a5Dg_@$Pd!ADUs3`R} z33#htEq|Pe~qEd&0-ekrE(#1Zpk%SR*s}(BoK0#|&**C6;<9vTOWl)+6 z_~%(ScT(N%8T-?osPJVUJ$SwhX*Dzc;d@9Q;BE?*iCLH7%Z3jxFROY7jnYbwG_2H! z*L=uP{l`G3`?Bl!Nlmt^?Kd$E?*SL{oOLk3gQWIx z$X{P#l3=2D@A$!sF9`l@)IAw)2u=+;og~&qDie{jDpQE-R)zwf)B%?U{fK_Gnv=Wn z+rtvg9Gx!W8>AR?m6O5Lk7@G63YDKkn*Yi0&G|8Qz`nWLxc0X^pCz1ktr1q?R~~7` z?!9&U;oo3uk*ny+wN5%afmEsmgY#rd2G1W4(2D3JDZ#|pkC4pn;1Rm07x16{S%bF( ztEn-3G^0$erDq{@W1#bga7l>-bJ;_|lAN=SJk84cohztKneW&6Ni$0#JQ7ziU&_hu zt#`ctL>yl&i6fMK<9-c320Teo_IiXs@aLml_a$+5MTiKUWpPX787Jlmhr?&XzkzhQ ze;9&y4PrW@;5MVT- zD;oKyRzgnGBBrKt&~vvIr>jiGVxx0daSj-+ckL|Zx+Iu_n?Lh3u^c=S&39bud*_#j zftY-=opeSXXdwqF`o;E((u?R7`z={NziifVQ)iOE1q<}`QoA?)s&n!w*_nZ92flso zZ!H=_MvAIr8Q+Jsot2gKaT})LW`AEq^kChhR<~R{C%JsrzqbdzObvt0O$6tyt})Z; z29TL2bRy}B3z?$9=fIFb)2U92-O@B~4kB{K6Gdd+YRQdtmmXvSbjFsh_Pcgg2(w|I zFaf2O?JBKMFh>dP{Gq+E(occb$gHf%y!;*WmF=c%X67QMb{x-@1Q?7P_*aPZi z)<~-H$v>XyKQZEgQ?DC#ouXC;rg=FeA6xaQczQe%bQ8mj5y)0sUADsF|Imz^0^S@@ zV#fIkmOYV5$6w}#-<0}Ctup=&|92;G##~K;50k-Emx_JAT()>HOxp3`XWKV||Ddw* zh5@ZEk)i7%77I>y{)^nyh6(UWnkLm%dNb`!(1sCDtIR!<#r5q+g4DW|gx#vehVM^W zC5M|&e%BkT)M~w6S`CgAepATO!PYPL$=2etjRXJ16N@IiMv~!kWP8uM)y=oe@|?_m zb{DB(l|}@@0+{j5p)t-S&$yG+)09d^%%DH|nWfBdKyS8RS+9#}xD8{W6lWufNwSpA z1*ddS-xn)(LG*HUJplWB^R4+0(vYTfZx+R9Z*0}z2NF#6*JP?sRT5n4Nbed`xDYue zfl*8-f_{+bJkX_{w?%?KXMZG_O5uXd4rPPTX!f@77(?X>!>!^uhB3+0BN?lV^kR@t zOZ_hJaPRe@eU-JR2X1ef=k(pz#Kka!N9*UDD;K6bn0*1btx~s$4i5%-yk26^PII$n z*FT)-St>?*b7t57ZnBm7CP(%%Ln@LvhD z7x_u(AK=PtpBqG0siNZX7|s^;4sU>S55z_qZ*lMt`Z&865j2ri%b~7HCm;1F6JcBU zI$u>BS3md(NTN=w0~J5I`)?U&&{0yb=s+Qw;Tg7UgEWF`7sgqCCqR$UBZTpb0=DB zspvNkHb38qUBsj7Uf_3{k+OCsEeELhw{UCitZH~vL79*CwvYw`GbM<3C)(xsbgp>pj&uJ%j>La4{EzXVD0KE+vJ_$Ax7OI# zH-vcO(nl@k{+u=6Uj6==x!`j_!n*R%;;8Ym=WhJX>#Jtw>j3k0l(v8Ac;^735D};# zL_`qs#8^aBT1-?L@ - - SoftwareCheck - - - - 0 - 0 - 1109 - 796 - - - - March Software Check - - - QGroupBox { - border: 1px solid gray; - margin-top: 0.5em; -} - -QGroupBox::title { - subcontrol-origin: margin; - left: 10px; - padding: 0 3px 0 3px; -} - - - - - - QFrame::StyledPanel - - - QFrame::Raised - - - - - - false - - - false - - - #Log { -background-color: #F2F1F0 -} - - - - - - - - - - QFrame::StyledPanel - - - QFrame::Raised - - - - - - Local - - - - - - - 150 - 150 - - - - Gait File Location - - - - - - - - 150 - 150 - - - - March IV -URDF Check - - - - - - - - - - Exoskeleton - - - - - - - 150 - 150 - - - - Slave Count - - - - - - - - 150 - 150 - - - - Git Branch - - - - - - - - - - - - - - diff --git a/march_rqt_software_check/scripts/march_git_branch_check b/march_rqt_software_check/scripts/march_git_branch_check deleted file mode 100755 index 0cdf130..0000000 --- a/march_rqt_software_check/scripts/march_git_branch_check +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python -import os - -from pygit2 import Repository, GitError -import rospkg -import rospy - -result = [] - -march_launch_path = rospkg.RosPack().get_path("march_launch") -head = os.path.split(march_launch_path)[0] -source_path = os.path.split(head)[0] - -for repository_name in os.listdir(source_path): - repository_path = os.path.join(source_path, repository_name) - try: - branch_name = Repository(repository_path).head.shorthand - result.append([repository_name, branch_name]) - except GitError as e: - pass - -rospy.set_param("/checks/git_branch", result) diff --git a/march_rqt_software_check/scripts/march_rqt_software_check b/march_rqt_software_check/scripts/march_rqt_software_check deleted file mode 100755 index 3476365..0000000 --- a/march_rqt_software_check/scripts/march_rqt_software_check +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -plugin = 'software_check' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin)) diff --git a/march_rqt_software_check/setup.py b/march_rqt_software_check/setup.py deleted file mode 100644 index 659c0ab..0000000 --- a/march_rqt_software_check/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python -from distutils.core import setup - -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['march_rqt_software_check', 'march_rqt_software_check.checks'], - package_dir={'': 'src'}, - scripts=['scripts/march_rqt_software_check'], -) - -setup(**d) diff --git a/march_rqt_software_check/src/march_rqt_software_check/__init__.py b/march_rqt_software_check/src/march_rqt_software_check/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/march_rqt_software_check/src/march_rqt_software_check/check_runner.py b/march_rqt_software_check/src/march_rqt_software_check/check_runner.py deleted file mode 100644 index 85294b0..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/check_runner.py +++ /dev/null @@ -1,82 +0,0 @@ -from python_qt_binding.QtWidgets import QMessageBox -import rospy - -from .checks.gait_file_directory_check import GaitFileDirectoryCheck -from .checks.git_branch_check import GitBranchCheck -from .checks.slave_count_check import SlaveCountCheck -from .checks.urdf_check import URDFCheck -from .color import Color -from .software_check_thread import SoftwareCheckThread - - -class CheckRunner: - def __init__(self, logger=None): - self.checks = [GitBranchCheck(), GaitFileDirectoryCheck(), URDFCheck(), SlaveCountCheck()] - for check in self.checks: - check.log_signal.connect(lambda msg, color: self.log(msg, color)) - self.logger = logger - self.thread = None - - def run_check_by_name(self, name): - check = self.get_check(name) - if check is None: - self.log('Check with name ' + name + ' does not exist', Color.Error) - - return self.run_check(check) - - def run_check(self, check): - self.log('--------------------------------------', Color.Info) - - if self.thread is not None: - self.log('Already running another check', Color.Warning) - - if check is None: - self.log('Check does not exist', Color.Error) - return - - self.log('Starting check ' + str(check.name) + ': ' + str(check.description), Color.Info) - - start = rospy.get_rostime() - self.thread = SoftwareCheckThread(check) - self.thread.start() - - while not check.done: - if rospy.get_rostime() < start + rospy.Duration.from_sec(check.timeout): - rospy.sleep(0.1) - - else: - self.log('Check ' + str(check.name) + ' timed out after ' + str(check.timeout) + 's', Color.Error) - self.thread.exit() - self.thread = None - - check.reset() - return False - - self.thread.wait() - self.thread = None - result = check.passed - if result and check.manual_confirmation: - result = self.validate_manually() - check.reset() - - if result: - self.log('Check ' + str(check.name) + ' was succesful!', Color.Debug) - else: - self.log('Check ' + str(check.name) + ' has failed', Color.Error) - - return result - - def get_check(self, name): - for check in self.checks: - if check.name == name: - return check - return None - - def log(self, msg, level): - if self.logger is not None: - self.logger(msg, level) - - @staticmethod - def validate_manually(): - reply = QMessageBox.question(None, 'Message', 'Did the test pass?', QMessageBox.Yes, QMessageBox.No) - return reply == QMessageBox.Yes diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/__init__.py b/march_rqt_software_check/src/march_rqt_software_check/checks/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/gait_file_directory_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/gait_file_directory_check.py deleted file mode 100644 index 105a538..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/checks/gait_file_directory_check.py +++ /dev/null @@ -1,33 +0,0 @@ -import rospkg -import rospy - -from march_rqt_software_check.color import Color - -from .launch_check import LaunchCheck - - -class GaitFileDirectoryCheck(LaunchCheck): - - def __init__(self): - LaunchCheck.__init__(self, 'GaitFileDirectory', 'Gaits are loaded from the following directory', - 'march_gait_selection', 'march_gait_selection.launch', manual_confirmation=True) - - def perform(self): - self.launch() - - rospy.sleep(rospy.Duration.from_sec(5)) - self.stop_launch_process() - package_name = self.get_key_from_parameter_server('/march/gait_file_package') - file_directory = self.get_key_from_parameter_server('/march/gait_file_directory') - - try: - rospkg.RosPack().get_path(package_name) - except rospkg.common.ResourceNotFound: - self.log('Package ' + str(package_name) + ' not found on local machine.', Color.Error) - self.fail_check() - - self.log('Package name: ' + str(package_name), Color.Info) - self.log('Gait file directory: ' + str(file_directory), Color.Info) - - self.passed = package_name is not None and file_directory is not None - self.done = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/git_branch_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/git_branch_check.py deleted file mode 100644 index 5cbaea5..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/checks/git_branch_check.py +++ /dev/null @@ -1,38 +0,0 @@ -import rospy - -from march_rqt_software_check.color import Color - -from .launch_check import LaunchCheck - - -class GitBranchCheck(LaunchCheck): - - def __init__(self): - LaunchCheck.__init__(self, - 'GitBranch', - 'Please confirm the branches below', - 'march_rqt_software_check', - 'git_branch.launch', - 10, - True) - - def perform(self): - self.launch() - - rospy.sleep(4) - self.stop_launch_process() - - branch_tuples = self.get_key_from_parameter_server('/checks/git_branch') - - for branch_tuple in branch_tuples: - repository_name, branch_name = branch_tuple - print_string = repository_name + ': ' + branch_name - if branch_name == 'develop': - self.log(print_string, Color.Debug) - elif 'training/' in branch_name: - self.log(print_string, Color.Debug) - else: - self.log(print_string, Color.Warning) - - self.done = True - self.passed = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/launch_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/launch_check.py deleted file mode 100644 index 7d03a27..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/checks/launch_check.py +++ /dev/null @@ -1,22 +0,0 @@ -import subprocess - -from .software_check import SoftwareCheck - - -class LaunchCheck(SoftwareCheck): - - def __init__(self, name, description, package, launch_file, timeout=10000, manual_confirmation=False): - SoftwareCheck.__init__(self, name, description, timeout, manual_confirmation) - self.package = package - self.file = launch_file - self.launch_process = None - - def launch(self): - cmd = 'roslaunch ' + self.package + ' ' + self.file - self.launch_process = subprocess.Popen(cmd, shell=True, executable='/bin/bash') - - def perform(self): - raise NotImplementedError('Please implement method "perform()" on the subclass') - - def stop_launch_process(self): - self.launch_process.kill() diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/slave_count_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/slave_count_check.py deleted file mode 100644 index 97bee90..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/checks/slave_count_check.py +++ /dev/null @@ -1,22 +0,0 @@ -import rospy - -from march_rqt_software_check.color import Color - -from .launch_check import LaunchCheck - - -class SlaveCountCheck(LaunchCheck): - - def __init__(self): - LaunchCheck.__init__(self, 'SlaveCount', '', 'march_launch', 'slave_count.launch', manual_confirmation=True) - - def perform(self): - self.launch() - - rospy.sleep(rospy.Duration.from_sec(5)) - self.stop_launch_process() - slave_count = self.get_key_from_parameter_server('/check/slave_count') - - self.log('Ethercat found ' + str(slave_count) + ' slave(s)', Color.Info) - self.passed = slave_count > 0 - self.done = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/software_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/software_check.py deleted file mode 100644 index ea2053d..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/checks/software_check.py +++ /dev/null @@ -1,50 +0,0 @@ -from PyQt5.QtCore import pyqtSignal, QObject -import rospy - -from march_rqt_software_check.color import Color - - -class SoftwareCheck(QObject): - log_signal = pyqtSignal(str, Color) - - def __init__(self, name, description, timeout=10, manual_confirmation=False): - QObject.__init__(self, None) - self.name = name - self.description = description - self.manual_confirmation = manual_confirmation - self.timeout = timeout - self.passed = False - self.done = False - - def reset(self): - self.passed = False - self.done = False - - def is_passed(self): - if self.done: - return self.passed - return False - - def perform(self): - raise NotImplementedError('Please implement method "perform()" on the subclass') - - def pass_check(self): - self.passed = True - self.done = True - - def fail_check(self): - self.passed = False - self.done = True - - def get_key_from_parameter_server(self, key, fail_on_exception=True): - try: - value = rospy.get_param(key) - except KeyError: - self.log('Could not find key ' + str(key), Color.Error) - if fail_on_exception: - self.fail_check() - return - return value - - def log(self, msg, level): - self.log_signal.emit(str(msg), level) diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/urdf_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/urdf_check.py deleted file mode 100644 index 6f1eed3..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/checks/urdf_check.py +++ /dev/null @@ -1,43 +0,0 @@ -import rospy -from urdf_parser_py import urdf - -from march_rqt_software_check.color import Color - -from .launch_check import LaunchCheck - - -class URDFCheck(LaunchCheck): - - def __init__(self): - LaunchCheck.__init__(self, 'URDF', 'Please check if the loaded joints are correct', 'march_launch', - 'upload_march_urdf.launch', 10, True) - - def perform(self): - self.launch() - - rospy.sleep(rospy.Duration.from_sec(3)) - try: - robot = urdf.Robot.from_parameter_server() - except KeyError: - self.stop_launch_process() - self.fail_check() - - count = 0 - for joint in robot.joints: - if joint.type != 'fixed': - count += 1 - - self.log('Loaded ' + str(count) + ' joints', Color.Info) - for joint in robot.joints: - if joint.type != 'fixed': - lower = str(round(joint.safety_controller.soft_lower_limit, 4)) - upper = str(round(joint.safety_controller.soft_upper_limit, 4)) - velocity = str(round(joint.limit.velocity, 4)) - effort = str(round(joint.limit.effort, 4)) - msg = joint.name + ': (' + lower + ', ' + upper + ') max velocity: ' + \ - velocity + ' rad/s max effort ' + effort + ' IU' - self.log(msg, Color.Info) - - self.stop_launch_process() - self.passed = True - self.done = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/color.py b/march_rqt_software_check/src/march_rqt_software_check/color.py deleted file mode 100644 index 57cc0bd..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/color.py +++ /dev/null @@ -1,12 +0,0 @@ -from enum import Enum - - -class Color(Enum): - Debug = '#009100' - Info = '#000000' - Warning = '#b27300' # noqa A003 - Error = '#FF0000' - Fatal = '#FF0000' - Check_Unknown = '#b27300' - Check_Failed = '#FF0000' - Check_Passed = '#009100' diff --git a/march_rqt_software_check/src/march_rqt_software_check/software_check.py b/march_rqt_software_check/src/march_rqt_software_check/software_check.py deleted file mode 100644 index f7732df..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/software_check.py +++ /dev/null @@ -1,68 +0,0 @@ -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QPlainTextEdit, QPushButton, QWidget -from qt_gui.plugin import Plugin -import rospkg - -from .check_runner import CheckRunner -from .color import Color - - -class SoftwareCheckPlugin(Plugin): - - def __init__(self, context): - super(SoftwareCheckPlugin, self).__init__(context) - # Give QObjects reasonable names - self.setObjectName('SoftwareCheckPlugin') - - # Create QWidget - self._widget = QWidget() - - # Get path to UI file which should be in the 'resource' folder of this package - ui_file = os.path.join(rospkg.RosPack().get_path('march_rqt_software_check'), 'resource', 'software_check.ui') - - # Extend the widget with all attributes and children from UI file - loadUi(ui_file, self._widget) - # Give QObjects reasonable names - self._widget.setObjectName('SoftwareCheck') - # Show _widget.windowTitle on left-top of each plugin (when - # it's set in _widget). This is useful when you open multiple - # plugins at once. Also if you open multiple instances of your - # plugin at once, these lines add number to make it easy to - # tell from pane to pane. - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - # Add widget to the user interface - context.add_widget(self._widget) - - self.log('Welcome to the Software Check.', Color.Debug) - self.log('Select the software checks you want to perform.', Color.Info) - - self.check_runner = CheckRunner(self.log) - - self._widget.Checks.findChild(QPushButton, 'GitBranch') \ - .clicked.connect(lambda: self.run_test('GitBranch')) - self._widget.Checks.findChild(QPushButton, 'GaitFileDirectory') \ - .clicked.connect(lambda: self.run_test('GaitFileDirectory')) - self._widget.Checks.findChild(QPushButton, 'URDF') \ - .clicked.connect(lambda: self.run_test('URDF')) - self._widget.Checks.findChild(QPushButton, 'SlaveCount') \ - .clicked.connect(lambda: self.run_test('SlaveCount')) - - def log(self, msg, level): - self._widget.findChild(QPlainTextEdit, 'Log').appendHtml( - '

' + str(msg) + '

') - scrollbar = self._widget.findChild(QPlainTextEdit, 'Log').verticalScrollBar() - scrollbar.setValue(scrollbar.maximum()) - - def run_test(self, name): - result = self.check_runner.run_check_by_name(name) - self.update_test_button(name, result) - - def update_test_button(self, name, result): - button = self._widget.Checks.findChild(QPushButton, name) - if result: - button.setStyleSheet('background-color: ' + str(Color.Check_Passed.value)) - else: - button.setStyleSheet('background-color: ' + str(Color.Check_Failed.value)) diff --git a/march_rqt_software_check/src/march_rqt_software_check/software_check_thread.py b/march_rqt_software_check/src/march_rqt_software_check/software_check_thread.py deleted file mode 100644 index 1dcad43..0000000 --- a/march_rqt_software_check/src/march_rqt_software_check/software_check_thread.py +++ /dev/null @@ -1,11 +0,0 @@ -from pyqtgraph.Qt import QtCore - - -class SoftwareCheckThread(QtCore.QThread): - - def __init__(self, check): - QtCore.QThread.__init__(self) - self.check = check - - def run(self): - self.check.perform() From 7129840d8fa49eae5ad78d0774667ea851412140 Mon Sep 17 00:00:00 2001 From: RutgerVanBeek <51922475+RutgerVanBeek@users.noreply.github.com> Date: Wed, 29 Jul 2020 18:53:40 +0200 Subject: [PATCH 45/48] Apply suggestions from code review Co-authored-by: Olav de Haas <23523462+Olavhaasie@users.noreply.github.com> --- march_rqt_gait_selection/resource/parametric_pop_up.ui | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/march_rqt_gait_selection/resource/parametric_pop_up.ui b/march_rqt_gait_selection/resource/parametric_pop_up.ui index 31c949f..63acb4a 100644 --- a/march_rqt_gait_selection/resource/parametric_pop_up.ui +++ b/march_rqt_gait_selection/resource/parametric_pop_up.ui @@ -70,14 +70,14 @@ - The parametric subgait would result to this version if the parameter is 0%. + The parametric subgait would result in this version if the parameter is 0%. - The parametric subgait would result to this version if the parameter is 100%. + The parametric subgait would result in this version if the parameter is 100%. other version From 254330b80bb5916d480f089ffe5c14d535359b5a Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Wed, 29 Jul 2020 19:06:44 +0200 Subject: [PATCH 46/48] enlarge pop up and small changes --- .../resource/parametric_pop_up.ui | 81 ++++++++++--------- .../gait_selection_view.py | 6 +- 2 files changed, 45 insertions(+), 42 deletions(-) diff --git a/march_rqt_gait_selection/resource/parametric_pop_up.ui b/march_rqt_gait_selection/resource/parametric_pop_up.ui index 31c949f..4a89452 100644 --- a/march_rqt_gait_selection/resource/parametric_pop_up.ui +++ b/march_rqt_gait_selection/resource/parametric_pop_up.ui @@ -10,7 +10,7 @@ 0 0 500 - 120 + 185 @@ -37,15 +37,15 @@ 10 10 481 - 106 + 121 - 2 + 6 - 3 + 6 @@ -57,6 +57,31 @@
+ + + + The parameter decides 'how much' of the base version is done. + + + ello + + + 100 + + + 1 + + + Qt::Horizontal + + + QSlider::TicksBelow + + + 10 + + + @@ -91,43 +116,21 @@ - - - - The parameter decides 'how much' of the base version is done. - - - ello - - - 100 - - - 1 - - - Qt::Horizontal - - - QSlider::TicksBelow - - - 10 - - - - - - - QDialogButtonBox::Abort|QDialogButtonBox::Ok - - - false - - - + + + + 310 + 150 + 166 + 25 + + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok + + diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py index 8466567..2344981 100644 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -8,7 +8,7 @@ DEFAULT_AMOUNT_OF_AVAILABLE_SUBGAITS = 3 -PARAMETRIC_GAIT_CHARACTER = '_' +PARAMETRIC_GAIT_PREFIX = '_pg_' class GaitSelectionView(QWidget): @@ -138,7 +138,7 @@ def version_sorter(version): subgait_menu.setCurrentIndex(current_version_index) except ValueError: - if current_version[0] == PARAMETRIC_GAIT_CHARACTER: + if current_version.startswith(PARAMETRIC_GAIT_PREFIX): subgait_menu.addItem(current_version) subgait_menu.setCurrentIndex(subgait_menu.count() - 1) else: @@ -289,5 +289,5 @@ def _show_parametric_pop_up(self, versions): return self._parametric_pop_up.show_pop_up(versions) def get_parametric_version(self): - return '{0}{1}_({2})_({3})'.format(PARAMETRIC_GAIT_CHARACTER, self._parametric_pop_up.parameter, + return '{0}{1}_({2})_({3})'.format(PARAMETRIC_GAIT_PREFIX, self._parametric_pop_up.parameter, self._parametric_pop_up.base_version, self._parametric_pop_up.other_version) From af4538aa16bb7930f02244747da11000c3bbfceb Mon Sep 17 00:00:00 2001 From: gaiavdh Date: Mon, 10 Aug 2020 11:29:46 +0200 Subject: [PATCH 47/48] Add single step mini to ipd --- .../src/march_rqt_input_device/input_device_view.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py index 39efcd3..b9125c8 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py @@ -85,6 +85,10 @@ def _create_buttons(self): self.create_button('gait_single_step_small', image_path='/gait_single_step_small.png', callback=lambda: self._controller.publish_gait('gait_single_step_small')) + gait_single_step_mini = \ + self.create_button('gait_single_step_mini', + callback=lambda: self._controller.publish_gait('gait_single_step_mini')) + gait_single_step_normal = \ self.create_button('gait_single_step_normal', image_path='/gait_single_step_medium.png', callback=lambda: self._controller.publish_gait('gait_single_step_normal')) @@ -275,8 +279,9 @@ def _create_buttons(self): [gait_side_step_left, gait_side_step_right, gait_side_step_left_small, gait_side_step_right_small], - [gait_rough_terrain_high_step, gait_rough_terrain_middle_steps, gait_rough_terrain_first_middle_step, - gait_rough_terrain_second_middle_step, gait_rough_terrain_third_middle_step], + [gait_rough_terrain_high_step, gait_single_step_mini, gait_rough_terrain_middle_steps, + gait_rough_terrain_first_middle_step, gait_rough_terrain_second_middle_step, + gait_rough_terrain_third_middle_step], [gait_ramp_door_slope_up, gait_ramp_door_slope_down, gait_ramp_door_slope_down_fixed, gait_ramp_door_last_step, gait_ramp_door_slope_down_single], From f8c03695d3bba24b7b66f55df64d9d5345f379f1 Mon Sep 17 00:00:00 2001 From: RutgerVanBeek Date: Sat, 15 Aug 2020 20:04:44 +0200 Subject: [PATCH 48/48] remove max frequency for ipd monitor --- .../diagnostic_analyzers/check_input_device.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py index 1640feb..c72b412 100644 --- a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/check_input_device.py @@ -6,7 +6,7 @@ class CheckInputDevice(object): """Base class to diagnose whether the input devices are connected properly.""" def __init__(self, topic, message_type, updater, frequency): - self._frequency_params = FrequencyStatusParam({'min': frequency, 'max': frequency}, 0.1, 10) + self._frequency_params = FrequencyStatusParam({'min': frequency}, 0.1) self._updater = updater self._diagnostics = {}