From 14bdc226541dfab1a3ec1284a7c59ea24034863f Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 1 Feb 2024 16:03:45 +0100 Subject: [PATCH 01/21] server cpp: add new communication warnings * unicast_receive_rate measures the end-to-end rate of received unicast packets over sent unicast packets * broadcast_receive_rate measures the end-to-end rate of received broadcast packets over sent unicast packets Warnings if this is larger than 1 or smaller than a user-defined threshold. --- crazyflie/config/crazyflies.yaml | 2 ++ crazyflie/config/server.yaml | 2 ++ crazyflie/src/crazyflie_server.cpp | 27 +++++++++++++++++++++++++++ 3 files changed, 31 insertions(+) diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml index e136fcd8b..adefcbfd1 100644 --- a/crazyflie/config/crazyflies.yaml +++ b/crazyflie/config/crazyflies.yaml @@ -77,6 +77,8 @@ all: # remove to disable default topic pose: frequency: 10 # Hz + status: + frequency: 1 # Hz #custom_topics: # topic_name1: # frequency: 10 # Hz diff --git a/crazyflie/config/server.yaml b/crazyflie/config/server.yaml index 6ec8a3989..59a3ebcb5 100644 --- a/crazyflie/config/server.yaml +++ b/crazyflie/config/server.yaml @@ -7,6 +7,8 @@ communication: max_unicast_latency: 10.0 # ms min_unicast_ack_rate: 0.9 + min_unicast_receive_rate: 0.9 # requires status topic to be enabled + min_broadcast_receive_rate: 0.9 # requires status topic to be enabled publish_stats: false firmware_params: query_all_values_on_connect: False diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 4a71e0245..1dfe1de98 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -181,6 +181,8 @@ class CrazyflieROS warning_freq_ = node->get_parameter("warnings.frequency").get_parameter_value().get(); max_latency_ = node->get_parameter("warnings.communication.max_unicast_latency").get_parameter_value().get(); min_ack_rate_ = node->get_parameter("warnings.communication.min_unicast_ack_rate").get_parameter_value().get(); + min_unicast_receive_rate_ = node->get_parameter("warnings.communication.min_unicast_receive_rate").get_parameter_value().get(); + min_broadcast_receive_rate_ = node->get_parameter("warnings.communication.min_broadcast_receive_rate").get_parameter_value().get(); publish_stats_ = node->get_parameter("warnings.communication.publish_stats").get_parameter_value().get(); if (publish_stats_) { publisher_connection_stats_ = node->create_publisher(name + "/connection_statistics", 10); @@ -801,6 +803,27 @@ class CrazyflieROS previous_stats_broadcast_ = statsBc; publisher_status_->publish(msg); + + // warnings + if (msg.num_rx_unicast > msg.num_tx_unicast) { + RCLCPP_WARN(logger_, "Unexpected number of unicast packets. Sent: %d. Received: %d", msg.num_tx_unicast, msg.num_rx_unicast); + } + if (msg.num_tx_unicast > 0) { + float unicast_receive_rate = msg.num_rx_unicast / (float)msg.num_tx_unicast; + if (unicast_receive_rate < min_unicast_receive_rate_) { + RCLCPP_WARN(logger_, "Low unicast receive rate (%.2f < %.2f). Sent: %d. Received: %d", unicast_receive_rate, min_unicast_receive_rate_, msg.num_tx_unicast, msg.num_rx_unicast); + } + } + + if (msg.num_rx_broadcast > msg.num_tx_broadcast) { + RCLCPP_WARN(logger_, "Unexpected number of broadcast packets. Sent: %d. Received: %d", msg.num_tx_broadcast, msg.num_rx_broadcast); + } + if (msg.num_tx_broadcast > 0) { + float broadcast_receive_rate = msg.num_rx_broadcast / (float)msg.num_tx_broadcast; + if (broadcast_receive_rate < min_broadcast_receive_rate_) { + RCLCPP_WARN(logger_, "Low broadcast receive rate (%.2f < %.2f). Sent: %d. Received: %d", broadcast_receive_rate, min_broadcast_receive_rate_, msg.num_tx_broadcast, msg.num_rx_broadcast); + } + } } } @@ -912,6 +935,8 @@ class CrazyflieROS float warning_freq_; float max_latency_; float min_ack_rate_; + float min_unicast_receive_rate_; + float min_broadcast_receive_rate_; bool publish_stats_; rclcpp::Publisher::SharedPtr publisher_connection_stats_; }; @@ -980,6 +1005,8 @@ class CrazyflieServer : public rclcpp::Node this->declare_parameter("warnings.communication.max_unicast_latency", 10.0); this->declare_parameter("warnings.communication.min_unicast_ack_rate", 0.9); + this->declare_parameter("warnings.communication.min_unicast_receive_rate", 0.9); + this->declare_parameter("warnings.communication.min_broadcast_receive_rate", 0.9); this->declare_parameter("warnings.communication.publish_stats", false); publish_stats_ = this->get_parameter("warnings.communication.publish_stats").get_parameter_value().get(); From 19f40f5ef1b5439faac6233e5ad1e50696d0e036 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 1 Feb 2024 21:55:38 +0100 Subject: [PATCH 02/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 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