From 19f40f5ef1b5439faac6233e5ad1e50696d0e036 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 1 Feb 2024 21:55:38 +0100 Subject: [PATCH 01/21] add initial gui draft --- crazyflie/CMakeLists.txt | 1 + crazyflie/scripts/gui.py | 145 ++++++++++++++++++++++++++++++++ crazyflie/urdf/cf2_assembly.stl | Bin 0 -> 166184 bytes 3 files changed, 146 insertions(+) create mode 100755 crazyflie/scripts/gui.py create mode 100644 crazyflie/urdf/cf2_assembly.stl diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index c8899cfbe..66e1e4ae1 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -109,6 +109,7 @@ install(PROGRAMS scripts/vel_mux.py scripts/cfmult.py scripts/simple_mapper_multiranger.py + scripts/gui.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py new file mode 100755 index 000000000..3941d8c77 --- /dev/null +++ b/crazyflie/scripts/gui.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 + +import math +import threading +from pathlib import Path + +import rclpy +from geometry_msgs.msg import Pose, Twist +from rcl_interfaces.msg import Log +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +from nicegui import Client, app, events, ui, ui_run + + +class NiceGuiNode(Node): + + def __init__(self) -> None: + super().__init__('nicegui') + + # find all crazyflies + self.cfnames = [] + for srv_name, srv_types in self.get_service_names_and_types(): + if 'crazyflie_interfaces/srv/StartTrajectory' in srv_types: + # remove '/' and '/start_trajectory' + cfname = srv_name[1:-17] + if cfname != 'all': + self.cfnames.append(cfname) + + print(self.cfnames) + self.cfs = [] + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + + + + self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 1) + self.sub_log = self.create_subscription(Log, 'rosout', self.on_rosout, 1) + + self.logs = dict() + + with Client.auto_index_client: + # with ui.row().classes('w-full h-full no-wrap'): + # self.log = ui.log().classes('w-full h-full no-wrap') + + + + + with ui.row().classes('items-stretch'): + # with ui.card().classes('w-44 text-center items-center'): + # ui.label('Data').classes('text-2xl') + # ui.label('linear velocity').classes('text-xs mb-[-1.8em]') + # slider_props = 'readonly selection-color=transparent' + # self.linear = ui.slider(min=-1, max=1, step=0.05, value=0).props(slider_props) + # ui.label('angular velocity').classes('text-xs mb-[-1.8em]') + # self.angular = ui.slider(min=-1, max=1, step=0.05, value=0).props(slider_props) + # ui.label('position').classes('text-xs mb-[-1.4em]') + # self.position = ui.label('---') + with ui.card().classes('w-full h-full'): + ui.label('Visualization').classes('text-2xl') + with ui.scene(800, 600, on_click=self.on_vis_click) as scene: + for name in self.cfnames: + robot = scene.stl('/urdf/cf2_assembly.stl').scale(1.0).material('#ff0000').with_name(name) + self.cfs.append(robot) + # with scene.group() as self.robot_3d: + # prism = [[-0.5, -0.5], [0.5, -0.5], [0.75, 0], [0.5, 0.5], [-0.5, 0.5]] + # self.robot_object = scene.extrusion(prism, 0.4).material('#4488ff', 0.5) + + with ui.row().classes('w-full h-full'): + with ui.tabs().classes('w-full') as tabs: + self.tabs = [] + for name in ["all"] + self.cfnames: + self.tabs.append(ui.tab(name)) + with ui.tab_panels(tabs, value=self.tabs[0]).classes('w-full') as self.tabpanels: + for name, tab in zip(["all"] + self.cfnames, self.tabs): + with ui.tab_panel(tab): + self.logs[name] = ui.log().classes('w-full h-full no-wrap') + ui.label("Battery Voltage: ") + ui.button("Reboot") + + # Call on_timer function + self.timer = self.create_timer(0.1, self.on_timer) + + def send_speed(self, x: float, y: float) -> None: + msg = Twist() + msg.linear.x = x + msg.angular.z = -y + self.linear.value = x + self.angular.value = y + self.cmd_vel_publisher.publish(msg) + + def on_rosout(self, msg: Log) -> None: + if msg.name in self.logs: + self.logs[msg.name].push(msg.msg) + + def on_timer(self) -> None: + for name, robot in zip(self.cfnames, self.cfs): + t = self.tf_buffer.lookup_transform( + "world", + name, + rclpy.time.Time()) + pos = t.transform.translation + robot.move(pos.x, pos.y, pos.z) + + def on_vis_click(self, e: events.SceneClickEventArguments): + hit = e.hits[0] + name = hit.object_name or hit.object_id + ui.notify(f'You clicked on the {name}') + if name == 'ground': + self.tabpanels.value = 'all' + else: + self.tabpanels.value = name + + + + # def handle_pose(self, msg: Pose) -> None: + # self.position.text = f'x: {msg.position.x:.2f}, y: {msg.position.y:.2f}' + # self.robot_3d.move(msg.position.x, msg.position.y) + # self.robot_3d.rotate(0, 0, 2 * math.atan2(msg.orientation.z, msg.orientation.w)) + + +def main() -> None: + # NOTE: This function is defined as the ROS entry point in setup.py, but it's empty to enable NiceGUI auto-reloading + pass + + +def ros_main() -> None: + rclpy.init() + node = NiceGuiNode() + try: + rclpy.spin(node) + except ExternalShutdownException: + pass + + +app.add_static_files("/urdf", "/home/whoenig/projects/crazyflie/crazyswarm2/src/crazyswarm2/crazyflie/urdf/") +app.on_startup(lambda: threading.Thread(target=ros_main).start()) +ui_run.APP_IMPORT_STRING = f'{__name__}:app' # ROS2 uses a non-standard module name, so we need to specify it here +ui.run(uvicorn_reload_dirs=str(Path(__file__).parent.resolve()), favicon='🤖') diff --git a/crazyflie/urdf/cf2_assembly.stl b/crazyflie/urdf/cf2_assembly.stl new file mode 100644 index 0000000000000000000000000000000000000000..02772d31eee4006fef671357ff447daf0f273ffa GIT binary patch literal 166184 zcmce<3EY;`_6EF5MaE;E=iyKx6w&=Ww<$t~lHpLKOc|n#8S2%5qRhfUsYvFdB=tPc zEg~eE%#|XU8mJJW@4D`Nujjq?`@VJj&-Z=${r-RJJbPVxtv#*1_uA`TZ;NX$zpUGp z=bgLL`Q0wNbf?2FIj_sP=XKlrfPEV8)7bql|9)E^sP;{g z_k8@(=zvDISCe4~ua)sTSCLqW>Q36)S4~E8Wv6|jb3eE#ZLOkBwnsEx**3m-oek6R zxNeghqCIarFhv*_HL>5lR`=XGpoz6ZOB7-9sD-_Db&q$r<)RX?CWF$gO-hF?U$v@dVs$AOX|2h* z6_~qive|yc#_bD-Wb4CdB3|y$aQx?3E2N#nY)@>qe>Y@dL1ue_vz?k??xwUyRJOe$ zu6X>>g3UzqXhCLv8C}WGb{H4yGI^v|FQfY=HxxLps0n6efwPjd)#UrC&fOcuBeF49 zE~6>K5WXFlC9%yCfX5mtVX~lXC;bmCwVkQO% zoW<}E$nha`Uw3l(ircc<4;@qVjOc9FCR&dr=18*HrZikhyE<=zG zwX$|_mMB8GOt#l5v!ZG5YKbC@_Pr}IQtQK+!4i{4+GbC6#Fu;YD0GV-PEUQ4_G(+$ zXRlc)eclCY6+S(G&&X>vd)4O2zppqkzVd}BqqOAO-;N!>-76hauC5vFWt7(1GI?dp z3F$i55|c+RqrHsok8hcr-uQ%grxGkw((dqf?QTp%?OA}*MxDI zJkGbjhA0 zf6w%^#5Lb=uUY8{kiC0Z{BTM;m+iIb{-GsCTRXOSrDJxI$@cT4-`B1wTWBTh1=7?+ zWn%P@24vw;kgM|6m=ol_CU#XdB;QUceY z>P{}A*$zWk%MIoCT5GLxSvx!;+d0$TrOjwl|HO3ua8}y9GTI}SV?0jF?glj>$YsA* z(<<#No1;2+O-9&P*1LvKE|XCmq4nzQ)A?ro#Q*xe)>^AvDlg?s`-zl|0i_$w){>^S z*79)Yv@usMqpckw0;0-0Cux&m<4;{yk1#IN&}0zheHqq^RmQb=CPsT1YT;@^RPAZ!u9m%)z1UT5!h6e2_>=NY_)FY`2jM2%HMMw2gVS zol|eZj$B+()WMGj?JfTo|7B|}K#!6=7u-n zpdG&DbF-UpRfKA2?eGZJ?*El{=k)et2x?+&s-~5l08)f z^LWh&M8aTV0j>L}-a3Y#vwZ-2kCnJPp_?pp0=*}JwL|2u*`8jeR*8LWf)8kf?3JpLQPWUwZWcr)9bX-RDiSYor!5S6opSpKVu zFs%$x+44zMZo=9>)^8@ykH^+kZo-C8F4NW{s@{ZcE-Auj-}0(A;i?F7nGBEc?dJcl zNNg@u<*GX79>c!U61BT#6TOV8H(|~=mZ&_f$+-oO@09wGtIE)NZ61g103zMbZB`o> zd(_arcdg}B5z3|eRk_uvwbl|v811!E>tn5@bfd{-8hS*wpQkd6iwNa1+P7oEsjI&U z!+w-+!kBNXxd~fIHU{ZQR?jHT{oilG-LN~i$4&Us>?Uk7v|hc7c&`Dx&I$XOEv)H$ z#oo1BxO4x%-h|8hV71-3e6OXZ95t>%(&4I*G2-bvPz4m)Gt?@lkHs*%;UvI)DL+iC!>1E(1Y_G4W z*V%5{YIk)sO(Xs{yw}!r|Mb0Ei3t&AoylcdDZomS`-@*Mw1J#@8yWS%AR8HW)U0dJFp>$HiU1- zQbeTG|GrV$azn6QLn!SLSq#f+5#+L(Jfc#sFiKBl7{a%_h8ULg45wUKTt@o4bIlN? zcg`B3^jNQ zji96M_PcKfqCaDa{wzn$WpNT|k1}$^qD|8XQ^v5Q<6&Hsu3ScYt*qUg?HZ-0T&9&r zVD2V}ze-zf2wNiA5 z$n0*~j?6Bl2<0-`wcz}bh9 zH07PrJD?;JtEt2R*N8))#MST z7gv-?XT7Go${_9BYagDLocLt@w6EIlxHOvh$kM3I`NJZk7hIA=XFk6;y0g}bsKG5I zAcg?Zy9R`E8O@rUTl8k@Jh zb|pWxQW=U45r2-`BzX(-mslMGYX{ed(SDYkw7fhXiLL{ymt49E0<998ujI0pleV7r zGVT!>Hfk)f(Ijo-?-3@2mciP=64kI~t%y*qR1=l$++ADTkZjs^|Ma|TtzEied|3wV zhspCJ*uTyGrJ-Bgkgg~jgU?z$U0&O|&OPF1ARb)h6ZVhIC7o9u zF%^h|*MK0G)#MS6K%@0C3ukkQ_R2<+HqmI0I2H95GrOy{)^t}(?GcUdTNK@V-#)od zp6pp(QS8wXix1C@t=lcCvsWp4boP{HW&qfc->=_`G>xltJda+QIHp zcUu#jS6gq~Ja^}+6QYLw#{zNR6OZRMn{sz;-^%ciJ>s{EZNv^oZtI-b23P3!y zd}isU-v*TCzT7@;n^|PeU0P6%J9~H4yZh$mPiYffyxt2y410ZQX|0LOcDul%P?yTvves_vHS%y}i6xo4C+^Yf>nRlDu(%C~-T@BDj@bqx{1b;6-#=D;oPs~mLT!T8V4t)NaXxC9Ka^oklcW-^NR{r-1gK}RSa$|r{ zu0uAvBs#j=VL8q*=SH+xkl$|VTltMHIX<@a%(V8z++{Ja5MJIjyJP_}^u~l*JL2Zgp zUEecVzr)(O-Q1tihu^&%ZMIvB+*$kG9J%hV1oCjwduK_0pW)Ns-gT zU2(;g6AC9z>6r{2QZILbtDUQL*|_MvQAgxv9`SHAY~5D^dCpzlyjT9#kM_!U{q~AP z_0@K4@J{nw%Vj4;XMeyI)$7*hiZd>Lv{=!qM>2Fyo!r=wb#hm<9Ut9tW0PFp#;-=t zd^$Fe=UkVi)AENtb6ftNch5??N2{X7PcDvHjvf(B9o#sVU)eUAedRNejuTFnE$%7p zIjM2!=muSrB|~fFZr!|IjwATah=-ylMz_onvGAp_(NR;I=Qx^p2VU!r(h(CHl$v79 z6`@?eomeM#RI55UKjshjTT%SI&Vb@~81rkkT^!BBDUxIUz+Ui&ZhlyfW8Pr*tD;ie zBIn24y}4CxmqBfE9Kp8#TokqZ`j2!3`{X-C^*%c!#}QOCw0dVr>6GEqO9hOeB9u#I zJGb-fsijdL^)G$8&)Lbcr&dOf4z8DDU+q)>_UQa&$K{Sc`aeN0IQP-gX{8&Vxvg{= zdRGzJSK1@aZNAo!(t;+fN~-u*=?>qP zW_~%i^hMWG0)%p@Z0B0OaC7eS4ZFt=Y|<&O_FTt59P=|Lo|c?4v3|0s-sR18SJS;j z=bLj6tvn^^(Ruyk&}W`6C_=e(6~ON^e`Ws3?ychicix}3a{~8s)g-hGKX0~v@=5bk z^`vc+a}HOTCbiN)Jiz_+51<;om=!R{nq*YoFbG<<-upQ=!E#t!X`B27nrm76oQS)OS_$V)_;{OS&Rxxuf4(%hq&xD9b#~6ZeqZm1d!O21=Q{7R zXS{BgLz70wcFAeIy1$0jXGPrx@$mX_()svi0YbS#tv>tY@MKKOjgo68k0_}oI{x99 zkNtkz_|y8w#TUPG&mlVd)WYc;gH1f{hPc_Ld*?4+)V82Ir=2EwI@|M!#nIpezeKdh z&h38R)p5t8`;_+VF{r5ftD@Cn;J)>rn~R@p*gcts6{V*PU3cL+?{js_c+n->B}4l! zD(dd67U%jievDk#Iyt{becQ(+OM16T+C4HdZ!NES-VJ3OH-BYmVfWTa|2ysv5Xu$W z-MuznKOWWm)MU^3y@OkdY8Yzu@we;6*LFBHX}!;f&GiHjaETprZcg9Y$sIeji}Mp_ z6!iR|=LEG~&UJsMcGCK%GvoZ?*#Sbi)IK`*=%B7i$2|t-*V(vPzTN5PMmuiNJje67 zo{-eeV+60hE&sy>o#IDNDwb3ewbUxlx&6*OB6+s!?#cdjmW;b+-rdn@bGOg&?ELVm z3!`_Z?VIC%e%8Xfqbok!KF596xg$T`CLViN^LQ!Npq?Q0{Hc4QbB*dXNG_}&$Mer< zmeaGLwl>@cZ~f%(_`Q}J#e?4+QPO&K#)M_#7ABoK?av4pl>lm!=S1vY146ls_A=f=t1sHIed@*05~EqK z`F*^M5kO4cxjI6*jP^3-!&CbqJZtnyX^GLPjLcu^Wqb?7^qLUDrQR>+4n)sRufg|f z_h{B@wAbp=llRX*h?PsNw8Us4LZ4-))gYA1XfNZW<-N+@T3dsx*Va5~+uKP~7yDOH z_DrzEW*=#r#~uNXaM{1BCAOl-W$V!+_7$zP#MU{vR9n-C|E-9vh2*j^AgwbdL}*=# z4iWu#YF`=)n?OI>WbDh)9?ScK{g>3&Y8T6{zcyPxt+Rv+d zfOvWh2y$6X9`PY&;rDCsI&*inUhs%l!TodgN~U{})@#3e#P!(Ehp)jSOfDNukEq{a zVCk5ddzIdVKT+?_Hru)P=}iW&WZT_U8ua4crAhE7DnhyRMuvFcR~MCvZx%|kb~_>F zsByH|TD{lmjm)_|Pu40eoG_^PCH#qsP%gbEI(Ku61*QAnf2(v5{E4Tk6J-&B?#kW^VqF3y%rBYIE`| z<~kl>J9?hhJ*hafMXCP3mKPPFTw(8?dRX7mfxBNBAAjyPdG+I)h2wcwwM9%(t-hrt z=Uow>QhVwHxWL#<{vyfUeK{!RJ1=k8HbgmQ)6+Oazg z&RssLYyAAr!vY_fwkg!=m~W3sCU@E{{%Q8@MO~>XFO<>z>Xyl2mu?sLxP4K8P_EE_ zby!~)&+XkRe(2$mfsah>imn^Hv3$E;GP}d6@kU)gY_4OV^=eN$w@=^N@v5ELCAA-( zQP8!mZ!9{;oa->Tc06tQnaL$nW(Nr6(s_(KPWL{gvwL(*+THMGN$XWR7_N`cuDPxB z`~{to&u~9igmS4o_;(kyiU0HEhDoD?-pQ#YQxBAS*qrM*_K5hJQM<%vA3Q6kuVHGV zbj{<<{LCZbi@WX~ZvY>FB9u!#Q_gL1SN-I^Qj_F^Jr@RkIJIipCVU?SAK9DmkqyWF zT=xw1iz(W<@8BaF4IkN(9`^=5GSx8D>YIBP6)wGRpCaEmZKvmMp#Bo|qd3?0lK1kn zcYHs8Bi=dfbXWD>A0m1@yR>l84$l_&&KV%YKNG$#JwLHm{;63F^3P+(v9lrHIn`^V zUJ+zN9PxN@_Pci%`OaxOGIt!USA8|k?Yews{A|J1mCnQ~nR*@7 zpA>lU+&*u++{!B_M6L0Hr5;oDcO&3l#3`e?6G z1>U?=x@x7~FT7@)@?&ZK!skjGVn0_zz!i8P&%gD#+{+g~n&T_kKN~EKR^GET;wzbY z>$FXHuN~dLw0XagrK>w#5FnIG+lQ#ISyM~*Ki^7WvaoUtUzppsfu@@T|l7=8xR{%H-9K+vIh9 z=*-ubE%oG013IrCKl;G)1w|;Ajt%mV{|)a31KVGpJhj2H;xl+xQ(LP$fUbGxCM~=nS#@jO z(y4e?Q-pHq>T+&>yc^Wnxm`TF_v+sbbRIi**yP&Dr_0Zb_pS17pj?5KaBsYSRWhbU z-~7r;-p{G-`c|MCI=Ah0ee#F(=or6_lZ@JP-3{~&!MP8wxh?-CUK-|~xcZj{)e!f= zMJFVe7dDBX!fT)I$h!BbeROWOuiGRSez{@X=)iY!icl_XlXDM`Jt8@4)Gomp?)sOEfHHin~B%`g>w}P<19jfmkpzL@1Z8N9U%&$Ivv3ZaZtor3Id5=tDZ|`Nf61 zve;w$-FwyCN2kS$M)=5OD^Y~fJa@s<49~~n%wO{6vc&~@J;?RuBTEZw.JVoK$ z4j;0`AuHG2wN|8W_Cy=a^9o+W;ZIzWd02Y4sGZ)v$W?!So%9An?}zToXUF0n?)v9~FBE37+ zWhj@nR(k>8>i=s~^NYsNyRcOjhpZZE`$DZ$#x}?Qnfe5%iOLHR%BAvb_N=)biqM*z zD`MXMl6jrA-%WSU5~H*AQC6%Xu~kGao%tq@zn9rTR3yPn%;uHO#OjC-vXNqmjWcN* z+3E;u2TKC2vNc~Fp<1at8*`87i8rrzPx0PbYa#Dy)(ad1qrGLAfOj1Dc+>Ym>v!I9 zY;6-^v|s1DwB0{{cMS+bE5eWYbMWn+lD*ejYw5Ez4O6YM8A@%PLmYB!amb1=S`l8x z*RVYN5`trCGme(bXmTmqxeZ}OKQb#?gRMzbujL`EyZYlS%E#Nv=` z<)|GbmyN8qiHJjvEe=`xJ!nT}wY{a@^u9%fI}wNcDemXCYT1jA%|5c&Ad5X7x%lv+ z#Uwk|75DQ~5QqE#?&mgtI2#n9H96M}_wx&~IAleP9B@SO@GSON(RkbT5vC9BD$lvIm(9#qAP#xIvF&5C zYMi^ZrtDj^*kfC1zIV6A{d_ybA-{(ExgzTS^Pr-|9xK|p3lPP+AL5X^A&OP?RfN{$ z+yS`JKZH2s^KheAgtlDK&OLDUz|tJVA{^3GCi#=8};zjXp@NdK+FUGrptpl#X==p03yJfM*Hpl$D>WFuPNr*%K3hxF1 zVzWyMEwk8TMW=DdC5uD0wau9r2|{ba2>{XRpEiFsKLoQxr62M}--5*+>niZ?22Bx% zoZ#I+tv=481&cinE!^gaB>#2RqxpL=f?C&E4-P17ki{OG?c(gg9#H#^(!moNb9*keUIH+Rxjxm^*5{3N1SH|Vf-agQtx zS?k?=w-!Zl7{aSkYLuh&^A9IOOj!=GSezxbQ_5hunDg zs|raLd;IWT7ZoffnH$qq#ci@UDFM840> z){o~Q4!QN~y~}4q?svM*b$@j3W%x@LArASwJ-5v3ey3|N+-oPpUozs>y7|Z79hp;v zt|&!g?jijBTiy@*3@1J8uk zrPkovwGS;?OtN!_e!NZmF5-}%M+~Iy!fI|3qpMp0? zYdP14%23(P9goQV(TGD%u%Z;9c308PF|vOK;*ifn9J21`s+DT%+%0(LdM-{757 z5o(DPjc)<)&N&`&$PXb7Sy<-35z7?y&Igg2f&W zUv*)@Vv?QvH@*dU6LH9AjUHUm)1>Zw;mNX2y$10B#38S9X0x202X!CRUcie9UM(#S zS?kg<*SX-_dY>MiOh+7Y4)2_bP^}d0+(6t=TW3+lwsLq!)-_?j`TfO8`Yhb_o zQ#j&~OBRQ0C5lkm-|i5HoJbsU>JcQD&LyMyyUzj93jIZ`tc3Q;P6H`ge2dNvKs;P$ zctBEwc!%u1%p1L-y$=9!$jMffamZz@3_%%2dj#T;6N^K(5^|Z}hg|l%M<5P4u{dNa zQH0VS@eVv{zhzOyR&UCc`AErSzk9?KAbws0qAJ?C*6@|Qx(44X5q3lNGVVuw`>-qy z*-FS|H*9j*?;e4e@Dh8N<6$L=P}(EN>Hk#(eo@=Q$z|)mI>KfNOKet>wt4Iknwdhm zY~7K|R+LAWPV@*Xm)Q?;nO&)l(EhRUCzqWpt0S~tMQBaVEyOny%pySTtb{#kCvw(n zzk3<~4N(>C+)J=-D>MJDjX_l|lgHojg#r*8BkusUvJy%+G*9h;p?);DO zHF%w~9hqF#a=$wx4mq(nWV0pdj+4ET5ux(%Wf=CSN!fmGBgI|V=u}4L5%yY5LtcY= zh(n$Rf1>p$TdubkTOX9|+_}hWa0TL!=}%OI-og}}#v$h|4%tSEE!10>Ss#DOxD0s> zW+M)HKKzM_(7U0cojVzM4L(I2a#O@1+dANwD?)3+-5F6~_ahGZIYfo2bgehM4<3ZP z29swDEzN~L(Z+-JL+{bHHvKvANqpnJ)}+Sy!{A%eyT0C1LvPf4L^{nw9P%$0920oe z)F-F+XnbXgxTSiFp386QPb=EcU^K&v2hlY?A&vRDQeubRel~~ic}Nj3bh)9@6Ufk9P(87eH5Yc z6z$yY{2~`|$glm}wxDCK?Fh%?Y(ydLaCnQ-uV*eVDnff#(awE(Xy1Gb#3A=W9I{zU zS^(A1ESx{@{^t7Te?lB`e?%cELffHe=MKkr!o!d1lb?3OprX!E)k?KRR2brxO6T2_ zKO1pNicoK@q7i$()8OL25r_N~;+AyPs#dBkZuE#knt(XueLuausOv*zsBC;2j&G6o zLmYDb+ZF`~^@mU5r=%={_o_}s;MVUJyY=3A{M6~;*ifn9J0QQ z>DpG?h3GbXi#!~0$VZ*Ac$^~C1EpwuKZxj^0f<9>645)_Le*WZ9bWt3BYOruvd^mc z$dpU{WoaC8!Z_rM5QnUGSG7`Yor{rg=@G;s^PSUneeMS8I|=xx2VC z-Z^c3aK};qjLJjAme048cX;R26N~z1?9AYA!k;4F($BB=&wq$_PDQBKNYOX}AbZjm zh(kUTuaIor6p|$6w{+kkxyt?<%3k zYub=G`Mz!Dmf9f>SrO_$UanB9dr$c>zr~{GN{eq%w~j0wg%f}xv`vb}*EM)6x)pClv)Axe z)Cg}y7L)wHycHo1IloyJhpesDdc&3@XHDMXkQJf6X+`7P4F4rko?jyAde#xtUVxt( zUn1>@IOJE(-zKl?L*?lzaPBmGiL@AT$h+bMpa|7U(eT#dyP{(ehkP>jbG-xTJXQ^z z+X>$lZHGAI;VXs(9(t9bvi}#K{^9r%>1)IxzxB!OflptxQf-~P5&3<#M;!7;GcIec zHc{u8+A-(GBfrn_h(m6RceMZ^uNjK|d)^ItBfrnH>n$se+b4}fE=LRK+74~|C&=$J zq+i|A;)!V-av7nkNYTzs?^8RD5Qp5SfB0^o^EI@hr{nvmDTqTp4d)WIwYnx0jaPkS z1hP0})mJsqHw4VYYx|TgL>%&0ILWAVeJcoegQfVQ>I1wqtcR0~B2*Ja zpInSMsZ050K&_Ww%kl<8KaBnEeKIZ3 zvu|Zy9DV`HF=4%oTBeO8m;LS$m0mLVsk8B53FCvg%aLY$Fs-#mNN(IT%cgN@y(+I# z0=>@4bqg{L^XABU`Hd>?YhK1J%PKP{E0?xbdm)s;uY|aRu`X?gwl74e3`Q{X&P=q* zb8h~>?kUGQE0@Z%+2h;MX16OXKSQiC6rnXCLI|F_PqO%S?RV3iqiM8XACmDT&4M0utc@8HD4W} z>qF(KhIl{6i_9B%Bcv>AA@6EdFUP=WZy6AWTw<;XBCOx(ld-i;gwcMTe+I<8H6RSF z2tVc}yks;)W);e?*3xHb8j{OsYMaI(Gaqs_grODTWn}q~({|X5qa`z%T#80+Ls&QD zLyoDH_KD3UB8>L)D$9pl4Pj_Sc&&bdg?lfvi8g=K4wB18*4xBQVBNmW?5_5^w!>&| zsW->{ynFVYu&pTeBC{6mGrn(uIo9~Cb)jyzg3nvq?UMIO$KZbcV3rS=mXotV5n7XT z?QuWPW%qNNaYT$j4&?|KLda^)$`Stp}t4=5|yREGi zKd*Y=e%>&U)-XQ=ED^<+96S_s;?r$~@ou2Afw|&7*fT0J52W>o-v=+kyTJu`H+VP8hfI6L7UDD+ z*lmK&Fu8_p^$ zDKMMvHOPVd#R%e)42-|tvpKHMy? zL4kR4ktyWYW%(=NVd;|PLpH><$bsCY?y>^&ODY->DaePs;(nh^mpL*Jhg@JjWUZGO zRyyu?bAg$4%lVK?mJitws-dEtI~ZP^^)oLH^NDnJwTsNZsr53C!iPsZTwpFm_$=X* zxeGp-?(oT|?i(Tpa;s%06_^`0Y)2hP zkl#TbGqrhr`N%9E^08n4k&d8p^+Ims`kx(A z^doq}i6?`@R$4p{*vpS@%fMqp)+66&LMki-mKYg&4@^5Cgf{n876{yRh0?W?OmlzTO4@cLDwbufgm1diko>U2v~v>>HTf%IAUB=tbG?{!9dn%v@D#xZFbzI{Z{P#a8K-k8v~UCP z`q~1ouZ%<1lkh*019|9zUkbFr;jXp^-Z@V^x=(%y;*fPuRV$%2;adPi_K!n`@p+s1 ze8_fY&~>is5pNTCgIphPkhKT+e8{#tGouIMkc-@-)Dj^ZJKi9l#T(=@yg^#axjs~e zo^bFoipc)w5ZV70;*b@gc308PH9$V(mk`-M`=fF`}{Mkpuav&$cgee|-SCk*7@Cx9Iot9KHpZfNuf5$F~4_n$*27JXv0b$o?3S{gYZY z%jtPg_d)Fi=JK~Rxi19J=*Uc!3fhUvP`#Dq6npZ z+)|bgIlb3fz2wpvZohlPez-x-T!ZJ1YemuMUBqkiFDbDdc0#5MJr&#U{;q~NOUWW10 zTWhm~B{nNb+dTFN$)+1Cm#sT;*^2TA{JlGiluh%haj#{GSqaj%&OJi4(*CjWCzqWp zLj?Z5TAESLTCNDK$+;b2-7J>VO4y@zB4@q!yO;5hd_7<#ics1kK8AmHUgqDmF{sLA z^7uPqKY-x;We->h>oPP?_eOge(pQ1+)yA3i8p6wfPrsZ&+4`L&Hfp48G|LF{gwT2Dxl5c*N-JuOjmA-Kk};bw@56o3e~@th22HmRK*4)?UX~TlLzH_N}i-^E%th zk7}iDa!wP!9~bx@HYI3iqKn_qRaV^ z^Og^pFI8-z-oniK_`v;WE)!|^q#3`=T1dF#X&L_rYI~5BYxNH5iIGWE%t854}g*+VtndTkwthe)z`y$SfbS zc?b2D8hWD`>C`KWbW-1fdhzrgjZ7hkTlyLK6IUT_Nj(M33VYzwOA7S1gtye@FZa)% z+J9teCh{TcJyYeWKLBx-h(dZ4QApF_DN;|0>b?_lBR8wnvfw>M^$}Av5#OKJe$D4Y zHcgZ()Czu|Jp4W-`h8S)m8WRl4ZS3CGlxL=Dt zr3JlwK4e=Ts+DRB|1RQ|#+`Rl=?BCu>8w_7t)k&uLfld}#4Wvme8{?LRV&*KyoEar zQAn>L3hCroJ|D8J50#;^@e+q`k&E~idF8D>AF?5IRw^11DfkxoIed%U%Euw6@eH&w z>K9XOF&_9%cnrQ1e)0Pc(tOC~BhwyLdHDN-_)hp8{=K3nd_H7*QPQ~-zOlUUPVM+$ z{Jo-w-u3yA4WaW@(at5vhujI#J7*($N9)q{q4p6Yi0GXOh~DXj=p999y#d-yLO$eA z5Q}r$AwD0n`N-6hrk*Keu173RU&P{UaiGtKT*_X?bZx8ca;^owMZOQ;A|IROLpFqZ zpcIX0T;xN(0ns}@BMwmuJ$7Kfa!5AHb3ZD{@(m50CEhkVGNBHz-7 zc<0m;i~48m%;0arKfOLRzb*1Dy@Pj7MX1k8(a3d=>`8Ybd(wrK`H)i|nd+|SzvqqR zNaRDl71?^)XZeurjYYk6>Sw~~HOr)P5;Ezi_f+3iLXX#%_)6$2d?oZVz7kS|dQugQ z*?vf`($UDh(<{q|Y`#$Sa)nwA!godM;k%+G*w0mW^*|~bnN^Xk=SO7g;Vao5$cOyr zJxdFGB~#tii-%i0zC`*8Un2d26M!PLO^SBzApf^B^B=C^t%zAqFaCe=R+PhA(SG=L zX=#zC;>;IAmSVI)d5@*qxCNIgc-qhTsIC z>qE!G&e#3~a2WC-uXp|x@vK#i0)%?Z6^#ru_^zmk?}|oZKi4~eddyYBaz5nP@*$gt zUS+7Z|BFvQ!k0)(@g-94k9|I5vsbE>YKvKde8{)s?~I=HR+ls%vc)9o9JAYme^;A} z{61SEzt1ySK4e4a7%2Mhc{jMN{q;$&4VLBB+S}(twrBxe+o5eQBEQea$nVpCn$L%9 z2wg>rc5WQLk7|kUqYfJsz8mO#4XtP`d>?fo-VG)q4q0ujt_elsyKQ6yYH(EF(iNBb ze8{%&|0tgi z*&;;L4(gtW*bjVzbsfIJ+BwUIYzWm|(axQRZ?JB{H(0fEJ|D8}mO38c&e;fGR6U3< zss`aCqb*e3^$h{G9r=)-L}sHc_UjV7X6Sh6IQ>1Z8HhtpzuM}yFpWblpBZ!q(DOq% zA9Bom$htap-67gm4*x zzdN_-q}K7h@ZQlU^A6%4%^UR=WSJ#0+9OUu{iV$Np(WH}J$$=Q>uj_~WciR&8La=K zEDqUdo{%W(qbw%5oDaDgLb;6gGMEO-H%yJGAskQZ^(S%l-x0k zLpGWSa$8I?d&vG(#MZmyx&)pU_8n=)3U|n2lG98G=Ce$*Y+8xdtMZTwsWKBntR?KT zE?Mj`BY)4xVvWT#B%P2`Ae*hfwd!DAF^)J-nH>abRAf|oJ+b20#^4Iu4XcL z3p1KygXkTf-8fx?HU{)r*xJ^0?hzLPfoxN&MUcyC@`x-Sa{69tEu;rB&`@S5GVtSF zt=}&Gg|{XI>$Ts#jNLIquFou-%|7}+mCJtj2`B%qY5!z%N#~VEY>W)b)67z< zt<4bRvYI?1%ZHrymCYsEE47J6d&EH4-G?%}t7BlgvrR^O#2Ltke0~;(Y&L^q&hM+u zufYt4jkB0!=W64A{^btu=ifsdvdty7T+t!oBizp)%i@p~pUvUhy-X+M5JTddUASM;}^Is5$JRSFQ zvt*pRwz|0E@Oy5HNyc3b_w!8=hdcxKb44f@zhp1RB)bO?#kvsLbh{#oRdrW%i1_ic zA^BqvhkP||^omffu;p1k=9;d$TQUor#cRNJ;@w~@;*j6W@*x{Sxh_X;uXmg2Ub0l&jTTC+ls&1`PZ^R)lz`KDWl#Ac7+4&KFxfSmQ zA7p<=QP;MjL&W>Yhx|_SXG;SyOH>n``TY89Mi!HdzcF%k-_m*0hn8BN*}1$sS3Mye ziQLF`XTa$dk>m#i+Yg^!8jmPeX3{W( za;a=&r-T=0|11vqapXg`IArEau-Idj=iC{{hx{hukh{_+Bl>DPHb!pbld_m(+@0ZJ zc>{6CJ>g+t=9ckU95OQ@^v`0CRUUFDES*-`9&yOmAyW1R=k0q z#~==QA>xpGARqEqSse1c$boFJ$GAroCS)!_Y$3N&K2M<;dk2u;V)5ya_K6- zj`P*Z(pJdcy6Ud`^LGB^ey*B?mf<_(L;f6b$h{}`4)#9X6LsAnpC$Yyd*km%b#7eF zhn()Iy4Qwla2)(4P5Rd@wR^9e4>?6BSGb}sL=5Bu$i@2EWvKDzCig-d@*hVw3lPc`YIQ%NCNIYu%g8A{ zAF^qp;~$QBmJd0xe8?PY?on#tbdDj*CSG3~Ar84V;*fRcw9_O{XN{0ez+#edZpS<4 z$B09ok2qxAUlpwu1Gm%{Z_ZtdIOIdN?v!R*urq_MyKtQ!gEzsrT+vV6#fP_EGKzKeXw%MgcL z@qX{%mJ(kIAPS1wg+HfCy8n2eq5LLWlvd?a8=UTJWoH5~;H$*<AJIlJAZ2;s8T(#aC+`T~TysN+0 z8W-zQE~CASLvXJ>vj)$daaBFTJ9h_ekPp=0xzqKbs{lDg(Q4-Tr60sfDBWn@)%3KC zbE5bFjEe~6GTOHz%ZD6z4XPsSChXr*vwXi$gXp)}>rVdl}y%dTxvDeH^lu z7|nXk@8e}m0^)k)uBBGS#k!QsXfFeCoC)GNVu zR)Y1~-tJ`>KYf-qmsn!6kF?Epj}Y%*;1{*ELM~g69?|Zn$~+TVV(Xk-s%@oC)XG{& zE*k^VHW$2%N=(>tMTdxEcWIv=0-Hsoaj_STX1#Vg_cA^N;{F;C%4M`4j~1AP$7cRr z>vxtYm(gBE4v3T1;1MR5)x>ib{%*#y%BW(ii!zKWrN!ol2pa=(+1QY_(Ig$_L)H?T z8RW7V;}L^@D#xBDHCYC7=}M~{Ka6=|Yl2)h9;CH>&i$ufUGvZ<+78t))ar=FW6SxF zRl3RxWmM`_5qP#$UMRy5jKJ0sl^4oDhK<-VY}h(b&vrWIvYN~JkkkFcWN>G(@u1(w zXg{yAe8{UskjrZFh;=XvhpoZu%-z{~!6UMK$f*poC3-?8m;LS$*I_?@VhtW)a@lBl z#2(0pd^!HU+7F0Bwq9h*ZMIXM-l^~fDDoP#MI7=B_!AYOT) z(MFA}HS0rvqTa}mO&56$j(BZQZXW!Jicl`SCzkUX=MUZ(IH|uqQV|V9P;ys z3R6w=KB)5W7pIU9`QL~`9-QSvCWVd|WllaDcPsAa&f^SLh{pyVhZ>i@Z z(y24zkeegpqWTl`zO4Q^M0q1_X%FO2{GM@3;#JdI>cPm3+$@VphKB`FNS*Lk*>}wH zA)8lC@BJ#xg+9`*WUN+z^k@9av(R!Vvn^=@Dw4YXdL2@|A5~|rK?to_J3o2 zX7)GM6`@?}(MjWw2z7rmeIOH>X4oY9b`KrtHq4PM@ zYQIKT#xI|HQ|Yu0dz5sBD_7{P?Xc_MVpqf=_dp!7u3FVF)M^-_kgh@;^2iTwPv7DB z8qSrf@#`gw@_vwhdgYNnn1zp?v#-ek~xliz&a9`vdy#LeL0YbTS9wQ><+CKSX z5r=&0jXoc;y$Puu4A)09@z)&@SyMzDa=VM~PV*s~-Bk@kt#Zh>G%WkO zuX+Mde+lzLnjgiv&yjCw0^*Q6;GNS>_f_xxAp&v8(N$R-vLci#d|jF_u~%sl;*dvT z$FZ{^SCqb5saM3gn~`tnfb8$S+K$W}N9$EzjdQyq-_mV}Ltce^2>PVRD^P=2g$ivmklYMi8$nI@M>wN3EDyB(l-4)Z!CFa>lubPjRl|KEz-2JP~oo>M>Q1m%g_+*Kz2aeEXB<=3hh{vLci#^rY^F+&k?M zhkRp}583>ms+D@bU`yKmnE!6!bNOBI_YG8Evw~ovojuU_)luO&^Tuc8YQvQP~Z$%C8R&;3=d#t|2|K+XdAiNd*h_|8@Yj`VC zdC2>LZR0JRL-+HeFP!Iww}ArAQ>#3AeY(3!8Y z@veq1krv=DBAk%rLuRBC*PwERT1~@uMJ>+1DZlr*dz9=Bz!QLKqH`K=UONxYy^X(b zaL4jtfrnl-3}yV^efnGBOQhQ8ZWm9Vdwby1R}Dk0K0$t;J72G#jGfe_x!OdvT{_$fYpPcf{^94mHmyXTf^KS3}^7{-#9P-qLTjtf)s%_UbkJlyS_t~#s-TWz2 zM&=ZuT)MiPTMO?7yCDvF)5}-?ZlLoRe-Yu`+Hps`8(d!H-9Wk2N+9%$*4Wo-5^}&!>_%qG#M`qV^3WDOM_~NZ0yK7v;c9) zZ;ki)kS!`i_dd0c_Or$|x(tsrdqC}cL8ia6x{ExH7+8M^m{Ti_${ti#$uVAcGr|@}(&=R8?J+^LSHIa7i{na3p%V^5RIdPpj5oE;l7)|N1 zU9RiDSEX7R&3cD!uu<-t>8m0yV=Ex$W(X}Yn(Y|YckA4k5v$TR;hVU1>qKaID#N%~ zmvWgruNAp}tY4>~S`ndKVLQG6;$I)FRSltBM*DWGjry&3Z?3g@?wD(Sk19HiJH)tV zzS_3A(H=2HdRI$yJd9>d@BsjUzDo9JUus$tf?O&)MU;DYVcpGhM~z#RYGwUKgwbBB zFEAc8^{(}m_8Z%T{iD7g^WJV{6Rpb-Ar1J-I9e>Z#F_$GCmCfZsX8@4<|Tnxni zy~mCJy-tWwE|rH{5oY3qKUY5!*>5V(BVJzvf?BDz&UJ!T*PK0W{91LJ##R?sj&YIJ z)rELCAZkw^H-6VD2<1{sgdJye{c=WY8-uDgDn!V5#EMWZm4`^DUwX!G|J1i|Wsfdn zcDUK)&bem09Cd%DyUQKhW9uB{eSEyj-BE9&ly)u$M8lu^rt4fwNDl-Td)I0rjrj^h z=VjFq%4M{-K0qXw{?a#{yY1dy8MUhLS{coHr=Gqtx^5#cFO`v;zRV-E#AvqT>K9f< z2j1e_l(r)|Y(?Mbg`Ey~$+%dra+y4@)gcS}Mr=KM*-D5onp|N!R)2ufE$v>oNqnR5tQJ{@%C1 znZlM^3t6wCjrJ|C4+Q#awFq)qO&-Cz{lAJtOSIllt8d>srJNT>5y}0ee?gE-wZ&hr>za#x zhgPrDX&PHyTocAcT2~ihov+A6M*o-bnT+kBhrW|0{0Qz0~hu?$hcUqa;faHjMVxJ+~wrxgIjKn zHvQ*?k@bC#`jT>u?Rg&t&EEZXmxBYSA=pI?PX9K|F0sRl-v`IxTts9i%;Oc{NL<_-_Wz{71GAd z|Aq+V(mt$2Fdx}PU(Ab+9WX7ei~r^SX1$NqDMhdS>%AIeC_=f&&Hr6ptF5Pvi&k9! zR9eFSZiW9%t`nEtA6?#Z#D7637r9RY%F8$$XNBR}2%fXmZ_)4z=SJ5p8W6ob{`V+) zV@^5-M*H5K1Md1ju@7oOkjsA0#sl2nWOMi0r)Eaqje0p9bJE{~<~YB<^MvT|hQqTl z=bYl)q6|eSm(gAZEL$0&B}P-m9uGbnJuvjnOa^9Qs#QD?AiNCKN=p=> z^Eh1}sm<*R8H!LYozwV>o@g~%p7!oN(d1~S1HMV6-wS$Y&};|Cg=6B^hr=32?^Z)7 zm(kVR!8$32B}TJe9h=l%rE3tYwyALupVH!QaF2;N0OTvczcCYc+WpI)cVkRVzby z8QE@-YNgiFXx3{rc^RuiD3{S*23L~*R}tT_*H_VQM|r!XmMFA7ZRf4BQ$~PLt_4G; zMK=_^C9?YY--=jE6rpwuUT2Y^2<6IeRf=Xdsma1=i6TM^x0%RLgmT?`$5T;*XZ+3( zwnIx4q5BGAQbmR$lxtzVK~b+=ZO2)F*Ju;uJvZ+ytkB+BhBX zWPsZQxSs~#MpDXK5tT4(@c^Cs}ha!|qW&8bu_Jq=InlU%pebq1NIIpwId(kt;e-+Svo$DS| z6M|g!yGN)^T({w%X!4&uQwwM3L~W$VKuRy1;UD@r(TIqO)$ zdCU1rTD5iVl`ZBqWBaSNZ5tDxn|rLvKMMrMg> zQs<3n(b*4F|?a+F4t>~N%+o1^MQhA7h{CwlGRvcrFHD`v- zSG9Kd-r$y-%Cd z%B8X~9w%>IZn@U0tBA9OW94O3_HRX^2<1|FVLP;5UA3Go9INUw6ro%y58qVFEYW(^ zs)d$X+o1^MQhA}D!PX%6C8Mc(xX$NdkE)y9QjLoU}LClK_qupL^j ztw+i*+G_>wgty_?=hmm;LUwVn0z1wX$8GGHjM8 z{nrrWvYI@Ct!C@ryZ^V*TJHwgnxKquCeqJesP%E}W>ou!J8Af}= zEXY_FcaYT~$ffd-E%eVOWt&)&Kan!5cS!>=`OYz{pC=hbD3^`7M|kcO5u1(TSzT`| zdJDrh?s7L!gmT$=jx|+EfL5{I`)nU1mz^0sVtu(AC_=f^I~>Ze^A78>(-LVrmv|YK zn2Nw>ss4a(Ui&4w@$dJm%_YtO@NA_nt7QjL^EwI+H8h4NjWzb$KQ#Lb+6SW#4kl?PS4zXK%8a^JE|rbH8HP1T>p*Fb!?)W3K3=v~J%UD;eM>R?ORGhY z%W9&h*tyPF<2PhBQ9bcS6A`{NL~Auoo~q|9gO=!B&1gS@^p(tqjK_L!-LxhIxlEo% zOab?{nf1|Cq$|~EZ?Dz^H*8jFuWCY&%YOHWTTj?BT_3!$*!h#v%?^?deU?*K#^rgX zB{l{`Snv9Fbo_Ap8WGB+vJnHh-zTPF6^SC0OV8TQU9ex}c&Kexbcj$Hicl`yOOPcEc4X7+ z{$V>ZZHDd0N_*R$d23U=sWMMtcNLmF69+2|+IV-6PQQ*!MA_Ek*?a@p@5ftHu=26nGyy?SOe`mZ6#Wi@%k49H-AQ7h{c%CLE*v`26j zvcFb`AeYtT5fd?jh#^Z`u5-y+OBqI&Wt3kUYC@39elN?28K*|AbPR0vQHIfF8Rd4= zgdmswUY1dAhs{yeYdxxTSw^`XH6h4lzn5i{+hJqNdToX)?GZR-#Jm}?XT57D3q7|roi(t0%J65nrywJOjIqfTFWbIE zgx)#y+?BrY@_wGS!%o7i*G}O?+wWe+XX{QWonL24e(E1>Vm%exNrq_E*15@br<69S zHzhyp&o;4^=$o*fgPq%@-jvc)b*JQ?tb$N3m5p!QQUB+V^B@p<^U{`UkKl`3)IS=C zsZ|ilr7!%>jRp6^K)nHkj-cK-?JU85bM8JM-UH&*DhTD$@yB0F1EL7Tb3kal`buV} z8ZYB=AVvZ)qY6T~R9@OwCG=IEeWms4E18|nyo|dc;}al?RS?Rh@{madh-ZL!1qiKI zU&-v0SzU%AluP9~*BZ0rpBVQZkfHVJEYUd@j)x+YOXXp0qYZmP&J&Pf>wq&{@6LMj zPG#gV<|U4~me`4dt<}{91lrILGA^!yP%f2??^u9X1Q`!hLFnr{&p{TKW5M=v1e zS3xM3YKXrj1jMsIj0QsM)z^1B&-oF&7>HMam{J9yTq@7GcYzoR1m~63tFP}Gm*X5} zdmgjB#M!P0s2ceu5*>42<1|F-Y?3?7w!gX!*y*N?N8zK2-8}fP(O$-aH;&F1GK7|x zcZgg@b5BHGP9P>^2rbd|5zgITfhbndiaU#OsfJ#w6VZ;BYmi|Gu1BN&9`!P0l-AG= z>K-Egh&GJ3{-kZ%S9(6zHW}@;>b~oS@vh&k9ziav$s-ng)HZ%IzhNpv&$Z_3A(zp< zcmErLTvn4uu%9}hzc{b7E~6>K{0v^J7lC*xLuiT791q*u{oLJZ^Xua=B`?ED*zZPj zJi^`ib;!67V?wR0UP?EF%J3sN1Bins9-Y7Pr`}_ECl*?!@(W#8hS48J; zRsJ>uhz>O&gllm|dl~Ef+&4mWzn38;#zh(IzhQrDn6+aeqWbMC2;*X1%4M{d@g;h8 zt9citGPJ~K%HUXx?Ph)TA`rXQfKV=@y^M1(7Q5EpKb4^+MyGv+u}D5$J8Q=fAii9u zI)Wo+Tt<5tr=VxAYE($)m6jMy861n`n_8KS&OoeJ146ls_A>Zw#(OLL*Bn}6G-Yrs zK3nIP?AtyfR{l^Op0j80`>EQXAkoyo`nf%Q=x zAzU0iBD@T&j{?7bVLPX;$>yPt zH@an$yZ^-_6i z=biiU_ZL^X6LE9$_r&^z3j8+ z4)!wC&LhTa=-TDpRfKY>JlGHEE3Nk_j78s{*G=1@c0SZf5z3|V@Dg`u<#=el9E*MD z*7JQOJ3}}giqPFatG>*ZK% z`mWzI)Xs-}r3mFxdHAb{haFdLht|un*s|TCbZx7hcW%E=Mwi>62<1|F$TJ~pQ0wJb zOziemDnspj*bYS~m&ywfS}(_<+mZ9U4A~jd^-*rQBFxTneW<)phStlmplu;S?L7X* z$k4UZ5yZP%tO(^&d7%ugcMFUKZ3|_nov%bth9ZFFdch;KoAyYh3?e9*byE{XhltLXJaK@>Rg~9Y)z*eub&2*Ww_N*5xi-Chc~sf*Inl^6 zf~_MbWvGUsR&_7fd5mdAF4jkcqC-T(W!INSP{&*~47GYhWCRG&o&CoDqwMOvYg(x$ zT2qM760KL;SBc;(;c7DJIKJO9J zJ3n3?K`pTp2f6HoV=^icT4HA{a_KxqCg}6a`H%z4ptgiPzH{4k(>Yz)S8RvYrCgkA z_Z_ibhPY+dzU6tPy{rAEJyI!y<3Y(b=86sx=Ux1Sb-?KauH6h4lzk9^?@Jzg&c_wTpW4&e>l=g_` zK*Vc6kjrZF2zt9FE$v%)_4mt0YrVQt>z?Ryq!7VgR)lid?_NeD-2Z!L9!Rqv)ZJRE zv`6d>#La6!kjrZF2zt97%{(l&CRnevR_VWnAeYtT5li4PxI6QU+S+Elris#j4M8re z$s@MG+3JGKi(^)e^_nJ1d&HLi1wk&W$s^jpk8vhWWgI~}i?CkPL}`zh13w1OajQd+ z%WCq7w|=bj6xrHly{3uMe+@w{tH~oCTwdv|wH=xDnkGvBH3Ye=CXaxxy|hQ>zp{Oa z^_nJ1d&C?dwpjy$Tvn4u+<=+BFIFyl*G`kH*ECW3uOY}~HF-ogtc`!v_dYVSaIDuf zQQ9LO2I8yCM^+PpT=u(1+=8|7d**eva|!D;O_VOnh#@0wc})m%+3y~4Db~g>nO9A1 zt!Y9TMtejXAo^yWyP6Q>vfn*|dO+BV|6QU>0Zb;s%X8braBwAuV zD{`4{$s;!FGSR+qPqf6mL*z0qPIVbtVt$xZM&`xwh#fxLqEd#Gn8$-$qdr(Scl0i` z(l?~aR&gw)cIz*U^s#>Wgwo`{% zRYfS5ogbg})-D@#Im&! z?Kka_O0C!qJKeJ`MTdx9t4=D<5^cF^xO%M=py#il#6SH_N#h3R4dg)YYJ_mmT0|N6XM7|YnzNb zc*BI6sZ~>(Xtdwe{u_c^R@48}*qMM!R#j(vGb6J~3n)$~0!~pQAky8Xkq8=5aYX+Z z!2!h~5vK?!UPgx`zY2&T@sFZ{8i|TFAk@8u$e<#~6hv?UR5S*Z(E(cd*LT+bPQAOT zZg=`R-Dh5$8Mfus<~TQqwJu76CAQT+Jm=%{ zk072G#KM0*V1>ODmtM}7y{lSnTECa!5)rm1XG|&Q1hGZ^wurr4ca0(m9S=Rr=-qGa zwpq6?QLXI7d1ZUe23e?U7z2@VJzFKRXgS@f_QEvSYlgo`9vo)GM>KK=>9>x z{=z@5u$SV}%lWcaH^g}RTS%OJw&mqWNjxe>FM7D^D|@l-By>Dfvtukw@GP<|!jUq^ zx%h6By%dXHwll}gF?u`IzT%2wTX7vJb6gz6@)-oZY-f&`B?HeAuFkdM96%IZ_h8U#nA_9zFcy>+E{qA=}Qsz3QG1?zj2;Z>rY$;P7qc zd@#JpfBsAvqGOZf=c=pvkt0AQQNjiRUYU57*yIT`~a9Gr;>Yjgg)2LO|I`{F+ z@rcbj{q~Na*XNJkX5L!oKCXM*_$8WkI`!`?m&nWgqq^J9TJ4bv{DXItmER!2d?vEx(z#!;L8_R$ zzLPa!q|mw#V$%!PiIEx4T@j{LcN5u;i|&2IQ{=6?33{h=-!^z%RWUcAmNTyH9FLB8T&nz& zu&tVy$Xb0WYIXP(=bZbUc(=$UlV_3XJU`Tq9YXiK#2Zf}xZK(W7Kb-mSW6Jz~RrmZydzX2sm@Dq-2bXzkpeM0&)jg+| zY4xa&XYWdz+Pdd1QCt)1AKN`KuDqt_u{T{*PY zxsPXG>C=o4g|_pR2k)}8OBBm-RV&ib^3>VxR#AJ2aNR}tc#iqf)ew_(>bLJ*bSabIleK%x5B32KCjhx92BVmwkBU@KaUy zoc(5dOj!5aCCcvpQQd9#wHo{`zEgd4dG8q{n9oGEJao^OTKC-bRV>r0iS5i0y65`N zc6Y@gVVgv@!)KJ%M0&AS&XL4#9kNW$ZkE#EhYTY~s* ztfC|smvku`tY#)Eu5)BxEk-b(iL6zbTQ>Yz)at)uO?1iRS!6oTkK^C5YrOdxsJEQ-E{KokTPZJ%tH4L=`%x}YEaoHK zy@Mq!q;B;&e!CYBuc^A{zxeAzhVKtua=oMGe0tn22+* zA=q!ETjC#Yx^eiNs(XIwx(mZ^Rju>q|JxPA1FHu5;>1O-xNNAm$Tc>kTVmTQ&Kc^h zZh5`v&J%|JRJG3UIrK}z2UiXB#fi7?b=pvGdr#8wY$;};b2ZRcL@pF)3w^}uYWz19*(@+)AI81B-nD7kf7ZY{aW(d zub(wMxayv}hVt_9B((BqJyGwvL~%{fPD1@;y}msj_qpNaRrlPqynH-~x{7u=T%x!p zXiwMblc#)ixI@)FH!Uw8pU&YD#Wg`Y3AI|SSIfOx%FDF8d_0M}!drhABPMD2`OnxR&@aa*+7sN4_-NscM~*ZgbF!*OHd_GIEf(WBpGI53gG1q~n`_ah^+s z=FwlfY6Y(>OaJlZLu;L%z4I%E)<7@i12NwF$4Ywl@%Miky-e$>|K3~o9qwN>(0gTy zkyHr{5eBwpqFygjpLa}g84|dIe4XK+{ZlovY`|Vx}GlYL_0_@AL(8# zv>U-AuDW5UH_A268*O*lP&!LG&u!GA7tf;Egg)CQ!CoNU=J1NFns}DT_9`lSal&(Kl5PpFmh$o$CEF|9 zbPf_eKPKtWnRr6!$9%{8{xF=Yy60@uf9?It;RaO$y_7SzKV>)7?Jv<+^m_g)?-=T> zb>;Z`7u+$_`}$d{^B=mF@6{TD`AC;?B2=20ojtn#g;xxv$C^2Ad(pf493)sn(y`se zOk^zf+dnK_7)p&VbBsr1ZbX7{Ntg06Sf(okxA(>X{mF6mOJ-_BT^$2@OD zjQffN!$BjejONjPrmn}bL=Wq6U z`Xrc-bo^TLpU>*QU&8FKIq96CwC)s_=i_+%v@(!jKGLCb@|W?N$XMrWaoSLteagYJ ztIa`zaY>i5SG547M{zj9}k#61ElBMCKsj62;;QXFGGmn0LSKs1u%qZN)W_IjF4aCyk2b z-j(e;;mkp0RT3WaNg9h!+@*S5(*B%xTkRbS?+r+J-!@6NId~7~y`czS2lL*5_faJH z6qt0I!+RXX^1ej2cT1UL%*+kwb<6pG7{0e^o%7j1sJGs;O}D4klT;GSN4kB!>*t0V z13xv=^9vu(XBm9f=MoaMlPLGzw@bEVZp5dK&LM)4#_iVCnO?qri14Z{JMO%Gbz*7n;Zl0Ce-h|2b}xa4MS_7w*`cc&v>0e!lA`@ruPy8hkv7_eK0SMjqv$m+#L+ z+s>6Ce#d06L9}I(a9k4Ej&&o4l+r~sXAbW(@rghwq4z2c(<#DqR}<1@c=t{HJ*W4a z%Fbt&Ug5G<+pYW$kKl-2?p=Agcau2whhyDy63j3d6F*W5oib3b`p%# z<{-gq2I)@sT*}-7vAzmx{oqy9cCK(${dJZY%8vb%u#=1q^p&CQL#7^ZD)?CmBxIEFs-;I zGRLLSYX9e5#~;KRt>XO{}$dC>94ek@d-HhzTU}Xj!+n1eog$9Rxt_Y zBOMp#(2f36s2hJI2;O3C;_MY>c z@V+zto<2cvoJoh?Qg|N{f2AGGSl+!TNBb#RRkU6%MQaj_OS+T?hidkXk>lhb7|Xjb zFO(8W=_4zpldw%9`|9SOykvCg&iMu?_qcrSV>^lcFL=r5 zkN;maHe7?ThXD{_A zug*T6<8f~gcb>O+4$&quN9gsBPKq}Uzj*hNXECm8!ggfK*Ijbx=w*@P*c%Q$i(aIi zL-|Vi>(EDeb5#U->@7bT?p1Zq|FYK4hnH2Yb05#|2(OONH=;?G61wLD>z=zrUUxkC zmf`P0KSObSJadHZ`6`J?I#!=l>*>xY$CcM@Oa3_2`}&IO#O#TOGwa8Vzc;O zxOLARS6-pdFn>zbI`{D;HdzfZNyj$^K6}Z+bE{^J z6JK$CJafcYjJA(4S&W#Z^WBW~Lk~%WE3cn!_=lnN zUKH2IGsl}(Lrl`8gzour>z+HVysq5)e}_^9Qd}R;9Ip=|)~v1_E+IiXiK_D75soXb zV-LJ(cx%-<_wgj&9|}!tR~0Cikf5D}RQ@-t>Pe0(uT#$a$?(>yb?)Oy=$8cEvkGF8 z?#>VWE-hoq{BkHgtjwX>-{&9F%)j%)O zJ@*s(hG1OMZ95o?>-kqUU3*^oec6u4VI7tM3C1N|%DbUS&RAT}Lyvy`sH%b9)`|q< zlGd1ywY(XN>-kHczVW{rzjf6>Z*wp^=iFfQpf2V-%i<_f+z!MLPLd2#Ie;;Po|`k2Etlp2NYY)2@ikD!#^ zx6m|+)Uvp7zis=L|K-?QhEEHf43&7Dm;Lsi-!`-cdZ*Id6ZCRxMfkdsImT@tU)5CN zx_3G1cuW)Y@*I!6?v!K#UuhUOoXZ~@aFC(w}4qtD6?W%!3ZiVI* ziF5A!^H4f6B1n%3ogeI7_7(fEB_2Qc)lkYKic5l#;`iv^a%!*A%$BoOtZgY&X(sWY zf4yrcts3Pc9ZCpGzvyp&4))mZmqRJGOxB9Ejs5N$9@*^x)bb}rJER>mNib3=U%BH@ zf6JmjYeleDtnIk3c%F+;|E#Q9=cHSLwc;FOPqzfUNHbq4TkM}_lv*B2GxJjBQLQ)z z?Yw&6o5w2V^zs!>w68?6+gj1fy8(Ims+~mWp7$e2FYjvP<=saTZ>WUYK`+i%@4J$C zFMdgYUK#^w`pBzYOWYFWAfefJLRBClZGZhc*YqxXfqmE#QH%L4>r5oUaf)xN|0?myK!TClmUGNmE7o>Q zXgo-$e|%*iJtj0BtQBe2u+u&7uOIXx&3v7f#bCpFujW{~-f@Q8&g)>PISkgDL73J| zG?8<+{$gRLJLs#IVsSOFojD#9_s#!XpMH?At+*yKhcqUh_~(2k;}XT<)!BC6j+umM z#Wj&RHi)NPPx*ZQR-@Mp#bV2CXO3w3z`xZjv?r`s|g2ZN)W_ISvekf2)!Bd{!*h%68^h zj4-XZCNjszL+3?jmHNt8VZ~ytZ1*|zyM0rHX~i{>IUXDH_I~wgl&?jK#ah|U9FJdw zFs-;IGROaoGd6x3psSU4kc!1x+0Gmn2Jx^NglWY!kvaZ8X5o(YiI-P6#bT{&XO33| zvBM0)wBnk`90$eu_kjBQoZgox7HefYbA(d*Xk~q}O~SU~n#dgQh*@}D)jjuWsaUL) z?acA=ApDDkBy20LiOiw-q(5=R8Ev%m=Am`ZeJ8{xcd|=);azKYJIgp{NA&WuC3*Q- zRubR4^9cVk(1>1siX$&SHA-TSJI6Wb<>w{x^7EM_{_gq*d4Dyc*NXq%X#Uw%>)cO5 zlE`-JzjF=Y>!7?ykNrh>?<_C&0{gHfyg#2L7^x-L4quT)vxbWk-Y1A4-4d)7$A)cc z3E%xFE(u0z363CZ#rAc2$lk+^NO<3-e5BhP-la~~inVPC<{-^{Ey323V5FAteo(dI z>}%)M_wF3)p3}=uyhQs6o9s3Rz5HxRUVcV4P0-6vapdKvM$-hn{Jcb7em*lz(98E~ z@>=}-AmKYY5v1Fevv=7G?8BDOnWOs;#U;T=Ey0$vR;=yfgl38E=#`IjOX%#>UFu}5 zSlhT`*?Rr%{Lo6R^FYr?CJ9C=Wt08J3j7>%){5;@j?_I@4iY*i^n^wENVn}^tw^({ zTY_GsnXgmkmgaJ|PvlDGbx>C_+p6K>glQ6a9Xvh0zw(HSQ^lOK!Exo~<4JrZh=2ZW zuY2C#5AxhLowd3wh(qGrHQT;?*E3zhxbh+$-w+Do2^Z)0GMHVo=dq7xj@pjynWamV z!vyUlo)-$rU)!zMJ@4asE{U+6#Iu7qdj`QACPc^g^us$ee!E>COE~5}o;jq_Y~6Eu z@yU&kXO6E1v3KOy=EC>v;u6JjTzQc$<+S+z${Eqanvbk9fDJ?E8$v~AJxY$*uoo`3kuuh_-0RAMj1 zWu#L0iwNA~knr)Wm53c;6g3_$VGi0!ghu=*G~!1dK|Sk{muY#aulx)szEiXsVv>&U z!{wMy<c@g zw^=rPUD~DW4Ml5unO04FJZlxY=fj_c;p zbn|7mTzTJ6UOt}0x}oJAIl8{0*XNGjX4yHX-Pg6l$Me4Siw{hN@pqj0=@l+vT-Azn zr(!;M%hJ@@W_H(IgpbQ!J)wN0<4NkKV}^XcdH;OxP};HfB9Y3fp?luHBco`V~G z<9@!QAGqHq*C>xup63{khG1Og>r~7Kp?khmy5|o&rYzh4pSPV?eXrfSEc@8{o6jrm zmk%z>_C9Qz&Q4;l-{&{a=w(_x>f_nFdhU8yd>c@;bBW@bP(Ir|F*@{j`ANeozq@kj z-g8;2%lBQm^nIIeJ})mH9}}T_-kl%xde2u^F5T?|S$7}LzS0xt?f>`a;l^v-w6jYX zSG6Kt%2oG_b+(z^br<2|vg5l3t09z+bbeN}^iSDW*Bnrmh3@&h&Q5!g$eF0#-6uv+ zy-P3mv}oJeSNfi_ROK(Y>#cuHg85A3%Ame_LH%y+_ddLG>3?jnq^lMC&GwjhTvcOt z342#{cVA_#^!?oj#Bbz&a=%|Zl?3yd$d-rhdH+n`^;Im>s)_B)5xVF7Gkp@aNn|_J ztNCYcL@(CLIg+^Y$N%c@^)&>0mvku?9=vSn#eaEfr*y*>lJGcX%in+MvZYrCA(dki zj7z$dFRo@LDz0;6Uxn^@uPW~xDzQx>YxT;5mM#5b)au+=;T&u7EHa(v$5uxyTY5(j z=S>h?aoTfm%ahOY)AND*eR7TRIOTc1{ZV`LpVyLLT;?mKDgt$TIH{cK4J7HFKP*(a z*9~P2y^kTqQ@taZ?`G6Dq&tFMQaRN-OM08b$MdZe6HoliLw9wFVlf}-R%KHfB#Qw;_Tfq(-Q1A(qn&p4?x=HdW&4Kq+qVM zy=6-aUGJbz=O7`KbiETkNsn{HZfUS}Xq&GLjdOWP!CY@*%a#_p-u9l(K|(6&dS82z zjyDdYcb{C}sosCdKAcNP&`v_yZBiIhJETXhw@c+E4SK!7DlZ>Tf{O1hAwhe(R?=11`?~USPs_{4 zlVHnTLV|Wr^zD$&z20Dzmuo05A5Vh1YAzu`dzz4bzTRM!muY$VcoNZ$zE&9Q5J}*AlNo4ieJt z*1P-4LAuRBFJ4Pp;w8vILR#Tc6i^P*rF=hT;uU|lsno{lnLLWtXGToXui%?wBZOeI;k#@V@@D?Gh zc)in|wYmVagaogzq}v?4dNHmv@AYna<_Nv;ekPJ&KGJOtUXdAB+WmURK66}+93+^J zben@$ddAhS3rH&BC95zC9=JW${rK?Oqhh{ z*d#qB^c>EsrM!Gb$@U7@=9naWeoWG(yzyDj=zCXs{d%KZUaXy9Xfq# z<)D|;to6pdyrhe*clVPx_m|K0omWFJAL;m}BxVV-vqzJG48Fo5ee3ibZBy5 zwlfxcTFTba@0-p+f^kWg@)qc~GZsfp%GOfg%N*1sC&9R+OL-OA!C0L6Jk#R6ZJbdg z7?*S@U;Wwrcg`N38%fVz+7GHPS2fP#Ql9&P!+U~WQpT6oglMmJ+46^-{)B#BkzhX3 zrF`f`cR$s&Vs@$FOW#g$c|MNkh~_Q{<|EzBlD-{`B{h7h{3!>|t~Lh=#wFe9(D!R1 zV@VBPdV|WrbG{ux5{yeaegO(+6k~Cv=4#irg9PJ}Zl6r}9MrXl=edvPn1@d4DDDZn zYm`e!&`yH)A0&7_+Rp1Bci|*#E3S#mL1k{2C>GC0+nGbxSzT|{Yb0zdu8GXStFucK zi|3>5%u$~dbva1bR$LRAgM>>Ii>ra{%)!1QVOw!c^f~%x0Ny7k7H5X-%yCcbee|0w z>MIhq71zXc4wonvXQJ)C9sMs(k+7|}CNc+~0k}l5ct+XI9Msb%VOw!cWDd1jtyjxk zqF7uRY)`j?gl)w&G2ITAC>B>Z+tckJVOw!cWRBlMg_h3~-Mfm*`(3ryf96IcJmw-u z+gtz64ZJRCf6lwD_KszUdjk^Qw@uP*4&DQLZz#go!Mr!P3pq&eDKP0ahxa&&<$Z~4 z@0K!$naO9CQsS1zkG%M7pp?Dt-Nx6b<|!xhk#2vX!p{vg27YR!rx8BhKQHNj)50Yr zXiq)M81bp2b97^l=a;3N_xw-vdr^A%`XR!rw(L;6xbT#IXGwzjNV{+KZ#?~|dlB_0 z?*XMQFDUv-PdR0Kh09vK z@ZLc`uju98m6v-riSf*ui_bE~PlTC|bSY1SuCt$oPS%8x#u>DGCE=BNk}hTQnnU-T zeikY(5{%U5AmJ;{Bpu36(7^VyP9Nif(NuVVgweh<5aM6TWv=EZ*bV&Ky*gCt+K0O=J#z zO6Fa-?`svydq#QL&K!#orb%Rut0IS<>uX-IPdqc^Wjk|FlbnQY5`B(dTivrkvDk9k zeUARhNy4_`n&@-%?QkzD7H6XEKF1(F&FETA!nWd?$Q;}Oc*ZFfd)IcKqi+WZ+lp%< zb5J9mW6RlYJ9FsU5AiLCt{o(7E3S#maejDfwr^RUwJYyZ_?eFHeR6+QXM2~!y9t$e zpP)UQ?bH|5uiY$-92ebp{w}^2iQry~_adddH)i62btZaOtXQ`BErP7oR-p*Ie1l6D zjtNy@?u2L6Fl%)|5W7^(WqNV9?43;JIH>9&A09bKFdyko_k8(}L)H3(Ab97+UT9nX zH{pG1{Qb!U;p?kvLOOmMFltreuT+{jg5E!fXzw|H8ZF;Gh?6D=jx*^}o)z9JgZh^s z7|XjCmUm&w(SAzy&B&pae|dsnT+*dHA-wMj z>SIALmUolN(SAzCz9PZ6q~nSlIo654M@J6E;#_KH3Hyo!nud{?ZNd-owbG?zn(eYg1g<_UuNNXNH7!#lLqmmfPp zc)z2#{3O1V#cIWxkS^uAh(8~HS40lR^1e+u_(^=5g9PJ}F6Cb${(kXS=OANw|EV1O zBtEo1!&@zvsyqqCB^|1c;r)*Idvy?u#q*piM*P-qcuTYYstJN|Ntbd-th>HTowc58 z_sVlVpM}TW@IX5Nm&nUcymZa5t>f{H=*Y22tnGh1sOSDa(>{lSiy6;}i!g^t&AHyb?c@_Yx2dv$wF_ChH) zKWVeU<8PcMOq0mI`qdRL8JxFgzJ10$E}#3@UNvD_J#8ZQCDBiV&^_>FcF93i zt;ccY_1n-8);s!&>*JZ@r>h|*=~6bjeXPCXxbl)#yfhOO*T*x*#;YMF>G+M2P-%W( z)x~jKc}as_@3t$hk7tgHgLvpFh)Fu$yblGM2UP_c$CcM_LxWyQ4vOpJnd4uB_U< zoy2eN8NV@dUX3fS>qCQH8ZTWA!YDTA8SaL!^bm6D5Z~nUiFq-LV|V@aej>A{OD$hX{mF6p>S z#reTlUbPih8uU_so6bRkaY>i*$Y+j!7l5(6&MWR8LxWxlm6?O{sv&SyBi)WUV{tu~ z2E7z9`yAu%QzJX$l5TS_mRD`XeNbr7OQAAzjG4Is3C1N|%HWyf^@_2$o=byX>XB~8 zcuge1xTME*(iz2AT+iPY8uU_j>~r)hH3`Ng-JTze#g&>Xcv~wHj7vJ6U&RjN&2`sD z!Znl{h3#y|0YN-`24R{+YFUKtdEatrUrFggC0?~|yBu{ql34bp&-bejy*$U{<++dqRhsGLvrAq+Z<1K+fV2A?^pc*i z6lvu3m&4avw(i&dr?cCZlaOYyRA@wy9uvCOvUk~6?8BCjVwqG(6qf`eb-L$Pb?nbU z5v&z!E6wGd`j)e=q)qOYlP={{s5DD|Oe!stwPJ1KndO7;=*L_o(vF!V7%6@$<&JxI z&i0SbS`n-jYdh{Mp64Q@c`iMiNjmP&?l`oc?W`5&7<;49bC+J6uimL85xVF7 z2+~UmY0@N<*X*@KIY?;sNi|Ib>9*zUUG@TddQ2#X^p!jlNib3=_piSsFxhg}inZ-# zN#Anzl~x0{oV1AXF9}F%$!8R6#oCs#&$f5;b609fUKvO*QYo*z{oZZ_b$)QnSu57I z?JE-MA72?rmvYVRhxX?OYekwh>~zojcF>D7^L5WMMxlE?RQ()F*E`N|+j$+-y;|s= zPZ6dyuT13J-7M~#H?4Z%zIrJZR|DIbLt4n8iQL_DlCZ6~CNhT z`*sYr`|~tmT5(Nej?i=-9a*1Qdd*NQw%m5+ctQ}z%pgoFu8GXCdEEcMt3F}zd7xNq zx$VqxFP`F%u&uZzGDrOO;7GqHs4@5XtXQm-?LNmy+Rl@NX~i{>IYRe*dFY;Z&oaD{ zDHdyGJ9C8Y`SQ>`pCU{vu8GX?so0I}U7x*r|Djl{mF+%9udPnPw&I$|9MmND`K(y1 zmF>(Cy63%q2?^VZYa(;3AG0v@Ub^1(RamiDE8Cf4!$ke5u8GX?oS22X*Jos2 z;S`ItvYk1kV*bDxglWY!kvW1GgzkAa9^RKI7HefYbLc5*=$=mzrWMyj=7{@`QQUuY zE1XwL#bT{&XO7T4ABFDu6k%F%O=OO3u0O%osL}dMZ(e%SDSzse>U<}}CwH=ScKgrV zh+clSBriY9N@9~UKHIk_ve!YBegl$4quT) zvxbWk-Y1A4-4d)7$A)cc3E%xFE(u18@0Z+nc|YbHLDq`xE9K#P|7|}LNqFC;e5Bi2 zd6zm_E7mqtVfXon_lYCsAkBPj4z`vABbD;4dtTPJ!}~$iinEWivXo6R+v(*eUZVYk zO?F!=dimLsy!?zTi5K2=L|-d<`6-UP{M2YV2fh5fL|%SAGfmLT_iFNL&yR6S=0+rZ zM<;^x*k9)$dzZbyK5Pl^=oOa)Beeuu&RVgy@vRrkT@t=`RzA{g4xOF4OP#D0Ya8#F zY<*0BM(I4zlY&Wtk=h&_bJmKrE#;=I*7v=^h=k4wJz-Hk(rpgbiZpw=lsE5v;mnyx zn)!N--)@(|mCWm)u4J}V!%p{n6iVq+glQ6a9hAzg)Na)x&IZSomyaj$!yvBzb`tdB zx$Wawt9J!)@FjaLU-*Q~iF4OO(R|?WV7~ zeTltP;<+Tkb`tA`vhl;7p5K!pVVi{PP8E0*y60UE$5IK$+{ZJ=M?yjQwUwY3pWOI( z=GZ7|bpIe;AG+r*nQRB?R&$v%&a~qCc;*O=?S;@i@7m!K*Lk^hon4~1CTJ&7Rpq-{ag{MGFCSksLODox=itD~++2|v*T*wQD08opP!7`F zY#(^Gdsn1bjw`};=J@ycPSN4t$?q-k%0k+<=u+aW9mH9?@cZM=+1asFVlTyIr1;iD z5YLJ+(aIn~&wVBdAJ1Be*k={QB;EJ!KsmT$VO$^29DftN>lMx=ife*)68!A}65h$k z%eLtF_Ghf1Z>Z|{d~QhEcILP-h{p%<{m?ykES1pHFc*wT%@3HRr;oHogUA4{!_uG8l8tA1Qx_%PsE0)++ zO&lp}_0dHLdf86x&x^P0w7f^kap_^(&cChdp4(R3&u+T;yfx56VIlR~JAx&)buat* zqqmv226`!v*(Gz_{{aV`X)nd4m-A(BX z6vT5cTxX~eoJGjX$CC)%^QXvL_XaFs+_qNfJx$Q7?cIIio$A{|#r#3>P7Yh^u~Exg z;)Ebh-0_^<LNv~yCiQ+q{N4(y5ECY{$&TWs4?3TcIGz7icT1nB` zy62BLrp#OS{J#z^^VT}QZtpU04fIa;d}Q5ome^MBI+E=C{9`V3wpBZOsqXZ$ovV*N zX$WQRZtm{)-Ic=^RNZsiihJe0D~HxV&+m)gyfO)v*jB#xes$&08tAFtzTd6!9QpVo zcD9$|(#!d>R?pn#nBlJV8L~@6*p^q@jvsA3o1mBN>?@7O-Ssn;A0ANVzg~6E?WLJ$ zI|+SOWZiR?xToc1JI`}H2Y+Ua$6I&&_Z71Uc`zB5e2V7<{3A zg2fW|mAq`{IVd&uAH;V?pZAQ@pE`?>7ju`gaVT>~e|2lSTd26CZD-5Bo$B9<67q7S zBydKJSi-n%t$y>675yqof?jR!-u; z_}=Kap>@youl~P#+a=WHA6+%j$NsS|X@~+Z_F+rBCA7_-S9Q-J{M}{7gP=O z#R+=JTmPT$olDK!8t7wxwSxq`7HvmY-jJro8F`C>if>K2zmK<61N5+^)J=$y`g`7OVvHMtsGZ}X0WN7W1O2$tAZ4rx|f1HIRKog(OEJ9AWJypCXrZRPL`mEEa8^)0uTV$sWX zpJSl@QePb(dgPC;y63hPSL32F$sE6qp0zTby%dXHwlfFEL-9qa#J1vcY})a#mk4^< z?sG`Zs%wYFTcs+otsJgxwxjC9O%e36J)J{yM`NuL+ltG4T|4^Twbx{=Ok@r!2x#tT z6jfqdIUFh5u^2%w+nIwc=W1zNahb1OckMMsn&(>w^;aMN*jX&m z6~EUyuh*-5FSX9ZZoMMme4Q7raBa(Z%~0Q{UAiu*4QfkXqc*v5JWEK>tGVtX$=rZf zGL5md&Y7>2UtlJZpciu^Ma)EAUo}^)bDkbgV7@lN1}8Bd z^|=vCNHk~H7>IU|pcmT*APBF{%I;NEwr9z7tw_+z^KF{&%Bd2saI$?yC2_wu@7;eU zOoCoMKa$w-38f!F&0Ebnm1y2t>zuU>rSv^^I6K?lwp?1+);gE6wKdQ~?d=`?8wyQd z(Tlm`my|I}SQDvPTkD)X-6=u$?I1y~wjI0k)Rjz#r?p~?%&r+V{F}X&R4H+Ij>&( zfwg*qC0^C!r5R!k^!U{B#3%IgiUhrwyVE`ITh5yBjM8k;SY@raUXh>|bC+^7)N_wcXuqa2zmK<60DVDDXz~)+DXLo znEsi}EJ9vBo&@hd7}w{ca@bxoLSBxP1l!?QitCkub`sn(%p&CF<9!bOPGh%w^;KVS zJxgf!Ir_WpS%kcNd^!i?diE)Y?dcq|2zmK<5-}bljfu`I$5LFMMYPw9ke4GRQT6n@ z9E|JLKsjtDQFZ7SCFJEuNyM1<s5{RbdFhsynH-~!=Om&r;dutdtCRfj*nL> z?Gs7R%VR!847^57-q-LhEUr=5x07hR7QK(+eV?y`%I7^}-baP*d4DHNf?j+&+_Ze) z{f=UJw*0T&hLF%8E^yi1Y zG*a}kT}SYq`}!RjpES;*r^bB5SYDmw)m{zWcg?3f9t}Y+<{tNL_w=LgKh*E)O>3R= z-moQD;{AhY?@Mw7KmLJ(&q{7?K!RT0)g-ZFJgK$rxt{H+&T1{62-?o^kOJoJ_4)2B zLS8X?R~e$9Q8USi*ZX+c|>qJ0^n->gNWt2zfbQ68e-u zzW}aU@htM4SJywd|170ym@Z0ajdG-1uReVDP5u7L`%h)(n%F+GeA>N(e)q}}kAZ0S zZni`9&p)xk;fXN4n7fpBV-@AoOwz2Or}%iM?ry?n*V z90x%In@=-Iw*+&LpqHQMYDi9c6S3F($#4ifb8&N6enpteJA3GovJ(k;OpB6Ty)67=G=q?AxG*XlD8=_|6<3}1I?=SsagLSBxP1b6zrb61I<6wuD~ zYITIX9BG>1?!|ZU%3*uW2zfbD647!gzIW>`uVlOq+Fmn4UXGN6o{L(O-1p9^yYF#n zCs9@9yIRd6Mu(apqJguHyb z&mm3nuH~FdZmn|I?sG_!d{IJPj+8`vqCe6n`#L{32A+M&VLOSqI!pa~Q9@phl*FY` zBWucfjw-Huly(wTBfe|LEJ9vBo&?9kW2?BH;k1)r%V!bt^6?~MM>bei$AkAAzTeUQ zs`-4U)S1}zm3J20w@Fjf`-j}a=^GZ;N1neb-`>U7B@w*m;JzzVRbnjSyNO*6?}QcC zyKvDyo;m)q!KKT87>d?AhAJ?3#jc5H);8Wm3Ju}&g1A&##rV|QI~nERJ~;GCf_P^T z-<=@n#oX~dnb5!9EYzT-l+HUZwwygu%3CA;HzLpF69m0{)m9Bl`QPw9Bd9Zi;0W?l zGVdjlsA|&-(!eG`FOGkFJ{UPph#aFJ7?+=tc~>)?g9N>puav7I$61l%%*esG{FKbQ zv&_K}Btb9ci#OaNM`%ZPIT)9pl6jXo-3}7;V!l%DjvT|taeB0aaXCvk$9iS%ZahfP zi}_0VOvDc@`sJUA96k>;!+Gz_JMVa>A&8TsW;X=E67M+FTAp2{>=biXTH9ZrAn3*1 zUCS4u<;&G_=HRFA-Us!)yI?hT67*sX;~QJij?YDovm*!N^3!+k=Q2mMVZnOgB;RRt3fa>SBdsKkF&P#D-!f# zzPK}ubyvUQqU#{naGu+?bN9MwU8zN|gipC_=k?=?h`%JLYY+L*&a()4F?al;%4pAp z`!C72=sbUvolie(XO7T4U$E{uOZ;?5Ubb~jEafW~?78s$`raK&cz!sy&YL&~&)aih zt%+Lc%3?3pFl%*m)XMMnF^37owVl_f%R*!O_Ndiy*T3fMS?y5WTjKYx-E6esDBqmq z{+#)2XRSghePpHdS%kcNJc-afA6fUD`&vKik(cf4-B+FclF==1%{M@25%Ti!BtrLm zWZiSdvaK9`&XBcQ8N_`tCYp&Xv90m&wLRzVc|m->zAft#^}B72M|*YNFvk0?n2nP; zOfW}|px(@^y60yxmfNA$x`x?~ts=({7xr9u&+(_8#S-UGTt+ITsx)`C+A*|=t$S`; z-w~Fcur<(2sY=jO1iftM_uDrJwUa}uS`SNXE5{w7Cu|M$_$F}>@0&r;%Xa42FqBB% zT(x&tVp};v_hV=c^io3ie7TjYn0K*MVlU;;SX@&z&_h8vh*yqv&wGMi zwll{aq15xys79r?mJ9B(06`HzT21{%!?$t3CE2{>2cYZ9l zQo6kqi(a-f#~CpeKd36^EU~Q|8jH|qsqGMP=?sEiwlhclU4Z4f8nDE+a(pg&)*9&D z`7wy|V~U`c?aU#S|Mg?dQVy2bRt~Q7ol^Q>gBb+9Y-f(Go_R_CxtjEdt$Qxr;M1$t zIoJ944bW@9;m-{QB2N_pmVXbqn^KD;|pcnIXy61ggF|NkKTIXEnBgc=P+MiJ*=*4_(%NbW= z@t~@8?iD=e?xmZ*reA$X(2M!vmwTTvUauHeV_~gxuJdiJNYIPn_$r67*uexGufz#D4W*T&~*fNmp&GyHKvY6G4Nw>tPsx+%s zY&mP#*6R7ocIsPBf?iUG9=AMn&wE0xlc#!-HEe73@Q0*Iv)Z9JR+}c>663ay&sybR z4cl5RN}%rQH~q~qKMN|&tQBj*`4&oryYJM`U6wE|BgGX5za*fu$=NxtI5y)}OwKD` zeN=Z}?Xn$XX0E?-s>Ih_*}l#vK^1eBc*h|x?{JccGpZkRmUyovFV5pq7GB?fPru*w z$%|`=M(|Np1AW{IwVbi&rMYG`qLyIqvPav#I^^1a>sC0;M5zotpepc_Zi%qcke81qp=UC;Rs9mi^>t7=Y$p+SsiWVm zf{>RZC9!#^KyMLh&^lARekiV6OFN0sJs&{#yx)17ke4GR@rQWA@-Ow7jL!+hb!%xS zaYGRC`)A#giCKiad_0Lq#Z%7x>Qhdi+luR&&`x6eAYQ!+LSBxPMChI`K=-`AgLF-l z!*&t}1QFWKT|2xEPI~!xpF`T}-P1wuMMStJw3DdX>WdQca-<}#TKgDjl6ST8xvjXa z3GF1luo^;Mj@0KETKC*nWW{w&X!ki*Nyy8Ql30jmi7%@v=Dsc|u4_U&iTNP*T?HX8 zM@r(w@kDr+`b5~fNyT+dXeaT~)e!P>q$J)RGd-RmcjMs|PH|lm+G|G0%aM}UD`xug zvF>?)@8p^&hwUU@6~qp!AmqgzPXD`0-I~a?)-_QM+nHncI5+-KpTo@}XujF@_uf*9p163gf(ny^B`-f)N+NX6dx9l?IwUVY#c__cedje|iJ!yBi>t)A z9h&Vd;rZcvwahVQT7q7_)0-yPyX-gi$T)*)<=wqvkzSl&%h{&3R(}iKa}xB@EK$3% z9f$3GVLu)^o8+ky*08PBj$5za&t07pjzzj9e)j5X`ZJ1S&KkD0iX6T-7?Gft&K0d+ z*^W*28>{@YR;&pl>GuuBEoTYiGEzJPxb7u`$2>h%%(<#@O|+d?wOcjLQ0 zgR82Fxl2UYo}4kIEJko%+0K?fEYy1c7^=NGQ(U6BoL9D!prZ9GLS8%GZwhl|k%M36|JaTt3kWmCYD$>7J_{_EKDWIbYW5M;Gq7T*~qyIQwkN%aM|h znsumJPZ9L8t=z4)I?p28ipzYZP+Q$zibXHmnPWwa-g9cpx#HMX4oAv%RF&qg9rjWz zdfCn#p|h=D66gr7&bAfTkut|(1ifr$4t=L6RNuRMXYY^{i#cp3aa<5j`)(4v%9xgy z?X1;xK^zyl=NJ8U<6RkR7M;Y!K|DBSie?G-JGSN3<`A*N41!*^vmJ3pjY3s^pd9KG z%~O@wRu1Otl+tGs^s=2f9vr_5uzpqMc3hQcoV6O!&X#W##QiG4XNacdWjk|(?)gZ% z=W2)J%4-&##9Vw&?c1@+iO{Up$|i!e?Ig|!VzdfEUXGN+)-e__CQ~`+rL)$(5Icn+ zX4Q&bwzIExi`D1$xaz1KE>U)kq({?s5`!Rqy9z>Hj+De>qgEHyc1Wjv-w$rIY{}UV z?0ykJ_Sql&^wRYXSZCSxr`@l!OL-I8L4sbk6)C>)8?*39bp%;rTRE=UeuHKDD?9Wu zf_Ux>f?l>W$1}qF{5soNVq0<52Ib5gA}*Lg(93q_xF-7P)_QejiEYJIpQ%@BJAyc< z{#t>(6pLQAGslV~y7h`Bw%v}`Jml`0qtjW`79;3oJ9DfZYvNY5kL3=5d^4ZIF=7`xo@>;?Y z+ls4x(=5##BD|K^OR?x>J99k$2DGjeR3V&(RVpq;#ny#o-4Hp-9On5 z<&hW9TA%-Yt$OVQmT6?*bGOgo z8LkqKhisoKNw6I(@w}3k+NZNP3ATeJo*DA;IiJLe+HyTlWQk|H3FUW=er4zgmvF|g z*T=2UQ#F=|R8J@_`!IfsZpoJY6EBvq-`FE<4wkSN*uFLgOJuuEtZkcvC9ENH$35UH z@9sZoUa_dKuV3eq?J?o2^DIJMKAwd3C9kf#aK}>J zU;e?3=Ff_2EbTE7JDF~mF^iCwk0()g8Qm_Napm>edT!fZGeTaDlmvHVj^#1P5%Zs6D!_AGVW-yF1O2ZZA5Eke81qu}JA9stke!5{u549z`m+dm`FIi!j+Wn7 z@8})N4o!@j~wz1}xz@(av4&^jX5$*Upmtv8QH<`bRTSEpNvo?hX1~I7`^0?RY%t zq}=;xzsnNU%Du~8FJ;#yTXroM!4mc~`_QA8cSS5=zp+PJf+g$)woiM8$lTL}R?f*WV5E4vKcBD+#JVOrKis>tbGB=RizgF{67q7SBx0A*KTl*_ z*F-sNuNfgPM@nMc_PLSIJH}-@WXIsuM8uWcwU+C8r-(S zYm~fvM#*Ncx9wmFdzyVXCUiH!64~xI_Q;r+>?`&H+t=>fS;984wrviUu!e1|Hp9N1 zCCtZ2rRdjgy_3Zf&IqU?^V)@pT74%MvPJ*Ax%p&CF<4M$+*wu=0HT$$KDu?YP>TF+>ke4I%gm(HJ z;aG~RRl)7B-PcNY6N?h^a-<}9U+Y-Pp;g-BVLOR>kJIItMaav?lh`os3$}@?r$*4R zltZh($KQ4mG3FzW`7AlZc)E=+AX6nMKIU$CKFT8%H_nu+E5}p(7+~t#PmT>m9v*bA3 zf3QUTqnW5)Xvc&11}tHZw&QU(?mt+fTIs5$9PD+CwEwi@!JcLx+FSq54Oqf{V~>mp z-5aoky} z4c5KE_&K%D3Fq+DK)HSO8Mi`vQI>c#Mf(iST8)`(xnp_m%FC-n5`V(Joh6=ECcFlB z`}TgP?=?zZKBHu_*T;2IJ6OV=W*?UFgeAxK-xp;G`;9%)=3oi?hwY2seO>0^Lqd+v8^0Gh-+Yc5>#{4&(*q>!Cs0*FWZ?z&nrc#*H~g(IkaLcTAsB_gZN&( z*4j(4=w&-|aL>RJ+lu?rxc;i|GDm!eWfb46>2?|RQY?De&Ky?-v1R>kElX^>9sAWQ zTq*J0tI@0KzTIAKhrN_rcIJ3owEX9F7tRvf%Ap?7Dv|A2H;9MUU7x)ai(a-fM;-Hl z$HOa*y%bktp%o)@)G?nT=w&-|)S1{3EU~S)8jEvc51%>eY@Z_NWjk{Ou{_R?j$nyx z#nqZSZpUH-y=-R=-m$R6wsM>oV=*2N-WAzPvFK$xbG#?+y#A?accDJii`8}D0?Xuy=-TW&js<5i}Q?PiEXz- zb3k?}Uk~EbaZOjP?4?-BXD{2C9I1WVX&Yt~onAGWXCGxYaSEMc2i+qPCLVGY-;R?Npp`7KsIRdZa;YTFtE z&dUA?{Ydq*mk4^<&hw+Lq8-5!+n!fG3%Z?*R_ZB&BWQc-9PFM`>nu_Uy?j09zHpl-51NuGN literal 0 HcmV?d00001 From 4260ba13458fe4bd77fcf27f2ba340ca43f5ccf6 Mon Sep 17 00:00:00 2001 From: julienthevenoz <125280576+julienthevenoz@users.noreply.github.com> Date: Fri, 2 Feb 2024 16:22:07 +0100 Subject: [PATCH 02/21] Add simulation system tests --- .github/workflows/systemtests_sim.yml | 75 +++++++++ crazyflie/config/motion_capture.yaml | 2 +- .../data/multi_trajectory/traj0.csv | 6 +- systemtests/figure8_ideal_traj.csv | 24 +++ systemtests/mcap_handler.py | 5 +- systemtests/multi_trajectory_traj0_ideal.csv | 52 +++++++ systemtests/plotter_class.py | 146 +++++++++++------- systemtests/test_flights.py | 62 ++++++-- 8 files changed, 294 insertions(+), 78 deletions(-) create mode 100644 .github/workflows/systemtests_sim.yml create mode 100644 systemtests/figure8_ideal_traj.csv create mode 100644 systemtests/multi_trajectory_traj0_ideal.csv diff --git a/.github/workflows/systemtests_sim.yml b/.github/workflows/systemtests_sim.yml new file mode 100644 index 000000000..e30cd8fc7 --- /dev/null +++ b/.github/workflows/systemtests_sim.yml @@ -0,0 +1,75 @@ +name: System Tests Simulation + +on: + push: + branches: [ "main", "feature-systemtests-sim-fixed-timestamp" ] + # manual trigger + workflow_dispatch: + +jobs: + build: + runs-on: self-hosted + steps: + - name: Build firmware + id: step1 + run: | + ls crazyflie-firmware || git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git + cd crazyflie-firmware + git pull + git submodule sync + git submodule update --init --recursive + make cf2_defconfig + make bindings_python + cd build + python3 setup.py install --user + - name: Create workspace + id: step2 + run: | + cd ros2_ws/src || mkdir -p ros2_ws/src + - name: Checkout motion capture package + id: step3 + run: | + cd ros2_ws/src + ls motion_capture_tracking || git clone --branch ros2 --recursive https://github.com/IMRCLab/motion_capture_tracking.git + cd motion_capture_tracking + git pull + git submodule sync + git submodule update --recursive --init + - name: Checkout Crazyswarm2 + id: step4 + uses: actions/checkout@v4 + with: + path: ros2_ws/src/crazyswarm2 + submodules: 'recursive' + - name: Build workspace + id: step5 + run: | + source /opt/ros/humble/setup.bash + cd ros2_ws + colcon build --symlink-install + + - name: Flight test + id: step6 + run: | + cd ros2_ws + source /opt/ros/humble/setup.bash + . install/local_setup.bash + export PYTHONPATH="${PYTHONPATH}:/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/crazyflie-firmware/build/" + export ROS_LOCALHOST_ONLY=1 + export ROS_DOMAIN_ID=99 + python3 src/crazyswarm2/systemtests/test_flights.py --sim -v #-v is verbose argument of unittest + + - name: Upload files + id: step7 + if: '!cancelled()' + uses: actions/upload-artifact@v3 + with: + name: pdf_rosbags_and_logs + path: | + ros2_ws/results + + + + + + diff --git a/crazyflie/config/motion_capture.yaml b/crazyflie/config/motion_capture.yaml index cf5e025c3..c9324ea41 100644 --- a/crazyflie/config/motion_capture.yaml +++ b/crazyflie/config/motion_capture.yaml @@ -1,7 +1,7 @@ /motion_capture_tracking: ros__parameters: type: "optitrack" - hostname: "130.149.82.37" + hostname: "141.23.110.143" mode: "libobjecttracker" # one of motionCapture,libRigidBodyTracker diff --git a/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv b/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv index 98c33752e..8a257c03e 100644 --- a/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv +++ b/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv @@ -1,5 +1,5 @@ Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 -0.960464,0.294342,0,0,0,-2.11233,4.6768,-3.73784,1.04989,0.172656,0,0,0,3.30457,-7.9007,6.65669,-1.94082,0.069979,0,0,0,-0.297773,0.623628,-0.493335,0.138654,0,0,0,0,0,0,0,0 +0.960464,0.0,0,0,0,-2.11233,4.6768,-3.73784,1.04989,0.0,0,0,0,3.30457,-7.9007,6.65669,-1.94082,0.0,0,0,0,-0.297773,0.623628,-0.493335,0.138654,0,0,0,0,0,0,0,0 1.57331,0.176622,-0.147985,0.0532986,0.0623229,0.343746,-0.500924,0.241338,-0.0403025,0.289553,0.0742267,-0.0522539,-0.0351683,-0.975361,1.50407,-0.788135,0.141456,0.043558,-0.0592408,-0.0401473,-0.00342225,-1.05145,1.64557,-0.878344,0.159958,0,0,0,0,0,0,0,0 1.68226,0.294342,0.17123,-0.0334701,-0.0192207,-0.0992382,0.118149,-0.0472082,0.00658999,0.084993,-0.0345305,0.0714366,0.00548031,0.800464,-1.18606,0.592827,-0.10086,-0.246232,0.00212717,0.0685892,0.0024846,1.23285,-1.76366,0.869779,-0.147011,0,0,0,0,0,0,0,0 1.92896,0.374489,-0.0338637,0.00252255,0.00888171,0.0111398,-0.0108929,0.00298726,-0.000251823,0.276975,-0.0784884,-0.0934159,0.000645144,-1.04135,1.33809,-0.584077,0.0868443,0.183524,0.0937667,-0.0541849,-0.00524925,-0.318377,0.393297,-0.166638,0.0242508,0,0,0,0,0,0,0,0 @@ -29,5 +29,5 @@ Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1 1.27955,0.296893,-0.0207413,-0.0288722,0.000206756,-1.45292,2.73635,-1.78001,0.396694,0.363893,0.0205828,-0.0396241,-0.00426978,-2.18054,3.96089,-2.52441,0.555328,0.000432,0.116162,-0.0271769,-0.0121203,-0.0967398,0.0511672,0.0231312,-0.0137364,0,0,0,0,0,0,0,0 1.25924,0.129979,-0.0386626,0.0254433,0.00229348,0.314605,-0.568015,0.359101,-0.0786867,0.096373,-0.177655,-0.00188175,0.0162637,-1.43193,2.92354,-2.00733,0.465729,0.019741,-0.0837562,-0.0114419,0.0108961,-0.89746,1.80018,-1.22397,0.282268,0,0,0,0,0,0,0,0 1.35118,0.155486,0.0338914,-0.00270101,-0.0039661,0.202739,-0.374799,0.237823,-0.0512712,-0.106737,0.0769914,0.0704076,-0.0152089,1.54198,-2.90366,1.8396,-0.394692,-0.101778,0.0286494,0.0412961,-0.00329222,1.31264,-2.35215,1.45212,-0.306802,0,0,0,0,0,0,0,0 -1.14424,0.2,0.0015442,-0.00224162,0.00123466,-0.00661606,0.0134679,-0.00982511,0.00245085,0.1,-0.0953098,-0.0830381,0.0218392,-2.55729,5.66335,-4.2252,1.06822,0.1,0.0445589,-0.0317429,-0.000623191,-0.213517,0.427027,-0.293881,0.0698972,0,0,0,0,0,0,0,0 -1.06974,0.2,-7.36408e-05,0.000107209,-5.42089e-05,0.000468972,-0.00106323,0.000841569,-0.000226684,-0.1,0.0736875,0.104178,-0.0410587,3.38468,-7.9737,6.38949,-1.73737,0.1,-0.0104492,0.00871066,0.00133714,0.089598,-0.209747,0.164384,-0.043835,0,0,0,0,0,0,0,0 +1.14424,0.2,0.0015442,-0.00224162,0.00123466,-0.00661606,0.0134679,-0.00982511,0.00245085,0.0,-0.0953098,-0.0830381,0.0218392,-2.55729,5.66335,-4.2252,1.06822,0.1,0.0445589,-0.0317429,-0.000623191,-0.213517,0.427027,-0.293881,0.0698972,0,0,0,0,0,0,0,0 +1.06974,0.0,-7.36408e-05,0.000107209,-5.42089e-05,0.000468972,-0.00106323,0.000841569,-0.000226684,0.0,0.0736875,0.104178,-0.0410587,3.38468,-7.9737,6.38949,-1.73737,0.1,-0.0104492,0.00871066,0.00133714,0.089598,-0.209747,0.164384,-0.043835,0,0,0,0,0,0,0,0 diff --git a/systemtests/figure8_ideal_traj.csv b/systemtests/figure8_ideal_traj.csv new file mode 100644 index 000000000..5b91fc79f --- /dev/null +++ b/systemtests/figure8_ideal_traj.csv @@ -0,0 +1,24 @@ +duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, + +#### wait on the ground +0.6,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + +####takeoff +2.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000001,0.000000,0.000000,0.232052,0.184839,0.030911,-0.176192,0.050572,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +####hover +3.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +####figure8 +1.050000,0.000000,-0.000000,0.000000,-0.000000,0.830443,-0.276140,-0.384219,0.180493,-0.000000,0.000000,-0.000000,0.000000,-1.356107,0.688430,0.587426,-0.329106,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.710000,0.396058,0.918033,0.128965,-0.773546,0.339704,0.034310,-0.026417,-0.030049,-0.445604,-0.684403,0.888433,1.493630,-1.361618,-0.139316,0.158875,0.095799,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.620000,0.922409,0.405715,-0.582968,-0.092188,-0.114670,0.101046,0.075834,-0.037926,-0.291165,0.967514,0.421451,-1.086348,0.545211,0.030109,-0.050046,-0.068177,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.700000,0.923174,-0.431533,-0.682975,0.177173,0.319468,-0.043852,-0.111269,0.023166,0.289869,0.724722,-0.512011,-0.209623,-0.218710,0.108797,0.128756,-0.055461,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.560000,0.405364,-0.834716,0.158939,0.288175,-0.373738,-0.054995,0.036090,0.078627,0.450742,-0.385534,-0.954089,0.128288,0.442620,0.055630,-0.060142,-0.076163,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.560000,0.001062,-0.646270,-0.012560,-0.324065,0.125327,0.119738,0.034567,-0.063130,0.001593,-1.031457,0.015159,0.820816,-0.152665,-0.130729,-0.045679,0.080444,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.700000,-0.402804,-0.820508,-0.132914,0.236278,0.235164,-0.053551,-0.088687,0.031253,-0.449354,-0.411507,0.902946,0.185335,-0.239125,-0.041696,0.016857,0.016709,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.620000,-0.921641,-0.464596,0.661875,0.286582,-0.228921,-0.051987,0.004669,0.038463,-0.292459,0.777682,0.565788,-0.432472,-0.060568,-0.082048,-0.009439,0.041158,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.710000,-0.923935,0.447832,0.627381,-0.259808,-0.042325,-0.032258,0.001420,0.005294,0.288570,0.873350,-0.515586,-0.730207,-0.026023,0.288755,0.215678,-0.148061,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +1.053185,-0.398611,0.850510,-0.144007,-0.485368,-0.079781,0.176330,0.234482,-0.153567,0.447039,-0.532729,-0.855023,0.878509,0.775168,-0.391051,-0.713519,0.391628,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +####hover +1.7,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +####landing +2.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000001,0.000000,0.000000,-0.232049,-0.184841,-0.030916,0.176196,-0.050573,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 diff --git a/systemtests/mcap_handler.py b/systemtests/mcap_handler.py index de15911c1..5e55a8784 100644 --- a/systemtests/mcap_handler.py +++ b/systemtests/mcap_handler.py @@ -41,7 +41,9 @@ def write_mcap_to_csv(self, inputbag:str, outputfile:str): f = open(outputfile, 'w+') writer = csv.writer(f) for topic, msg, timestamp in self.read_messages(inputbag): - writer.writerow([timestamp, msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z]) + if topic =="/tf": + t = msg.transforms[0].header.stamp.sec + msg.transforms[0].header.stamp.nanosec *10**(-9) + writer.writerow([t, msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z]) f.close() except FileNotFoundError: print(f"McapHandler : file {outputfile} not found") @@ -62,3 +64,4 @@ def write_mcap_to_csv(self, inputbag:str, outputfile:str): translator = McapHandler() translator.write_mcap_to_csv(args.inputbag,args.outputfile) + diff --git a/systemtests/multi_trajectory_traj0_ideal.csv b/systemtests/multi_trajectory_traj0_ideal.csv new file mode 100644 index 000000000..5cc458941 --- /dev/null +++ b/systemtests/multi_trajectory_traj0_ideal.csv @@ -0,0 +1,52 @@ +Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 + +#wait before takeoff +0.5,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + + +####takeoff +2.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000001,0.000000,0.000000,0.232052,0.184839,0.030911,-0.176192,0.050572,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + +####hover +3.5,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + +####flying around +0.960464,0.0,0,0,0,-2.11233,4.6768,-3.73784,1.04989,0.0,0,0,0,3.30457,-7.9007,6.65669,-1.94082,1.0,0,0,0,-0.297773,0.623628,-0.493335,0.138654,0,0,0,0,0,0,0,0 +1.57331,0.176622,-0.147985,0.0532986,0.0623229,0.343746,-0.500924,0.241338,-0.0403025,0.289553,0.0742267,-0.0522539,-0.0351683,-0.975361,1.50407,-0.788135,0.141456,1.043558,-0.0592408,-0.0401473,-0.00342225,-1.05145,1.64557,-0.878344,0.159958,0,0,0,0,0,0,0,0 +1.68226,0.294342,0.17123,-0.0334701,-0.0192207,-0.0992382,0.118149,-0.0472082,0.00658999,0.084993,-0.0345305,0.0714366,0.00548031,0.800464,-1.18606,0.592827,-0.10086,0.753768,0.00212717,0.0685892,0.0024846,1.23285,-1.76366,0.869779,-0.147011,0,0,0,0,0,0,0,0 +1.92896,0.374489,-0.0338637,0.00252255,0.00888171,0.0111398,-0.0108929,0.00298726,-0.000251823,0.276975,-0.0784884,-0.0934159,0.000645144,-1.04135,1.33809,-0.584077,0.0868443,1.183524,0.0937667,-0.0541849,-0.00524925,-0.318377,0.393297,-0.166638,0.0242508,0,0,0,0,0,0,0,0 +2.03711,0.374489,0.0286391,-6.1852e-05,-0.00368623,0.00253097,-0.0050055,0.00272644,-0.000441277,-0.358361,0.0262046,0.110055,-0.00620508,0.544041,-0.672761,0.279349,-0.0393627,1.0461,-0.0452504,0.0303272,0.00292994,0.0964916,-0.116635,0.047237,-0.00652766,0,0,0,0,0,0,0,0 +1.08507,0.4,-0.00978883,-0.00352811,0.00421159,-0.147076,0.385891,-0.320935,0.0884478,0.1,-0.0261795,-0.0932209,0.0135473,-3.39392,7.64989,-5.90562,1.55714,1.1,0.0120733,-0.0160302,0.00109914,-0.170679,0.405338,-0.316775,0.0837347,0,0,0,0,0,0,0,0 +1.46341,0.4,0.0345966,0.0216228,0.00040938,0.113468,-0.193811,0.109932,-0.021215,-0.1,-0.00953811,0.0938259,0.00686414,1.78784,-3.00192,1.71647,-0.335021,1.1,0.0162005,0.0183118,0.001431,0.543281,-0.905734,0.51742,-0.101032,0,0,0,0,0,0,0,0 +2.19353,0.492638,0.0469231,-0.0199807,-0.000892996,-0.0338512,0.0362701,-0.0130911,0.00162474,0.203839,-0.0288206,-0.11848,-0.00554452,-0.630569,0.72139,-0.276928,0.0361384,1.209968,0.00986131,-0.0243065,-0.00119445,-0.0998581,0.115095,-0.0441976,0.00576199,0,0,0,0,0,0,0,0 +2.40126,0.48694,-0.00615544,0.0115191,0.000423929,0.00796213,-0.00925373,0.00321793,-0.000373305,-0.469909,0.0557579,0.149918,-0.00106415,0.538482,-0.564538,0.198219,-0.0236403,1.119622,0.0192151,0.028845,-0.000113746,0.115626,-0.12072,0.0423492,-0.00505007,0,0,0,0,0,0,0,0 +2.46094,0.515442,-0.000365976,-0.0122673,2.25527e-05,4.3861e-06,0.00181356,-0.000737567,8.47618e-05,0.463845,-0.0412104,-0.17329,0.000264977,-0.593703,0.60121,-0.20494,0.023781,1.330891,0.000348253,-0.0330759,0.000313802,-0.0979748,0.0999974,-0.0342066,0.00397451,0,0,0,0,0,0,0,0 +2.68427,0.48694,0.00486563,0.0132321,-0.00114426,0.0129524,-0.0144619,0.00482814,-0.000528691,-0.71593,-0.0408684,0.184692,0.00619732,0.442694,-0.405637,0.125531,-0.0132641,1.142929,-0.00451953,0.0270264,-0.00218942,0.0121858,-0.0179685,0.00647746,-0.000732907,0,0,0,0,0,0,0,0 +1.92558,0.505486,-0.0481952,-0.0204291,0.00284946,-0.0470553,0.0773259,-0.0377233,0.00598077,0.718761,0.267399,-0.141554,-0.0117975,-0.800345,0.963882,-0.403221,0.0582575,1.098904,-0.131338,-0.0396354,0.00782912,-0.4965,0.661823,-0.295209,0.0445271,0,0,0,0,0,0,0,0 +2.0699,0.421491,0.0186218,0.0156054,-0.00129697,0.07458,-0.0953844,0.0399616,-0.00562765,0.302142,-0.266958,0.0381168,0.0177516,-0.0552536,0.0911841,-0.0450511,0.00702141,0.771628,0.115857,0.0879918,-0.00554767,0.814447,-0.973674,0.396022,-0.0548837,0,0,0,0,0,0,0,0 +1.9596,0.48694,-0.0304046,-0.0213345,0.00170875,-0.231304,0.295149,-0.127553,0.0187408,0.120541,0.123032,0.0122544,-0.012462,0.149347,-0.218046,0.101473,-0.0155924,1.505486,0.00192474,-0.107286,0.000790515,-1.01863,1.26183,-0.536142,0.0779269,0,0,0,0,0,0,0,0 +1.70183,0.333009,0.0183142,0.028215,-0.00185434,0.43896,-0.63547,0.3139,-0.0528803,0.232223,-0.140993,-0.0449547,0.00936592,-1.12957,1.6455,-0.81818,0.138527,0.832615,-0.101997,0.0790866,0.00096767,0.497639,-0.719611,0.351991,-0.0587304,0,0,0,0,0,0,0,0 +1.34472,0.48694,-0.00625689,-0.0317257,0.000382684,-1.03393,1.83762,-1.13117,0.238966,-0.226478,0.0101228,0.0610516,-0.00764983,1.25229,-2.30188,1.44081,-0.307367,0.917626,-0.033286,-0.0550669,0.00313228,-1.52482,2.7969,-1.74891,0.372958,0,0,0,0,0,0,0,0 +2.1222,0.333009,-0.0545061,0.0199991,0.00480515,0.107972,-0.116334,0.0436145,-0.00566624,-0.072397,-0.0307885,-0.0689067,-0.00236443,-0.48434,0.57055,-0.226496,0.0305878,0.717497,0.028035,0.0816772,0.00612047,0.73381,-0.837942,0.327938,-0.0439369,0,0,0,0,0,0,0,0 +2.22722,0.421491,0.0660872,-0.00287208,-0.00340824,0.062709,-0.0713995,0.0277343,-0.00364623,-0.495874,0.0534612,0.0981472,0.00118253,0.675836,-0.73877,0.276221,-0.03532,1.457563,0.131649,-0.0799577,-0.00792167,-0.366161,0.385528,-0.140404,0.0176255,0,0,0,0,0,0,0,0 +1.85734,0.540865,-0.00634081,-0.00724899,0.00147892,-0.0808568,0.10675,-0.0483186,0.00746342,0.379252,0.132248,-0.0957692,-0.00873243,-1.07307,1.34465,-0.588112,0.0887744,1.038957,-0.200084,0.0246642,0.00970083,-0.25658,0.36452,-0.173423,0.0277121,0,0,0,0,0,0,0,0 +1.69138,0.496281,-0.00160133,0.00618968,0.000136705,0.124512,-0.174729,0.0851269,-0.0142547,-0.1853,-0.289684,0.024136,0.0175717,-0.384504,0.663858,-0.362455,0.0651215,0.81169,0.0702784,0.0295687,-0.00247486,1.21881,-1.70715,0.833757,-0.139995,0,0,0,0,0,0,0,0 +2.19933,0.540865,0.0144117,-0.00391837,-0.00119932,-0.0275664,0.0268387,-0.00940817,0.0011576,-0.385893,0.235345,0.0702187,-0.0148152,0.588274,-0.67496,0.261551,-0.0343838,1.323729,0.185085,-0.00841054,-0.00925475,0.039381,-0.0748132,0.0351087,-0.00510156,0,0,0,0,0,0,0,0 +2.68201,0.500322,-0.0412104,-0.00763103,0.000713901,-0.0520821,0.0470691,-0.0146416,0.00155911,0.387882,-0.0842107,-0.121138,0.00454433,-0.324702,0.302031,-0.0946549,0.0100916,1.366876,-0.264264,-0.0765883,0.0107036,-0.298247,0.281606,-0.0893991,0.00961875,0,0,0,0,0,0,0,0 +2.21932,0.292621,-0.0377479,0.0107761,0.00185615,0.0523879,-0.0498758,0.0171324,-0.002072,-0.664787,-0.0756608,0.114634,0.000204595,0.482644,-0.530616,0.198653,-0.0254146,0.289226,0.0179677,0.111474,-0.00732309,0.507412,-0.56779,0.21534,-0.0277882,0,0,0,0,0,0,0,0 +0.826943,0.365479,0.0796986,0.011073,-0.00345361,6.10559,-17.7498,17.8943,-6.18313,-0.128196,0.0924843,-0.0686475,0.000538459,-0.967377,2.3937,-2.12002,0.661123,0.899606,0.03983,-0.0650002,0.0145722,-1.19604,3.65303,-3.72378,1.2885,0,0,0,0,0,0,0,0 +1.70943,0.515442,0.0778242,-0.0130839,-0.00452718,-0.0331128,0.0135171,0.00336571,-0.00165812,-0.128196,-0.0503162,-0.00225906,0.0131073,0.046757,-0.0481522,0.016617,-0.00196654,0.899606,0.0424298,0.0647269,0.0125499,1.45743,-2.08038,1.01596,-0.169609,0,0,0,0,0,0,0,0 +2.0764,0.515442,-0.0859543,-0.0188315,0.00474201,-0.258721,0.311102,-0.127276,0.0176996,-0.128196,0.0471026,0.00501662,-0.00187713,0.203127,-0.228639,0.0905749,-0.0123697,1.418091,0.0269611,-0.0986299,-0.00460336,-0.709972,0.831305,-0.332664,0.0455369,0,0,0,0,0,0,0,0 +1.34033,0.241704,0.00169625,0.0267376,-0.0040746,0.496448,-0.922,0.581167,-0.124661,0.125962,0.103337,0.00817997,-0.00308402,1.45034,-2.64983,1.66366,-0.356691,0.813306,-0.0951879,0.0808931,0.00400333,1.56751,-2.73421,1.6614,-0.348002,0,0,0,0,0,0,0,0 +1.27955,0.296893,-0.0207413,-0.0288722,0.000206756,-1.45292,2.73635,-1.78001,0.396694,0.363893,0.0205828,-0.0396241,-0.00426978,-2.18054,3.96089,-2.52441,0.555328,1.000432,0.116162,-0.0271769,-0.0121203,-0.0967398,0.0511672,0.0231312,-0.0137364,0,0,0,0,0,0,0,0 +1.25924,0.129979,-0.0386626,0.0254433,0.00229348,0.314605,-0.568015,0.359101,-0.0786867,0.096373,-0.177655,-0.00188175,0.0162637,-1.43193,2.92354,-2.00733,0.465729,1.019741,-0.0837562,-0.0114419,0.0108961,-0.89746,1.80018,-1.22397,0.282268,0,0,0,0,0,0,0,0 +1.35118,0.155486,0.0338914,-0.00270101,-0.0039661,0.202739,-0.374799,0.237823,-0.0512712,-0.106737,0.0769914,0.0704076,-0.0152089,1.54198,-2.90366,1.8396,-0.394692,0.898222,0.0286494,0.0412961,-0.00329222,1.31264,-2.35215,1.45212,-0.306802,0,0,0,0,0,0,0,0 +1.14424,0.2,0.0015442,-0.00224162,0.00123466,-0.00661606,0.0134679,-0.00982511,0.00245085,0.0,-0.0953098,-0.0830381,0.0218392,-2.55729,5.66335,-4.2252,1.06822,1.1,0.0445589,-0.0317429,-0.000623191,-0.213517,0.427027,-0.293881,0.0698972,0,0,0,0,0,0,0,0 +1.06974,0.0,-7.36408e-05,0.000107209,-5.42089e-05,0.000468972,-0.00106323,0.000841569,-0.000226684,0.0,0.0736875,0.104178,-0.0410587,3.38468,-7.9737,6.38949,-1.73737,1.1,-0.0104492,0.00871066,0.00133714,0.089598,-0.209747,0.164384,-0.043835,0,0,0,0,0,0,0,0 + +####hover +2.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + + +####landing +2.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000001,0.000000,0.000000,-0.232049,-0.184841,-0.030916,0.176196,-0.050573,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py index 5655aedb6..4b34d461c 100644 --- a/systemtests/plotter_class.py +++ b/systemtests/plotter_class.py @@ -9,8 +9,8 @@ class Plotter: - def __init__(self): - self.params = {'experiment':'1','trajectory':'figure8.csv','motors':'standard motors(CF)', 'propellers':'standard propellers(CF)'} + def __init__(self, sim_backend = False): + self.params = {'experiment':'1','trajectory':'','motors':'standard motors(CF)', 'propellers':'standard propellers(CF)'} self.bag_times = np.empty([0]) self.bag_x = np.empty([0]) self.bag_y = np.empty([0]) @@ -20,13 +20,16 @@ def __init__(self): self.ideal_traj_z = np.empty([0]) self.euclidian_dist = np.empty([0]) self.deviation = [] #list of all indexes where euclidian_distance(ideal - recorded) > EPSILON - - self.EPSILON = 0.05 # euclidian distance in [m] between ideal and recorded trajectory under which the drone has to stay to pass the test - self.DELAY_CONST_FIG8 = 4.75 #this is the delay constant which I found by adding up all the time.sleep() etc in the figure8.py file. This could be implemented better later ? + self.test_name = None + + self.SIM = sim_backend #indicates if we are plotting data from real life test or from a simulated test. Default is false (real life test) + self.EPSILON = 0.1 # euclidian distance in [m] between ideal and recorded trajectory under which the drone has to stay to pass the test (NB : epsilon is doubled for multi_trajectory test) + self.ALLOWED_DEV_POINTS = 0.05 #allowed percentage of datapoints whose deviation > EPSILON while still passing test (currently % for fig8 and 10% for mt) + self.DELAY_CONST_FIG8 = 4.75 #this is the delay constant which I found by adding up all the time.sleep() etc in the figure8.py file. + if self.SIM : #It allows to temporally adjust the ideal and real trajectories on the graph. Could this be implemented in a better (not hardcoded) way ? + self.DELAY_CONST_FIG8 = -0.18 #for an unknown reason, the delay constant with the sim_backend is different self.ALTITUDE_CONST_FIG8 = 1 #this is the altitude given for the takeoff in figure8.py. I should find a better solution than a symbolic constant ? - self.ALTITUDE_CONST_MULTITRAJ = 1 #takeoff altitude for traj0 in multi_trajectory.py - self.X_OFFSET_CONST_MULTITRAJ = -0.3 #offest on the x axis between ideal and real trajectory. Reason: ideal trajectory (traj0.csv) starts with offset of 0.3m and CrazyflieServer.startTrajectory() is relative to start position - + def file_guard(self, pdf_path): msg = None if os.path.exists(pdf_path): @@ -47,22 +50,11 @@ def file_guard(self, pdf_path): def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): '''Method that reads the csv data of the ideal test trajectory and of the actual recorded trajectory and initializes the attribute arrays''' - #check which test we are plotting : figure8 or multi_trajectory or another one - if("figure8" in rosbag_csvfile): - fig8, m_t = True, False - print("Plotting fig8 test data") - elif "multi_trajectory" in rosbag_csvfile: - fig8, m_t = False, True - print("Plotting multi_trajectory test data") - else: - fig8, m_t = False, False - print("Plotting unspecified test data") - + #get ideal trajectory data self.ideal_traj_csv = Trajectory() try: path_to_ideal_csv = os.path.join(os.path.dirname(os.path.abspath(__file__)),ideal_csvfile) - print(path_to_ideal_csv) self.ideal_traj_csv.loadcsv(path_to_ideal_csv) except FileNotFoundError: print("Plotter : File not found " + path_to_ideal_csv) @@ -91,9 +83,15 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): no_match_in_idealcsv=[] + delay = 0 + if self.test_name == "fig8": + delay = self.DELAY_CONST_FIG8 + elif self.test_name == "m_t": + delay = self.DELAY_CONST_MT + for i in range(bag_arrays_size): try: - pos = self.ideal_traj_csv.eval(self.bag_times[i] - self.DELAY_CONST_FIG8).pos + pos = self.ideal_traj_csv.eval(self.bag_times[i] - delay).pos except AssertionError: no_match_in_idealcsv.append(i) pos = [0,0,0] #for all recorded datapoints who cannot be matched to a corresponding ideal position we assume the drone is on its ground start position (ie those datapoints are before takeoff or after landing) @@ -101,14 +99,6 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): self.ideal_traj_x[i], self.ideal_traj_y[i], self.ideal_traj_z[i]= pos[0], pos[1], pos[2] - #special cases - if fig8: - self.ideal_traj_z[i] = self.ALTITUDE_CONST_FIG8 #special case: in fig8 no altitude is given in the trajectory polynomials (idealcsv) but is fixed as the takeoff altitude in figure8.py - elif m_t: - self.ideal_traj_z[i] = pos[2] + self.ALTITUDE_CONST_MULTITRAJ #for multi_trajectory the altitude given in the trajectory polynomials is added to the fixed takeoff altitude specified in multi_trajectory.py - self.ideal_traj_x[i] = pos[0] + self.X_OFFSET_CONST_MULTITRAJ #the x-axis is offset by 0.3 m because ideal start position not being (0,0,0) - - self.euclidian_dist[i] = np.linalg.norm([self.ideal_traj_x[i]-self.bag_x[i], self.ideal_traj_y[i]-self.bag_y[i], self.ideal_traj_z[i]-self.bag_z[i]]) if self.euclidian_dist[i] > self.EPSILON: @@ -146,8 +136,7 @@ def no_match_warning(self, unmatched_values:list): def adjust_arrays(self): ''' Method that trims the self.bag_* attributes to get rid of the datapoints where the drone is immobile on the ground and makes self.bag_times start at 0 [s]''' - print(f"rosbag initial length {(self.bag_times[-1]-self.bag_times[0]) * 10**-9}s") - + print(f"rosbag initial length {(self.bag_times[-1]-self.bag_times[0]) }s") #find the takeoff time and landing times ground_level = self.bag_z[0] airborne = False @@ -157,16 +146,16 @@ def adjust_arrays(self): for z_coord in self.bag_z: if not(airborne) and z_coord > ground_level + ground_level*(0.1): #when altitude of the drone is 10% higher than the ground level, it started takeoff takeoff_index = i - takeoff_time = self.bag_times[takeoff_index] airborne = True - print(f"takeof time is {(takeoff_time-self.bag_times[0]) * 10**-9}") - if airborne and z_coord < ground_level + ground_level*(0.1): #find when it lands again + print(f"takeoff time is {self.bag_times[takeoff_index]}s") + if airborne and z_coord <= ground_level + ground_level*(0.1): #find when it lands again landing_index = i - landing_time = self.bag_times[landing_index] - print(f"landing time is {(landing_time-self.bag_times[0]) * 10**-9}") + print(f"landing time is {self.bag_times[landing_index]}s") break i+=1 + + assert (takeoff_index != None) and (landing_index != None), "Plotter : couldn't find drone takeoff or landing" @@ -176,26 +165,44 @@ def adjust_arrays(self): index_arr = np.arange(len(self.bag_times)) slicing_arr = np.delete(index_arr, index_arr[takeoff_index:landing_index+1]) #in our slicing array we take out all the indexes of when the drone is in flight so that it only contains the indexes of when the drone is on the ground - #delete the datapoints where drone is on the ground - self.bag_times = np.delete(self.bag_times, slicing_arr) - self.bag_x = np.delete(self.bag_x, slicing_arr) - self.bag_y = np.delete(self.bag_y, slicing_arr) - self.bag_z = np.delete(self.bag_z, slicing_arr) + # #delete the datapoints where drone is on the ground + # self.bag_times = np.delete(self.bag_times, slicing_arr) + # self.bag_x = np.delete(self.bag_x, slicing_arr) + # self.bag_y = np.delete(self.bag_y, slicing_arr) + # self.bag_z = np.delete(self.bag_z, slicing_arr) assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size after trimming" - #rewrite bag_times to start at 0 and be written in [s] instead of [ns] - bag_start_time = self.bag_times[0] - self.bag_times = (self.bag_times-bag_start_time) * (10**-9) - assert self.bag_times[0] == 0 print(f"trimmed bag_times starts: {self.bag_times[0]}s and ends: {self.bag_times[-1]}, size: {len(self.bag_times)}") - def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): + def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str, overwrite=False): '''Method that creates the pdf with the plots''' + #check which test we are plotting : figure8 or multi_trajectory or another one + if("figure8" in rosbag_csvfile): + self.test_name = "fig8" + self.params["trajectory"] = "figure8" + print("Plotting fig8 test data") + elif "multi_trajectory" in rosbag_csvfile: + self.test_name = "mt" + self.params["trajectory"] = "multi_trajectory" + self.EPSILON *= 2 #multi_trajectory test has way more difficulties + self.ALLOWED_DEV_POINTS *= 2 + print("Plotting multi_trajectory test data") + else: + self.test_name = "undefined" + self.params["trajectory"] = "undefined" + print("Plotting unspecified test data") + + self.read_csv_and_set_arrays(ideal_csvfile,rosbag_csvfile) + offset_list = self.find_temporal_offset() + if len(offset_list) == 1: + offset_string = f"temporal offset : {offset_list[0]}s \n" + elif len(offset_list) ==2: + offset_string = f"averaged temporal offset : {(offset_list[0]+offset_list[1])/2}s \n" passed="failed" if self.test_passed(): @@ -206,7 +213,8 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): pdfname= pdfname + '.pdf' #check if user wants to overwrite the report file - self.file_guard(pdfname) + if not overwrite : + self.file_guard(pdfname) pdf_pages = PdfPages(pdfname) #create title page @@ -223,7 +231,7 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): title_text_parameters = f'Parameters:\n' for key, value in self.params.items(): title_text_parameters += f" {key}: {value}\n" - title_text_results = f'Results: test {passed}\n' + f'max error : ' + title_text_results = f'Results: test {passed}\n' + offset_string + f'max error : ' title_text = text + "\n" + title_text_settings + "\n" + title_text_parameters + "\n" + title_text_results fig = plt.figure(figsize=(5,8)) @@ -321,18 +329,41 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): print("Results saved in " + pdfname) def test_passed(self) -> bool : - '''Returns True if the deviation between recorded and ideal trajectories of the drone always stayed lower - than EPSILON. Returns False otherwise ''' + '''Returns True if the deviation between recorded and ideal trajectories of the drone didn't exceed EPSILON for more than ALLOWED_DEV_POINTS % of datapoints. + Returns False otherwise ''' nb_dev_points = len(self.deviation) + threshold = self.ALLOWED_DEV_POINTS * len(self.bag_times) + percentage = (len(self.deviation) / len(self.bag_times)) * 100 - if nb_dev_points == 0: - print("Test passed") + if nb_dev_points < threshold: + print(f"Test {self.test_name} passed : {percentage:8.4f}% of datapoints had deviation larger than {self.EPSILON}m ({self.ALLOWED_DEV_POINTS * 100}% max for pass)") return True else: - print(f"The deviation between ideal and recorded trajectories is greater than {self.EPSILON}m for {nb_dev_points} " - f"datapoints, which corresponds to a duration of {nb_dev_points*0.01}s") + print(f"Test {self.test_name} failed : The deviation between ideal and recorded trajectories is greater than {self.EPSILON}m for {percentage:8.4f}% of datapoints") return False + + def find_temporal_offset(self) -> list : + ''' Returns a list containing the on-graph temporal offset between real and ideal trajectory. If offset is different for x and y axis, returns both in the same list''' + peak_x = self.bag_x.argmax() #find index of extremum value of real trajectory along x axis + peak_time_x = self.bag_times[peak_x] #find corresponding time + peak_x_ideal = self.ideal_traj_x.argmax() #find index of extremum value of ideal traj along x axis + peak_time_x_ideal = self.bag_times[peak_x_ideal] #find corresponding time + offset_x = peak_time_x_ideal - peak_time_x + + peak_y = self.bag_y.argmax() #find index of extremum value of real trajectory along y ayis + peak_time_y = self.bag_times[peak_y] #find corresponding time + peak_y_ideal = self.ideal_traj_y.argmax() #find index of extremum value of ideal traj along y ayis + peak_time_y_ideal = self.bag_times[peak_y_ideal] #find corresponding time + offset_y = peak_time_y_ideal - peak_time_y + + if offset_x == offset_y: + print(f"On-graph temporal offset is {offset_x}s, delay const is {self.DELAY_CONST_FIG8} so uncorrected/absolute offset is {offset_x-self.DELAY_CONST_FIG8}") + return [offset_x] + else : + print(f"On-graph temporal offsets are {offset_x} & {offset_y} secs, delay const is {self.DELAY_CONST_FIG8}") + return [offset_x, offset_y] + if __name__=="__main__": @@ -344,12 +375,13 @@ def test_passed(self) -> bool : parser.add_argument("recorded_trajectory", type=str, help=".csv file containing (time,x,y,z) of the recorded drone trajectory") parser.add_argument("pdf", type=str, help="name of the pdf file you want to create/overwrite") parser.add_argument("--open", help="Open the pdf directly after it is created", action="store_true") + parser.add_argument("--overwrite", action="store_true", help="If the given pdf already exists, overwrites it without asking") args : Namespace = parser.parse_args() plotter = Plotter() - plotter.create_figures(args.desired_trajectory, args.recorded_trajectory, args.pdf) + plotter.create_figures(args.desired_trajectory, args.recorded_trajectory, args.pdf, overwrite=args.overwrite) if args.open: import subprocess subprocess.call(["xdg-open", args.pdf]) - - + + diff --git a/systemtests/test_flights.py b/systemtests/test_flights.py index cfd84f1c1..8f7c20d10 100644 --- a/systemtests/test_flights.py +++ b/systemtests/test_flights.py @@ -9,6 +9,7 @@ import time import signal import atexit +from argparse import ArgumentParser, Namespace ############################# @@ -30,7 +31,7 @@ def clean_process(process:Popen) -> int : '''Kills process and its children on exit if they aren't already terminated (called with atexit). Returns 0 on termination, 1 if SIGKILL was needed''' if process.poll() == None: group_id = os.getpgid(process.pid) - print(f"cleaning process {group_id}") + #print(f"cleaning process {group_id}") os.killpg(group_id, signal.SIGTERM) time.sleep(0.01) #necessary delay before first poll i=0 @@ -44,9 +45,21 @@ def clean_process(process:Popen) -> int : return 0 else: return 0 #process already terminated + +def print_PIPE(process : Popen, process_name, always=False): + '''If process creates some error, prints the stderr and stdout PIPE of the process. NB : stderr and stdout must = PIPE in the Popen constructor''' + if process.returncode != 0 or always: + out, err = process.communicate() + print(f"{process_name} returncode = {process.returncode}") + print(f"{process_name} stderr : {err}") + print(f"{process_name} stdout : {out}") + else: + print(f"{process_name} completed sucessfully") + class TestFlights(unittest.TestCase): + SIM = False def __init__(self, methodName: str = "runTest") -> None: super().__init__(methodName) @@ -65,17 +78,21 @@ def setUp(self): self.test_file = None # launch server + current_env = None src = "source " + str(Path(__file__).parents[3] / "install/setup.bash") # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" command = f"{src} && ros2 launch crazyflie launch.py" - self.launch_crazyswarm = Popen(command, shell=True, stderr=True, stdout=PIPE, text=True, - start_new_session=True, executable="/bin/bash") + if TestFlights.SIM : + command += " backend:=sim" #launch crazyswarm from simulation backend + current_env = os.environ.copy() + self.launch_crazyswarm = Popen(command, shell=True, stderr=PIPE, stdout=PIPE, text=True, + start_new_session=True, executable="/bin/bash", env=current_env) atexit.register(clean_process, self.launch_crazyswarm) #atexit helps us to make sure processes are cleaned even if script exits unexpectedly time.sleep(1) - # runs once per test_ function def tearDown(self) -> None: clean_process(self.launch_crazyswarm) #kill crazyswarm_server and all of its child processes + print_PIPE(self.launch_crazyswarm, "launch_crazyswarm") # copy .ros/log files to results folder if Path(Path.home() / ".ros/log").exists(): @@ -92,19 +109,26 @@ def record_start_and_clean(self, testname:str, max_wait:int): src = f"source {str(self.ros2_ws)}/install/setup.bash" try: - command = f"{src} && ros2 bag record -s mcap -o test_{testname} /tf" + command = f"{src} && ros2 bag record -s mcap -o test_{testname} /tf" record_bag = Popen(command, shell=True, stderr=PIPE, stdout=True, text=True, cwd= self.ros2_ws / "results/", start_new_session=True, executable="/bin/bash") atexit.register(clean_process, record_bag) command = f"{src} && ros2 run crazyflie_examples {testname}" + if TestFlights.SIM: + command += " --ros-args -p use_sim_time:=True" #necessary args to start the test in simulation start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, start_new_session=True, text=True, executable="/bin/bash") atexit.register(clean_process, start_flight_test) - start_flight_test.wait(timeout=max_wait) #raise Timeoutexpired after max_wait seconds if start_flight_test didn't finish by itself + if TestFlights.SIM : + start_flight_test.wait(timeout=max_wait*5) #simulation can be super slow + else : + start_flight_test.wait(timeout=max_wait) #raise Timeoutexpired after max_wait seconds if start_flight_test didn't finish by itself + clean_process(start_flight_test) clean_process(record_bag) + print("finished the test") except TimeoutExpired: #if max_wait is exceeded clean_process(start_flight_test) @@ -113,14 +137,12 @@ def record_start_and_clean(self, testname:str, max_wait:int): except KeyboardInterrupt: #if drone crashes, user can ^C to skip the waiting clean_process(start_flight_test) clean_process(record_bag) - + #if something went wrong with the bash command lines in Popen, print the error - if record_bag.stderr != None: - print(testname," record_bag stderr: ", record_bag.stderr.readlines()) - if start_flight_test.stderr != None: - print(testname," start_flight flight stderr: ", start_flight_test.stderr.readlines()) - + print_PIPE(record_bag, f"record_bag for {self.idFolderName()}") + print_PIPE(start_flight_test, f"start_flight_test for {self.idFolderName()}") + def translate_plot_and_check(self, testname:str) -> bool : '''Translates rosbag .mcap format to .csv, then uses that csv to plot a pdf. Checks the deviation between ideal and real trajectories, i.e if the drone successfully followed its given trajectory. Returns True if deviation < epsilon(defined in plotter_class.py) at every timestep, false if not. ''' @@ -134,14 +156,14 @@ def translate_plot_and_check(self, testname:str) -> bool : output_pdf = f"{str(self.ros2_ws)}/results/test_{testname}/results_{testname}.pdf" rosbag_csv = output_csv - plotter = Plotter() + plotter = Plotter(sim_backend=TestFlights.SIM) plotter.create_figures(self.test_file, rosbag_csv, output_pdf) #plot the data return plotter.test_passed() def test_figure8(self): - self.test_file = "../crazyflie_examples/crazyflie_examples/data/figure8.csv" + self.test_file = "figure8_ideal_traj.csv" # run test self.record_start_and_clean("figure8", 20) #create the plot etc @@ -149,7 +171,7 @@ def test_figure8(self): assert test_passed, "figure8 test failed : deviation larger than epsilon" def test_multi_trajectory(self): - self.test_file = "../crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv" + self.test_file = "multi_trajectory_traj0_ideal.csv" self.record_start_and_clean("multi_trajectory", 80) test_passed = self.translate_plot_and_check("multi_trajectory") assert test_passed, "multitrajectory test failed : deviation larger than epsilon" @@ -158,5 +180,13 @@ def test_multi_trajectory(self): if __name__ == '__main__': - unittest.main() + from argparse import ArgumentParser + import sys + parser = ArgumentParser(description="Runs (real or simulated) flight tests with pytest framework") + parser.add_argument("--sim", action="store_true", help="Runs the test from the simulation backend") + args, other_args = parser.parse_known_args() + if args.sim : + TestFlights.SIM = True + + unittest.main(argv=[sys.argv[0]] + other_args) From d468687aeb891faa2830f1c7cd29638716c5793b Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 3 Feb 2024 22:29:11 +0100 Subject: [PATCH 03/21] server [cflib]: add Status topic --- crazyflie/scripts/crazyflie_server.py | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index fbbe8fe72..4ba4dcf4a 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -26,7 +26,7 @@ from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType -from crazyflie_interfaces.msg import Hover, LogDataGeneric, FullState +from crazyflie_interfaces.msg import Status, Hover, LogDataGeneric, FullState from motion_capture_tracking_interfaces.msg import NamedPoseArray from std_srvs.srv import Empty @@ -74,17 +74,21 @@ def __init__(self): # Assign default topic types, variables and callbacks self.default_log_type = {"pose": PoseStamped, "scan": LaserScan, - "odom": Odometry} + "odom": Odometry, + "status": Status} self.default_log_vars = {"pose": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', 'stabilizer.roll', 'stabilizer.pitch', 'stabilizer.yaw'], "scan": ['range.front', 'range.left', 'range.back', 'range.right'], "odom": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', 'stabilizer.yaw', 'stabilizer.roll', 'stabilizer.pitch', 'kalman.statePX', 'kalman.statePY', 'kalman.statePZ', - 'gyro.z', 'gyro.x', 'gyro.y']} + 'gyro.z', 'gyro.x', 'gyro.y'], + "status": ['supervisor.info', 'pm.vbatMV', 'pm.state', + 'radio.rssi']} self.default_log_fnc = {"pose": self._log_pose_data_callback, "scan": self._log_scan_data_callback, - "odom": self._log_odom_data_callback} + "odom": self._log_odom_data_callback, + "status": self._log_status_data_callback} self.world_tf_name = "world" try: @@ -569,6 +573,21 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri): t_base.transform.rotation.w = q[3] self.tfbr.sendTransform(t_base) + def _log_status_data_callback(self, timestamp, data, logconf, uri): + """ + Send out the ROS 2 status topic + """ + + msg = Status() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.world_tf_name + msg.supervisor_info = data.get('supervisor.info') + msg.battery_voltage = data.get('pm.vbatMV') / 1000.0 + msg.pm_state = data.get('pm.state') + msg.rssi = data.get('radio.rssi') + + self.swarm._cfs[uri].logging["status_publisher"].publish(msg) + def _log_custom_data_callback(self, timestamp, data, logconf, uri): """ Once custom log block is retrieved from the Crazyflie, From efeffb2a70998bb19136e741ca45c7ca4c575b3e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 3 Feb 2024 22:29:36 +0100 Subject: [PATCH 04/21] Status topic: fix incorrect constant for can_be_armed --- crazyflie_interfaces/msg/Status.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie_interfaces/msg/Status.msg b/crazyflie_interfaces/msg/Status.msg index bebd42a71..d7c2f2352 100644 --- a/crazyflie_interfaces/msg/Status.msg +++ b/crazyflie_interfaces/msg/Status.msg @@ -1,6 +1,6 @@ # Constants -uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 0 +uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 1 uint16 SUPERVISOR_INFO_IS_ARMED = 2 uint16 SUPERVISOR_INFO_AUTO_ARM = 4 uint16 SUPERVISOR_INFO_CAN_FLY = 8 From bcfa55372911b25f28b0f1cfc42bfa5106ed6b94 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 3 Feb 2024 22:30:15 +0100 Subject: [PATCH 05/21] server [sim]: unify naming convention for logging --- .../crazyflie_sim/crazyflie_server.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_server.py b/crazyflie_sim/crazyflie_sim/crazyflie_server.py index 13bf4ae2e..8050737b4 100755 --- a/crazyflie_sim/crazyflie_sim/crazyflie_server.py +++ b/crazyflie_sim/crazyflie_sim/crazyflie_server.py @@ -210,7 +210,7 @@ def _param_to_dict(self, param_ros): return tree def _emergency_callback(self, request, response, name='all'): - self.get_logger().info('emergency not yet implemented') + self.get_logger().info(f'[{name}] emergency not yet implemented') return response @@ -219,9 +219,9 @@ def _takeoff_callback(self, request, response, name='all'): duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f'takeoff(height={request.height} m,' + f'[{name}] takeoff(height={request.height} m,' + f'duration={duration} s,' - + f'group_mask={request.group_mask}) {name}' + + f'group_mask={request.group_mask})' ) cfs = self.cfs if name == 'all' else {name: self.cfs[name]} for _, cf in cfs.items(): @@ -234,7 +234,7 @@ def _land_callback(self, request, response, name='all'): duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f'land(height={request.height} m,' + f'[{name}] land(height={request.height} m,' + f'duration={duration} s,' + f'group_mask={request.group_mask})' ) @@ -250,8 +250,9 @@ def _go_to_callback(self, request, response, name='all'): float(request.duration.nanosec / 1e9) self.get_logger().info( - 'go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)' + '[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)' % ( + name, request.goal.x, request.goal.y, request.goal.z, @@ -269,11 +270,11 @@ def _go_to_callback(self, request, response, name='all'): return response def _notify_setpoints_stop_callback(self, request, response, name='all'): - self.get_logger().info('Notify setpoint stop not yet implemented') + self.get_logger().info(f'[{name}] Notify setpoint stop not yet implemented') return response def _upload_trajectory_callback(self, request, response, name='all'): - self.get_logger().info('Upload trajectory(id=%d)' % (request.trajectory_id)) + self.get_logger().info('[%s] Upload trajectory(id=%d)' % (name, request.trajectory_id)) cfs = self.cfs if name == 'all' else {name: self.cfs[name]} for _, cf in cfs.items(): @@ -297,8 +298,9 @@ def _upload_trajectory_callback(self, request, response, name='all'): def _start_trajectory_callback(self, request, response, name='all'): self.get_logger().info( - 'start_trajectory(id=%d, timescale=%f, reverse=%d, relative=%d, group_mask=%d)' + '[%s] start_trajectory(id=%d, timescale=%f, reverse=%d, relative=%d, group_mask=%d)' % ( + name, request.trajectory_id, request.timescale, request.reversed, From 8db6586d157ba8bbbddd7483b1cab19dcd7faf7e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 3 Feb 2024 22:32:27 +0100 Subject: [PATCH 06/21] server [cpp]: switch to the node logger so that logs are published via /rosout on humble --- crazyflie/src/crazyflie_server.cpp | 100 +++++++++++++++++------------ 1 file changed, 58 insertions(+), 42 deletions(-) diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 4a71e0245..b4e7e65ea 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -39,13 +39,19 @@ using std_srvs::srv::Empty; using motion_capture_tracking_interfaces::msg::NamedPoseArray; using crazyflie_interfaces::msg::FullState; +// Note on logging: we use a single logger with string prefixes +// A better way would be to use named child loggers, but these do not +// report to /rosout in humble, see https://github.com/ros2/rclpy/issues/1131 +// Once we do not support humble anymore, consider switching to child loggers + // Helper class to convert crazyflie_cpp logging messages to ROS logging messages class CrazyflieLogger : public Logger { public: - CrazyflieLogger(rclcpp::Logger logger) + CrazyflieLogger(rclcpp::Logger logger, const std::string& prefix) : Logger() , logger_(logger) + , prefix_(prefix) { } @@ -53,20 +59,21 @@ class CrazyflieLogger : public Logger virtual void info(const std::string &msg) { - RCLCPP_INFO(logger_, "%s", msg.c_str()); + RCLCPP_INFO(logger_, "%s %s", prefix_.c_str(), msg.c_str()); } virtual void warning(const std::string &msg) { - RCLCPP_WARN(logger_, "%s", msg.c_str()); + RCLCPP_WARN(logger_, "%s %s", prefix_.c_str(), msg.c_str()); } virtual void error(const std::string &msg) { - RCLCPP_ERROR(logger_, "%s", msg.c_str()); + RCLCPP_ERROR(logger_, "%s %s", prefix_.c_str(), msg.c_str()); } private: rclcpp::Logger logger_; + std::string prefix_; }; std::set extract_names( @@ -129,8 +136,8 @@ class CrazyflieROS rclcpp::CallbackGroup::SharedPtr callback_group_cf_srv, const CrazyflieBroadcaster* cfbc, bool enable_parameters = true) - : logger_(rclcpp::get_logger(name)) - , cf_logger_(logger_) + : logger_(node->get_logger()) + , cf_logger_(logger_, "[" + name + "]") , cf_( link_uri, cf_logger_, @@ -228,7 +235,7 @@ class CrazyflieROS bool query_all_values_on_connect = node->get_parameter("firmware_params.query_all_values_on_connect").get_parameter_value().get(); int numParams = 0; - RCLCPP_INFO(logger_, "Requesting parameters..."); + RCLCPP_INFO(logger_, "[%s] Requesting parameters...", name_.c_str()); cf_.requestParamToc(/*forceNoCache*/false, /*requestValues*/query_all_values_on_connect); for (auto iter = cf_.paramsBegin(); iter != cf_.paramsEnd(); ++iter) { auto entry = *iter; @@ -285,7 +292,7 @@ class CrazyflieROS } break; default: - RCLCPP_WARN(logger_, "Unknown param type for %s/%s", entry.group.c_str(), entry.name.c_str()); + RCLCPP_WARN(logger_, "[%s] Unknown param type for %s/%s", name_.c_str(), entry.group.c_str(), entry.name.c_str()); break; } // If there is no such parameter in all, add it @@ -301,7 +308,7 @@ class CrazyflieROS } auto end1 = std::chrono::system_clock::now(); std::chrono::duration elapsedSeconds1 = end1 - start; - RCLCPP_INFO(logger_, "reqParamTOC: %f s (%d params)", elapsedSeconds1.count(), numParams); + RCLCPP_INFO(logger_, "[%s] reqParamTOC: %f s (%d params)", name_.c_str(), elapsedSeconds1.count(), numParams); // Set parameters as specified in the configuration files, as in the following order // 1.) check all/firmware_params @@ -351,7 +358,7 @@ class CrazyflieROS // check if any of the default topics are enabled if (i.first.find("default_topics.pose") == 0) { int freq = log_config_map["default_topics.pose.frequency"].get(); - RCLCPP_INFO(logger_, "Logging to /pose at %d Hz", freq); + RCLCPP_INFO(logger_, "[%s] Logging to /pose at %d Hz", name_.c_str(), freq); publisher_pose_ = node->create_publisher(name + "/pose", 10); @@ -368,7 +375,7 @@ class CrazyflieROS } else if (i.first.find("default_topics.scan") == 0) { int freq = log_config_map["default_topics.scan.frequency"].get(); - RCLCPP_INFO(logger_, "Logging to /scan at %d Hz", freq); + RCLCPP_INFO(logger_, "[%s] Logging to /scan at %d Hz", name_.c_str(), freq); publisher_scan_ = node->create_publisher(name + "/scan", 10); @@ -385,7 +392,7 @@ class CrazyflieROS } else if (i.first.find("default_topics.status") == 0) { int freq = log_config_map["default_topics.status.frequency"].get(); - RCLCPP_INFO(logger_, "Logging to /status at %d Hz", freq); + RCLCPP_INFO(logger_, "[%s] Logging to /status at %d Hz", name_.c_str(), freq); publisher_status_ = node->create_publisher(name + "/status", 10); @@ -415,7 +422,7 @@ class CrazyflieROS // older firmware -> use other 16-bit variables if (!status_has_radio_stats_) { - RCLCPP_WARN(logger_, "Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero."); + RCLCPP_WARN(logger_, "[%s] Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero.", name_.c_str()); logvars.push_back({"pm", "vbatMV"}); logvars.push_back({"pm", "vbatMV"}); } @@ -431,7 +438,7 @@ class CrazyflieROS int freq = log_config_map["custom_topics." + topic_name + ".frequency"].get(); auto vars = log_config_map["custom_topics." + topic_name + ".vars"].get>(); - RCLCPP_INFO(logger_, "Logging to %s at %d Hz", topic_name.c_str(), freq); + RCLCPP_INFO(logger_, "[%s] Logging to %s at %d Hz", name_.c_str(), topic_name.c_str(), freq); publishers_generic_.emplace_back(node->create_publisher(name + "/" + topic_name, 10)); @@ -453,7 +460,7 @@ class CrazyflieROS } } - RCLCPP_INFO(logger_, "Requesting memories..."); + RCLCPP_INFO(logger_, "[%s] Requesting memories...", name_.c_str()); cf_.requestMemoryToc(); } @@ -489,7 +496,7 @@ class CrazyflieROS if (p.get_name().find(prefix) != 0) { RCLCPP_ERROR( logger_, - "Incorrect parameter update request for param \"%s\"", p.get_name().c_str()); + "[%s] Incorrect parameter update request for param \"%s\"", name_.c_str(), p.get_name().c_str()); return; } size_t pos = p.get_name().find(".", prefix.size()); @@ -498,7 +505,8 @@ class CrazyflieROS RCLCPP_INFO( logger_, - "Update parameter \"%s.%s\" to %s", + "[%s] Update parameter \"%s.%s\" to %s", + name_.c_str(), group.c_str(), name.c_str(), p.value_to_string().c_str()); @@ -535,7 +543,7 @@ class CrazyflieROS break; } } else { - RCLCPP_ERROR(logger_, "Could not find param %s/%s", group.c_str(), name.c_str()); + RCLCPP_ERROR(logger_, "[%s] Could not find param %s/%s", name_.c_str(), group.c_str(), name.c_str()); } } @@ -594,7 +602,7 @@ class CrazyflieROS if (pos != std::string::npos) { message_buffer_[pos] = 0; - RCLCPP_INFO(logger_, "%s", message_buffer_.c_str()); + RCLCPP_INFO(logger_, "[%s] %s", name_.c_str(), message_buffer_.c_str()); message_buffer_.erase(0, pos + 1); } } @@ -602,14 +610,15 @@ class CrazyflieROS void emergency(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "emergency()"); + RCLCPP_INFO(logger_, "[%s] emergency()", name_.c_str()); cf_.emergencyStop(); } void start_trajectory(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "start_trajectory(id=%d, timescale=%f, reversed=%d, relative=%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] start_trajectory(id=%d, timescale=%f, reversed=%d, relative=%d, group_mask=%d)", + name_.c_str(), request->trajectory_id, request->timescale, request->reversed, @@ -625,7 +634,8 @@ class CrazyflieROS void takeoff(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "takeoff(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] takeoff(height=%f m, duration=%f s, group_mask=%d)", + name_.c_str(), request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -635,7 +645,8 @@ class CrazyflieROS void land(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "land(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] land(height=%f m, duration=%f s, group_mask=%d)", + name_.c_str(), request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -645,7 +656,8 @@ class CrazyflieROS void go_to(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)", + name_.c_str(), request->goal.x, request->goal.y, request->goal.z, request->yaw, rclcpp::Duration(request->duration).seconds(), request->relative, @@ -658,7 +670,8 @@ class CrazyflieROS void upload_trajectory(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "upload_trajectory(id=%d, offset=%d)", + RCLCPP_INFO(logger_, "[%s] upload_trajectory(id=%d, offset=%d)", + name_.c_str(), request->trajectory_id, request->piece_offset); @@ -670,7 +683,7 @@ class CrazyflieROS || request->pieces[i].poly_z.size() != 8 || request->pieces[i].poly_yaw.size() != 8) { - RCLCPP_FATAL(logger_, "Wrong number of pieces!"); + RCLCPP_FATAL(logger_, "[%s] Wrong number of pieces!", name_.c_str()); return; } pieces[i].duration = rclcpp::Duration(request->pieces[i].duration).seconds(); @@ -688,7 +701,8 @@ class CrazyflieROS void notify_setpoints_stop(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", + name_.c_str(), request->remain_valid_millisecs, request->group_mask); @@ -824,13 +838,13 @@ class CrazyflieROS auto now = std::chrono::steady_clock::now(); std::chrono::duration elapsed = now - last_on_latency_; if (elapsed.count() > 1.0 / warning_freq_) { - RCLCPP_WARN(logger_, "last latency update: %f s", elapsed.count()); + RCLCPP_WARN(logger_, "[%s] last latency update: %f s", name_.c_str(), elapsed.count()); } auto stats = cf_.connectionStatsDelta(); float ack_rate = stats.sent_count / stats.ack_count; if (ack_rate < min_ack_rate_) { - RCLCPP_WARN(logger_, "Ack rate: %.1f %%", ack_rate * 100); + RCLCPP_WARN(logger_, "[%s] Ack rate: %.1f %%", name_.c_str(), ack_rate * 100); } if (publish_stats_) { @@ -853,7 +867,7 @@ class CrazyflieROS void on_latency(uint64_t latency_in_us) { if (latency_in_us / 1000.0 > max_latency_) { - RCLCPP_WARN(logger_, "Latency: %.1f ms", latency_in_us / 1000.0); + RCLCPP_WARN(logger_, "[%s] Latency: %.1f ms", name_.c_str(), latency_in_us / 1000.0); } last_on_latency_ = std::chrono::steady_clock::now(); } @@ -921,7 +935,7 @@ class CrazyflieServer : public rclcpp::Node public: CrazyflieServer() : Node("crazyflie_server") - , logger_(rclcpp::get_logger("all")) + , logger_(get_logger()) { // Create callback groups (each group can run in a separate thread) callback_group_mocap_ = this->create_callback_group( @@ -1036,7 +1050,7 @@ class CrazyflieServer : public rclcpp::Node uint8_t id = parameter_overrides.at("robots." + name + ".id").get(); update_name_to_id_map(name, id); } else { - RCLCPP_INFO(logger_, "Unknown connection type %s", constr.c_str()); + RCLCPP_INFO(logger_, "[all] Unknown connection type %s", constr.c_str()); } } } @@ -1063,7 +1077,7 @@ class CrazyflieServer : public rclcpp::Node void emergency(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "emergency()"); + RCLCPP_INFO(logger_, "[all] emergency()"); for (int i = 0; i < broadcasts_num_repeats_; ++i) { for (auto &bc : broadcaster_) { @@ -1077,7 +1091,7 @@ class CrazyflieServer : public rclcpp::Node void start_trajectory(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "start_trajectory(id=%d, timescale=%f, reversed=%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] start_trajectory(id=%d, timescale=%f, reversed=%d, group_mask=%d)", request->trajectory_id, request->timescale, request->reversed, @@ -1097,7 +1111,7 @@ class CrazyflieServer : public rclcpp::Node void takeoff(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "takeoff(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] takeoff(height=%f m, duration=%f s, group_mask=%d)", request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -1113,7 +1127,7 @@ class CrazyflieServer : public rclcpp::Node void land(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "land(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] land(height=%f m, duration=%f s, group_mask=%d)", request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -1129,7 +1143,7 @@ class CrazyflieServer : public rclcpp::Node void go_to(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, group_mask=%d)", request->goal.x, request->goal.y, request->goal.z, request->yaw, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -1147,7 +1161,7 @@ class CrazyflieServer : public rclcpp::Node void notify_setpoints_stop(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", request->remain_valid_millisecs, request->group_mask); @@ -1254,7 +1268,7 @@ class CrazyflieServer : public rclcpp::Node RCLCPP_INFO( logger_, - "Update parameter \"%s.%s\" to %s", + "[all] Update parameter \"%s.%s\" to %s", group.c_str(), name.c_str(), p.value_to_string().c_str()); @@ -1324,11 +1338,11 @@ class CrazyflieServer : public rclcpp::Node mean_rate /= (mocap_data_received_timepoints_.size() - 1); if (num_rates_wrong > 0) { - RCLCPP_WARN(logger_, "Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate); + RCLCPP_WARN(logger_, "[all] Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate); } } else if (mocap_enabled_) { // b) warn if no data was received - RCLCPP_WARN(logger_, "Motion capture did not receive data!"); + RCLCPP_WARN(logger_, "[all] Motion capture did not receive data!"); } mocap_data_received_timepoints_.clear(); @@ -1377,7 +1391,7 @@ class CrazyflieServer : public rclcpp::Node { const auto iter = name_to_id_.find(name); if (iter != name_to_id_.end()) { - RCLCPP_WARN(logger_, "At least two objects with the same id (%d, %s, %s)", id, name.c_str(), iter->first.c_str()); + RCLCPP_WARN(logger_, "[all] At least two objects with the same id (%d, %s, %s)", id, name.c_str(), iter->first.c_str()); } else { name_to_id_.insert(std::make_pair(name, id)); } @@ -1400,6 +1414,8 @@ class CrazyflieServer : public rclcpp::Node std::map> crazyflies_; + + // broadcastUri -> broadcast object std::map> broadcaster_; From 94a1d93ded5918f3df18e11ffcdc26db494dc3b4 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 3 Feb 2024 22:33:04 +0100 Subject: [PATCH 07/21] gui: add support for status --- crazyflie/scripts/gui.py | 156 +++++++++++++++++++++++++++++---------- 1 file changed, 116 insertions(+), 40 deletions(-) diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py index 3941d8c77..830d94a34 100755 --- a/crazyflie/scripts/gui.py +++ b/crazyflie/scripts/gui.py @@ -2,7 +2,9 @@ import math import threading +import time from pathlib import Path +from functools import partial import rclpy from geometry_msgs.msg import Pose, Twist @@ -14,6 +16,9 @@ from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener +from crazyflie_interfaces.msg import Status +import rowan + from nicegui import Client, app, events, ui, ui_run @@ -31,58 +36,55 @@ def __init__(self) -> None: if cfname != 'all': self.cfnames.append(cfname) - print(self.cfnames) - self.cfs = [] - self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - - - self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 1) - self.sub_log = self.create_subscription(Log, 'rosout', self.on_rosout, 1) + self.sub_log = self.create_subscription(Log, 'rosout', self.on_rosout, rclpy.qos.QoSProfile( + depth=1000, + durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL)) self.logs = dict() + self.supervisor_labels = dict() + self.battery_labels = dict() + self.radio_labels = dict() + self.robotmodels = dict() + # set of robot names that had a status recently + self.watchdog = dict() with Client.auto_index_client: - # with ui.row().classes('w-full h-full no-wrap'): - # self.log = ui.log().classes('w-full h-full no-wrap') - - - with ui.row().classes('items-stretch'): - # with ui.card().classes('w-44 text-center items-center'): - # ui.label('Data').classes('text-2xl') - # ui.label('linear velocity').classes('text-xs mb-[-1.8em]') - # slider_props = 'readonly selection-color=transparent' - # self.linear = ui.slider(min=-1, max=1, step=0.05, value=0).props(slider_props) - # ui.label('angular velocity').classes('text-xs mb-[-1.8em]') - # self.angular = ui.slider(min=-1, max=1, step=0.05, value=0).props(slider_props) - # ui.label('position').classes('text-xs mb-[-1.4em]') - # self.position = ui.label('---') with ui.card().classes('w-full h-full'): ui.label('Visualization').classes('text-2xl') - with ui.scene(800, 600, on_click=self.on_vis_click) as scene: + with ui.scene(800, 400, on_click=self.on_vis_click) as scene: for name in self.cfnames: robot = scene.stl('/urdf/cf2_assembly.stl').scale(1.0).material('#ff0000').with_name(name) - self.cfs.append(robot) - # with scene.group() as self.robot_3d: - # prism = [[-0.5, -0.5], [0.5, -0.5], [0.75, 0], [0.5, 0.5], [-0.5, 0.5]] - # self.robot_object = scene.extrusion(prism, 0.4).material('#4488ff', 0.5) - - with ui.row().classes('w-full h-full'): + self.robotmodels[name] = robot + self.watchdog[name] = time.time() + scene.camera.x = 0 + scene.camera.y = -1 + scene.camera.z = 2 + scene.camera.look_at_x = 0 + scene.camera.look_at_y = 0 + scene.camera.look_at_z = 0 + + with ui.row().classes('w-full h-lvh'): with ui.tabs().classes('w-full') as tabs: self.tabs = [] for name in ["all"] + self.cfnames: self.tabs.append(ui.tab(name)) - with ui.tab_panels(tabs, value=self.tabs[0]).classes('w-full') as self.tabpanels: + with ui.tab_panels(tabs, value=self.tabs[0], on_change=self.on_tab_change).classes('w-full') as self.tabpanels: for name, tab in zip(["all"] + self.cfnames, self.tabs): with ui.tab_panel(tab): - self.logs[name] = ui.log().classes('w-full h-full no-wrap') - ui.label("Battery Voltage: ") - ui.button("Reboot") + self.logs[name] = ui.log().classes('w-full h-96 no-wrap') + self.supervisor_labels[name] = ui.label("") + self.battery_labels[name] = ui.label("") + self.radio_labels[name] = ui.label("") + ui.button("Reboot", on_click=partial(self.on_reboot, name=name)) + + for name in self.cfnames: + self.create_subscription(Status, name + '/status', partial(self.on_status, name=name), 1) # Call on_timer function self.timer = self.create_timer(0.1, self.on_timer) @@ -96,17 +98,39 @@ def send_speed(self, x: float, y: float) -> None: self.cmd_vel_publisher.publish(msg) def on_rosout(self, msg: Log) -> None: - if msg.name in self.logs: - self.logs[msg.name].push(msg.msg) + # filter by crazyflie and add to the correct log + if msg.name == "crazyflie_server": + if msg.msg.startswith("["): + idx = msg.msg.find("]") + name = msg.msg[1:idx] + # if it was an "all" category, add only to CFs + if name == 'all': + for logname ,log in self.logs.items(): + if logname != "all": + log.push(msg.msg) + elif name in self.logs: + self.logs[name].push(msg.msg[idx+2:]) + + # add all possible messages to the 'all' tab + self.logs['all'].push(msg.msg) def on_timer(self) -> None: - for name, robot in zip(self.cfnames, self.cfs): + for name, robotmodel in self.robotmodels.items(): t = self.tf_buffer.lookup_transform( "world", name, rclpy.time.Time()) pos = t.transform.translation - robot.move(pos.x, pos.y, pos.z) + robotmodel.move(pos.x, pos.y, pos.z) + robotmodel.rotate(*rowan.to_euler([ + t.transform.rotation.w, + t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z], "xyz")) + + # no update for a while -> mark red + if time.time() - self.watchdog[name] > 2.0: + robotmodel.material('#ff0000') def on_vis_click(self, e: events.SceneClickEventArguments): hit = e.hits[0] @@ -117,12 +141,64 @@ def on_vis_click(self, e: events.SceneClickEventArguments): else: self.tabpanels.value = name + def on_status(self, msg, name) -> None: + status_ok = True + supervisor_text = "" + if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_BE_ARMED: + supervisor_text += "can be armed; " + if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_ARMED: + supervisor_text += "is armed; " + if msg.supervisor_info & Status.SUPERVISOR_INFO_AUTO_ARM: + supervisor_text += "auto-arm; " + if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_FLY: + supervisor_text += "can fly; " + if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_FLYING: + supervisor_text += "is flying; " + if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_TUMBLED: + supervisor_text += "is tumpled; " + status_ok = False + if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_LOCKED: + supervisor_text += "is locked; " + status_ok = False + self.supervisor_labels[name].set_text(supervisor_text) + + battery_text = f'{msg.battery_voltage:.2f} V' + if msg.battery_voltage < 3.8: + status_ok = False + if msg.pm_state == Status.PM_STATE_BATTERY: + battery_text += " (on battery)" + elif msg.pm_state == Status.PM_STATE_CHARGING: + battery_text += " (charging)" + elif msg.pm_state == Status.PM_STATE_CHARGED: + battery_text += " (charged)" + elif msg.pm_state == Status.PM_STATE_LOW_POWER: + battery_text += " (low power)" + status_ok = False + elif msg.pm_state == Status.PM_STATE_SHUTDOWN: + battery_text += " (shutdown)" + status_ok = False + self.battery_labels[name].set_text(battery_text) + + radio_text = f'{msg.rssi} dBm; Unicast: {msg.num_rx_unicast} / {msg.num_tx_unicast}; Broadcast: {msg.num_rx_broadcast} / {msg.num_tx_broadcast}' + self.radio_labels[name].set_text(radio_text) + + if status_ok: + self.robotmodels[name].material('#00ff00') + else: + self.robotmodels[name].material('#ff0000') + + self.watchdog[name] = time.time() + + def on_tab_change(self, arg): + for name, robotmodel in self.robotmodels.items(): + if name != arg.value: + robotmodel.scale(1) + if arg.value in self.robotmodels: + self.robotmodels[arg.value].scale(2) - # def handle_pose(self, msg: Pose) -> None: - # self.position.text = f'x: {msg.position.x:.2f}, y: {msg.position.y:.2f}' - # self.robot_3d.move(msg.position.x, msg.position.y) - # self.robot_3d.rotate(0, 0, 2 * math.atan2(msg.orientation.z, msg.orientation.w)) + def on_reboot(self, name) -> None: + ui.notify(f'Reboot not implemented, yet') def main() -> None: From 1eb3ab65d9761cce429b29e08aaa57a68ff75906 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sun, 4 Feb 2024 22:05:42 +0100 Subject: [PATCH 08/21] server [cflib] unify logging messages --- crazyflie/scripts/crazyflie_server.py | 85 +++++++++++++++------------ 1 file changed, 48 insertions(+), 37 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 4ba4dcf4a..ac7882d15 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -67,7 +67,10 @@ def __init__(self): self._ros_parameters = self._param_to_dict(self._parameters) self.uris = [] - self.cf_dict = {} + # for logging, assign a all -> all mapping + self.cf_dict = { + 'all': 'all' + } self.uri_dict = {} self.type_dict = {} @@ -224,6 +227,9 @@ def __init__(self): StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) for uri in self.cf_dict: + if uri == "all": + continue + name = self.cf_dict[uri] pub = self.create_publisher(String, name + '/robot_description', @@ -354,7 +360,7 @@ def _fully_connected(self, link_uri): Called when all parameters have been updated and the full log toc has been received of the Crazyflie """ - self.get_logger().info(f" {link_uri} is fully connected!") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] is fully connected!") self.swarm.fully_connected_crazyflie_cnt += 1 @@ -367,10 +373,10 @@ def _fully_connected(self, link_uri): return def _disconnected(self, link_uri): - self.get_logger().info(f" {link_uri} is disconnected!") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] is disconnected!") def _connection_failed(self, link_uri, msg): - self.get_logger().info(f"{link_uri} connection Failed") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] connection Failed") self.swarm.close_links() def _init_logging(self): @@ -406,20 +412,20 @@ def _init_logging(self): self._log_error_callback) lg_custom.start() except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' + self.get_logger().info(f'[{self.cf_dict[link_uri]}] Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') + f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.') - self.get_logger().info(f"{link_uri} setup custom logging") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] setup custom logging") self.create_service( RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri)) self.create_service( AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri)) - self.get_logger().info("All Crazyflies loggging are initialized") + self.get_logger().info("All Crazyflies logging are initialized.") def _init_default_logging(self, prefix, link_uri, callback_fnc): """ @@ -438,13 +444,13 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc): self.declare_parameter( self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency) self.get_logger().info( - f"{link_uri} setup logging for {prefix} at freq {frequency}") + f"[{self.cf_dict[link_uri]}] setup logging for {prefix} at freq {frequency}") except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' + self.get_logger().error(f'[{self.cf_dict[link_uri]}] Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: - self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') + self.get_logger().error( + f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.') def _log_scan_data_callback(self, timestamp, data, logconf, uri): """ @@ -652,7 +658,7 @@ def _init_parameters(self): # crazyflie with get_value due to threading. cf.param.set_value(name, set_param_value) self.get_logger().info( - f" {link_uri}: {name} is set to {set_param_value}" + f"[{self.cf_dict[link_uri]}] {name} is set to {set_param_value}" ) self.declare_parameter( self.cf_dict[link_uri] + @@ -691,7 +697,7 @@ def _init_parameters(self): # Now all parameters are set set_param_all = True - self.get_logger().info("All Crazyflies parameters are initialized") + self.get_logger().info("All Crazyflies parameters are initialized.") def _parameters_callback(self, params): """ @@ -710,7 +716,7 @@ def _parameters_callback(self, params): name_param, param.value ) self.get_logger().info( - f" {self.uri_dict[cf_name]}: {name_param} is set to {param.value}" + f"[{self.uri_dict[cf_name]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: @@ -727,7 +733,7 @@ def _parameters_callback(self, params): name_param, param.value ) self.get_logger().info( - f" {link_uri}: {name_param} is set to {param.value}" + f"[{self.cf_dict[link_uri]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: @@ -751,12 +757,14 @@ def _takeoff_callback(self, request, response, uri="all"): a certain height in high level commander """ + print("call1 ", uri) + duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f"takeoff(height={request.height} m," + f"[{self.cf_dict[uri]}] takeoff(height={request.height} m," + f"duration={duration} s," - + f"group_mask={request.group_mask}) {uri}" + + f"group_mask={request.group_mask})" ) if uri == "all": for link_uri in self.uris: @@ -778,7 +786,7 @@ def _land_callback(self, request, response, uri="all"): duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f"land(height={request.height} m," + f"[{self.cf_dict[uri]}] land(height={request.height} m," + f"duration={duration} s," + f"group_mask={request.group_mask})" ) @@ -803,8 +811,9 @@ def _go_to_callback(self, request, response, uri="all"): float(request.duration.nanosec / 1e9) self.get_logger().info( - "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" + "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" % ( + self.cf_dict[uri], request.goal.x, request.goal.y, request.goal.z, @@ -839,7 +848,7 @@ def _go_to_callback(self, request, response, uri="all"): def _notify_setpoints_stop_callback(self, request, response, uri="all"): - self.get_logger().info(f"{uri}: Received notify setpoint stop") + self.get_logger().info(f"[{self.cf_dict[uri]}] Received notify setpoint stop") if uri == "all": for link_uri in self.uris: @@ -855,7 +864,8 @@ def _upload_trajectory_callback(self, request, response, uri="all"): offset = request.piece_offset lenght = len(request.pieces) total_duration = 0 - self.get_logger().info("upload_trajectory(id=%d,offset=%d, lenght=%d)" % ( + self.get_logger().info("[%s] upload_trajectory(id=%d,offset=%d, lenght=%d)" % ( + self.cf_dict[uri], id, offset, lenght, @@ -881,7 +891,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: - self.get_logger().info(f"{link_uri}: Upload failed") + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed") upload_success_all = False else: self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( @@ -895,7 +905,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: - self.get_logger().info(f"{uri}: Upload failed") + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed") response.success = False return response self.swarm._cfs[uri].cf.high_level_commander.define_trajectory( @@ -911,7 +921,8 @@ def _start_trajectory_callback(self, request, response, uri="all"): rev = request.reversed gm = request.group_mask - self.get_logger().info("start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % ( + self.get_logger().info("[%s] start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % ( + self.cf_dict[uri], id, ts, rel, @@ -1002,10 +1013,10 @@ def _remove_logging(self, request, response, uri="all"): self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop() self.destroy_publisher( self.swarm._cfs[uri].logging[topic_name + "_publisher"]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging") except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ") response.success = False return response else: @@ -1015,10 +1026,10 @@ def _remove_logging(self, request, response, uri="all"): for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]: self.destroy_publisher( self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging") except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ") response.success = False return response @@ -1042,10 +1053,10 @@ def _add_logging(self, request, response, uri="all"): "_log_config"].period_in_ms = 1000 / frequency self.swarm._cfs[uri].logging[topic_name + "_log_config"].start() - self.get_logger().info(f"{uri}: Add {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging") except rclpy.exceptions.ParameterAlreadyDeclaredException: self.get_logger().info( - f"{uri}: The content the logging of {topic_name} has already started ") + f"[{self.cf_dict[uri]}] The content the logging of {topic_name} has already started ") response.success = False return response else: @@ -1073,11 +1084,11 @@ def _add_logging(self, request, response, uri="all"): self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency - self.get_logger().info(f"{uri}: Add {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging") except KeyError as e: - self.get_logger().info( - f"{uri}: Failed to add {topic_name} logging") - self.get_logger().info(str(e) + "is not in TOC") + self.get_logger().error( + f"[{self.cf_dict[uri]}] Failed to add {topic_name} logging") + self.get_logger().error(str(e) + "is not in TOC") self.undeclare_parameter( self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") self.undeclare_parameter( @@ -1085,8 +1096,8 @@ def _add_logging(self, request, response, uri="all"): response.success = False return response except rclpy.exceptions.ParameterAlreadyDeclaredException: - self.get_logger().info( - f"{uri}: The content or part of the logging of {topic_name} has already started ") + self.get_logger().error( + f"[{self.cf_dict[uri]}] The content or part of the logging of {topic_name} has already started ") response.success = False return response From d0b4ad555da155aadaba4961edd2e9b8041d776b Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 5 Feb 2024 10:30:07 +0100 Subject: [PATCH 09/21] fix flake8 issues --- crazyflie_sim/crazyflie_sim/crazyflie_server.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_server.py b/crazyflie_sim/crazyflie_sim/crazyflie_server.py index 8050737b4..93dd698da 100755 --- a/crazyflie_sim/crazyflie_sim/crazyflie_server.py +++ b/crazyflie_sim/crazyflie_sim/crazyflie_server.py @@ -250,7 +250,11 @@ def _go_to_callback(self, request, response, name='all'): float(request.duration.nanosec / 1e9) self.get_logger().info( - '[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)' + """[%s] go_to(position=%f,%f,%f m, + yaw=%f rad, + duration=%f s, + relative=%d, + group_mask=%d)""" % ( name, request.goal.x, From e996e62258d88b8bbfcace57b78fa9b647099015 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 5 Feb 2024 10:46:07 +0100 Subject: [PATCH 10/21] remove hard-coded path --- crazyflie/scripts/gui.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py index 830d94a34..5bd156634 100755 --- a/crazyflie/scripts/gui.py +++ b/crazyflie/scripts/gui.py @@ -1,18 +1,17 @@ #!/usr/bin/env python3 -import math import threading import time from pathlib import Path from functools import partial import rclpy -from geometry_msgs.msg import Pose, Twist +from geometry_msgs.msg import Twist from rcl_interfaces.msg import Log from rclpy.executors import ExternalShutdownException from rclpy.node import Node -from tf2_ros import TransformException +# from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener @@ -215,7 +214,9 @@ def ros_main() -> None: pass -app.add_static_files("/urdf", "/home/whoenig/projects/crazyflie/crazyswarm2/src/crazyswarm2/crazyflie/urdf/") +app.add_static_files("/urdf", + str((Path(__file__).parent.parent.parent / "share" / "crazyflie" / "urdf").resolve()), + follow_symlink=True) app.on_startup(lambda: threading.Thread(target=ros_main).start()) ui_run.APP_IMPORT_STRING = f'{__name__}:app' # ROS2 uses a non-standard module name, so we need to specify it here ui.run(uvicorn_reload_dirs=str(Path(__file__).parent.resolve()), favicon='🤖') From 8bd3ca6190f22c7c4e01996e7534add720e4e8e1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 5 Feb 2024 10:46:56 +0100 Subject: [PATCH 11/21] update documentation --- crazyflie/launch/launch.py | 5 +++++ docs2/faq.rst | 4 ++-- docs2/overview.rst | 2 ++ docs2/usage.rst | 8 ++++++++ 4 files changed, 17 insertions(+), 2 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index 8a9290c11..4eec72a92 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -134,4 +134,9 @@ def generate_launch_description(): "use_sim_time": True, }] ), + Node( + package='crazyflie', + executable='gui.py', + name='gui', + ), ]) diff --git a/docs2/faq.rst b/docs2/faq.rst index 8270b9343..7422be127 100644 --- a/docs2/faq.rst +++ b/docs2/faq.rst @@ -45,9 +45,9 @@ Crazyswarm2 was forked from Crazyswarm. However, there is also heavy re-design o In Crazyswarm1, a simple visualization of the setpoints for high-level Python scripts is supported. There is no support for simulation of ROS code that does not use the high-level Python scripts and no support for physics-based simulation. In contrast, Crazyswarm2 implements the simulation as an alternative backend. This will support multiple physics/visualization backends (optionally with physics and aerodynamic interaction). -- **Support of Distributed Swarm Monitoring (Planned).** +- **Support of Distributed Swarm Monitoring.** In Crazyswarm1, a common swarm monitoring tool is the chooser.py (to enable/disable CFs, check the battery voltage etc.). However, this tool was not functioning while the swarm is operational. - In contrast, Crazyswarm2 will allow common swarm monitoring tasks without restarting ROS nodes or launching additional tools. + In contrast, Crazyswarm2 allows common swarm monitoring tasks without restarting ROS nodes or launching additional tools. How is Crazyswarm2 different from Bitcraze's cflib? diff --git a/docs2/overview.rst b/docs2/overview.rst index 014cfc93d..cc413fe1f 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -67,6 +67,8 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | - default: odom | No | Yes | No | +---------------------+---------+-----------+---------+ +| - default: status | Yes | Yes | No | ++---------------------+---------+-----------+---------+ | - custom | Yes | Yes | No | +---------------------+---------+-----------+---------+ | - Add/Remove Srv | No | Yes | No | diff --git a/docs2/usage.rst b/docs2/usage.rst index d8278958b..52117f0ae 100644 --- a/docs2/usage.rst +++ b/docs2/usage.rst @@ -155,3 +155,11 @@ You may run the script multiple times or different scripts while leaving the ser [terminal1]$ ros2 launch crazyflie launch.py [terminal2]$ ros2 run crazyflie_examples hello_world + +Swarm Management +---------------- + +The launch file will also start a swarm management tool that is a ROS node and web-based GUI. +In the upper pane is the location of the drone visualized in a 3D window, similar to rviz. +In the lower pane, the status as well as log messages are visible (tabbed per drone). +In the future, we are planning to add support for rebooting and other actions. \ No newline at end of file From 55e11dada5b5cd106c168ed9869b41c6630247a8 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 5 Feb 2024 10:56:41 +0100 Subject: [PATCH 12/21] enable status topic by default --- crazyflie/config/crazyflies.yaml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml index e136fcd8b..f102bd78a 100644 --- a/crazyflie/config/crazyflies.yaml +++ b/crazyflie/config/crazyflies.yaml @@ -72,11 +72,13 @@ all: # firmware logging for all drones (use robot_types/type_name to set per type, or # robots/drone_name to set per drone) firmware_logging: - enabled: false + enabled: true default_topics: # remove to disable default topic - pose: - frequency: 10 # Hz + # pose: + # frequency: 10 # Hz + status: + frequency: 1 # Hz #custom_topics: # topic_name1: # frequency: 10 # Hz From 60524ac0c50f01c68ec223e81e3cbe5725326c8e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 6 Feb 2024 07:13:24 +0100 Subject: [PATCH 13/21] Kimberly's feedback: 1) unused code removal 2) Better error handling of tf errors 3) Improved handling of simulation time 4) Unified error handling in on_timer --- crazyflie/launch/launch.py | 4 ++ crazyflie/scripts/gui.py | 82 +++++++++++++++++++++----------------- 2 files changed, 49 insertions(+), 37 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index 4eec72a92..624ae65fd 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -138,5 +138,9 @@ def generate_launch_description(): package='crazyflie', executable='gui.py', name='gui', + output='screen', + parameters=[{ + "use_sim_time": True, + }] ), ]) diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py index 5bd156634..9bc434aad 100755 --- a/crazyflie/scripts/gui.py +++ b/crazyflie/scripts/gui.py @@ -48,8 +48,6 @@ def __init__(self) -> None: self.battery_labels = dict() self.radio_labels = dict() self.robotmodels = dict() - # set of robot names that had a status recently - self.watchdog = dict() with Client.auto_index_client: @@ -60,11 +58,13 @@ def __init__(self) -> None: for name in self.cfnames: robot = scene.stl('/urdf/cf2_assembly.stl').scale(1.0).material('#ff0000').with_name(name) self.robotmodels[name] = robot - self.watchdog[name] = time.time() + # augment with some additional fields + robot.status_ok = False + robot.status_watchdog = time.time() scene.camera.x = 0 scene.camera.y = -1 scene.camera.z = 2 - scene.camera.look_at_x = 0 + scene.camera.look_at_x = 0 scene.camera.look_at_y = 0 scene.camera.look_at_z = 0 @@ -80,21 +80,16 @@ def __init__(self) -> None: self.supervisor_labels[name] = ui.label("") self.battery_labels[name] = ui.label("") self.radio_labels[name] = ui.label("") - ui.button("Reboot", on_click=partial(self.on_reboot, name=name)) for name in self.cfnames: self.create_subscription(Status, name + '/status', partial(self.on_status, name=name), 1) # Call on_timer function - self.timer = self.create_timer(0.1, self.on_timer) - - def send_speed(self, x: float, y: float) -> None: - msg = Twist() - msg.linear.x = x - msg.angular.z = -y - self.linear.value = x - self.angular.value = y - self.cmd_vel_publisher.publish(msg) + update_rate = 30 # Hz + self.timer = self.create_timer( + 1.0/update_rate, + self.on_timer, + clock=rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.SYSTEM_TIME)) def on_rosout(self, msg: Log) -> None: # filter by crazyflie and add to the correct log @@ -115,20 +110,38 @@ def on_rosout(self, msg: Log) -> None: def on_timer(self) -> None: for name, robotmodel in self.robotmodels.items(): - t = self.tf_buffer.lookup_transform( - "world", - name, - rclpy.time.Time()) - pos = t.transform.translation - robotmodel.move(pos.x, pos.y, pos.z) - robotmodel.rotate(*rowan.to_euler([ - t.transform.rotation.w, - t.transform.rotation.x, - t.transform.rotation.y, - t.transform.rotation.z], "xyz")) - - # no update for a while -> mark red - if time.time() - self.watchdog[name] > 2.0: + ros_time = rclpy.time.Time() # get the latest + robot_status_ok = robotmodel.status_ok + if self.tf_buffer.can_transform("world", name, ros_time): + t = self.tf_buffer.lookup_transform( + "world", + name, + ros_time) + transform_time = rclpy.time.Time.from_msg(t.header.stamp) + transform_age = self.get_clock().now() - transform_time + # latest transform is older than a second indicates a problem + if transform_age.nanoseconds * 1e-9 > 1: + robot_status_ok = False + else: + pos = t.transform.translation + robotmodel.move(pos.x, pos.y, pos.z) + robotmodel.rotate(*rowan.to_euler([ + t.transform.rotation.w, + t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z], "xyz")) + else: + # no available transform indicates a problem + robot_status_ok = False + + # no status update for a while, indicate a problem + if time.time() - robotmodel.status_watchdog > 2.0: + robot_status_ok = False + + # any issues detected -> mark red, otherwise green + if robot_status_ok: + robotmodel.material('#00ff00') + else: robotmodel.material('#ff0000') def on_vis_click(self, e: events.SceneClickEventArguments): @@ -181,13 +194,11 @@ def on_status(self, msg, name) -> None: radio_text = f'{msg.rssi} dBm; Unicast: {msg.num_rx_unicast} / {msg.num_tx_unicast}; Broadcast: {msg.num_rx_broadcast} / {msg.num_tx_broadcast}' self.radio_labels[name].set_text(radio_text) - if status_ok: - self.robotmodels[name].material('#00ff00') - else: - self.robotmodels[name].material('#ff0000') - - self.watchdog[name] = time.time() + # save status here + self.robotmodels[name].status_ok = status_ok + # store the time when we last received any status + self.robotmodels[name].status_watchdog[name] = time.time() def on_tab_change(self, arg): for name, robotmodel in self.robotmodels.items(): @@ -196,9 +207,6 @@ def on_tab_change(self, arg): if arg.value in self.robotmodels: self.robotmodels[arg.value].scale(2) - def on_reboot(self, name) -> None: - ui.notify(f'Reboot not implemented, yet') - def main() -> None: # NOTE: This function is defined as the ROS entry point in setup.py, but it's empty to enable NiceGUI auto-reloading From 9f136a2e17786e0b5aff403ba772f4c02fbeee0f Mon Sep 17 00:00:00 2001 From: phanfeld Date: Tue, 6 Feb 2024 14:59:38 +0100 Subject: [PATCH 14/21] first, untested version of image publisher --- crazyflie/config/camera_config.yaml | 58 ++++++++++ crazyflie/src/image_streamer.py | 158 ++++++++++++++++++++++++++++ 2 files changed, 216 insertions(+) create mode 100644 crazyflie/config/camera_config.yaml create mode 100644 crazyflie/src/image_streamer.py diff --git a/crazyflie/config/camera_config.yaml b/crazyflie/config/camera_config.yaml new file mode 100644 index 000000000..2c801c64a --- /dev/null +++ b/crazyflie/config/camera_config.yaml @@ -0,0 +1,58 @@ +image_width: 725 +image_height: 500 +camera_name: crazyflie +camera_matrix: + rows: 3 + cols: 3 + data: [383.76372, 0. , 363.38408, + 0. , 381.38288, 271.61177, + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.001747, -0.009650, -0.002608, -0.002890, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [380.19111, 0. , 360.28396, 0. , + 0. , 380.987 , 270.48483, 0. , + 0. , 0. , 1. , 0. ] + + + +# image_width: 533 +# image_height: 320 +# camera_name: hoverair +# camera_matrix: +# rows: 3 +# cols: 3 +# data: [295.21347, 0. , 284.02365, +# 0. , 294.30945, 196.91373, +# 0. , 0. , 1. ] +# distortion_model: plumb_bob +# distortion_coefficients: +# rows: 1 +# cols: 5 +# data: [0.029797, -0.026754, 0.000635, -0.001295, 0.000000] +# rectification_matrix: +# rows: 3 +# cols: 3 +# data: [1., 0., 0., +# 0., 1., 0., +# 0., 0., 1.] +# projection_matrix: +# rows: 3 +# cols: 4 +# data: [297.20145, 0. , 283.47583, 0. , +# 0. , 296.96598, 197.18161, 0. , +# 0. , 0. , 1. , 0. ] + + + diff --git a/crazyflie/src/image_streamer.py b/crazyflie/src/image_streamer.py new file mode 100644 index 000000000..ed5ce88ee --- /dev/null +++ b/crazyflie/src/image_streamer.py @@ -0,0 +1,158 @@ +import socket,os,struct, time +import numpy as np +import cv2 +import yaml +from ament_index_python.packages import get_package_share_directory + + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image, CameraInfo + +from rcl_interfaces.msg import ParameterDescriptor, ParameterType + + +class ImageNode(rclpy.node.Node): + def __init__(self): + super().__init__("image_node") + + # declare topic names + self.declare_parameter( + name="image_topic", + value="/camera/image", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="Image topic to publish to.", + ), + ) + + self.declare_parameter( + name="camera_info_topic", + value="/camera/camera_info", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="Camera info topic to subscribe to.", + ), + ) + + # update topic names from config + image_topic = ( + self.get_parameter("image_topic").get_parameter_value().string_value + ) + self.get_logger().info(f"Image topic: {image_topic}") + + info_topic = ( + self.get_parameter("camera_info_topic").get_parameter_value().string_value + ) + self.get_logger().info(f"Image info topic: {info_topic}") + + + # load camera parameters from yaml + # TODO: could possibly be done in the same way as with the other parameters + config_path = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'camera_config.yaml' + ) + + # create messages and publishers + self.image_mgs = Image() + self.camera_info_msg = self._construct_from_yaml(config_path) + self.image_publisher = self.create_publisher(Image, image_topic, 10) + self.info_publisher = self.create_publisher(CameraInfo, info_topic, 10) + + # TODO: just for testing, needs to be moved + self.info_publisher.publish(self.camera_info_msg) + + # set up connection to AI Deck + deck_ip = "192.168.4.1" + deck_port = '5000' + self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.client_socket.connect((deck_ip, deck_port)) + self.image = None + self.rx_buffer = bytearray() + + # set up timers for callbacks + timer_period = 0.5 + self.rx_timer = self.create_timer(timer_period, self.receive_callback) + self.tx_timer = self.create_timer(timer_period, self.publish_callback) + + + def _construct_from_yaml(self, path): + camera_info = CameraInfo() + with open(path) as f: + config = yaml.load(f, Loader=yaml.FullLoader) + + camera_info.header.frame_id = config['camera_name'] + camera_info.header.stamp = self.get_clock().now().to_msg() + camera_info.width = int(config['image_width']) + camera_info.height = int(config['image_height']) + camera_info.distortion_model = config['distortion_model'] + camera_info.d = config['distortion_coefficients']['data'] + camera_info.k = config['camera_matrix']['data'] + camera_info.r = config['rectification_matrix']['data'] + camera_info.p = config['projection_matrix']['data'] + return camera_info + + def _rx_bytes(self, size): + data = bytearray() + while len(data) < size: + data.extend(self.client_socket.recv(size-len(data))) + return data + + def receive_callback(self): + # first get the info + packetInfoRaw = self._rx_bytes(4) + [length, routing, function] = struct.unpack('{:02X})".format(length, src, dst)) + chunk = self._rx_bytes(length - 2) + imgStream.extend(chunk) + + raw_img = np.frombuffer(imgStream, dtype=np.uint8) + raw_img.shape = (width, height) + self.image = cv2.cvtColor(raw_img, cv2.COLOR_BayerBG2RGBA) + + else: # otherwise set image to None again + self.image = None + + def publish_callback(self): + if self.image is not None: + self.image_mgs.header.frame_id = self.camera_info_msg.header.frame_id + self.image_mgs.header.stamp = self.get_clock().now().to_msg() + self.camera_info_msg.header.stamp = self.image_mgs.header.stamp + + self.image_mgs.height = self.camera_info_msg.height + self.image_mgs.width = self.camera_info_msg.width + self.image_mgs.encoding = 'rgba8' + self.image_mgs.step = self.image.step + self.image_mgs.is_bigendian = 0 # TODO: implement automatic check depending on system + self.image_mgs.data = self.image.data + + self.image_publisher.publish(self.image_mgs) + self.info_publisher.publish(self.camera_info_msg) + self.image = None + + + +def main(args=None): + rclpy.init(args=args) + node = ImageNode() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file From 9d9ec2636054b97392b595176c28efcde058c8c8 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 6 Feb 2024 16:38:44 +0100 Subject: [PATCH 15/21] watchdog bugfix --- crazyflie/scripts/gui.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py index 9bc434aad..9a7d4b9e5 100755 --- a/crazyflie/scripts/gui.py +++ b/crazyflie/scripts/gui.py @@ -198,7 +198,7 @@ def on_status(self, msg, name) -> None: self.robotmodels[name].status_ok = status_ok # store the time when we last received any status - self.robotmodels[name].status_watchdog[name] = time.time() + self.robotmodels[name].status_watchdog = time.time() def on_tab_change(self, arg): for name, robotmodel in self.robotmodels.items(): From 9efb8ed7df87ca16eaa55ee1c31dec27b4240bdd Mon Sep 17 00:00:00 2001 From: phanfeld Date: Tue, 6 Feb 2024 16:58:35 +0100 Subject: [PATCH 16/21] working implementation of image publisher --- crazyflie/src/image_streamer.py | 37 +++++++++++++++++---------------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/crazyflie/src/image_streamer.py b/crazyflie/src/image_streamer.py index ed5ce88ee..204d824eb 100644 --- a/crazyflie/src/image_streamer.py +++ b/crazyflie/src/image_streamer.py @@ -56,7 +56,7 @@ def __init__(self): ) # create messages and publishers - self.image_mgs = Image() + self.image_msg = Image() self.camera_info_msg = self._construct_from_yaml(config_path) self.image_publisher = self.create_publisher(Image, image_topic, 10) self.info_publisher = self.create_publisher(CameraInfo, info_topic, 10) @@ -66,14 +66,16 @@ def __init__(self): # set up connection to AI Deck deck_ip = "192.168.4.1" - deck_port = '5000' + deck_port = 5000 + print("Connecting to socket on {}:{}...".format(deck_ip, deck_port)) self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.client_socket.connect((deck_ip, deck_port)) + print("Socket connected") self.image = None self.rx_buffer = bytearray() # set up timers for callbacks - timer_period = 0.5 + timer_period = 0.01 self.rx_timer = self.create_timer(timer_period, self.receive_callback) self.tx_timer = self.create_timer(timer_period, self.publish_callback) @@ -123,24 +125,23 @@ def receive_callback(self): raw_img = np.frombuffer(imgStream, dtype=np.uint8) raw_img.shape = (width, height) self.image = cv2.cvtColor(raw_img, cv2.COLOR_BayerBG2RGBA) - - else: # otherwise set image to None again - self.image = None + # else: # otherwise set image to None again + # self.image = None def publish_callback(self): if self.image is not None: - self.image_mgs.header.frame_id = self.camera_info_msg.header.frame_id - self.image_mgs.header.stamp = self.get_clock().now().to_msg() - self.camera_info_msg.header.stamp = self.image_mgs.header.stamp - - self.image_mgs.height = self.camera_info_msg.height - self.image_mgs.width = self.camera_info_msg.width - self.image_mgs.encoding = 'rgba8' - self.image_mgs.step = self.image.step - self.image_mgs.is_bigendian = 0 # TODO: implement automatic check depending on system - self.image_mgs.data = self.image.data - - self.image_publisher.publish(self.image_mgs) + self.image_msg.header.frame_id = self.camera_info_msg.header.frame_id + self.image_msg.header.stamp = self.get_clock().now().to_msg() + self.camera_info_msg.header.stamp = self.image_msg.header.stamp + width, height, channels = self.image.shape + self.image_msg.height = height + self.image_msg.width = width + self.image_msg.encoding = 'rgba8' + self.image_msg.step = width * channels # number of bytes each row in the array will occupy + self.image_msg.is_bigendian = 0 # TODO: implement automatic check depending on system + self.image_msg.data = self.image.flatten().data + + self.image_publisher.publish(self.image_msg) self.info_publisher.publish(self.camera_info_msg) self.image = None From 597e943b330f813122430988f899329587f99405 Mon Sep 17 00:00:00 2001 From: phanfeld Date: Wed, 7 Feb 2024 09:45:19 +0100 Subject: [PATCH 17/21] correct camera calibration --- crazyflie/config/camera_config.yaml | 48 +++++------------------------ crazyflie/src/image_streamer.py | 5 +-- 2 files changed, 9 insertions(+), 44 deletions(-) diff --git a/crazyflie/config/camera_config.yaml b/crazyflie/config/camera_config.yaml index 2c801c64a..69527384f 100644 --- a/crazyflie/config/camera_config.yaml +++ b/crazyflie/config/camera_config.yaml @@ -1,17 +1,17 @@ -image_width: 725 -image_height: 500 +image_width: 324 +image_height: 324 camera_name: crazyflie camera_matrix: rows: 3 cols: 3 - data: [383.76372, 0. , 363.38408, - 0. , 381.38288, 271.61177, + data: [181.87464, 0. , 162.52301, + 0. , 182.58129, 160.79418, 0. , 0. , 1. ] distortion_model: plumb_bob distortion_coefficients: rows: 1 cols: 5 - data: [-0.001747, -0.009650, -0.002608, -0.002890, 0.000000] + data: [-0.070366, -0.006434, -0.002691, -0.001983, 0.000000] rectification_matrix: rows: 3 cols: 3 @@ -21,38 +21,6 @@ rectification_matrix: projection_matrix: rows: 3 cols: 4 - data: [380.19111, 0. , 360.28396, 0. , - 0. , 380.987 , 270.48483, 0. , - 0. , 0. , 1. , 0. ] - - - -# image_width: 533 -# image_height: 320 -# camera_name: hoverair -# camera_matrix: -# rows: 3 -# cols: 3 -# data: [295.21347, 0. , 284.02365, -# 0. , 294.30945, 196.91373, -# 0. , 0. , 1. ] -# distortion_model: plumb_bob -# distortion_coefficients: -# rows: 1 -# cols: 5 -# data: [0.029797, -0.026754, 0.000635, -0.001295, 0.000000] -# rectification_matrix: -# rows: 3 -# cols: 3 -# data: [1., 0., 0., -# 0., 1., 0., -# 0., 0., 1.] -# projection_matrix: -# rows: 3 -# cols: 4 -# data: [297.20145, 0. , 283.47583, 0. , -# 0. , 296.96598, 197.18161, 0. , -# 0. , 0. , 1. , 0. ] - - - + data: [169.24555, 0. , 161.54541, 0. , + 0. , 169.97813, 159.07974, 0. , + 0. , 0. , 1. , 0. ] \ No newline at end of file diff --git a/crazyflie/src/image_streamer.py b/crazyflie/src/image_streamer.py index 204d824eb..6f831895e 100644 --- a/crazyflie/src/image_streamer.py +++ b/crazyflie/src/image_streamer.py @@ -19,7 +19,7 @@ def __init__(self): # declare topic names self.declare_parameter( name="image_topic", - value="/camera/image", + value="/image", descriptor=ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description="Image topic to publish to.", @@ -61,9 +61,6 @@ def __init__(self): self.image_publisher = self.create_publisher(Image, image_topic, 10) self.info_publisher = self.create_publisher(CameraInfo, info_topic, 10) - # TODO: just for testing, needs to be moved - self.info_publisher.publish(self.camera_info_msg) - # set up connection to AI Deck deck_ip = "192.168.4.1" deck_port = 5000 From b3271b992670b84ea63454cc69b791335f9e3979 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 7 Feb 2024 17:00:55 +0100 Subject: [PATCH 18/21] fix regression caused by augmenting cf_dict to contain "all" --- crazyflie/scripts/crazyflie_server.py | 32 ++++++++++++++------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index ac7882d15..a38dd6808 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -364,7 +364,8 @@ def _fully_connected(self, link_uri): self.swarm.fully_connected_crazyflie_cnt += 1 - if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict): + # use len(self.cf_dict) - 1, since cf_dict contains "all" as well + if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict) - 1: self.get_logger().info("All Crazyflies are fully connected!") self._init_parameters() self._init_logging() @@ -707,38 +708,39 @@ def _parameters_callback(self, params): for param in params: param_split = param.name.split(".") - if param_split[0] in self.cf_dict.values(): - cf_name = param_split[0] + if param_split[0] == "all": if param_split[1] == "params": name_param = param_split[2] + "." + param_split[3] try: - self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( - name_param, param.value - ) + for link_uri in self.uris: + cf = self.swarm._cfs[link_uri].cf.param.set_value( + name_param, param.value + ) self.get_logger().info( - f"[{self.uri_dict[cf_name]}] {name_param} is set to {param.value}" + f"[{self.cf_dict[link_uri]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) - if param_split[1] == "logs": - return SetParametersResult(successful=True) - elif param_split[0] == "all": + elif param_split[0] in self.cf_dict.values(): + cf_name = param_split[0] if param_split[1] == "params": name_param = param_split[2] + "." + param_split[3] try: - for link_uri in self.uris: - cf = self.swarm._cfs[link_uri].cf.param.set_value( - name_param, param.value - ) + self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( + name_param, param.value + ) self.get_logger().info( - f"[{self.cf_dict[link_uri]}] {name_param} is set to {param.value}" + f"[{self.uri_dict[cf_name]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) + if param_split[1] == "logs": + return SetParametersResult(successful=True) + return SetParametersResult(successful=False) From 6b14385737483b51d6f52cc552402e7a06a488cb Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 8 Feb 2024 21:34:53 +0100 Subject: [PATCH 19/21] do not launch gui automatically --- crazyflie/launch/launch.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index 624ae65fd..8a9290c11 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -134,13 +134,4 @@ def generate_launch_description(): "use_sim_time": True, }] ), - Node( - package='crazyflie', - executable='gui.py', - name='gui', - output='screen', - parameters=[{ - "use_sim_time": True, - }] - ), ]) From 62fc28f4ddb996a1983cf93942a3dcf928f1bd1d Mon Sep 17 00:00:00 2001 From: phanfeld Date: Fri, 9 Feb 2024 12:29:50 +0100 Subject: [PATCH 20/21] loading all parameters from config file --- crazyflie/CMakeLists.txt | 1 + ...amera_config.yaml => aideck_streamer.yaml} | 4 ++ .../aideck_streamer.py} | 64 +++++++++++-------- 3 files changed, 44 insertions(+), 25 deletions(-) rename crazyflie/config/{camera_config.yaml => aideck_streamer.yaml} (86%) rename crazyflie/{src/image_streamer.py => scripts/aideck_streamer.py} (79%) diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index c8899cfbe..de8eaf697 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -109,6 +109,7 @@ install(PROGRAMS scripts/vel_mux.py scripts/cfmult.py scripts/simple_mapper_multiranger.py + scripts/aideck_streamer.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/crazyflie/config/camera_config.yaml b/crazyflie/config/aideck_streamer.yaml similarity index 86% rename from crazyflie/config/camera_config.yaml rename to crazyflie/config/aideck_streamer.yaml index 69527384f..280ba9163 100644 --- a/crazyflie/config/camera_config.yaml +++ b/crazyflie/config/aideck_streamer.yaml @@ -1,3 +1,7 @@ +image_topic: /camera/image +camera_info_topic: /camera/camera_info +deck_ip: "192.168.4.1" +deck_port: 5000 image_width: 324 image_height: 324 camera_name: crazyflie diff --git a/crazyflie/src/image_streamer.py b/crazyflie/scripts/aideck_streamer.py similarity index 79% rename from crazyflie/src/image_streamer.py rename to crazyflie/scripts/aideck_streamer.py index 6f831895e..c97df2960 100644 --- a/crazyflie/src/image_streamer.py +++ b/crazyflie/scripts/aideck_streamer.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import socket,os,struct, time import numpy as np import cv2 @@ -12,14 +13,28 @@ from rcl_interfaces.msg import ParameterDescriptor, ParameterType -class ImageNode(rclpy.node.Node): +class ImageStreamerNode(Node): def __init__(self): super().__init__("image_node") + # declare config path parameter + self.declare_parameter( + name="config_path", + value=os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'aideck_streamer.yaml' + ) + ) + + config_path = self.get_parameter("config_path").value + with open(config_path) as f: + config = yaml.load(f, Loader=yaml.FullLoader) + # declare topic names self.declare_parameter( name="image_topic", - value="/image", + value=config["image_topic"], descriptor=ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description="Image topic to publish to.", @@ -28,46 +43,49 @@ def __init__(self): self.declare_parameter( name="camera_info_topic", - value="/camera/camera_info", + value=config["camera_info_topic"], descriptor=ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description="Camera info topic to subscribe to.", ), ) - # update topic names from config + # declare aideck ip and port + self.declare_parameter( + name='deck_ip', + value=config['deck_ip'], + ) + + self.declare_parameter( + name='deck_port', + value=config['deck_port'], + ) + + # define variables from ros2 parameters image_topic = ( - self.get_parameter("image_topic").get_parameter_value().string_value + self.get_parameter("image_topic").value ) self.get_logger().info(f"Image topic: {image_topic}") info_topic = ( - self.get_parameter("camera_info_topic").get_parameter_value().string_value + self.get_parameter("camera_info_topic").value ) self.get_logger().info(f"Image info topic: {info_topic}") - - # load camera parameters from yaml - # TODO: could possibly be done in the same way as with the other parameters - config_path = os.path.join( - get_package_share_directory('crazyflie'), - 'config', - 'camera_config.yaml' - ) # create messages and publishers self.image_msg = Image() - self.camera_info_msg = self._construct_from_yaml(config_path) + self.camera_info_msg = self._construct_from_yaml(config) self.image_publisher = self.create_publisher(Image, image_topic, 10) self.info_publisher = self.create_publisher(CameraInfo, info_topic, 10) # set up connection to AI Deck - deck_ip = "192.168.4.1" - deck_port = 5000 - print("Connecting to socket on {}:{}...".format(deck_ip, deck_port)) + deck_ip = self.get_parameter("deck_ip").value + deck_port = int(self.get_parameter("deck_port").value) + self.get_logger().info("Connecting to socket on {}:{}...".format(deck_ip, deck_port)) self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.client_socket.connect((deck_ip, deck_port)) - print("Socket connected") + self.get_logger().info("Socket connected") self.image = None self.rx_buffer = bytearray() @@ -77,10 +95,8 @@ def __init__(self): self.tx_timer = self.create_timer(timer_period, self.publish_callback) - def _construct_from_yaml(self, path): + def _construct_from_yaml(self, config): camera_info = CameraInfo() - with open(path) as f: - config = yaml.load(f, Loader=yaml.FullLoader) camera_info.header.frame_id = config['camera_name'] camera_info.header.stamp = self.get_clock().now().to_msg() @@ -122,8 +138,6 @@ def receive_callback(self): raw_img = np.frombuffer(imgStream, dtype=np.uint8) raw_img.shape = (width, height) self.image = cv2.cvtColor(raw_img, cv2.COLOR_BayerBG2RGBA) - # else: # otherwise set image to None again - # self.image = None def publish_callback(self): if self.image is not None: @@ -146,7 +160,7 @@ def publish_callback(self): def main(args=None): rclpy.init(args=args) - node = ImageNode() + node = ImageStreamerNode() rclpy.spin(node) node.destroy_node() From 172938185a506b0271e2420d36f4630980a3e024 Mon Sep 17 00:00:00 2001 From: phanfeld Date: Fri, 9 Feb 2024 13:31:37 +0100 Subject: [PATCH 21/21] script needs to be executable --- crazyflie/scripts/aideck_streamer.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 crazyflie/scripts/aideck_streamer.py diff --git a/crazyflie/scripts/aideck_streamer.py b/crazyflie/scripts/aideck_streamer.py old mode 100644 new mode 100755