From dd3a7a822a615c80f3b5266a051cef80fb4c6a61 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 6 Jun 2024 12:51:40 +0900 Subject: [PATCH 1/2] refactor(simple_planning_simulator): remove static odom tf publisher (#7265) Signed-off-by: Maxime CLEMENT --- .../launch/simple_planning_simulator.launch.py | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 0070646a20d49..a8eb88f55df19 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -14,7 +14,6 @@ import launch from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -78,22 +77,7 @@ def launch_setup(context, *args, **kwargs): ], ) - map_to_odom_tf_publisher = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_map_to_odom_tf_publisher", - output="screen", - arguments=[ - "--frame-id", - "map", - "--child-frame-id", - "odom", - ], - ) - - group = GroupAction([simple_planning_simulator_node, map_to_odom_tf_publisher]) - - return [group] + return [simple_planning_simulator_node] def generate_launch_description(): From 043a7e9a9a33d98763a2d1e1812b6fb913c931a0 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 6 Jun 2024 13:49:58 +0900 Subject: [PATCH 2/2] feat(gyro_odometer): add diagnostic (#7188) * add diagnostic Signed-off-by: Yamato Ando * fix diag time stamp Signed-off-by: Yamato Ando * style(pre-commit): autofix * fix spellcheck Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro --- localization/gyro_odometer/CMakeLists.txt | 1 + localization/gyro_odometer/README.md | 15 + .../gyro_odometer/diagnostics_module.hpp | 65 ++++ .../gyro_odometer/gyro_odometer_core.hpp | 6 + .../gyro_odometer/media/diagnostic.png | Bin 0 -> 41146 bytes localization/gyro_odometer/package.xml | 1 + .../gyro_odometer/src/diagnostics_module.cpp | 110 +++++++ .../gyro_odometer/src/gyro_odometer_core.cpp | 300 ++++++++++-------- 8 files changed, 361 insertions(+), 137 deletions(-) create mode 100644 localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp create mode 100644 localization/gyro_odometer/media/diagnostic.png create mode 100644 localization/gyro_odometer/src/diagnostics_module.cpp diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index a832383ff4761..1b142b254d2e1 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp + src/diagnostics_module.cpp ) target_link_libraries(${PROJECT_NAME} fmt) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index e9e390010f084..aa6f2a96f4838 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -34,3 +34,18 @@ - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. + +## Diagnostics + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- | +| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none | +| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none | +| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none | +| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none | +| `imu_queue_size` | the size of gyro_queue. | none | none | +| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed | diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp new file mode 100644 index 0000000000000..49b881900b997 --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ +#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + template + void addKeyValue(const std::string & key, const T & value); + void updateLevelAndMessage(const int8_t level, const std::string & message); + void publish(const rclcpp::Time & publish_time_stamp); + +private: + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = std::to_string(value); + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); + +} // namespace autoware::gyro_odometer + +#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 2926589bd2da9..3b2da504f938a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,7 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "gyro_odometer/diagnostics_module.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" @@ -52,6 +53,7 @@ class GyroOdometerNode : public rclcpp::Node void callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concatGyroAndOdometer(); void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr @@ -74,8 +76,12 @@ class GyroOdometerNode : public rclcpp::Node bool vehicle_twist_arrived_; bool imu_arrived_; + rclcpp::Time latest_vehicle_twist_ros_time_; + rclcpp::Time latest_imu_ros_time_; std::deque vehicle_twist_queue_; std::deque gyro_queue_; + + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/gyro_odometer/media/diagnostic.png new file mode 100644 index 0000000000000000000000000000000000000000..f2324f4079d0db7f1c407841cbc2c925e645eef9 GIT binary patch literal 41146 zcmdqIV|OM^^fwyYHYT=h+s4GUc?A>OoY=PQWMbR4?UVa|&U5hM`2wfc>Q$?&@uI4F z@A_efE6Pj2!{ERG0Rh2FNs1~10Rcz;oLEp0KRs0_Yi2(Un6t2yD%8&bfHDdDxyE)8 z({xd>H*;|}bTS1px3jl3rE@lRGBvexwy<}(2JPYhd5FbQRntYp$<)xr(%z0p)za4V zrxOT>nUR_Gu&t4i?I<&hh3P1@h=_^hC_R9U`7kVmk^Q(NfdqSp8VHC8NJ>;l)g$v_ z)71riWEQUE1_?L$8Ey z3pw3e?ft!*vDZNG9UenZ4@EhTA&|-Vx!(3V&7!a6@Ba6Bh+$8&vpHgn2`~#lOaAy) zQEmGkfULt>szvYKN)5_fvFMFf;i?Z9`jVtbhCD_ymwB?l}KB!A%N&ad@1m>9!pc>#w|oJR>pmXqyX zn?Yu2gVG!Wsamjs_25f+mJ=-QHFNldFR_gg4UWJgsN+~vZX)SD+kSZ5oW89zI*;FY zX58I0EDM>R2PZ^t4b><#C;#b6DMj@+2Vpy>+lV|bkFe|V%6^*EXR>d z08^`TM>Yq8^X?YZGFMWSxqQ#rTGwr<0RkJqSKa2gh=EL}0iG5czT3X#C zp2+3ybZ5;SW{9kf=FNcsL2h)u(p;+EkBi_1&&p0v%IhT{3-oe=<%_$4MaEZ|*We~{ z`v!V*-`AsrIQhPEFCwsj;QA+d3PLY_lIbCKUUIT248Vi1?N*hk_0_x;X2k}7JIyO; zs;l{DS|>F)HVZ-hXTY|Me>HDUQE|-AVxQ<1uzviu%~Po`Jk$n}<4^ySKER!CRf=2L zr0zhsg*C9GKdt5qp{vJ(a#<17KPiqV@onMqY6qFkjdw@)u_6d7l|WP$aJx%S0z~w< z@C&fe$qSNIWq61ZrTyvKznfI4JX*r5GQ#&jeC%AgXtfWfMO~@ox zsDLUeYt63CC$m$B13^iuuC>k#i6^(ksIkfOT6_88GbnJMq9p-QUsT*?kYF>kUS_%^ z@prYcbD0yac~VhHNw0YXGcSyXNJFc(w(JI+n$*6Y|8FTx=F*wm)V?-qvktWpOs2H+8AG+%XZ?};wYuBKwqOMi$1Js_+tt_ zL*u>#(P~ESa?}mBdR8`Jc>kk40-kLM$N6QWX`|kfs>Dw-bxWG=e6mw2y%1nd=8dK@ z&*cSVUKzDJEYNK>;y|M<27kP@P)S#HEr#jJ>HS?F?&z5DdfaOEphkV7cuOb$=}gjM zr~Qus$3yXOEg`EyoRoR{P8N0Krb!^gNl46e zV}O_#$BW}LqQVV-ukSZ*tHY2G`S|N`AnR@al9+CrDaG@}+Qc@0r8tEvlE4ei{a4bq zSNgVh?RIe#MFz*mBXD42(TiATU2i8EnLju@7J6=>ZLb!G=~xS9ZFECGT%!%^ zJH07?2ZCjHOEjUl6_t?CG5vVjhwwT%yg5Z}yZkS-tBVe%z`{Qrp+Sn!;8}maCcsFf z_D#POr$Z774If8SEqqaElitNw zFNeaZd)JMW>tc=$OkP~q6hE&s$u=a_g;uO=s!iSycQ~aaUpp!BV3x}u921kI*Vz*J zEcya?j+CanPW|e$=-v-C$|NB#v@~Xeu&s3tw2ZugH=tDDPB=nEBFbI3j4SeoVZR&B=zYAX2%R$fKBH(x5A8t{}|JLg5c z1w8KYn}ia(SI`{1y`QAmjM;;E#+H)WTkv{b_%7V|E?x`|+KDjplh&=t*SnG}G*C*tIn! zx+Aj^-*bJ_1wFXAddX@jQpm?76~;Z<=tme5QvLc8A?%9u89^3MrV2OvV{AtZOYy;qJI?))R(HMvZd7Ab0e4JZ=lHsDbrHj zzeZCu+b(7R{HokrV59YthG=jelayIQc-^0O=6-SZKe@?p|dGjJ_ zcuHhyt;l{DP%si69#a$(+g$f~;`ZQt1SC{k&m`#tWaB}=n0t<+>!2twINuHmUMe9zEMUJ-6K(xmahi`K@E`mx16Q;A>GS(YQIPn%K_ME4O6Zx z)6RT?yBEM3ha#DLK6Km1L4bSZq=V|jG0uCE3~y6*{{7Y1a!n?f!_dYv{JW?a9= z7_R7vH%Z_2bL4c5_km=~$|B&r>fRdpbV+BBl7$&0#fH6?dDcU1$bJ)3_ZK3F`|ox(OZ?q$dYf}ua6u_k8AJhlZmyQy>rs>8tKBH4JA-}f<7|7pNzQ*t zhYt&A)?@Lo9L^FK((-;CzwlL8A2+oK>Q8CBUObMB^6Du4amXzHd)#i)N48A()Q*=`0Oj4n&&qxFgt1Dqv zkIy>N#{|~SN^My)+#Ajs4$oOMH7d;C$xeSZ7}b7}LO3)bP1r+i*3Har=i2lih8V`x z=0%1pqlf{jjgbTs+w8Xc!$Sg&hmN3=8pjRMp$Sh)d@q;%j~f^r=@mzEM>QP@EbHKR zR0{vI6f}wErRcMmLn~UYkMltN92Z~A9@O8RdY7UAckxr!0&Im<<7S8au>yL5?1sM?Ew+bi_8IfP6Bo^t6!jvIc;AC}z3}`z zYS0TrHY_UCl!}^)bQHPRFSruMr_Id1Zp(%~TE) z;EYBE&Cl4p+Cw;Yc7jWCtXAGT)~9GR;|>ZEBVV;TEFju7m5Nm7$5wMpx3H$VS+l1lFD3@l-t@;MB04|$d%?LcsuZ}JNmg}dc5~G2=n=Q2UptHx5>#TG zh-MA{y&}VLy7|}#BzdPxP_#No!ivAHsZ^*cnO~32z(jKyIm;K=oTU%ptyQw}*|IKh zzp%8Ad)T$btDyLOi;E;+cQ(4A@rXmtY1I@K4p6T;`yEK-W+8#k5au*vC#NP zKklAlw)H`frTdSVZftH@NFsSHg8t7ez*=c(k|pO3ZRF-rAOt*~&}OfME#S0LLn&`z zma3L{`@_y#ed~TSbQeVfFqc%sXIrXHSytzkui?PumgCNKzH*e~cV9(^w_c6sbHS=w z?$YRtXr#LN=YyorhL}MtR1_nac+6BcTN&0c(A__|d-%2|8^%n72IktA`nH zC&)|*&Y__|zA7(Thq9uFD<-4b^y`G$Q%l3kLaot>W(b3XO|{ z2?%|PudUfaEf&Le4-3Qdw4!5HX{@6%7v^xLEBH0OAWjFlECl7tvW!E1cO$FwJy3?) z98CPBU!C^@!}8}10+s5W!J@$lyUytZ7Y9V>(2~V(UY!~?WZECH0xWV6@PBg@KU@U{ zA3Cjas#{Q%a6lnU-_B*30>h}c=GrQ#v{XQGzlNCo%Z6Xt=@G&N)&>fCpsr!TeBTra z-XahDKmdtU+gaRy;|!|pgG)R1xUYBLba;Ya9Qux{;y}t4I|wd>6w#{NCpD*zpJiv`yb4{8?*^qZrWB42oF%zy?XZyv$AslPvCda(3t^wiDYRB z$Q(&_Pd{LVBZ84-m&o;!C*w~jg*7WxRUNK5o=T8ytk$?y0s7>`{nzuIVcJtex#*?r zZd0AxhN9zoUu?U*(NWjaBi(RggrnK4DVLPQavc@?g^UV2zIjOIOHPVK*TS(1T#b#? z*FxCZYBo&%U(p&VbqbEw?aSXWkObWZer!L~(=Q^8TFL!-`R1hD+{9Fh#{|((3};Uj z^2uY0x^}(%x%tp+X{57=kGH7s!io3uZNo6}U!;7S0RyMzHThuSZcXI(jp0ex^aj=v z;c==iaMa_{hyACg3;rzjCNbv%MIq>9k_+u2b$}ttkXFyy`GiUy>pk9P2Z!7eKd6C# z4a=f9D5-&j0mM1U1Se)=fv{Y85V9h^&jRZD${u`lpk!i^)^))-j|7!7PG#V5YHxuM z$PN{TKMBXusX?=%w?9RRNlZ#YG*hSd$-d#4j^ z$E*15Rl0=;z339gFC_vRn4ftUcM1pwB(j`aLzimpHiTd7R^=8_jn9q{IE}pbsY-h- zVF|=%U62Z6|3h@ck(|f&MXkysJ=HS(a4b$ty8h^kxq=Z^3&_D|oO8L55iesI3C3|H zJ(n9YnZWU&NeTQ&3No35m}i(5OsJ0^rVtoonKenU^zaDPVEDarHLF&dEGX9`)oDRK zxolSVCs~v-a~0qQ5>2Xd)RIfDQR~sz4!MbzJJ-Uav5_oQ%;amrRZcuxya+MDkaR@Pn2ufHfe%_2&p!ZW0 z+aDxmXw6Wu2C6hlg3)^69ZTT)*U22p^%EtixwqI+y%ckt8oVbPF987|vfVetk$Uh81gQcsEbdksM{F%x!fd!8!Ine%H{W8!H zI?jlLN5VMZpm_U(cxDM*XW@ z@9~tNM-|Sqb85g+a|ILs0vQATwE~crVc9T)lcq^IUKL;RwFl;Ah7`oUrbUplR+ES}~1cvPNy;mp`61=7)^ z#?m8Cu+5y!SQXzE26Q&5`0a60g;-$<&hiV)_Q0^4S9qeo2|Vy4!SPSyK%Wvd`5&F({O$1)Lt6iQg<2h*Mtz_q1LQu=KmGC!Wt*?1i%#(c+RL*&n4p)O^uKBim<^Fn79Eqef%`)bm z8|i(1^ByG=L4DU8f<-P2*_e|d!`OE>kljDo^0EhR;;@-SPIH(nU1p$iHklpqqM1q% zmV}#aX>O!M_Kl-yaELQ0B;!o7XXvP>1?cHlRJ_658c^XDL<%|%%;{|JBj>|(vqt4zM&S<)LfUc0&YB}QHSS;*c`PhN>@W7dBA-RRp zbrgGcs9>13qlu;Dq1wl~7f%Nk(bu&jMJiq`b+iJu&}cm^|lGPGZsDjc;6y z(cNS{G4XX8#h(?9cWvtQ(mt2%CaiX~dKGrjZ*17%BYUFy({Jtf3LanO%&DLP6kWQG zK%r5nmg$K%$&l>0fsS}IH)~L|KNEm#=eZ;`7xC$h(BywoSL&0Og6|+{C7vPRofd{_Jyh{r0vIF=U6^@GI&JmRu_Nz6o*0p1!$W z&&XxpTzHqzt|;z)LXhLPZ!P0jaE*4;V8*9}K{^!1w|&}F&0vsJB=YypcGV&4wm>)? zjr&eMmXjEgo^*9x9=(wf8y%@su>Z4r|H}a)IUbH4ij`CV`I!vfJL2{(V4qaIAwSM;n1Gw_L0E`L zxC@+hp!hHP+S$^)I!6)7$Lbe$c4u(Fh2VWvJ>B1dNt#k6wV5P(abJxLN(B|<-I*E8)W}a`y zU?H$qhYv{9gT;MjT)1L^@3{K>^m0>6pRRkC|D{Y&vU7H&KT{J$ck@E(d&&U?YiHIS zocUK7@rr&H%bW6=8+qW64Qn;;*9X3RLc-og`X;5dX>4^I_)6UfV+UF7quKUaken8JSVA8B%FIBet`Sk2p5HG% z2L;A2whqQg549mvYw$zoh`rO0iO!!WS(4nGu=a}x!I8-rOIOGitX@3I+2wzjci)a` zW{RAE@E*BWFe}0k3YLibZcOYQZlB#1IjFG+p1l4?6SU74jCwC_UOVuPCAL=HAJzI^TWX^0f%hyTiSJ4Iz z=illxyF&V|O`>^H)KYM+*)tpYQ`TFm4;JWm7o@$!UB3SI)U5ZXnQqtF=KfuBEn(@C zO*rLcEa$b&q`BW*&1m&rhL}3hidbS-JjuVW>=;JZE4u?Dvvxq#K2Oaq}Ya#T|l98`PKo4N4q)kzI0O5Ig+5eVmi;DkQH z%ltouneoF9Q3D_ONfG`L#P&7Ezgb2@$5&RByijN@czdU%OQA;p;HUDNye$Dgs0H{Z z_y!uK$2W%A+K>tVjxlbynj%k3KYB3PYIh1ZUq&JxS{X<1A+p2C@* zv!ERul$(Cz=?e(yc`=Mj@J|pT{2Brj%a0lDt=i|8HIf%I=RoJkx=!O_r_j(egQo5%q< z?%vTqt*^`SnJc)Ve6i4Kx5XAH`Cy4p23E(+4h^pUq543=xN^hLsDiJ^1jR>Ln91^x zYQ*GrUR=!e#dH2i+a|aQh5I+%1qP=f7CVUwleztnbUf;iEesNO^bu<& zrxQe2X^2G$;A!6DCr{!cDRv+@74s_fwz&Nc%Gj^_E@hftB!wjOSpVzz{PbB#%>cf^ zw~@=p-y0Yjboyb4CnrDhgeYuSG%r*#FLb;D(zC_1d(@QiuO)xd10^)3h}hDT!{fAJ zY2G_;`LAl4o!$W5?79Y0cH+p2BFv9!k6})s7KkHgW?#x&%6c@INJe0f6=GMgyw~iC z<^^~r!137VPcR&)eSm@G$}dNT5b>2MS8Db|eUbYcWxal6l=4Jds7z>}`MS+$@+JOJ zd&6%xP94N<8v>OK{XX*CJ2PaS2WK0|B%(vYNCIKJoe_vCCXrP=XM+${V^h3DJEQJ2lL3c4rZ&#)=|S=Ja|k3- z;AZ--M}!mb>7lcpq*mKMcQN=zoL(Dt0WHxYC6NF)t*HrIGH=!#j8I25=V@9pq019U z*KW{mq(YSL%_GCfxPK*M3Uh70b`BA`h(3BAK;So+Q5bXu^WjQ`wf6`5HfQUMos(Rt zM3vP|6}y9A5&($SW)lN#51p9vs#bv*LrIMIpYZnx|0vaaxV)MwAnx(jqt8S;s;ufXsH}%MsV$Ur~&Qqx#a23G>~z zg)H>b9=dxoc2wp;ba}$vbI8e zwfq{U(}g1e(9U<|as@s;Q*74-bFiV=yE-Ve=52!Ku{%N1qTdkCRr=rQ6-5X|V<|u} zHlnh=0=+5{xqw2P)IB4|(^1%rsPnpY2XQ5wLuvFz#8(w?iTuJz55pxmZ8ks%Xa$tkt!^1U$Xvi|6o20_KE$A09aYdEI7JR;s}@PJ?tJ0;WI09)vfUBEc5X+fZ#%T~OJsZbA}mG}TP zX1i*fAW9C2R&UOM;WccsEv-NAe)laV#RDWtQ(G|FG#a9;I1#3p%tTzOgA(`T`9%#u z0p22Iip9%73rQMA4(oX@uJiI2n%^|!bo^hkZ1M1CUGu2G?-Y5 zz7qytER6AViD5`)(Vn z+2Acr;Dov5T>E=3E8G-+!@7+%SO+}6rw&G^Z_d0W=F~aK;2Cky_Qh`32tS>c(y3vP z)uIZ(ePiPqaTvcc$T;lOac~1x;C)9%3T#UlnCvgv2W@pi#peA6k8tx5q>=6(3Lp2b zA3_omJ#R)@G=CgmwhJ)Q((tyr5p^a_>Y266y`4C%&V~1Te`har*@CK&qB-ne_4ovHXfne*X#>uAOHRf|&9(~}+4zO}%wgJfRrZr<%)+r9A7Xsi<> zv1Bl581zTCD>!P6oZlf+CKt&Y>olz#XqHD=m#8aEURZoKjg3#VlWXXorS<3i=xvEi z`uo$-7nEh=P7}AfbZ6kK|BADcSpN_K>uVE9bkl}qmJ8}M`lf>rfM8}xuMyveW(?hPm5(DdI<-;>1m7hby#B~ z>z4F>gQE?!fVZklU1ydrAJmWE`d50NjI~6au?tbwqgA;}%tNHrbUKAB#(z07aG%uX9oy1G})Ami3kXm&$AvyEcfZ)Tey!&{w{&hot>%}>_op*|gTI%@_pESRfTTFQI zu-}U2Z8vjvbXMcw?EFXp6Y`i0w<`T+1-;7V2|ka#=Lbi78bU(eftpQJMFRW3Sa;E? zmLFrFLHz$?5d4pP4>$P1xI~(y0m1)&xmh^&C84Xk(UXq&gTeElO0^oyvbQUozA4z* zpZwx6|LY8+R;>l{M z7Nt-4r>!mRH6Sjb*t@&+(3L%^*k(pWW+?>O!26hec;9F21_UsY>JN+tnmaPMyXn8W zePMrFX#Eh4orNEPL;C`=vqRv(rfYklsKpvRruX1Bzfog5Mm1_pH~C4s4*2G3U5P-l zJ{9Y)urAm?oY!9^KITp7Ix(2(>4dhSbb{bkKOaP*0qEPyOTr0-nXz-I_Cp7mC&NLm zSc}!XfXk+tHLpCEhS<6T-M0$40RXDbu&bb_KF`TR3{==vz^#sbe`XuI5#*~-GSBBl zY&ZB31*ku;28V*+HTSMV!>sVNCfwmV;7^l=Q_em4pq~=VS;?GY!NH9PBk)didTGhH zF>V6RZqC(+iUuN~!Syoyj@=?y5BZVnZpq$WahXSroNLmJTYyS~zQc3+(8M!>QQyC8 z3r%_?yO^QdS$$7FJO1H3=*OMng@VI?&|umc;bKE#LTpTRBh_VX$}2yo&X5sW*&9+=a%>V~#@$c9(K&W7?a!<)*{orw3avrU5PIhkT}_Pz<;yZ7>z``DMz zac#=EgQR&)9%DRlKnK)F0@U~LG;MFFq;IL{DzWvgih0ch1+Nr*Dv|5aHasL_yQj{C zg9qkzO;$8xc5i1aoIL;?NHAcFg!SI+a1CPKJy{~+qtGD^>z=C};0X_Z9x9mL z#?snm%}*YM6P>P8L#(#C^D4}tKXfNFdHci-*?6&xx(|US!PJAqDam!pzaapPzvdf# zj_#fNEFoZ^S<3(tA>cjEx-$`rKWj9nRhsSf(5$PMGGCjC*u;!pCLeussxYuy&>iX1 z5Nd2PEngeS=^r0o-k+r(JVaYpSBJ;tM4IE#7+2*=6nC%m25(gPhcvpg`4!i#23gp= zE5z#+IrSUo3q~L45?L3CdIz3c(Mp;5|bSKtr zHm+-Pfkl_Ql8)w$N4Z9Qez*ZmN1jKR1U99vjF1nl?Ve_~eP6lwxDMaBAEkS{keTEo z)3QoQ$@XyU<_(ndE2GawdpM&Rhwc(ER{vNU-Dx+rDt@+Fgtyd==X2S(u;#&>ze_X~ zu25`4Y)>ln%I~glIeOMNwNTO2=y+=?y2;SSQngdhuCLe2KTG^jJO7hxX8R}-ks;;eHm%w)`s}W9(UlxxJcsd(iRV%6FPy)ent1Q9};DPk;bDJ5%$G zQEbW(hcPbng#*#ubAS4gKpM@a^$e^tjxxK+*B~BGtXe*{aHT{*W_DH?OT+Vmrb0?5tlzo}I0lXlEMA zOwV*(q2XJb{TTpQIZM39Ixht9$-aXsH~%@apjKVUPiUD%4>Z*tSmjmdd>gN#e}=Pp>R2_hQlkU6ggY_5uP2% zk01feF3d~W9#C0TD6!Rq{q;)tOb)xYNdACJcbA$)g8h_{{XcR{Xbs+B^SC8>&$wN+ zHzAoL>e(qc_f%^L*@0P@@D9(86F@~WEDG_;b zWf+6wizF5NJep`HGBRBOUy$bqq9J_7lkY7Y# zU#pm$Dh|i;5H?A%@eU-|At3s#@SLot#vf)Dkqn`#V^lo9;bo%)BIyJ=gDsYMukf&W zZaxow`x`=T(H$d5P}nB{jFEi#-WboTB!`=Vw++AK;H#&s0)WgVS|P*>=2x(Er`4v1 z)@c|^*NxC}?r0rms)~3dthj`OA|gD zE8zAZ{SG5UidjC$wPm2C-`w;DDsf(HwEg>hK(gM4gwbGM|%fKx)}JL3p;Rn7~@e|B5o{ zX-*S4fy;8P1gnV79H*1OoMR>{Zro>@)rKZVo*okF?(sMWens#*SQ~mlHDfUH2y->{ zcsXyEI`s*v5G9v+htG~-f+Xrj7zW_QLU44$oBeU%?N=+|!Q{n#QX0450%t!#EHlM) z%*w%Rc*;X-Y%jKLWnb^NJoa#7J3$pn(A~~@pp&`W|L1JyO4orpoqY9Q=yJ zNzI{B6VdkXK{xuP%C}AIzuDg`Qg^rc?9*LZSo=K&RM9NffESwOYwp39&GFYm+-z0v zu2EBW(*!Erg{6@4+L+8v)|B^sM5l2DAI-J32=rmIMW=r)ZHXXC`?}QSHoii2X6oSE|+ZmspySR@%c!LcXzf5Jop*^I+z{PZ^Q$;2J3nBbW+u zS0RYR1qUCmAJ=fs7nRkHz+udXLMn2oQeU39@fc{KW!gGcwzb)b=*(R&@+Q23sjAF+ z3blzD>Y7#{Fi2zugT;iDHZ#1*$r$boS^4BIBv72a-iTa# zf!8s;g(eqF{45>QMJS@u*A~n~tQ7D;N2}vJkXrcB$!0tGm_&+L7Qf1k8l(>1>!$)9 z8!%#vPk1bc#$c*(MlV^-S)_1M>JTeTeCz}>O$fpLlfmP6_~S8>|q%S3t-g_z&764Qys08aIZaRz0>|YwplV76HM6 zJ36NOPOk5I_MCyJ76^l>?hXf^eU=H`S|sPK$PjvGiF+z1nfC+^sfs}Hv9Ul4YleK) zTG-NcF zMqqyj79a^?qay(pZNhas(>-uJ2DO0>IeCJ2xpV?%tODW6TJ21b>hN(a=~M8 zyAM}+dK%*|v~EzVsTV9Ypv<40gq8^+U0_tDj>Tw7HN4yr9h~^po2>CMk^0RiZ;B{^ z3r@7%7rx&@44OL%mDr7!PqG+y;H~+6?xf2CGE5`G$Z-U0A{F>g+BA1EIcn4(2(B04 z%$9N`3rdFGFB5$1h*E}xr-svvV!+Et$nxEu^V&G&LP{aX|0 zYero8O8M3CTfDCH1d9pNxUr`!3#c^w*oDK7%t6-fxv z8NKrI>S5Dh83UXJ>a-~dx6)vSN)W^@UJ!R&6VV82Jk#tBCoQ|-@3|?G)2?x#07As4 zmcy{U66PApR=V2AZga0gM_68~Oj+F?=;O`6Uxv(>#TCP1#sdb?E|ljWA{xYVVBAsn zt+WIJ2veaz0s1z0p$#&A#lg6b zch{mK^%plLXyRa?_^xg^yDQZgM|Mux7&#Im+xbk!hl?$^{TvdQeZRj-TRbz@)m?$^ zvxXGRNC`s&%|*BfnQjLc{xhN#o)*to1&Vki$8uI*(l8P7QO>tJE<65#vP7|+gF za^lWmvYej*1F5|~h@|Te8=R~s8{R9i7H)3u-K$ZvW-dYtNkhX-BL#ULNwe;wU7(?; zX6HeW^fx4Rw#76k1!25NJq>B@L@CH!(uZcIX;O0p3bGN1CG)0=_Mxv`p zvsGn&Y8^%IZX_u@ci@p{ztq7N1pc4`5>t-`1Y|xmjsf^ZGauUtBqdh7u zP*zqfa?drl@MOp84AQu_*rA{3=GBro9iCD4b(yiW*vB4C8CW)B--7*4>)Gzj8skG< zZ?H#}P$`lE%@d9LY+j5z_Am z>hean2WqXH&*H0t%MzR!IbNn_JT|xAt^LBssXw%=l(Ib6M9y~gBoZbco0mq4#RpTW z{={a(z`u%fiF&!!%By4F*$aMG#QUy=9P3Pa6Hl%GCOXZ-%T9lZ?l94FdY)^Jp&S}= zawFsU^NtL+m5lj5)uzTR^HbubE19OflJVC|0Y2xe?V*OI>#t7Q`98z2D1y+$vEejl zV|Xu4^ih54uO8B)2BZjTnPW5Ho*jB&55mazg<(uT?7NSK_I^g2%yo3VZ*?_qrRf|Z zsD2qQz5sAp$#ypk^QnW)nJP!Wn=xGHuk8G6f&VNp_lV%IxGs9xAyYNfT%-7(?8OD! z71GygG-t2gA>|vInT#=?Ee*s_{nnzYQU6sKWAHtjA!A}b+aHJ^eW?t(l6Tr=!(#jp z2H(Q$KQ+5zy5{}oM{Neg?GMoX0unA+=n>@?&MX4?lmEk>NrL@2#z~edXD@O1h6&yu4BzEy>6R3<2pTU+mZ%Usyr^eccv$f0 zG3vcrVczXuWxeOf@e0#ixgJ{RN8%UzEh>SD36^9TNhsgBK|~XaP$@BSXtKf6j*~I1 zEh*C-L3qY9z}E`$M%$^~dau+SbL!d@M@n@aI|m8ce8{(zM)Xm0_`X$ha~(d~fKs>9 zDZu>*|EV*N@x_2P`GH+`K7H6~m9n$uh%EoTg|Q0SUaS=@^1V(Ir|u1fquuj@@v{xo zTnG^TaTdU3vtjEx5-N4A4?OCZ)w2xP__Bc?%)HTjcu^riUSt?LkjdrF3M0o~YaU_at}g!;l8pHfc$(tr zjM%g8z25Q_@j|2`Z3N2u@p?!|cuAB){hEv*G6+^cE+|D4q78$BQC2O4%1fOQxke%) z&MQWqJMVHoekT#4D)R?(**;WNBb=Hk+ErWHGVT(|pbhOWw<}a}6q~(mRQI z>hE%j!+MfQstw!whv6N#9=?X2DautPZZX~#Ha3}!sechR-ov4hsmCk+V5_F1>Of85 zg40z?qpuUCmrASL-;G}LVDsMT@bbUB0OVF4Yp@0CBvA(dZfRpipV1lmvSVT?gu;ox?ys@vI78zE_@HbzV+W-g zkor-`)D^t+8Y?uETD8xE#mz}J$^{rO^qbI>ru;^VU*7S(ZYMUdluSgUFKqYJ7@7p< zBA4{_M;Y$%>h@g;^Tbh>HhnRm>yezs9B=1Wrt*uux$@@Mp|8EV2H*X3(LD8D=zdwL zJLiECW8()`WC@)_P31d98t$Ff>WB?fj&{{c$>PrmtGd>qXjdgW+}e{AUyRY##y~4& z;tj=h%RaDg;4=tavS1@U$-yX>2#8gJGO0`9ox3eCw8Y}+qP}nw)4cc zotyuA_P%xAea_wY)~!0@%lfcZ&6>4pRjr@q7>JVYD?cF<>~LFUjGzdFPO5UNDu|gu z;`-&|e)W2)cq-^p@l&1n091^CSX4sE0}(Vmxw?H&ZuVMM+5xs1ItG*CalaDB44k1i zHh?-<W|r))Ajo|rb5=A%m^RfdfU8euQ?yF zK(ztv*F%qQxTz8OHN8DK*c?p&dQPksl*Vv8)yp1YbBe>gKdek3<=XObACJ2vT$#t4 zP^Tzm;%hB2JSp!-1_=Y55jpu#D3RxFbG?*tpm>hiURUyrPBKp_j$|y9;l^?9cC40U z`gm;p(Vbx~T#tq-uCzy1_V!|=gp>_R3>~*3?}(hDcyC%T_z8LK5kKr7iJ5ir_ks$n zLXe6h9@tk?#l`1$j?gF8Wd@S!MpykpGaHL%wykx?P3PHiG(}YpCZqPJ3(tu=U%t~O zB3Z4vgN7O9yG1>)t4}f%sakHXXNHeXrj2fLXz@0Nmb^#BHD90uou0S|Q-)60*)J(ZEBFW;29*{1KE&@OhfqJv=ikr@2$KQJ=Sf_uQU74`f|%gD_&gbpD1xTziS3kqJy2p~F9=OWmR>>in2XW) zNBNyHTT-^CwGr0_*(DF(c+HjbPFp;{e(vdh3@ZQpBbJTq-XQ-}Tm_Dz$|^vK9EGHc zF0o8V=n$WnO2fN8#0w%4K=42r=B|;nWdaNPS2h~iBStVsbGhM>4BVqECeFF`{U~EB z+k!G;L&D0F(41l@1@X>>tk@iVJZ9O|rhu&GJ!16nC++rfCt5{0$zDH=|JhVvI;g*u zIKK+VY^SQ_=)k+kRk@*2u_e8YCUVlG_Rj3fQgQj{!Tj6v*{JVcGzT?%7IdR^iW^~M z^Ku$?hQcSvceau%jK*-q?QQ=ouHO1a;XCwL9zX9e0$zfI`Z3_Fj%F=jkrm$Lv&F8r zPqsF@%Ygx>IOd>(bGsDL=nP8V14?buT2C+^V`Mp9Cu4nh{Ru3~pOnB2USC*<`H)tm z?z8X3YTg$mK_sI@GQifnMrKe{`8rtJbT}YN%T<4K)fC@;SxVlON^L- z)pzd+r(L~6uTNiuVvig#flV+egOHm$y>5*RF+ax6qYw$U9x*XYA6tsv-eyRqQj>oV zSO3vh3J`U^YnRA>zTqp=aG}}_up*W}{?6DerUU>wqKgN%F+O zK9;zrf^-U$EH#NLYAS!L)4i}Hc$#impl#lVlolX1J{IP7Pp#2=9n?qEGJRCFn{JB( zRlDKOFg{JkmIl=^%n@{~%&Z}~o*ZS1qp_X`7qKJ^))Fq^`<8`3rKVhq<8F&{W3o)3 zJ-gtaXKJR@6+&@DXX`G~-dHp$Hp&s9{tzakpVNz%!da!@`~jQUG%I2c2-yti)7(-V zXun$)L66mSBE;4vSEq@4Ki}f&NFh&IL65few|jjB*JFDC_onWx#nHq>PGopJ{`h1s zh)dAPI_6$jNJ=_j08B_o^+&R3+iF|eA12I780?9i-fle|3yZAi(Bo0hzI)^plv$IG zu=Ds*qAN(KJ(D zVgoN_$rr|NYxFtCW)#(lt1t!J@<9B0{`9D2>Yp+)3~3QPgdl5dix=*hkdGuf*FFBy zZ~>p|(iR%d0X{hnV}7u=XA0J>$|)`1g-0M^+}QuL-~W!m=>>DXVW`evPw1nCP@wsX zT7g%T6aM7yZ7GSCdRsb23s$p%{O*d!FY9uUmDnahVa0$8!&mv!$)3(a#QRZku_mtS zb?i|qc!`+(_3y`x(EtiG#fS;FN?9=PWR!_Q-LAgdUU4Z(4|iT54jN>o+KoGW!xB?F z^WMcRu|9aUxd_$p?rx1D{W0Yeg)qeVLBhcuAUyzRR0M>BEV%bwvQx zn2&*FA(3?}OrEN!ZvV=_8**aU-b@7VL>OXzVkmPbD0Y zssvype8nn|&qtRG zN}WOBt9w)I<1$BEvKkPS>`~&!ytPD=Im8GGqB~H+6@^1Wej8H_o{l9s(o5p?rNRv8 z_5+&k1GHjzIb6Q)G@B?W9lP3sGgRN#jS7x(%D0_u7Oz9h`+7yMiVs)!3p;~A%C$EW(_hRygG>1-Z zb;Sv$pj(L($M6UN1nfdkr_kw}b-S}R&_qkna+q_loVJslvt`%9w12S(hbfZB4D!f~ z^~G9)OCp(WJg=yVX!4?RdCiUS>5tmET)-g3hmWYL-n7!i1^zoypPCT=pGTyCj>@khSdUWF0TWLL~ui><{_^2X^Xvf=! z5ftnmu5oNs^&L&vw1w-reAp%NSe;z&IZ)3tQ=J3Nqe$s@ge;=`{lv9TMeBusSpl4U z%;QpQYmdBZMvtu-Y7;@?Y=rE7V4&7PR-H`kc2VQBMW&(R60m@QY=K+0sFQI1DYZZ0 zU?w}Q-fx>h` zlZedZCH{gk`jdb|c&xv58g6|Jh<4wm;fV zQqI_N0JTROEvAx^=dmbi#3jEI8Z3sry^%(n0kbhTr{ZbGva`Vpp6;w*o$uqTOTP9H zuhhD6zI_`1`ocFvIGiqn(^8k8?H^3LFV0&jlT){oZ`MMIBj(}(ML6QY?Gu@ySzcfP zKDY%L6x=1qs%&_^hpf;s4zDwYiz$Dl30|DXf2Ij02^2oaA|3Q)&29$>YtfmZR{;t? z-*7zV>tSTpLxsGtW>VoFP_1sJ!KIDnru-;A=WWYBFXvgn0>~mncMh1y7*e4Rr_ij{ z*nUJm&@se>jd z#s{QgB1J`WU2kXI<>y&5q?z5G)uQft6SIj}kD4f{k6lOI0!u8Rpky&+JM;y9f7N!@ zVQ0#~WCqum032ArC$a>WqU&n2th#?-V4^QN6x1h8ymR4Iy?s`#T;m-D`I`f|rYTCM zlFiaYuiPRv@;? zQS2qNazz;mlu(S4P}CF_OKSVKN7&<1)unuJCK%IZ25SMEx8`hbs(@rMq6< zl7VC8WSM1Lu;wGDQwB~kryYYlywm8il>0bXWM%|K0-Kh@)8k#aS(Ic)ja73Ad#s|I z963$^B{YXK9l@(jG$xkF@P#GGt;Gwv8{_#t7#?oH{TlIVnV z2`N&82@5zQ-lN-z^kgkOQCXi!BV?OQt+7SuPt-#^S&kd0PU|w! z*!;gf7IZpm@+SJt$jBGDa!m1~kHRKiK4ByXoiNzy_nqiw#uITASs`9sRXe4^y~f>u z-lT^f2B1}f@jXS=UrU zzp6SG_X-e?Db99;Sb-DYOV{J zViWZd)9-mdRVx=Txk8VPmf%l}VfN@{Rl|TZe(0@#6HoWNbAD_kfuP4R_7?%iOl<;s z2*AnAi8Z=(-m}J3cmrtrwmy2y!1*%cI86Nj9QAP@1Z1*Eh^3B(15Ljh<(f9}(}^Ep z`t$^l8M;3r=j3^2pw?R~gsN0%%vR^pP&QDpXx5LmE!ILR!-ys`6|*#WrB}}$R|6_1 zmuXUn-`8RNuGJGijeQ&JK5j+Oxpnt1T_szDf}{#{=SrJ3u7i}Q!ZDDygqf>ykDI< z7vyU*LExQs`&tWLU9ZD$)37i>Mg;ki0;sA~wv(i3j+d=NgKs6+`_{z4ZDcvm-wF7r zjV06ht?P$o7NsO|QALN$4xA}F)0G!-mG0_=Bj9XbA(o5uT11vxE3iY+j;yrpXNrD# z+{hM6KOqtGJkajrZ!^B4$!1sOU~b=X8fd>Xk#kdXY+Gr!piNYRm7l1BXuCr#Hr(6g zHDiHMtPRfPTuM-erwvx=s^D8cCbe^vpb=@y)&j)bjH&VkO32;3YP3iQIUxwtXKwPO z&$#U3p=(st=j({Ig9E{P$izZBhI6KA&Q@QM{5lqf#kp3z?f?8% zAYa;$=9ys*#H_fRX5*GWMD{&?Bz?WMFqK6_zz(XwgQ?&oh(IGTei^nou#9DY$D7D? zk%4iZ1-*$TRPvD4-u&HCNyXT~;=|K#z^fr4gvmHET_F zH#LKa%FS3TywV=iImQkeF2@qlnvkg#!P`r8sDQdtsYIVQ>Q)Po znBN-iJq;?e7}IFP8TmtU{Pk`l1yrDGYXs^B%gqnJjv-W$02_Zt3$Ugd>INJc!}~D4 zkDI2g!srgS}e`E$t6Mk1x%q&TX3{s*Y$BU%bUF`PmoDNPhqeo6RV8T@DM?N0<_W5 z9YeiKl<`1+$eX<|ck5GsEKvr8Q;*vMVr9AZWcoU+R!#Bs8BZ|l;vcJ#HHLx`fybKn zVy8nrUBwXhed7Ez9Dog+aU(>*Fu=ywQBzFad#@(L>4lrds%Gz=I94V|X4E4ICR-sV zmiX6-(IL6uT&q2tX2CI$<*$@AW$;o&*-=In73}cmcW!zLzf`7oB^T_ttKO^Hph^Vri zQKNBq)I<*!mB{Ex!?H!{{*hQHRyn`S>Z(EllbS?94$a@=ah{|lU3}~w=|BW=N}IJ_ z{S654-)utE5sp+)Y6f5IlY}ia%$n`=rdoBu?dc8s>|Vq$WD7$27xj%|JQGl2;b7RI8wvGdzox zf`GWGgk$OfvSqAKmL$|M4o64l5m9YSzZE0-GOJ5g@!cP8L7yOcH)DlTU$Z%sV{3A6au*Gp1X}r~zkshmrTxpU$|N zw&bkj0ket*#srMr1u86O4wVic^B6WiW&gfN8LUL*UC|s+ynIR@JYMSLO0#GxOkJ3q zHum+Ow3j@0MKa%b$1}mef)EYOuIPYKs=9Y^{ODSXN;)&=a)3xPjmXuOh7T_^LYyN> zFa5E78DG_W=iA=o%>|6QkUGENoZU4L8^pcDls;M}{kAq#?JBRuVfh`Xf7~u#Uw-zp zgn@XEQ?G2oT`CAMqW;YYD))3Kznan;Ct>e&i@=Up|BIsukH;i~Vi`Htb|}1>!;yjaEo~I@c3bxNs+X`5 zjX7MEW6tLTj@Lp1oXkd;uq(oB0>T4YCvoXN@Nrf68wxTqZnWr*6YGCs<2Kg+9vegJ z02W(FcrsaFzp$teO%%U+aS6lq&lV)S4{=$}Rn)o=)+t&`h6_=E;e4T$NJ0bl7kK^D zK<@MZ`&9xp>uhz)Ee@vY!#UYkSpw#!P_kH$6k*fT(~Q3ae{XjM%uXU_vR){njwbSc ztq*5>THYHsdfuvzKAIZvKUN?g{TH~-QA0M)$^I%i>Zmb?di$)^k6vz=3F`_ z+!y4gfU>rvS`!!ck&iyt%M}S(u`}y05RQ?=QSHD-MpV!2eZ11Pu|Y-tCiyICihAGx zWU7DtCenG2G*k=4xk2r4{;2n5{?w03t^QWQK*sDP~IfyAc2b`6gOr%OdyYPQvY2bx@2yhDZ2 zx#Q_c&jZcCJM`gV7gT#fuFoB!Qo%0~$Y9U#L9kcmIX4N} zH2OPS&G|m3y{YhjhRp*U%k&Ng055EwcXLjCP?Afwu;wYmkq4(Caj}-(#(M}i!i+GJ zx5;m&rf(+?+i6Unh-Ad@nnC^250mIZf_R$Y`&uQN_TQ-pLxWKjq}*cwe|0yHg+cK= z!rigfOPjU}+(f@*yua~vWlv}OHq4bLzJ6D79;LZ`qmMd}a?bu2H0(-i1I$tSG5SFO!9$V$1HlRaSYo4Y#zL*mqY~${x0^ar{{CbQ?gG&yIA^X?S$}L? z`S!w#XH|0;0FTBvK;vf!U37I_HkDiA8ELz^-UxR1tJ990XOmZA|4`&+b^KcC{$}7a z(t^sy=J3-U*1)#}he-GBPY&MewqnV5i*C;Yh(RCk_&-T}yGa_(#AnZe@I@W`d}|ii zDO!(}k;3b%gPPhRH4J@F3mZ9mD4C=}yg4~J1~+>nZl5u6{WqNt0s@H6UOp%z&*lo= zc=%@|{-f5(KSzEEs3G=mau5k)`Jm~~V+0rukQ!`}rS&T->bTEsKMKRbeottyb8bLz z*XGz($?HJxIEoiT6Zol*AYYdXjq}qwZ)(8J;UkQ3DvXcx0Rh$v$6vk-Wf8YQXo`c( zUpzmAga`KfeD00E7vW>$)xfYk@xLswjBg(w=AZ*ZWJvw7I;;W$B`$X&eNW`8GP-#f}Zmw6r6`FSeRrY?3aL1I%*iujIOjz1G1t=T&Yt2 zx0>Wnn2mv*J;IY|id(O!*0Kyp0wJ4Z*S1m>*Y;+W{5@U9tXC=K`L$4;uJv}^!PxvH zhc6%4iO;;|NPj-GJ4!Ze4bd1Unf#==0Wk|tl8${U@pLC?b6+??zic4lwT{4_J}g~p z4Ck0GBxS-;0BBPg+@ffvn6Eb7r}-46Mbao;LI$${L<*9arA5KzdR& z@^5_x)=C^?^nWo7i1j&P+OS zAE@yzsEksrLyWwE2Y2s|U=Vagd5sQ2Nr=LQlMT@WX9{0m*O!mR_xApN{S`Nj>&aZq zNY#rriiHnCumPW|>A)WUrAGLGgi1!F0Ox?e4wE|ScqF&){k$hRj`P5+Th22g-9gu2 z`K%x3o3;=WB5(QE8)Ll|i4yyTe%dkCbW zsvWU>SOjfaGZR)agAqA!Pn0gcie^7R<^9QV!9KQ}O0I`B;!4^9eN!KOOybv9@BcB6 zJB+qyo+N-hWU-`^OR#i8b(=#i$+6wt^$-x4RhqC~9ZOuEPfTc&Tzy?0oW-;b0~~B$ z%G&R+#&83WU>naBzNfdOhK_E6X(DoT?PG0vd7u)Q&eAUzi+6d)C*Z+u-5cBg{83`+ zQf&{!sQm6kqxqj%0AlrOXQ#nrw*aEDBBpv{w8AXv-}z24-K9xfjE<8gy-3t*YbC*L zt^s%7ix3{?a}dYl_bp$S(JBh;+fbJebQM)?1hKlPdmpgdY&S}Tj!9@%xk~*!nG#(k zIVhDMhRa+CQ=FK(cGFo?7N%5(RyHBCx^dgv7n|L5Ho>At7L4wF#x^AVvB}tdp#&GmM#=~U zF?nLl^O#+}O#c{_IwMrd%*{M=A>bO2V*~VXq{-|}hGa`L_<*SDSANo#p$B_*$?S&< z{vr)9$Ho7x3%kjKw)AhdrtdR#@a%d~$tS^Uofei6!MbDXz$)ar8vbZRr;s$pVycer-syL2 zZl{1|uS%-Ob|JgTvk_Ow=9nh$3NDy9%hiCAar-WM0G zxt2q!hm}Yk?x+>z#L;IMSEo)phO$g$2`I7YQb&n?>m^Xk_k2dc1T12$wZUdiccD>U z-cLgRd4o~4&KD}(cW<1qJ(;Wlw2-I>GVfC?C2G4pfGrNCcU2|{Z6N#hpP;nu$Xfe> zK4n4*(u+>^!vbm9MT$=MdxzdMshz&fNH?c`NB^%FfnC{L7@J9zp5_rIudPR%8z|T9 zMWqx!smT)Dq&ej*;K>Lz@Jw$^wrZ>K)Vh`X6LjWLFkOs99G)QCl#2z`)FdLAIIj$V3iPN?O2F=1>iBl780}qyTWpyvPwT0k&r0J z+k;0-z^b~If6*{fh{6Fn(QC#JW$aj0@i9_oii+4U`39%xY4N_%DEtR|S;pG;@( zqIbS#^R%obT^%-XX;e0kM2cq|@lJ)hsD6$|Lp^JUdWJmjn#3>xNb^9UEU>=-e83tv z@qc5pE}5H^PJ3B(wLPX z)31>u!v5GV5wbz$+fjX|9L!~4vp_o9g`Fe6O^81-vN{MOYSe%71&iYFKhyST_(6;9 zEN;H%t;Tolauruwo++NW$g8?VEWCCkGg-6py)}kaI1JUCKT3Ndq4tjL+xoz#aD{31 z(;1#subfG__{w+EkrBP_gt;OVgiWPxGU1rWL%g* zVB&Bl;FC~*rMw~Jts>#chV1#LC4bA1fj%C|Pqyt6=^b3d@}NcVQ3S_B33g?Tx)cK% z(|R^v!w)XJdJI~Hy!q)m19$E+O_rsnl12=AiL>^6B7;(#oQbMjDV$4+LI}GOwTBR3 zuH+3vUSgt9g$!qu42Q2!(Ltp7c`QyzqLIXE&ij)`dVWAa+|H~Kpo_Taj-+4AoSGYT=&oW~ z4p~qz-x8oV|KF7oMf;2izfDyErOf$?(@4zU5`U#C&^8ZuxAgw{eMYebM(ym=dJo|G zBU9AmyE`3r;JMf_+`#Kx$rF(U)^ai<|#Cqhx+E_WT6}| z3<@FMoRO7oH!1YT#hB00?5a%J55OV1;&a!iFG+fF#(~(-sq~-%YC`MD6@2L3xc?sA zh647dwb$;ei~ZO!(h!ID8~accp8o#n!pt+Qq4=^VI3+Z?ERL`b!~de}DC;I1@^4}t zN9hpP-TEE4ayD;WScN4BD!G<6Ne}1p&SBcQb1KV~EI5@#SLS3fvl>f&#w4_d zp{W%Nyw6o;vs+H8%8_j6g4`^c*hY}4dH_EO<1%L}cqU4_oipArWs>{6lhW@C)dZ4uK#Dg?6#$&H3_9M<<8c#;d-YNC*WrScidt*@& zK93$Ja~Qlz$q_><9h|D{p3CA82Li4j>L1QOJ#PM3Caao}8NiId7BYM>w{msRv52*75tnuQwYY!OEQT z0UYZTrf^#-F1qvxaiJu;GGs6Cm>{wd!|%y%*SXW9gUN}zf}VsVw&XE4)K98)TFztr zdne{C`XII{WPGs_-QtZ8WZSyLW{|`vol=pJYY6D%p_l2Mt>D>fKCFr_Vvp% z1Z@rMIKIdOmsSSvm<|_6RQ!dprBH_Xz_G;Pa{|NWPxjl3wB~F`{;8B7=?PBqFe2C3 zSX3;6fM5wk(^{nl>f-k$6~zfM-PYsRm z5>Gz&jdkO4o1GVVDb|q}O&2@#8HQwy6_O>Q)5Djjf#VvQnd` z`Q*8y*@=o5Uj6I0<8mu=I9QKqvmj>?APDFns?A`J) z>`6t={aZ|659d#5lMx~KvVt3@+q+KBlSYIW2vkh1D+@PvV7bka9hY_AES;#tqO zhZNO%T8Q;DK!t)AA2fzLTb~;FNfV*# z{$PW4^^f3*0hOh0gkxl3DSO7PL2s-(Xo!5QGoC-BwRN-bYl+=qob&VDuCRF&V zHN5_5(G2T0*Naoe@aFg%^Knl5+BulFgyqz{t}4@~vCBjIS_T@bRwvn@813O9Z1zwR z+VuKDS~3Sq1se}vo7nV_(e2OySVLlLD1G4|)0k>f<&! zNZBZ`e9&kn1pdcYuepKXzMXYy$>d?V3?W2u-~GXV_`Kikq_i*ybjtFo6We4`yR%l& zylGkc>*OJT4@u+>g!LG_cz6l-K{*Rl)uCR1Wc2%^AP^o_x$K3Or4@z#mZpM&@RNym zsG!t<{ggP9u0|%(s?Vp$)BteBKwcYg`{jL;;r85Ib=GtO<=mRUit$eJN! zRz+=El>J3h>H!Inr()*F+S}o-$#BvpNGOg9pN#gkoezFTTTFb)=H;IqB2?j#ow>iK zAPr`&k>$0-KZOU)Yn~z=a$Cz1rZLln_AycX8~nScwW(!jDP>8#dVJDeSJnR?HLlOL z1Y)m68HPp`HS+Q#{xgq``_o4 z)lRX(=qA%{6^X-G1lI#9UtV{rj~}#a#WwF7e^LWQ#@U;NV5(Vzal7&l-bav0H%oJo zc&luX)SKc=143syIpwb>cmzslWpV$dfDPD?BgK5>4R+0b*mHoVr^6U&^9)zvMIH7H zc(g>qiZ~FWxp+<$`)1iSPrDAl2YyA|M?XHEUR4L$y6H9l&eyG}b-fbLG*PRyv1f~S z^j8MNCDS{BiGB&uN3|hbb-oyafj1Mv(D=;__Au71ol~*z3}zjaJMj)78GMmBia?HZ zI_FNSgW-9Q@Fx@K!`ZW|DorFhZ9;n!ORLN9QlkG)niG6_(tI7)a`PpOYV6leI1R_! zdt(;nFji}!=VTM{Ez-pF7RGQeI8B+hAyLcEmHM^Y1$^P`F2@wFqg0#mo>zGvge}e2 zE5RC{m*x}JrU&W5s(FSkZUZ%Q?*dWBxxIa@f$c(uNw0_N01HM6bbH6zm($c%3!W3T zBKrd)W>hKcHHYYQ32SrB>xXyedb6cpPJ6r$Lwgf6LhBYH@Xrw^b!beDjn^0a3IM2) z61~&Cr$<^$LF<#H_-5}HDS`v)0J0Q_WzMqPol^p>kdEHgBu{zJRd+>pWVE4#2DT?< zH8)1sJKY7xdwG5c|125U6Zzk(FT;bX{F%rSQU4*ntl?avDp$HTOArtA+h0luiO)%v z_G$(fS}>)y5?3FTxnXx0gL8yEU6CUq?CtI03xU_vP>9*=+qdNfRRbJ`RQm0SWWW)o zO0Nx<*G%B*vS@sKL~|rJy^1wInU0s}03^crfZ7q|_yNTs!SuRnl45b` z`akKhQmN6^kQP$Xu=v0o69sjM@6mQP%gLpfzW`7a=8Un5f|#oEe$nC*QUa5l63dWC z0!B5L8>#5TkO`JPi9;iLE#n2>{q)vDGD&&bwklEkY4G?5BxulIMl5JeP}s&<5gIJM zTYH^X+h6xL8Q@n15fc!J$x71U>j*>MD0FKo$;=KbKELDoXF2>2j3HcS-{zf@}(*Aj9bjU3?jVnG{6;iA9^6KP%LmXeT2N^6wwm(MZXVpXa zZ6uWQ9hhxdz!f+h8>z7ns>zykg?`2Knvu$QKGs6L(jJsWcD^0OqPW~*ig<4`7MZS# z>lXZE9FD`9mT@e@?D5W|V__lt^w(Z$Bld2~jY5dGLnmpeTT;=RFe$xvJUC!ae7i@k z&25X41OX`t_UMLo6taI18oRX3{<0=)3W9Txr0U*G?@ceLm|lMjT_-B11d+xyRZ@J697KFZQ@rM3;vyy z&k>yzj4l-Fu)qdm?=W>7y)GjE_49-Lu;cC%jV#oS*l{eu=eaSNXQlc48$R+=+^TBI}~35-sly#r%vbp z4FT13aqyTc*`IZ86vZ%KE~kA2f1|N}A&j|WcYWT@;~FoDQ;nTFQw{{P_(pS|$|8hU zlW{KJgk??H)`ZiqP(vZj4O5@){->@;7V!3T-45(Yab!L*yFa#Rl(KNti~ykwpX7aV zOI>C@W^z_@Iczw5Lta9VCHTBzW18JrnQSioah#@G(Avh8a7sqf4b9 zk}fD}YwNk50htPTn{kvm`LdmXXtohj<5_u-j!xYZ8<3{}nR!=ZCrH$7g{NC%AFT}T z5`{9{iL6gw2UNM0C;1s^uvKvGxlxdWVZGBMOjnhw-W4zWGOZK&C3&!xHFRJzf;E?; z{`%zsJi#KU&OyO6(-cJ{f}6*0wnW{|vh*#ATy zdbmT(r<;)kGgp9Ze*4(3LZW{zpD=3Ub_>oy5xT3-rS`r%)V|Ce+;f`l zYIwbL=c0I)DogZPT^nt8^3GCb3Y+(Zy~H|yYXzd3c5&7pt;SLY?odULUg0E8VhN9p zMn+=_3RHqNu9k~BR*MFR0t3!on38*FO1E;yNEsNADl}nJm1+sf*o_H_a_`G!3jpR? z93HV(X`GZSSmX03x-(1i-$zosb!m3{beyi~P2zsV{jF?gsa;P!&!<__Saxv&P>P^g;eLWrlLa)PYL7ccqKR5VgwGgsW|b3vnb-Wv;2t{J?Hi#pSm| z2m!0MU!1LVs*Cw=W1fIR3iTRs9&`)Jj`nyK#lOL+EXk8RK!dyqIn;{x@We$o3jg6( z(Rg~V>tyyTaZwE?yb-c88@cm_eQciQKi-`#NL0coY`_` z4%0VZS(UTK<3c~p|C2jXufazB2SsFrVvbbYPXMa%6pE~5OnT4mQ=y;|9zNs^l{0~* zUEQ50Bd`Y14i3>I{r%fJ$G<`e9WxjKeaNA^RYC`wnzjO8?{^Y&oj;~30YJ=avHN)Q zg6Q~0|0B9kE%qz+43%S*5YUMxIcJoZSn^BwDu2ucjnZ@IjL=j{-$zx^a0dZE#w(NZ z_2dT)IoI+(NonNJ{r>}D>u1v`_5YeIajgEY$&!&{^M9h*MQJ{cCi=XOJc4cTdV89D z_T~Ilo{UT0cp`uDKo`L2_5n3y!F9_#4#;_A-jh@M*HPn=REbiEgtB%g5+>K?Vcpmt zJUmpZy9&0{ST>1!R!u2)$q7hMzQyG?m#-BYQWtufN?#AWy%whxkfh4RPivg8d}Ii}Bf1AVRd ziJloXjAZJ)UVwlb@_si#fb~X-Etk2 z^J;Ks&gdJygV%%-=|pnRCs-B!%R@Eq>31r!`+}G(A{9;?)_u#l)6=*B2yV&}Y+-|) zt+s_}F~rzP+@2zv+WICMO7V!XFajfx{C5#PE|k^O&mjgTJ&vIwSVytgNbsyoqQiAM zsmvP^-FBV~>1V}&^ZG`{SBZJR?62n%K7m#7}@s+qJSh( zer)5)hJ&}ykA&E0|1Ig;+c5(<+ADcB(AfN<-8RVOZ>Lj==9O{L;qTAfnjKX8y7m|w?q2a9 zPso{=km9Y^BwW0ME(f)@uI6K0+}^hIHl(0%n>p)zCcHJOCl+{#0w<+OZXacc;B*?3 z{U%?NREpC^eQKlA-Sp&KushYq7LxH+6q(aMoCo?pn&Ui`2P+=&3C}~>uSAeLOmPau zE@k8l@owTC>j0~IGN?n&xf%2pw-%ULXU^c8DjVE!`yWq4B(Pk^dIaFnDdk0*w)S|8}v&78EE?{!dVv-UhKyZyN zeoy0sGd!|c!z+PFwQyH3wm||*s$Dez2*UKee5%Hw|lhbXa#%6d)(atp$0Jt7pdam2Idea581WVuO) zY|-mxuE}UBo3VfPI5jIoyZsbPa>l6Z!tSMunkh}jBF>Mi!7;ad~q9 zpCo@z<9ZQvYh+p&mykBdM&vcRp)Xk>gWXh6u%C~|j({ioR%MBfmpkz4+gZOPzC3wG zMmyJAg2V9jnrPlrHXuUD-$@b$2Pjj!$zK|&i(;F!iLcW!lxy?W!n1;T?2_k>(bONd zA~aKvTPF>GC9XTWQP<{<#+rs2!LMeVmI_wG5hAsm3~}}GKTw5~o1zX@?G==@Ug>uF zpMM}uevXRr;KTNHfl`{Kg5AN#@Fup`oo>D(#gp=`pkOR3lkRwV7+nh`vPL3U8 zr>Y$X6%c7eTHC#%p-pA%FUA469V9o8)Fvx2oUMw@#9B*KFCgIp`id39U zjiLhEqx)3*N+K$V5B&gj5bdCu_T{lAD^T|g&`1T+whZ0Jv=HzHX4;N90Dgb^1S?!c9(Y4EGlDP2?0p zxa@GTG*X3Rw|crqheSsCWH2FiCk+J&Ja*ABPl$=Ne@)rY@GD!V%W~=#^n73@ec~j< zNL)-Vx!v7o>{+DU%-1SQSs4G_={w`I_^likk}>-|kC?zixQV8Ej#NmzztQ z|7NzRfUx{1o{~zuC52FI8LRSX26YwITt%q7`xNB*gc^t_{ZwHd##W6YerTat%u? zRl`3Uf#(_-ru6YiIo@H~iiLk3X;up%=4aGL1c)PYS8p2rELh z`WkX?21h(xlRjDpwasjfv~v|> ziL&2bvzDlG5|em5ozMF(cLRn^DOgykVA0I9?J?$g~m)3tmJTedEWanM>z(UpW>y+%s~SWaV4;OhGXN}!AS(XJY-N9hRb5@()#o|C_dU;pE4*w!S$HI)Rf2xLqKFHqSl`?FR9{!8s^e{{ zQF(UVuZzg?U1p9#^JmYpgLte&h3><*d6xu;Vn|AX!EOSM2>pnj@+cG^KM=&)J=P!m zUR>_sI>9FD=jjgLr>XO*!swR{{%y_)T>TO~=jg1Ih!Sg^iMkg)`-?K;0L9@}ItxxB zByPKNPaUCQqd^)W%plU~^DPO9M7d59#FlYwSpJ&!90x5%M? zMs5T!=qIl3ST0>ts~P>f!nr+`d?BA3Tz)n^eRR)2P<$^wjm@R~-ROdk{{oes*@hDJ zH-Sw|)z`(8iC>PN2DK%UHD}Gg$>Q$xoK6;d5gQvD2TMne*jRR9<6cda(J zBb(wxu&z&n;|vv2(u&w$dz6E7C^>%hcf6AzA~E9%?GOZmI{kpodAkoHbE1yq?K-z6 zwOe2f>UGM=FRC}BmzbHte|F*)@63P_I(Li4gM;}4IPb#CfL}@(HomLe?ZK91Oo=$I zPYmW}1hLZlRJhFN3z;*#Y*vCn_Sywu%$94d=_&ZYMvLM~ z2W6hQ6)q`{cJLOMmy0lC3$%gfOY@_mqMBDtr4G~$-C{jN>mOR4y*GQEbB|kF8#(S3 zHYGHd;7s%eZZR)Uufg%4ou*~kDL!mIPi+XYuf`Q-NHJVJQC;k8#Al^G;mfNmJz97i z{>?HgRTn|p5S?^R3O72RP;loifUNpVr%YBrmhOZAgekuU{}nFUkP6`f8-s_rH5iuct@#{8opi{R z%kG*$)r(PEJri*zMU;V5@(13An`ZEj@4~AQCd+^ z(H=_kI)H6M9Pc1g7yNga)s_ZB^490gO}gsp5||yDBrbV-5&okNsDHKCp#6r&;%}P3 zM|t|cE?;?SwAz0i!;*0a|9XFweC!>}7mRH>z3HD90H@c4zgqDucyp zf-c_U4MNEY@4^6;#Ho4gbUr1Cmb0#*15Z zZP2e$z*J*JDi#+zSsLVH8mI@Fi*Hc7D_BoLYoRrsLv}mAaz+A76cG4O#EF&T+?CE; zYNV?8HYTg-;vi?@d@|Cw8xGw@r*c!2n|DWZ4GwJEFR9%4sO7wcJ2C??tDi-acZ5*~ z=W@sD${NG<ol&CBRXv zU-6CL(_%rgbI2Uh<{k-rJ53kX%^IgDz=tojw3)-mdeTFYWIE%8B^|Permxh=`j72h zRaIH-8^F}g=yq;h$9mSUp|0QE<8ru@N?fMXCcM)b{j}3aXg|sfcJ8X=i6K@(c$pCv z>8b)6rZkf1a&3U>&1#bQy`a=K;B~KHhc_D*JxOAj2;(8a=#dt*Sw`R5%M5UQ+U-`Q zr_(jyKzGO3i|lO)X+L6tfPubx!UyZa%sr1~PWb;z0{>6hs}WfgjyP4YCw9m!?d=gn z`VgNB$xWU@cBvH%eR5n@UkAyW!_w;5$)5yIk6A&d^GV^{BIl#}g>!o!9A*LDfFf&V zbN#^HR3RT8lql9s%C$H-){UQ;Z^NZa=*(XBRqh@Mk_>tW3l#l!7Y$PF5J|&inDXpq zM$$7*7V}b}8%nTpk`reIWQ( za#kC6wu~IZ$xkmkD{EY_FDQtoPKjCnCjU~}=DRD{`>$O}RFMwlc2;(S>E#4(;CW4$ zNm*uK7uNkxQ;o)Jfz+<}wRiK}LBb$Dw>zG52VHf1m#y~x^gsH-x0u_Bj+SrsX=g+U z;U)s(?=hAc#M*B6t!-45~V$O1!McmabU}TEsd>ZA|a+X#8`VCp9`gU!Xp(jKU z>qP0NZ@`6a_XNOEAz0ERQz%iixI@wR*g&+wXjr0O+J#zDAmEfk{$F(*YV-cG%`CQ} zzm0gYJVodCSl#Gu*d?%Ai)hYsb=R2TxXYS`@cbG|RZ~ zA&tqp2<0NLY47M%=9>y#pJPXdltDL}4Rb;)u6&<7CF(g%^#t9cz(s3tzV+zb@31!| z8nU`Da(js0+4blCu9fURXSRcC2>K|YSXe*>mavs(6hm03YnJ<_ug5&>g8>;?q~tG# zotVnoHt&ti_26t<^?V71(#Iy|9eqXn-KaJd?q9kd#%o!E@*1d8s@w=oipbn`H-=*u z+O-54YI66T#`S3EbnNWWgxFlP^G%^Oe+x-ZZWcTJQW&SvF=XOB;Pcm|kGyDZp8Ph_ zTY(P2#B8UmJZ?FIt;vZbQAE_rU#9I{SDk8|+c{=<*|LntGO0L! zmyF`P+@F6*L=vyJclQk6Jw+x{Mc9$X-eN`|+Z^UfJ>7nOpw01WEXBAvOGtI3SIf?fkwTeiK^jO&FDG&FWM!Fsvc*%SS>8tdeP6=#8X6iUDu+kAwRZ7b z1pqD7Y~0~0M~~Y>M|#dj^J(}@$Zgw?qlNNi??zWPE)W0Mn2>EuW?NK}&Wr}Up2J^N z{S{#rcyDb;Shn9DnPcat9vOjBMY1Ls;2(69jevHErkq&zJD}vj1Wp_W zA42^J;e{Od-NCCiJR;&pjNq$vN9iR?pr4CE?caeuj$pV<;QCLQ5Rx{@^g4i&$L&)| znSUFAjkb>+t|)@#Bh3DXI57DhE%GtLJviGZV7*=_>rI>+RHOVg=j(1mtzH$v-p>%f zPKnWugpp{DN2<_|RLpvMXBdcKF&2|dK0hYKr5b5@6JwnpffKou`ZGVwsBW=MTf;WY zt7K01q%PK@U~`TTW!2`(E31sQ7I-s`Ho6)j)&PIt4z$D&a7U_A&5y0(Q_HCnIA1!< zP~|+&H8}AU*#DW9OmwKI&1yYN!nfv?(w9Y}dHAKO44Bh}xGLO0oSJMHupsNJ%vmh( zJn%tbJGVq2319rg`W?9N+%V%s&hiJH#SdsNoiZJmh4$ojIEE~|OmO02?R6XpFUm4M z+5x`DR=9izH(lBK52=m6lIT~F5#M+|Z+uzh=IyQg5aglPnCBW0Dn7I1->#e}=Jn@^ zBY2lNu)8;TjQ76t8B*gws_zzP_Omlt%Tuj{?0@N%VI6_*a;rS%s?W6qtKTC$CloB4W~6js8)q z5H4aN;DVQP$;EuTk=t~oB6b0uc)6@IuT0GexZj&EA-@R&I zMA|&IdT;5}&Gd-c0WqW74I6Yt$AEJ`kJr7JNWf$DR&#m-NZp8bhR7Jp9nJvWdZsry!f zz>4Kgi7aqQiTFs&dcjmQm!eGmrU*FH+LR9P;DlveVeZ-m9@FQ>Mr`C%zgRlDvbu`B zZJ-=sb6MS?YR}lTcl^0E8_r3$TXVW_rQW)2?fVwDc32jWhJdEU0}ZCPMostKWYmO- zct7e$PqzDbw0fNOiN$2czcr{2KQg$6wYo7vZ}~1{mQ>y&dK_JPABkq1b*Iq6M+3qq zE^*5IxqxJ-r|!@_7{+2i05Qwww9k= zef+rYWQnsxKc`eWt(e3WmxG%w2)r+Sat)&-0j+EMRty|7p>^nHFu?@MuSCv$CQ!C! zp4jvVD0{Z-Dt$`Q{%0fae?`OGghwXwo$Dh}TuAMBIh8H+jJujNax04e4f?*A zowB>{B(NJ)ok-Jja;XKUe((7u^))+5%A^!Y2G-|^{~b);8PkhL33PANP$H}w3PLhR4NPL-wb~4}(Zb=s)SMH7DteFFA`}km84v=Os z6b;+Br~>)>iK`KchOh#!huih}A}4DRL-s|eTxN9F1g7PWK-YOB!};#G|4)rvw&>ZY zZvyM9AcvIadz^Pq#mn5+T#a~niOC}Ci5}Bjh(0_&@b*L-L%B|*dPSOlf8OzItelT> zzby6@fY96riIUS|#OutXXq-1H(b~+Bl6LTiRf$^-%q&y=*{G+9z7p=sNbomdEmY;u z=rph+k`uKAB_HhpHknMY$#-1urBlSTyjDZNIq#jnpe+C()fU(f*x9sK=Zt-~pX|{R zytj2NI`9<4W0xLw4AkoQ2H*~2Ud93BNoL|cn-W>fDSr-NZ)EuDWIMBZk_%w@QNE$y z`BK1EJd@kUU-blCB3&#~@QyhWIxTsIW3@i~KdH3TNzt8ehANt}m>0|nnPgv6P_&L> z=9GGT(s>g5KLqgax6#-woZ9l7(OTJk+atb}Ik2=95cj14G$czQYoRln!^fScDwnO6 zBRTE3*uk@OqR}UDF6u`iRc4PL2vzq0G<(8TaV|6kQrrl`JA?M;8XejmbHgqv>pXV8 zEx2v`^0PKG|6b!^;}dNIShO57_c?Q;n!PbLJ~!ueK_Ae2Vs_}vRZH)~1AL}=dP$ax z1w`Ci7~L`gArk>amEu9Y-WFtwqqAnO`a@(oL{)Tb*r#Y+14BV$j*Mb-$#d3k_No&) z{LuRko&>`}k@b=BX5f~hH&pLXPFa^4ZAcB1W#K_`KA;iL^Hr=&~E4t-=h|^J>F)=Bw|H&*jU@%cWr25l6 zr*79?EB1I-H4kkzaio0=RSxWQ(h~Tdj9F-qepgFY)H?`B;CdYT$4Q-z+LZg?w$5VM zCI(cmCHs70?u{(8xwo&Px(QI6OZpll-!&8D+%MrOnZ38h^c5`Ax4j`EY6)lM6x0d` z`VTT*wH0)5@cZy#qJB0AOw#|Q@SN2X6J+YOKNW$Mt@{lz38Gh7iW^HIJOM;!p4S^} zLHDEW+i|ry42}jmIEWk3LrlrY8B}ZQn#l}5oN6gM-+J=2tj0jc%%E2MCj56i-llJ6 z>lF%N6vOXJ5=uR!KINK1?bt{v*0!%?#JT7HF{I~Wiw$%Rq8eT7)EV6?EHAlpgNI6ad6OX+TtI8v>l z?PEIxOauy$Rs14yqvRP@+}4Z(+gE_JGwcEk3;K=XUl8;beUqUAB%NxQ!S; zKZOnz^Xb&daba%Oq_fR=0CIW}^%Kfj$_?9c)#foJsPJ$B;Ok?$hAdE7qnEA@Dg87! zn_b^G*q7iZ$oNE~9Ns6PP6auKC`D<;dsak2Gje6}^k3Qc)PKup(Zl~)J;;~-)|Rkt zB@3!Lezmj*w}XF$@EW+^k-^+7?lZB}s5NT@zK&cj?I zpd9E9ozeap9COjH5?}l>5^31p+sKY3B8x*!@^>hcLGI7ZI8k8k4%UosMoXeg5ktK? z%92?%xz6BjSXF-M9r?Y8P&{*`A>+jksd-6zdK3s}W{iUuo!i=$5G zibzaqi_rj%`a((}%r4{Lkf$>=Oy{ajQ$b+G5AWK4p~WSIC)JeM!-$wmXExK(pD29z z>}@$C3mKT3X-NMG^lB8=!ssBv!mrvhdKxts*0&F0;==eSvn@Vr*^j1ahHX@RkiE0u zH$HK=^=w<;nu-hA&E&bhGJa)4FgYtBL&l{E@mu;_?ZM>xs8jnf{)uY4p(c-bTF+!< zFS4_YNt{x7t_8qwUq^2js16%cZ0&zDBIDI*No|0DxzGD#^wwfraANyQ?3^50dKn0Z zd>lvOhEcSsePb8jezUROJu%OfEK%64(DGw|fli`9`+K*r-4SscUiDUCI)Y#h!QBmWPJ3b4^W+y(;uMXf`$L+)bLbB;VoK%j2m zx?FLsNS6|t$0x=AymL;J{)@hYU%oidP*Y)wS5@_|9XDY4VLDv+x_fm>UzU)xyZBjg ze>#KeN5`bJ|A5e8SOW#<$&9!=vJ8#QMg&U|$DwQ`<`5O$-@<&FM75z%=ro8t=uZament_cmake_auto autoware_cmake + diagnostic_msgs fmt geometry_msgs rclcpp diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..9d88d8e6833ed --- /dev/null +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gyro_odometer/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + addKeyValue(key_value); +} + +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +{ + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = publish_time_stamp; + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +} // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index bda7af74b8489..1852d70bced71 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -25,11 +25,26 @@ #include #include +#include #include namespace autoware::gyro_odometer { +std::array transformCovariance(const std::array & cov) +{ + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + + double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + + std::array cov_transformed; + cov_transformed.fill(0.); + cov_transformed[COV_IDX::X_X] = max_cov; + cov_transformed[COV_IDX::Y_Y] = max_cov; + cov_transformed[COV_IDX::Z_Z] = max_cov; + return cov_transformed; +} + GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) : Node("gyro_odometer", node_options), output_frame_(declare_parameter("output_frame")), @@ -56,6 +71,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); + diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + // TODO(YamatoAndo) createTimer } @@ -63,186 +80,195 @@ GyroOdometerNode::~GyroOdometerNode() { } -std::array transformCovariance(const std::array & cov) +void GyroOdometerNode::callbackVehicleTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + diagnostics_->clear(); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); - double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + vehicle_twist_arrived_ = true; + latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + concatGyroAndOdometer(); - std::array cov_transformed; - cov_transformed.fill(0.); - cov_transformed[COV_IDX::X_X] = max_cov; - cov_transformed[COV_IDX::Y_Y] = max_cov; - cov_transformed[COV_IDX::Z_Z] = max_cov; - return cov_transformed; + diagnostics_->publish(vehicle_twist_ptr->header.stamp); } -geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( - const std::deque & vehicle_twist_queue, - const std::deque & gyro_queue) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { - using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + diagnostics_->clear(); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); - double vx_mean = 0; - geometry_msgs::msg::Vector3 gyro_mean{}; - double vx_covariance_original = 0; - geometry_msgs::msg::Vector3 gyro_covariance_original{}; - for (const auto & vehicle_twist : vehicle_twist_queue) { - vx_mean += vehicle_twist.twist.twist.linear.x; - vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; - } - vx_mean /= vehicle_twist_queue.size(); - vx_covariance_original /= vehicle_twist_queue.size(); - - for (const auto & gyro : gyro_queue) { - gyro_mean.x += gyro.angular_velocity.x; - gyro_mean.y += gyro.angular_velocity.y; - gyro_mean.z += gyro.angular_velocity.z; - gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; - gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; - gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; - } - gyro_mean.x /= gyro_queue.size(); - gyro_mean.y /= gyro_queue.size(); - gyro_mean.z /= gyro_queue.size(); - gyro_covariance_original.x /= gyro_queue.size(); - gyro_covariance_original.y /= gyro_queue.size(); - gyro_covariance_original.z /= gyro_queue.size(); - - geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; - const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); - const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); - if (latest_vehicle_twist_stamp < latest_imu_stamp) { - twist_with_cov.header.stamp = latest_imu_stamp; - } else { - twist_with_cov.header.stamp = latest_vehicle_twist_stamp; - } - twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; - twist_with_cov.twist.twist.linear.x = vx_mean; - twist_with_cov.twist.twist.angular = gyro_mean; - - // From a statistical point of view, here we reduce the covariances according to the number of - // observed data - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue.size(); + imu_arrived_ = true; + latest_imu_ros_time_ = imu_msg_ptr->header.stamp; + gyro_queue_.push_back(*imu_msg_ptr); + concatGyroAndOdometer(); - return twist_with_cov; + diagnostics_->publish(imu_msg_ptr->header.stamp); } -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::concatGyroAndOdometer() { - vehicle_twist_arrived_ = true; - if (!imu_arrived_) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + // check arrive first topic + diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_); + if (!vehicle_twist_arrived_) { + std::stringstream message; + message << "Twist msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (!imu_arrived_) { + std::stringstream message; + message << "Imu msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); - const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + // check timeout + const double vehicle_twist_dt = + std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); + const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); + diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt); + if (vehicle_twist_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", + vehicle_twist_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - if (gyro_queue_.empty()) return; - const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (imu_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); -} - -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) -{ - imu_arrived_ = true; - if (!vehicle_twist_arrived_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); + // check queue size + diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + if (vehicle_twist_queue_.empty()) { + // not output error and clear queue + return; + } + if (gyro_queue_.empty()) { + // not output error and clear queue return; } + // get transformation geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); - if (!tf_imu2base_ptr) { - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), - (imu_msg_ptr->header.frame_id).c_str()); + transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); + + const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); + diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu); + if (!is_succeed_transform_imu) { + std::stringstream message; + message << "Please publish TF " << output_frame_ << " to " + << gyro_queue_.front().header.frame_id; + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - transformed_angular_velocity.header = tf_imu2base_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - - sensor_msgs::msg::Imu gyro_base_link; - gyro_base_link.header = imu_msg_ptr->header; - gyro_base_link.header.frame_id = output_frame_; - gyro_base_link.angular_velocity = transformed_angular_velocity.vector; - gyro_base_link.angular_velocity_covariance = - transformCovariance(imu_msg_ptr->angular_velocity_covariance); - - gyro_queue_.push_back(gyro_base_link); - - if (vehicle_twist_queue_.empty()) return; - const double twist_dt = - std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; + // transform gyro frame + for (auto & gyro : gyro_queue_) { + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = gyro.header; + angular_velocity.vector = gyro.angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_imu2base_ptr->header; + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); + + gyro.header.frame_id = output_frame_; + gyro.angular_velocity = transformed_angular_velocity.vector; + gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); + } + + using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // calc mean, covariance + double vx_mean = 0; + geometry_msgs::msg::Vector3 gyro_mean{}; + double vx_covariance_original = 0; + geometry_msgs::msg::Vector3 gyro_covariance_original{}; + for (const auto & vehicle_twist : vehicle_twist_queue_) { + vx_mean += vehicle_twist.twist.twist.linear.x; + vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } + vx_mean /= vehicle_twist_queue_.size(); + vx_covariance_original /= vehicle_twist_queue_.size(); + + for (const auto & gyro : gyro_queue_) { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; + gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; + gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; + } + gyro_mean.x /= gyro_queue_.size(); + gyro_mean.y /= gyro_queue_.size(); + gyro_mean.z /= gyro_queue_.size(); + gyro_covariance_original.x /= gyro_queue_.size(); + gyro_covariance_original.y /= gyro_queue_.size(); + gyro_covariance_original.z /= gyro_queue_.size(); + + // concat + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp); + if (latest_vehicle_twist_stamp < latest_imu_stamp) { + twist_with_cov.header.stamp = latest_imu_stamp; + } else { + twist_with_cov.header.stamp = latest_vehicle_twist_stamp; + } + twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id; + twist_with_cov.twist.twist.linear.x = vx_mean; + twist_with_cov.twist.twist.angular = gyro_mean; + + // From a statistical point of view, here we reduce the covariances according to the number of + // observed data + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = + vx_covariance_original / vehicle_twist_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = + gyro_covariance_original.x / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = + gyro_covariance_original.y / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = + gyro_covariance_original.z / gyro_queue_.size(); + + publishData(twist_with_cov); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); vehicle_twist_queue_.clear(); gyro_queue_.clear(); }