From 627a8b6cb2f9140f5412272c19f562351319bb26 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 5 Sep 2025 21:13:48 +0200 Subject: [PATCH 1/7] Add debug topic to visualize whether MPPI critic has an effect on costs (#5485) * Publish criticsStats Signed-off-by: Tony Najjar * linting Signed-off-by: Tony Najjar * change header to stamp Signed-off-by: Tony Najjar * make unique_pointer Signed-off-by: Tony Najjar * typo Signed-off-by: Tony Najjar * Add readme Signed-off-by: Tony Najjar * add to readme Signed-off-by: Tony Najjar * fixes Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- nav2_mppi_controller/CMakeLists.txt | 2 + nav2_mppi_controller/README.md | 22 +++++++++ .../nav2_mppi_controller/critic_manager.hpp | 4 ++ nav2_mppi_controller/media/critics_stats.png | Bin 0 -> 119317 bytes nav2_mppi_controller/package.xml | 1 + nav2_mppi_controller/src/critic_manager.cpp | 45 +++++++++++++++++- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CriticsStats.msg | 5 ++ .../gps_navigation/nav2_no_map_params.yaml | 1 + 9 files changed, 79 insertions(+), 2 deletions(-) create mode 100644 nav2_mppi_controller/media/critics_stats.png create mode 100644 nav2_msgs/msg/CriticsStats.msg diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 931af234887..c28c711dc50 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -59,6 +60,7 @@ target_link_libraries(mppi_controller PUBLIC nav2_ros_common::nav2_ros_common nav2_costmap_2d::layers nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 68db5b73719..630aa19f214 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -59,6 +59,7 @@ This process is then repeated a number of times and returns a converged solution | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | + | publish_critics_stats | bool | Default false. Whether to publish statistics about each critic's performance. When enabled, publishes a `nav2_msgs::msg::CriticsStats` message containing critic names, whether they changed costs, and the sum of costs added by each critic. Useful for debugging and tuning critic behavior. | #### Trajectory Visualizer @@ -293,6 +294,7 @@ controller_server: |---------------------------|----------------------------------|-----------------------------------------------------------------------| | `trajectories` | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence | | `transformed_global_plan` | `nav_msgs/Path` | Part of global plan considered by local planner | +| `critics_stats` | `nav2_msgs/CriticsStats` | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) | ## Notes to Users @@ -328,6 +330,26 @@ As you increase or decrease your weights on the Obstacle, you may notice the afo Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. +### Critic costs debugging + +The `publish_critics_stats` parameter enables publishing of statistics about each critic's performance, which can be visualized using tools like PlotJuggler or Foxglove to analyze and debug critic behavior. + +The published `nav2_msgs::msg::CriticsStats` message contains the following fields: + +- **stamp**: Timestamp of when the statistics were computed +- **critics**: Array of critic names that were evaluated (e.g., "ConstraintCritic", "GoalCritic", "ObstaclesCritic") +- **changed**: Boolean array indicating whether each critic modified the trajectory costs. `true` means the critic added non-zero costs, `false` means it had no effect +- **costs_sum**: Array of the total cost contribution from each critic. This represents the sum of all costs added by that specific critic across all trajectory candidates + +This data is invaluable for understanding: +- Which critics are actively influencing trajectory selection +- The relative impact of each critic on the final trajectory choice +- Whether critics are working as expected or if parameter tuning is needed (e.g. `threshold_to_consider`) + +More detailed statistics could be added in the future. + +![critics_stats](media/critics_stats.png) + ### MFMA and AVX2 Optimizations This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index fed85867905..237649bedde 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -22,6 +22,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav2_msgs/msg/critics_stats.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -96,6 +97,9 @@ class CriticManager std::unique_ptr> loader_; Critics critics_; + nav2::Publisher::SharedPtr critics_effect_pub_; + bool publish_critics_stats_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/media/critics_stats.png b/nav2_mppi_controller/media/critics_stats.png new file mode 100644 index 0000000000000000000000000000000000000000..c373f8e0dcc5edfb00b450e2f6c7d24ea562eec5 GIT binary patch literal 119317 zcmdSBbyU<{+xJaLD5WS0Qi>7+A`C;9A|Q=~bi*JF2!cqLDBV3nh(R|)NQtD>fD%KO z0!ky@JbN(C^NRaUV6zQ<=1q^c}ScAEAy4h{~P+`YSMI5>os zI5>EG#CYHpODgLW@CUA=n(Q5%f-br_92_Pbxx2SDTyPg_ceTzByx;ad_F?7d;Tf2o z4e}D{;X~)$F{kXKqrTbhFQED_!77MkWOf@zOW6W{sr{L+Oll;7sqTAHZBvhLFH zkSBJ{9FTNV=qwZ@t@v_T+&3`L*kwmgGHB&Hu%%-;u{U(AGq_w%QUz!?3cn2GB5 zyKo3Rd~xz<@@W1xL|R;Jh!Jt%WAZ;wF{LTsVDfrjn~&}LD*hV9C(1-VD+ebhv=xPS zPA;5937I!{Izap$4yKdH?zA$=U*A9sK2IC+>#p6bcOAV+%9c^k;!2aWJG9(qCWZfs_;> zo1MbNqn}^}>#@I0Y)SlU;!I3j;JaQPDaj4H2Av@~SvghWENLAJi>k~My4g6OHcWwcy04cW=V;f z!32HQvuANmP8H6}oX%SHf^`MUPaW6m0*%#<2SUVyxEFd^g_+Irexwm%IE7rX|lp|B9_^2BBQ8Bdi2aq zXQz{6Bi@W|(}}r^mM%^|+`cf3R~~^n{j@H8@7|)8x@U>C(o==SZ!FX6Y!cSrpXFbd z9tyfo9xxwk%9~)zVH7{)#>FY~<9nT^WBHSM66+}-F_g27~n>*C^UI^WJ zL)B|Z9jj6BQdF>p8IC!3!B#JIJVGhd!aj=aZ!5xe={`(OPA)EA!h8a;SJ#xmt~Ba9 zT#2elGoI;6nL(qE*GzkL490AG9B-IGi=pg}ly6aXlxua@S}&51Di< z?a;?MOMdEye$#f;1MkC4EwFx1mpLUKe~Hc9Qxu}{o8ps}!Q@@v8?Ln+$!W9lGZSY> zPOYdk9f^)7=3;Twc|4bFzK!AJ<8j|y-mLT+J2cCjrK1Zk%8I?-+?+c|>ujr+(atJ#&oCsPCQGiqU3$!Yp;fsGT8g+ zs=mtPAsWxd14Hg5Go9v<31AN8>&p^|V6a;RW^w&G7qgj-Ov_+wY^k_s=#~tP9;7UX z*&Thfx4<2PQH-)EAmPq?-GHI@P{zy-v2`zLFr$hIX!!`cnd?$3l4m;A z$|kGrVv0Fv7}M-$g>S`ob>AbU2t(_tue*I6Ebk_i^%pW#%b2aW`-q4`KA}wJ5z_IK zF6x1vR=J7ekAgRvmou6JWScX+%QJS2l{VY_nsZBt$_EQ}f}}0yK&)JvAcRt z3yo@PZC2Wlht&g$wj@yc@S?eNKdqG}u0uE=-acczn{MUS2OdQR^d*CN!96aUxABq!jz$|)gXgnFf@e%tf=%}1)T zq-||d;BX1lWe49iJ9UafC0S5!|MD2wR{SWHYns^DY!HjRD3F$R5z(y(yPr&grU z>wsz7ny84R7O4oR3M4GK}} z)Gm6jpFXA0TQ+8h0*+^fh+=tI)U0ok+U!D%P+ieO<(5y0iA@_q>0HN&v<%b|j~z8N zE$XMEelSvWB^&9M3$u#zaeCdkKNLTMz&A5Him%PGUz)yszc3#$8>?HRoeS*^yYWVC zHi033_}w)}Rnvn{R&aUwZ=_*Q@1}PSmAHE@x9%i1#nQkBx;xA_NwXK0q{x4Tn;mYj z`1Io<5nuSIFpFP1rJI*hKP|=x+IqIlwsEawJ{$^)7qo(qq#C1zYXq`>M=?W6RlQqi zcXXe0m6da2c0Nt^n<3uWf-A16KbnHnY2g`{_#hZX*KKc^7H#QB;J29y`LRw>z!|wf z@(u+nfQq5-moISKkB$ENf|t2F;D&*d3$H&o@0j3jm)h=J>yDhZX-x}jct&DjR@0TQ z@izHvDLicQhl1uVUrINdrH6h@)6;&InvJ{2-4&{)TXww^!|A<^8Dd8gi8e|wAjQob zrGG@nW>Lc*l$*dz7tYw~4L`bqkiL)(M-kNyK_1$mb-U2eoKR&%o$YqX{rr^Ytjae> zYrRZOI|xf{UMh_o)mP*c*$Ov`U`EAlWAfKb6GP9ai9O z$Qc$njpS@34yYeQ!EnQ(taKT?t`r@@W6-u3nMHg|Nn%~168+M6N& z*=0UHOsHs2VoXeK4RND^R5LOPtuNvnA7d#Rw>{zf5FL1)nZm3J1#_J3bP~h$!i3rg zz#knY3`a%P!sh7))O*lY;Gb*$(6Q6V&hWgUg+LBb*u&XH^o1EX>dSE504JZ@>dvWC zmpeM-+bbS%<_~HLf3;i6*I9e^WuWl$!~KHHeqru?=VDmSicc_IGNQt|w|vZ^c}7oQ zWn8^yZEGZ+mG4YuRH3i6Rdra+IRXCb&ixtD?ZMGY9S-W_qOay)g*xvZX=HnOL4Yqt zFUMb8EEf$#ig_!!N=sY(Kwd`_9`9~J>B!XRp8d?jd`bxdp;F1SkMLY*Gb_trg>__- zhju!)6NG7~fxIG%H{yAS>gs16zfzL4D{1AhxVX68-Q8Iu+u29@&BzPq^K2d+X@NMi zcK?XI=VaNOUxQpAr(l_XS4O#{I!6jVYyX_Rx~)|mF-@YhR+`axHe>N_Z?c~+wAb^J zx`28wYbD)`Nhio5&iZO=IVYK0Yaro{dvwfjYA7FvfaSdr2~FYGGF=mD@7cI?4{dR# z@GT$R7!%*zWGKyhW_l5hsT#;@F3bKZoKWfug?lbL$jL8Qm7Xq%x4ZVK0%kkr&kExj zE~#dMV7}JEUI_j8Y};)otjh?sMZpTYCHUN0`_LAb^(F=|{K-GkU!DIIpr@}$=FxoN z^LHLy72)eIzLx|&W9NwvOnGrNOnopKqSm@?C39U_UvF!vE%-X( z{;SVYFq?vhu7s!z8-vf79!xC^*&e;iFGr6g|W5wfdxw*^Q%qXUrrdOv;dVb08Pm!*m*xp7;G-GZC|-#hXso z!f0q{K-x!hQv#gB`MqW3GLZ5ac^}SXpHp*Vel`c(K9a(9P(2&Q5|t#9uk!NY3_{6| zg%A@;ERxP8v~g#AX;eo!53y@^UXxJsTzhY!eE8#rK_pFuH2Ytr&qHD+l&J6j{dTUy166OcF&T-_V)I4-5<-Rz2p0U-9|Q%iv%XFIkfk{I0EV@KVqDK-cFc zVnu6HvVMLOh}Dxu;FAs(yryh!|E_1>g36T;PWab@DcH}-X}x^$%0T{W0RP>dQ)j?qN~iwA$7m(If=z@zHYn2}ziRH953C)CV=8y*buM}TUM+b7 z=Mw0`5bE2~$mYz9QH-9Rp8L|luEKQh8(E66{aTx$H6)T53YCq&X&`gs`>ckRx0bY)@m29xiT)aK6LyVb9UnXh&s5UxUuk-!WALnQ!_UaGd6eCrx>4sw zGZSg{vSeaUm9~p{yI)mIzRvy68A?g~E2BEMm%q&QT87zKK6#?(wCVh|D~kE!-quoP zlH-m&S@!yvv+VxX(zjvDIQk)HSDBgXj*rZBhl!h! zDZ)-II}F{aheJr{i?_-qQ&l=BSgzyy9aFA0o!Y%KYL;kZ0|H`Mxh3Q{yQrCqVRKqr zgH>T}|4ypGAtq*;6v5=lMr^xnmgwgDiXR1qDV_87Cznm^r~Tgisn$)$eZxiNWJr@* zMmz%NvDPckLoAgeLYYx|lbQjsmbR=2f1tT(tFM0$c$T@93OQEoT<7wGal!+&Fj%~j zz|}EOTi6lL#m%N&`S@#sv~<$x(+_SMR4KL%6`3%%=~VG~HwQl>yUbr;WK`<08{8a{ z@a}1u%a5LegPDbai}`vLS0uc5UrUD=EHM3T4+$t&&%!aa`_*2rI&0LmHsaB6IIN6t zYu-cHBn?;AOE$%hM{rKFTBjBnbQZ1(5gqk2;edjMz3 zL%5Y7H!7gV?(;g`%M(#7Vi?MO)S9rSr zXbCe~9zqqCZw<$?S2B96)(;kcz4zklM-?`VO=lv{`Yf7tx5|!3ef-H5)^IoGMhcBK z>iq)RIE2U z6*_L@QGSm^@j6AVB7H>9p(0>EeeNZ89g-&Jl}$O zfWaxq%g3E2qF{L7jh-p7=;(e~_hflgdS;&|flJS8d9)d2$<5YJ&21=b*3wy2bT5Fp zZ;F_VoSf%qa49^ENmf<02n};SBsg`d+DkvT7;SYqDp|XuvQk8>{A)WVNv^BbePrDG zxGq*G=(#h&s&k_AsHpX+R)(pyDqcbNur3d&Y^MD{=mI7`>C!i8iC_s`)T6PgdL+UK z_wAl%Co^X!heFvPZ|1lA0g_8+Zx(X+p$5K-+Okntmp?|n;tT1kfYFa9Iexpju{eZk zw4OhWi;G)de}&cpRw2?VYxe10hHtzz701#jja_vgKU*Fm{G&PXgy^^NT3h0${od_0 z_c~BTZny!ORFW|1o_CD){IiX3HEy)pyy3w=YHZRltXDRt3MRSQp1j9l=m?Hj|iVX_an(l{UmL zyZH^6nGcaI)?g5kpu>2(&T>Dy?2pZ786xWGutIG! zDWyj)eT;5XVky` zO({vhPOoe}1Pt;bx8brX|J;L}{Rvi~6!hVqXP#b;c{t-sFoztA zs`D|D)ne1L=&HRv|LY2C3i9oS?+-jj%1zTV`e_A?KYV!htREceAD$23#>U30{$xp4 zukK~#IwfMtV3{f}FN?W_RxN%1Y!>)d-4giaK{)Id#3|&hF2NI$z%~L94vGgz|F)TE ztvsEIM`Ob__e9iZZ}tun%$4&#x??oVpT(XeC6Qbz=F)^{sxE>VayPAU<{2`-wj*xc zAVqKBbivr}W1>#;jy&x25lx4N1b*-B+YsSV1&jZjgo@jss;9zE@yU~8d?Io&uY--% zsg*BZsq!%&^eUF=gf$!&FexJy-YF2v@w$}za&lHujg4UE;F!EJUHp_T*cy#6-wY;S z1h@#7)yW^R?7g#{i0wuS39xwX+shS0&p0@8fxRR}MovHBUl}gjZi|_?Y4je6L>_Gp zxZhiE45;g9L(D(n&&?|+3TbH>#%pv0ypQgLee0o{*F^zs-pR@N!ef57jZ2v*9U+ZvoPSx;zvVbZVZ9!o zk)QX|_=|g#KYVtEk4lZC6~R0h&uj)l@@QtxadtPzlLTi7G5t=))~YfzB8Ozm2s_#H zb=>wIW}<|+Y~Sh<>g4-oDyQUwxZ>e5TZExl z%5`h85`p*%0^>RQF{9dnMX+uHk4BMV?vG5rOagb&mB<7C{wX$r+xw-klm9T1URWb0 zo_}q;mZ;i!MSTUThk`9N2B`35-mMwjo_N`gPI+%!{{<{i4yYh7Hivs*DygCzY}#ec zD@Uv^fqNAMHe1Z%oQJ9QtOj!=xisy^7<7x zZ1Jt=4O=P>unwoRw4bg?;d{c0R|uNLVEoDA>3&*%625U zO`J0fy1&+uf7cy|@p=^n->2Ikg@%X44?nAwK|9*_s{3iRHJw($t1S6LJ3odVz|(x8 z)gG1QH3yf&9y>aJ??SSF=^VQW>AA9nr0G-~yP8dnYk+6Udy)g3_q+AB3g1M2!E`DL zWYFc0Y4d0xh0{@Jlk~`IiyxW0ieRz1`J!hyuCpO$eH5j-J@oAPyW1C#KWB6+vlM2l z^0444Et|-GuR=n>H5PIQ;iP^HP;Ect>pAn=48G-m0DPXbv=M{&K}zpXa-!fpGxWWg zsm5L;(yOpQJURxl2`rrt8-zJ?p^+|pWl(07hvf9gJ6-c3+!%WBSV@wiCwaR&I<}`H zp)fu^zPi^O>46WId5Xwp?C&dWHiYP=X_dmv{nZciaX_^E7&%#Qf{+`VE#}|w1*DaKk>JnmO*$owK(U^jV3@s~E#wsGx&*>+>kIJ&*UfV0XtJP^ zg7nWItVIQ5?S4qxWI}jLCylGQV-s=4O`%{};2b-h@;BIIQ(yEmbJ}p=^XjE8yWv|p zn_rs!6}2w@j9Qp{qr-!BZe`2$nU1ocAiq4!hv(1V8hO_quY9ArOv?a<&%6Rv*F%lh z`tjS1KhSdCp6QqX=ZNauIfhG@YCwPpxmXw-6Tl8SJbYh1y+VQzp{d2VyE(Amg0X3M z9wc#m&@t6G5k)mvcRUh6@sbZJl$%37ymTe_BKTPFWxbi*hkFa2>3p|t?QG0Bf}w!) z>>{Uj-Vc4G)L1Pnn3p+@tcfNMQQz6e6>WX?({&~mDv*V@)G_*@Zs9c$Mq6$gU(S@X z#U8Wwd)6N}7X%DzT%5ppuXL2i{Hz#^MhMt_28my*NdP^AsCKRE`UB_wFJFMH%B?1k zPF`(|db6;wA}hOHF|>G@-)3<_BAl-k!By-#N9NM1OqQo5Gubc`&F8j$}#eBWF? zLgtzi#>B=Zdgql>{*b`L;%5&t80ckyX;!8({L3u-q4<_21kcH?b31tw z!=v=Q-Sut>1%(d%Fu5e{xycb6YwK{sIxl8!yQelbqu*X~du%OQ7>?Tvj4RHTlFesj z3sspm%g11BT0#*Z`?eRjD=)9>dFYwm*GaNH*Q;{Y7Ni*dq%<(jCljHvYm5Xs0!2R;Ruot zM7G|w{;O#Ul8P*XAycGq!mb8l;>-6^`~7T_XH_tYZ#o0T_Q5O+PN^no^OV4bhN!?o zZ!c^Y_ta!P?9EoUY@Modk`{P$pl0M<_wF6vwQHR)6v$^MH|9{^x{bgLXAqE!(b0s% zHczX^ih;EO2mfe1kAqWxdqqhlfMP%}1q~dk?GnZ9+w}r=7;sd#ry5zC%(;vV?Z38g zWukUJ_)1Mpo#jvR>`rZOXxO}l{S;s#sp6i+@BzaB<|nO@CU0{w+d+x~f>UDdHz??H z-n*ifr`vje{0NF*lqefo+}z|+F$CWv^0YLt`Le*HFxogP)R4M*Zv>qXW^iXQ^QBiR zzpcN+?osr^>dGB*rL(!%B_&2aYQZrVFur8z2Qf=?8hQDMCYZOCkXM43X>d@6sjsW}?B*LPsx z1La^!ZP5qA!;Jo93iP*(CqW$^9HI~eb^r0D|FyJ&soB?ZJ8mJ59g!roI2@CltL3ez z%n*+h{?hP50VEKoPg_?EEP@OT1YwHH@z$uJ05SzJ_pLABIGCAz4QG_7blbd03kPT3 zvgd6wEJ!oti<8zry$WYa3UrbYJ6UmD!Vq z`M$>xQKVABpU!rkiWaignB}e;EgxD0*~2@hg`2LY)ry7YS^4b7^Bg^KAYz^x78d=f zVsW(aa;skI9&eMWJi@Wr;)ieF$q!e;pb$ZmbSmD{Af`DN4uJ|1m~nmI8h4F97W2;D~d{>?9gU6P4l zP&e|H;N#Qhk#%)NDJVFD$$wtt&8A1%;GDzTikLNO9ehj<3viCg06#aB*xE4OZ zpjKC8Uz>^D4zwNwZ3vkeK4im%v)E`#=P?nMiF&&SP0c+zZ~s)*-{k$z+99 zH0nl+!C@Omx`GxGf6`%3I)`Of@=I~C?kVEKMU?CIvRG?JlJq$I=}ddS*Wv5NMrPoU z<6}!}l#IYC=4isfi9tk0Ub=YE8-yMNM}&m1KIj-DsYby-Uf70CQ6A2jfR}r^%T2uhOvUQ8H(CF+ z^`7V}b{oW6oiC`!ecTCK@t71YTo0Ve775mf!R9f*J=1DKLYmJ}QhoxOQN@JUetyOH zp2YjZn8;i2P|BZrUro3zo}&{1|{pd4IW z@NF&}Kzw|A=^Y9+^Fattrhir20NzpFC@Mw>DPc))Dy|f`_0MN_utIh$wlE=INl{Pjj>h%cmK7 zgxRBeRd~$G%v_C9{=D#tZ23&^6Owyn#NW?R#*hGATp|6^^DF)I#Ok^i&qOGmIDKPu z>3-i6r7(nIgiSbt{Jw00k1Wuj*|hXrsUOIz@}Y4N{;9j=lQyYw1JLvnZOM526%_Jt zL0wu$Cq`t!L-_IcuBz!STCE)EG!&E6OyIQ$b}oN@5T`l&v8je1)DLPVBdO)U)BPMu zRh}bXRuud;hvtm$GeY7_Df?N12V?mslduj&K1nEsfu!uSKC-A$nND{Qy4NZRj!;S~ zUGj_iKhmOCrP*cB4dQW>VHl#Dy(yPUF7_p~7WRK#pXyNPLet#mAgRlvLpOB@`b-~j zU`H5dO8IxC9;g|=h8tSx2mM;?Q?;NDQd@dl++81#_v0B!!jyfmT8taeeC!peO%PC$ z;PfCnA@zeMtdpZxu0FVMB9DOC&xVSY2$>S7C(t!BcMZa2DjV8S zMQYDpTamZ&ZFE+gvRu!rDPaGphJbVjUyvo_EfT?ku+JgvOa5S8b;^>A(knm`9g$yU zY5$;i5*bef9T6l0m|Z9hE>AQ$CUikQXHGx$g{I@wRSH}kQd@iK4>~_RX*E6^hV#)p zN(0h+jY=7)H&oh66_a_m4J(vcyTzK@q+Qmu`n@M{?)Q@NJj+%H|8mxH5&A~|DbQ`$ zZ-3?^GN*N$9eIXwePB2c`%}db%0uAN#Yn566=M_yHSw*LjiF=DP=v&FQfadvAB;f+ zbFJ)^I(}9Spl%5Dxg}uot&BEJehg@m@gFKV^(r62VoUW zl4&i8Cdtdtv&Y~}ITQCC4Cc5KT0+SRtbSQ-q7XAM*h}?&*JXbMLbgC-X-z1-oArZ7rN&Y0|I%u2o8sTIzE0Df{@7nNKc%K+=5R(Ye;&$=CK=9hwGg zxR*;1?z1XFVC`koFXo3r5)PD#W(~Di6~Q`_qh$GILyhO7-?%i`*^1}%CrlkBKyjB(K!Jt z4ZrjFFDxt+0aP^?u9eNbTTUNKk{9A?&8{{j=f4cUgFm!#^F$Wi{dpASpb#bADGjnL zA>aNdXS!&a4#<6xjCxMplX)5SP_EzVgC$TO&N3%fO|vmPZZU+kTWF=eh&y*)$x$-n zmF0V`>)%#`vi-`r`6QB)q3l=oFY*C}vc9bGzu0fEIH1np*6Iv-yGNfWBo;%DSAy*)u?cM(ty#_XpKf>j zzJ|tH8{=r6-bn%zuLadWu4t5{h{?GFrOCYd)BDpXOu4 z%bKT4RqzoS2rT@vKs^j~@qC{6lPsjhJq3lA0^Meo--atmg>1L`&Yr(3z9*27_JImX zSQms~kDhlnhDf^f;hHmO80^2DCWX%i_dx^BmB0huC*#92$W@{U|8VfQj8~Y-puKF6 zFI_C--dw4D4OvQgfb>~h4BFh>-rwBUw$j!po$JenOxurDfyB-$yRgnkU!V5eInX0n zdiwFbrzQoXIA~~ME*j;58X5F@MSj=fAblRKJhOngP?k+6Tj#|C-UwQ}9(V4jNj3UO z1?0RGa~Fbj4uWF)%$Wghunfb(Dd;O#OxC|~3qD=EM&ow47ypV;%v#i?NElQNLSom5 zean;1z*)Ww$YpyGQR8aZkxF9TG>At;ucs}YDIUmctiD51*?8ocgO`VhxZ5H~_E)c2 zl`3)=$*$MQN1~O|$MPUFZ^)nQvh&jL!Y~Gg;?{{a(%Ww=GRej#Bo%d-)_(jr8x4C2 zg?IJmN&xXoNlvcXeo9TPM=}XrFb?0^m<8JwJaag*M^*}laSvODI!jDGs7uixUl6K z3(~sI(R1>xF#pvQ;Ud`l>Upcb2ETe1&tM2yHWc|ZMRN@0t3zG#VupI9jvM2k<#-JO{P5h;IC^#QFs#2CYuC;UW20pE2|Kn=f< zADTY|X4jQ0BugVK<{8^VNpdG`$-e;^v@>%*XTnz+r~fj3jCcGuJq{nLA?RHi*(zCMXoYx{}+vDI~ zJ5!lJq{vX|W=;==qJ|bD7@*{|8$kt7_k*46o}^-EwfnZAu5OhY4T%=`z^mR?vA~j} zjQh=SOMqqbUH^7_2r@TA2iJF*br5%qvIVBJnz0Z7z8wv9Ac(x#Fk%LjnPd9 z^K4*Nrjb?Qc&!DBF4L+G{!oacI@`}!_5qpQsQ&g3wIumLmiUPUoSK&Q(Se-dK0+p0 zsmAEA?IOzW*|6G+EIg_V`T`v3<8 zN{iQ_>DE%N+v?2_Usub&K>u+3^Gxb*==CowjVzx zal_bPB^-|dhqb!erSpWinhS>IeKcj2}LquSpnlg}8mcl4Bbs(}M zhN!0k36l*V@0hri4q`!c+yEMP2>yftiuIx}fBx&{5UCwyPyjLpg+8YJ3pPq)nPkM| zS$3yZ*bO8kcq_dgvFL~&=%dTd7Ako%$2;I6khW>LwJ0<-71i>xsHnB5C=e)Qk&*TR z%zT1^O^;Z+jmull$ElI9a9@OTKQOGmpcWJ7t43-7|FVffJaz8~%I?lrS{T#`Twtp_+2Wvq47;aqu!{_y?crB$?STO z_wE#HEuG81p`;!&^@{hwJ;1$Rv|L)EX#6}g*A4;QC8WfoRE94rw^V{$?=wc$`pimG z$#*p&Jt?i%O&_`me!{%MN0#otNeuLVkb1Y z=S0*Hi)n6PRzDHZq^zjt!k{6cp$A9%F;-U0fzTweeH0Xax{ZsQQ(pj%u|Y3U5EHke zQ_79&?=(kA%WQ=O_jp}p-j|Un&ngWJ%mSN)p|>n^8})^r0|U0ud{QnfYO_3ehEWl?thD{>NVD2K0IK5{XB2u>qQc5 z;&A;xPg583bu{}c9zXuUEECG4^6;i!IrhR1uwCj4wl~$oNDBxXI37J`dW}j=9O3Np)Xo}C2)e9R>x;+%G zz;HY6TOHPx{<(@&!yaruMaL>q@F9IF<3`FsnPvn^*D>NPlN5S1-BM zaEitUl4#0|+gg)0I03e2RZ%l@1p-*9(DnUE1sHmww=Ru@fbN4scdBPu*+7P3o71)- zsk+r*baQdl`VC)*zBSizD#dI2ake}t3+CP?G z(U2iH^^TnPIrWz%hIFbnb4 z-t*Qrz3tkd8B%Txt#xgo!Q?ec1hrfM>Lm{&OS(@dfmuL`szXM8Ig0t5+YeVIm;mB&hmB)CkNF2A7q|? zb3m&?0S?g4^G>k7#>c!2`fp*p- z7c;z4YQD%1UmxYTmX7R^Qy|xJx5%0omcHeLzAZKbT$lr{la|uoG`2&Di;YM`rIMNw z3_E{A2qw>c+c{3lFp}#)d}reER~AQMj*#ZHE|HZ>w_>efwDy#$1bui+y~klJv)8Iy z*Z|eG%=|}v1T;kk@!7A>-E}ftu4+S0Noy&vIZBmfu52!0QZ51h;2%ZRfN00r{$Nm} zH?U^(yp+4R>=@xetfED2%AP4@Q6-v!=) zlbTyk@@LKcQTBvm-Fv}{(#3bx!*&`jP*SFeNCBm=ir~2g-tYE8@a;=#W4^c2or+gm zV}I&|p@@E<;@I=b^FCQP3sS}Q{s;f4di;xM(BV?31BkBbTCMl--6KfA)_`iCN=^}r z5MFv)^~MKo5U7yf9!IO}bMM9;DOTB2#6}*5kYa<(a9BPT-5S-o zVbpOEi%0Gi+)76x3KF<^iL#nMouI#Jqz#etjjLte;c{C!GTK}VEY)!7p$w^htc zOpKAIBr_;Huq74Cui0>eX6qsFC%2-S&dZe%2j0#RIW34qUr72yv#Pd8x(XBpXIlR~ zMG@*yA7^QjP9WV&7isk!VW-eWgXtu-{#C4DQ}oaSu+ z-vblvo2+jesYrPKLOQxVgqo|?<;NPJLk0>A7$GbQy^Vmk=!nlR(#5ZMba(>_9kV9m zWM>z9k|kq6=8h&^c1U0cec|s-)u5+7`Kw0-Ijp;SMnq-LcMvOJ{t>h`LIQUgfLq0C zzy#)npI!SRu_j7092q3eV^b@q`?{#WKylDr0wM{Ep%vgwK;v_6M1V!O>ev@Q zfI-5`ZD-XPH1Yw@XZaF+y!*|FLCkuz*4^mg!{@z2isDiM8T#+__s5(+-lG6ntx=7O z>)JGXT%0EOoL%s4d3kxeu__cmbO7iS$pTQMyu3VNr};cUF}iIeg0PI;bH~{`=H^CP z{pnqg1snPTuF+o3 ziM2lwWh-<~uPl&WK)T!iw9T4K6d0iGQW;>ax>BlLcGom>p{vVd?iLmo0N(?8ltQi! z1#X2ypk~x6yxF&*sxwsb!WMu8pg?RwW^W_U!l`M7wm-jViDB*Td|6lYYHA8#QG6*6 z$(`~M{j^zu5PXZRm5+Bo%D@txh;1VB+;ft}wZvYk@I?}74uF1r97!hKNk`yqISlAJ zB2QxhWrY6Lodg=`#r9LCDz)9qWBWifhWU~ zHhR1~dI~hPFJ7$k@;u9~$qS8poCWS^g!a_`z5lhlI_9A+Kdu4ZxZ}7Zw21luO{| z`^>bWlW@|T*L$BuU&;A1oe*?tM1az`8iYNKlztk{=k2l8233};s?!oxvH`-rJqqj0 z(J%9K`X*$;B@TJFyNuc`+qu1uY+Vt#dZ2GuKmQzF1|)>zA0NGyU#2F+^4T{BYpSFk zfTpI$fHk2|Ff=4QywYg_dz0d0L^lI~7ytzLQrwfre$rTJq_lLybzK~=N$l)aK%0}x zj?T^wl`_ElB>{3d>r`Z*VIQfGv1bbY0otPl;fqD6SCU`o`HIF#26{0a3U|Nbc;Scq z(9lc6swdZZ9!K^L<(0!8KgI{>SGqz1sAAXGW>$b~bzcwFkpPEVhetE)ZG~BR-hpVj z682h?J+VY*C?aN`@XOxacvS&2PeAP5S8@=aKW;$MC|=`{28}_}$oX)*F6`YAJSas@ z0U(tBN`zv5ijElvYniz`V0C0LHh?bK+uQftm*GGIccOQ{ID}3)iT^8bPe3YS5VU{z z!N>QSL+a9ad8Yu(fF&fDkJxaEWygGI665`jwM zEgs-3fhJn0U->^kK-Tc5^L+@{b?U5q8X+Q6G*+In(au4`v^Ca;0lf};W0IaYVfCQ&EJj=eZ!S66E{%+@d{|oc=fx0@y`H6_lxY*dZ*c#W>^{u7qxw$vzt>RL} zb4p6AyHn-M0HV8V{b-rb8G{9Z-&S62eUh(ZHP^MaW_J$_+@FP|iUy(dohvx-7Avy; zC6EnjZm%_)O;1KOxExIkYYou(cNom9c4 zT@)z#%Blw$7r%V4w=mYe%k=8s#SM^7tTD(z5!jORxe0nyh<5;VVf})xWngcOkgAQ{ z|Dc0KGSdElQ0zZpt-1=Bq4#B=oYr~l&Vz#3vNI8&Kc%kgm<*P0vFz%4MkbgwLlpSs z6-j7A?RVid10ZYg0Bil!y>)de8({4{+Ni<79iTruuZ$ZC1^e&B|RfU!gJ4byrx^~P6Pj=QGi8C8`xfRWyzz-glbe^k&v;Di!mgv z@4FE-pGjAs3#-b%W@Tf7)?Z4jru&a(4EZc)3-~W+y zXCy7dCR|qRO$ie4HgY)hl$0N=#^%<>Kpy(^OFn@)&#ILm6bNhtA;IbRGWG$}QY&a%?#odM*%((&mgwr8|o?nxD8H%>E<5(FkolbJ#fG;PPvP z1_Xs-ih1m-OX>~gfZ%nZd>U-`9HIv1EEO!A>4~vG7na6c$qe*%bngG*k?)p zzIz<&QT2Ueh- zdNVJ!v2BPhvU$|`q%v7K`(IBjGs_mgCH!EPVy1U{ zd31mIwrlixh}QIvy)QJUE1eLRFKQ9wn+lbGY4eSY}(JZ-C&b1k-v?g;o# zAUBo!Bj-M*FFkxBmQJzgIpKLM<>a;4c{&Wb@VR%(|Pvek-y6{Ugk??2# z>K=**LLGS4o|vT5DR<5KaL&H}|7t^G(o5Fp@e2R9)DpD1Pnd0G%_bI|+J|!Tb+#Ff zq4q;RVwbkn6;clXB7WR2vY4{G(t;j}DSyLLw)1!~uW(}SsGuiO{bYoH{A7!j#pfjq zSg;Bzb6P4Kb2ITg`IFdFbA~TciF`_=|3peU^`DstM)cJXhl6Sag?m86!Ki65K}&=@ zm+R}~{9&Zdn*gwh5yz)Pnfk|FGu>BNW7Nu_H3J>vUmM38EBc7Md)D3?7f763;`(Q| z6dMwid4^-g-hrRUoM$`b-=+Xq2=xC>o-MIk57ffM+z4B`_E;?R8e8p-?iISEV@Kz7 z+`TQ4`rytyskY0GXYQMoiT}geTR>Hnwr}H<(xM_Fp@=952qW~4i~$C?Vbk4O;L@E9lDzj^(4>a$sQJ2f)O`Im z1Ny%Y`v#PHN^uPlV5r4zKURP)Bg_>Ldfc6RYD#vWnTi#t*@ad59f3u+?rBu*hTa|Q z6nHYVT|=cKtFt+|tJ)wmJbJ!hW#xI-Xh2Sl=t4{R1rhv@I!Lurq!r)gZx{6+;?Mok zE6nZd9XReta4cND#sA4L9c?)i@T>tH-)QXikFpmKjKME_yDL|x5pI0{cB|m-@KxTh z{Url^v~ZPioPfwpJ5zbqZ8s0dM}+@c`RK^Z!S!-G>v+>Q+@08cogsiSN-C`W}ieiDz-Os5O+wy;hvOtc@ z!)i7bNR=Hgyln*{NX{x6k=jgko+Nvxg{xTBl;g&!is4RX=VIUTA2kvds!9&)@RXnc z0eO+=q5C+%Zfif07Op=A`GN7(Ck~=zG8*oe%Ia0+*F`LFPaU1dJ=Kv^7g!OS!uVZXOLMs2$oo`Hxy0_5a7338sv< zCF+93n$Pmc-d@|eF7f^+a;6Vx3`+wg@@nVE>0bW?MGG+{^Vct5tiKhkLca&Q{98c} z?1&J0!JC|%2V1KVAXvNgQsoCFiX+~-E2U}F%WJ^e89peRrdRGjEgPxYwHd7zGJ7az zF!2u5m3iv#yFlZl{=N`gG79urGc)H(O2kfDybcSy6W<{td){uQEguw!5wDJWtfAw1 zOgO_DIELdE<3%_mWTLrykwq+*FQ3Bps=aRL=Lk~Lpbri+efiY`r|9U!Y({1Piwp<| z_;Grrx0PR1*%+;3^P{MLWa!b<9yY1>|LdHnE?T|lkgF@)(Nb$?glnYi7q3{J@=^VH z^=K{GF$;%N>67^X^k}Li{!T&06n@XOK0ADLZ`?WITV)*Nzh4-z(wd(}Yk@`_HUO=@ z_MfbOWKGX??C`k#id|W`izSh&dhYSy15DeAn`lb#;>@I-gFG?4f7mPhsj5lk!vppOKG$>3s0PR;_nkYdr4 zxo;KDhcE=V>kk6Ts3<~9J~x;!3=Pet)WP3CBB-ib?fAp#A??|-t3Ywh$fyQ(Vsk&6 z$?enNa=-;Tx-MD^zQ?n+ujF(3!5$`Gw2k&ERFJ7T7ya-N8UWTYtFesO>txP=h#gM({MZ2*MMj}V-k zB>c9>fqAJ6e?|u!49Z3?fHX5N-v;QuEc+DaZ&oIb-%?Px-ftl#rMAQP;`{eRfBzr6 zCfTtg@C5*WHbTMxeHTFJ&mD<%_jW+f2?LbxGx-Ac-`{q1h*379$<0cTI}K}Q1ir>_sBVfrx@R2mho>73o^}9` zrJ2tfTcRUJ3=IrC54Nf8pUn5rTp2}xSx;Dukk#j>{(($EFBj;jQupsj{pxMxo>JJK z-~ZJtbBg_@weir0)r}2`+Bznxx`z)RV)5j6@AL}$WSVLm(mi`JUIIi8-}W0LK=l%< z5Du-;Je!T-lTN!oxOl&PEHZ$lTgvG#vFL-05uOJF2u#OHK`)BjeLdsG)vF%>U7P%h zZl1jy9)3gBp%f&otQ^BMXU^oDbs#3jXAtwekK*MO?osD~D|Q#@p08F!Hd9M~7_^hZ z67e_WW2FTj?w?pVMH3K(8_g)=n`a7`3yrr5b*H%M<|MV5dJ*SZ1uiGS9~N5TvS7wW z8B|z?Wbpjv0E1kJ4j~uf^xvzOJ1={#PKZ@+Bm3SPj&f1oTV7iM;FhVBPIycZ z+y~r%4{2`hst{%f*cgaj$w}-NhBOqxe9G3=6b1}1ASu!0Aq3)3nGkrgRX?8l$rnX= z)5U%XNS{V!&rYUFmR6Q5A{=1+Wk30q3s7I;6DM->4wis3f9qBVF8*_bLrQ8e=z6W; z=iXgxn5chXVUZtB2|v{gKqss|jMdO_yDs2#0{>2MjCHVIv-%`PZV(q&ti zteSL+R#qB8#%eWM&2)2T@?He9*prnH@(G?Kxr&?joadpe2P0^Hy(Wa4 z!-4c0+IT&OtdUHg_OT=u8o$bhE46nSE z<7g9Zg1IItnwT{|2*51_I~ZQ~l^g$O1{8gS%n;pANVotFiy&|I8r{oH#C6?4j^9O?E*jz&PzY| zpBzjAtP6%HG(t}Ff~g-q_<`aVED_)r!Cq%=n&a}el^ePkV>wdk4|+$Zr5qv>`;!)D z&wM{8$UO<7M=`IF$IBzW;02d;EP^Y&7n4J7f7f=!!UDr46{U5!bK+GCjUYEW6B7`5 z?|~8!=7}_7p3UI7#VpfUe`FulO8NH5_>Yh9Zc`nl`#QaOhrZ_7TR08GI2i5k^{Wx*+1;G((nxZoO*~3Np)iWoucf4BzQ|!ce zW23;?nNo?+O4n34tTh!{fe^Z!=$n;Nw10f6?^?j@uKPdUnnT}48Rw7nN=oQy!K9$`$fIdb`#)bZL>m`hY;&$_k+HBq^If(wsPyaO4YCm~;xp9?}98KPV!LE9Fs#x6=! z0(4GrATiE1>JnGIx%!6SzpbrbavTyJxkhBVKL?&*b*IdyB}R~`p4YpPq@caeWvFi{ zwq#ej@WaAwcG)pAOVn&?-)nG?)A-KRN5%i(-@WgX+4h~*z70ZzDv3LzBuB)C>UNhVQ9iV>j5F`(YkrxSk}Pzu^vw`!l}jtEMz?M|?}phO z-!yjmaweR{rN#nuIu?risnhYNQTiJLG}-)?q~dQ7D3>*PT?BzSmH~`sf-hhhL1-bP^EOsE zmF(E({hYO1$BYm~h)>Nl`Y_sN$I{AN7_x*o)LXhzyOF+gO`|=gpuDTk0x9!pu!DMu?vaj`=0J+g*yhlMKcD{ur{QZyM)*rIUqZtq2pfr6^%H1D< zFf&~%c>%BR+yF1oTxCX>{y-yLS`I3jxNXC9^auaKDZTx^jRog({Zm`(dD3eG9{|8wjCo}~W;u8xM- zTTuffaXj~3lc5hy!g3u3@55U7^?+^4J9hCMQ3gzY_UL3=X`G_5Bcq}Fe{(X0eKCMb z|8uv_cc3mQ zFyfw=R&(-&-WJ-EC&x}CgJ%6a!HsQkV1|R;4|eQ^wMm1|%mN5U|DYx69GYz=0dsZ@ z50x&9)30PO(z}T&+M&bgF4r}`BM%hzuA0p?FnM3k9Wq# zuKk06UWal)!?*o!vghc&2l!^sYwE7zqCm5cyxQ5fiYgk2%i8BGgcl<(}MqZ!% zuaT_s7J-6RhqS*LOYnT?JT7l~oM7*8p~^*>O1dV)qs?oSf!`)2x${d#f4Q5@G)?rG z7e!Z}dK=scUJJ{38KP)-hKJ^KFjg|9q(zAJ_&3wZ^i?W*9e%9_>QpSEt`Cv8_ta;a>DyHbH&>e>? z5m%a&+as^&X)?d#BShHpJu8ZD@->tGCPe#`(`vxch!8RC0wy8mZFi&4H>9s`G^;Ax z1(YCfKGSnLt{V4lU74`ab(S?hI~uIhC@-?x)Oy_SiVHr~Mw%0Sd!G>VvLi@lFx4dm zu}I`5E%B+p_y;(fek5N z7B`{*bx)<|tjw7^;$#CotzaJZlRb&1@F(+7`sLyNbK{lkT0Ay3HTl(STZ?6M92{0t zQ|Vw=9{D8W?c4AZ($e>v9|zT=B8qnQwu9jMx*zR15RL&EuKMG7uU=7}4MOlcqrqZ{ zSZoDJp_XFfo0bj*&%_Pluv4Yt5hk@V6r z1Qw-WW~l3#S?-+QG&m@4gl3kPd-Rc;l2G}rddM$2YZs??XQNv zV6H*qH9iZN!?T5jO0XGz{z>ka%%kbXKw1Dt8cal13_)8Vx}WIjHHWA6JYuolnc)X7P{ z{#&xn@zeJ(GAXTLLKU4`K0R**>H{xX1Y`kAZu@%^cYGNk0fEQnaQxS=gg{*$EHtKT zAB2_hR!xO{SzUdQt!fU-onbmerd@6hT+#lBEFbSXkmc0r>LVN2&S3g2+H!( zcCkY^reXy6L`6jobhM}^Bh}_As*?3{prS1+5y{OFrl#f(oLThoVdUTleHEd!N}QI8 zHnsAwEZYUv;)j&)TfuZC>2fMxzPxnbDu|j{ymFEe?t`bPDs1SAGBwTRJ&iDWr*>hf7;pI!>WrBf_CHBt+&ax`B|m9Z`2R zg%}-{nm8o(Y{~MaZS~Du4Fv^Lx81c+v5}WwxI+)6*rN28!Pv{x5?5Enp@aAaR~^Y4Ob10{rFL1Z!BlGdP5EC_8FR;l_&G|Ix8dN z`>c8HQY7q#mZqklz(9DCYaILGGmG8n>3uyzk@4r-2Px)qPk>Ok zbaG17K}>VuTj^p^M#eXbK64V%paO*3R)HgYuSxS78XqAfLFoDZUD*ijXMrw2uyctG zm%J9+gz!Yosr=&Ub3WtT=z4?tiI?wM^>S$vNuxgoW#0NEQ;<~ZT?7Zz#dr@0l-4$W z(zsjK!7G$8*Vg?B$e=)`iOjjRVzjTEi{P@kc%%EvId#^_dfzvwS8~~ zt!2AvfDpZ`Kk><@?n&>teiUfM#5{*CoI2&7B_D^Pr>5p2o3aBPLqJGM(%1r7P7)=* z1LegS2`wIz0%S8+7_zZB#x~*AV-rKawJtOCt5*-{LqeJ&(l03my!Hj@^7sCt(N3Pm zH2H)%+sAjLzWVu->sB~P+-rI?@RJQ(<1GTd zL}#lYuZrd%nAR_KJKaDKy_HfKIBENu`^Hs51v&!NGiL=rPgB-(!n}NqH!YV2C3KVi zyjs4FF4!;rlr}Ttxqm-Iw^ChO(Jn1*T2)Riz6Uj4hD#LOo4}8p@P_2q9Hmi+S%5w3 z=f|3n3*2)(ivUArX68h~yen5^vvYGJN;HASEae>fvr_vBSyu@Xz9buS*Q<12BzS=2 z!T<6;QxQ+W{r2HvdT+}U=IGeZDApQr6yrZr@`H2)>4_6w@K}%{;A;eoFjxJ3^Z4vt z`GhIk$3(gPjp3_~j+FAzwp{EMadc)!$MZZP-kHf>qq9N1us&;~ zj|FamV|dyqDp3VltA$XM8L!bp6IC@9Ys--1r71@75yYm-kTv7>9QUE}Kh}2X;#aFe zU(}Ltu$e``_sF{!fE7l*SKeA!s6>(rYI+$6ADoHZE;D1Vu7E&Iu1T{9vI4kXpF!+= zCR`)Nv|4X^zy^o8&KA++!~{E=+~A^UbFPM|=@W=z85ySZU%Dx=QBW#1yq`urb;err zgg75z39=33jJg-mVNv|!qF3hN^O{NR26h@|V!{wIwb%Ak-^Y&@z&}*g-e+%j!v-!^ z!5fxu0f_Xa=29g*=6|V<>hx(H-OZ)i)?dvskm9L#c7fHQWrF@nw?PtcV02M4yYq z&mhNRCn1fM6n{B*RBfRew0Jaa@IP_R{hZ2%J9{!B>UEHW!LT%9&-_W0UPxxyoOING z64#d|n?Nt3`4k(-Z67?)dh*0?{RH~yQyZuSn6IRMTdYn^Wg)`6oLPj!T+on|U_a8G zJsbau8?5T)laiJ@9neO7sPlpkD;-Qu-K5@oRMgc)iOoI@pne5*Wrr3tr?L}a`H{!0PDS12kF+xx2-WuZaJijE5%4Cj#x}K>Vs6^Tn1QWUI%0V z1STDs4;aho)|X)5LNa>%y1kLGIrw6iL+of7;EZL3n8qzpPc%W6Lz74yU5breyjfiV z-bIlwZV7r1Tr5Z`_@qJb<%6t~LU&xc%PXKRyX4G@;QU@jDeprO5Lkvgc9DjrXnABY zRytnDdAv2+p%LZp?+-$X1Bew85_PMT&3?|(I_1j2)=kCzI>M>dm6i4|Tz_bB_Vnpe z2b0b?A##cf#oyt8G%cQrVdufpMab+BcHn*a`%5}iP<%#RVP@{dV6>~AaMuI@uPRhZQi;HVA zAb#XLB&!m_d?vV=YGfNO4e{!+{;i9Y_t$chnO2!E`w{R;oISo8$%`$Hb7;zt4~3@L z2>;f7_#l3`KVyyxy&Y3zAc+CZ6;v6pS>1juaIx>wMPd4hlfroL@GvqnZ(G_vWl=`P z&*NxNh)g*m9Dt{e_oo8W2jP&~bZ7hbu!EA;><%e#<|T-gtqAcRpzrzAPv1u0@E#j< z$a84U8-HDM^5<)4=hExT0~z^`4%v5Q~un( zG-6&(3WN*W56W8E;466E{9B8-0VdzL`5Aa5nP_*x_Owa|ng1mb9j zNnAO_i+vOVF1F-C;Z>LP97x+A8ztY zONowDCh%6ytWnxBQ+=7cC)0rH$bZz4@0 zZ{+Sw2e!k0V~OstFUoVH9Nteb>qgSJJ|_r(i{TMcI^mjAUaVVhrEkojn(dnIINmmG zi!@(D~I0~_PaF{ zhk06ys2hgX^5-d(M2gZcCOhGl2=4h53C3>7T#yl}9qByz*sME`>Gv|x2azoGiT)m_ z;EENgrb(vXA!qX62nuVLBw;}UwY#a?D8>g8AH7asLx;OtFyeuHg0z6Fg!7GF9`Y;w zR<`299GBRRdGk=cy#Ex({7U5d8{D=X8n&08j;gByWKV23Ta00d-h5PU#srxK`pk)U zd4cbF3d%3(-4+$+!!5jUj0_{Qeas+>ryEx4;wG&H#&^3AQKoSU$U>}5v`$opOcYH)S zSFhQE-qD&;fv!RI9E|{}**O&QFF_UUe+5;79rPw?O_LE00@Y9!E(R5AQ!%@GQ zNnZHuRU+hEaU2UN?_^tQB@rb?O5Z@-KlQ!1C3cqn;XXxS0f}ZuInW33Tf}$;Udf3} znO6m1OV55m4=Iy=9Q8i1sS<)h${WrmWW*iWA6YP&?sT=*G8atdIWOwQR*3X@v7AT` zO_}IBa&E?VLd!|mjL)jhgGcXQ@pe9%B>J2(*`Eqw;*4NJ_Lp$p1}~Xx5jFs$gL9(0 z1D;TxCaHR9N#O5@pmyM!H!Mn&pLt;#a-2&0ltMiX1~o_T1f`qf(=H`#4x>@K_gV+e z=N}4c9cD0mH|*`cDdgB|FJ)xORBIADA~+XQnW&F@6`OO~O?_pEX4@*Ty^{$E<|U5w zIA@w`+HL0JRQP`?Kk>{bzmYKEn=q0~bT7OE^l9z7#WXMTFcGW%jaVmRebXr+Iy^YD zVFIB7)$^b4<@>kPIrV6({bWz|JMZE#{h8UXd6**wG!vTKZ2ZyoER&I@1ir=KTKpY2lGBWP*hF{)$Rv`C=lO@AL=K(^K6T zpWVbgTfSa1C3ny;a0k>tLrpz5*oYhP%)j{j_Wnad6JAkpTilgne0C{ZR1fW%8wDnF zM!YHN0dYQ2mv~DOdBsDx8c#Id6+g_f3hDnzE5M`*#rr1BpNscnfAM~^__o8lS(Yus zRcTBq7B?{$^*wvKhA&xQysnEia_t23?@SorZ@+VPzqG=9x%iY``U3{}AU4_H+mU#L zHU6*THAa^h1h#T-j7a3zY|5{=Q7^{P>d|nI)z<|{jGjM-SU7KGDOvLE2u_m1w6=4- z8w}0QuYtedxys7&+3lpxupj5(cbl6dut2-VK5Ty{<(5kuVXEMsmL&*Tq*FZ82~wB} z2TkA^?jf<&yvkVxV22gtStJj4q7lTTN1~WP)Sp27FF+$UMQKKxpyp>*dJ{uTDAA< z`XT?$ub_jKPv7G>h(|?VYL?CfSFboR%g8xCjpO7fMv?{$=C{#ph^fkdEU{hRH#Q7P zmrQ%6et5;=bnT{CpG@@Zu5}Inh(P$+{ZaeV2MD*W_i*0zZgnE)Wfa?HM zn%uy^Zql>Y?(~_WQI24%C~PpFZSHpe^a z9$^c!{E&#OvTro%D#?jzxA2YDQ#d}TDD+Bpe4?p`d3kKnpTPjUzueGCLcGri>Ngp9 zuj!dHH`rm`rufw&>19pP*TyH)m-hFqEN1GAa&O$TsqE}?TVJ@R%TIRBU|G!c+n4+$ z*U)%h_S?5PaGlDFJ5Y@G4__fNP45f5(x3axmD}(2Sfl7z-C|8XYIdy+=f~jwpqiYz zz);@n+zjEPM0r&UJOXlDT(Sj2=&tLDOVsUyVZPrpX?n2(rgBXd`PTq{yvU1v^G40q z_O4>*CY%`ZRQ|4>-d%-`AbCAKq5*IXp+DI%*XGCGrlZ0Z*G#s{t&g{hP!{E|DzEDt zw|y+^b+8Hc1HhM5JUx>d5Oc#2-1VX}#F0oRjeb@|eR}eGXyOAXLW>7=t7RzPh}n*< zz@b!@e9I`5R#Q`^fUZjIsMt!3jgwA=>e~0FA^@6H^IBi2%HKyhZCV@Nxf6+=uCh0F z(j0iKJpF4?2r!+n*PCJV?sguFPU~~6r?elrbLye*)L5JILtO8Dm8hM zyCHEg(Nx#60zq{P<`l;J7QWX=CRk&z5vb_+U%oH^KK2QztvlBBRMoI7c?KjC z=W2}dkJ45boQ$Fg*WP(u&$7NRzgpLswzMzu7?b|t(&e7h@V&Qa7Om^CUt%sn3NibQ zNhj_bT21s(O7s*`MVTC>WMhkf{QGzAd;!XbV2D=rUNSmHSJ=o7$Xak1UxTqiZA**A z!JfZ{M*p|)W5@iD69$Rhx|Mj!@FBB@r-Xp;70*Lnrx6qFE&@7EOBMH6RmqXulvG(b z7{uggdV!ALzXd4n!onjJ|0ZQ7(_zQ&}B@r6eXj>TW5F4htxBM)E5 z@Xil!MyChg1OPED@OtSraX(~oWOmS|_Kl+_aZ6=GE6tljI`f;4r&;i=Tpa_lie`=d z`{%kITKvlIkLZ%nQ7aX$3eFdDsuKQh1h^-wcHpmTFrGJIt7O|ayf-4Rd~x$9WHa+G!S1FZT}>&qV;UxgWVkOhU4Kuv--z zOc5{af=MlN$K)TL%fGm`NdbOdoLperA>22x^BsIi;c%3KL60=~M4S(Kjad zU%u?$TtYZEe=#o_1In3kU24d}l=+v+Tb-SEOJl9k3Jy{H9|SaNp8? zck7{yMa{`+9vAyAd9nSWt}dsIdB9BGBry-I>5ijreU9?@OhbBKS>_1|oG`1`d+(Fq zJneyN;!Jnlc~n+j$Z=?Jl}}9z$k7><Xd zmnL(supj%rZgQv~k#Y}Ol6$lJf-i_HJ!81v(!#E-QN-YR74-iKmJ5vll>%?d1PH5q zF`pbftzUjhKPya&(~S%t&MPo8w2k=us@e*!~7v_EqH`0RVu>+m9d!|>_b76Dw@ zWt*fH0lMUjB}h=PzvZ)1)f`x$;h9cL_X>*_}LUHSB9%6lb#{2R|8=IQyN*nw7#6&zWFpJ>fDY}BWR#Etq zo=wnm?+Xm|8d2jLrU0?SQ7ry`cZy3zL!QsNmq!mTMZ76Ho@-30Q8YGIfWb73kJl-R zKgvlQKS;T!1bObJ)4~%E4<#9y1vqOkgyFCgxwidlPged)A4z28HLhLHZvS4(Z{~FPh-cwro=(+4dc8;ii&dh`MrHsPjvsWmfb4Fhd}JQom%ez zmjfZbv%`t~F0I(hm;rwk#`rAq2iJ0b5SzK%Nst=q{s%Z^#>GzXU@1qD%n0RN}%z5Z8# zRTie($fX@`Y()7~D?0k{+D;2_R$mml$aL%5Nj>VWlc^=Q&K_Hhl*Pt+jlXIq{>!A# zwG}l!GS<7%oi5M7z##Th+O*mB=IU;`@G^SsS006vcpQ7^ij_y(F1hZ4z;?B? zG<^%_$%rSAQ=6M%PQ|scx~lcwK*htOyDPQDWh5jgdgYr!`G>99wm8YF9+Og54<82B zc#XvgZ9wS;KL(@;6zalp!e>c%)>~YqdfLS9#=4c`w^CaD8YnPihe>7+3U~_4CnZu} zA8#y*k#82wh;Lq9$6TZp0H(trP4O+&!N8cvbSw~ZrW9U!y96x}4(>%b-TajLh2CvT z@Dkdcun7;t!DBuNL342XOXz2t%=tV8brzoqv+&0=X4Xb%0(loNm&3U0JWx0$#m8;! zcfX}5CFLM)AAGNWO1((UorW#pH>QUoa6L?nTC3g4I)OLgyaeY|&)D1ld@=$Pdpj_M zH);2zAPrJ4VXMelIFt1}ho-#-3hRfC@Tj|+4CdzMa6ag$y@QY}ef!|%GPM@=o;zxJ-R5bds6{$BO8PJ zPB%8rs}rAOYBu!-c&^UB<5TqH2fr@&AG3G$9SSfqcO@b*$2HD;QR2f6=}gFB0MJ8| zg^yj)gUbas+Y%-};O_6dsFtf`K6yRz)5RLokjbY38e@GOGI&+}C`O8jN7Mlql&%~W zu%%ydVU^aQ65$#S-3zsBZCV!oJ0ctN?kl#)}&soP{0Tix3Czs&@!bPGmKJF64-iROJqnmIE zqughuG7GAH#8!!sefFjPYE6m9tgBS@SXm^>QrMutd+a%5G%r*jr;5tX_J5g6E@x_u z$_z9YWD>ZmKynM$>CexDVOrBOJ%in_?}BmEc_fBU5;#kOVy9GBJ+1^0;8zNA@1Ih~ zMKcz}20uk8Jr3s0<~@SN_70kQob}>zc-7QMX|(8KJBE0br~sF<1!M2|%L5m)&1*eK zd=cG($^P@$N5SQRCvZr0yf~_Wr-yFcLo`_VAe4-ZVhxVvL|=M@I;I+`{8;Caq|@n~ z=r)_TG9HAOruz<#C1LNc)e)m*c~lfROhP7)@2o-r>v)t@+DTmnOjee%q)ucHZp{>4&im>mzz4_ptADNwR3BYzy9YVkgA+e=MhZ$>4N=v>ag&Wx?vn7gcuou zJ0CJ}3k%9kQa_`{KeAr-#yN?DwfrNw$EDai$USDe9Y2q<&0~AS#8u} zR0Z718!EUJnj9KizF==N^7lNhfU2HK)i|lnLxpL|oIjK6laxWEL7E!KQz@Y|a}vgM zM?*(*#)?AZ$g<>4NKmiC<*JH2P58uk*9pnP5XBSBDp;_Y<*&fVuU7v7HUrdI8x+1D z$rdAV;+PvT3|k@I{Ijkp2{S%IFnc$YWK+fmXo;D(=((whzRam zg0vSaNTv?9|F1ahB;Q|v|2VfC6^D*tT06p(&%^=}`FH|po#lV67)hMEjp+=hZ`clI zc)t<41AYei__=3!C`Nn-9<~4%G9V}$w=h6;D1`(&vq|{`RoB&_dgXTI#xL&-{yc3L zx1!h5HyXQ^qHl^i-vcQQCU}BODtyn9Bn>zs+1en?&lLPI)iq}%WhnK}?T5Ka+thw{ zh^2?an3n5_H0nN;mgq@=muh~;F0ioRJ>^G;e{~e$yoJTqJ^liq`0o=Im=zJdom>Xv zu$!*}p8`K7P;RJI7Q56mn&+>Ghu~s*z0)FP+L^M(){N%auF_mvbLU!9lkKpGh2blUIunJ7LCg4j7x`6-hVt2 zQ9zCUOqW>9%jRLt?$4+f;xG8L?aorw#TbfhuO$ECfdSb#eXQ+kqqSnhXQ%76jh(T{z?g#4+J-MBso*GdU%D59AKT=9b;Fk1cc^t4!7hmwH5*+b;z8y}a9g9PQrs4O>UN_0mpQAC}cvhYXD3L5hW*TC13ArxDMRd zy|1xbbL)t}rmflhzd zzRJ~}N8KcR^!vcr`51PH+wCh5FhVp^{%x%aQ9f`xOjb>&Q21 zN%%x3PE@#+eeUb?nre;(c=;Pj#O3ET78Wixw(@uH3X0dI)43&Lj@a=*!0Xe?t8~RtQx#|^Whow z3`;*GRJTNtrV*bsluuic^aqjuP^?VlE*%wUU-i| z(P1YkFN0<75Y6gZOfN2O_NpDL*>_4#P0a<)D4gncdYp=qrwiz~_o72c`Ykd@?*RSVqDi3wo)&NWyFS6@0LPrfT42Xcxrk|#a7X4rW~7@`7tUYf~5qxWrYKpkWPSzMI=f|OPugNYEp;d&8yB<>4`QCM7ASK1Dq{I6a-96QMl=bttaC zr-c(Njl){9Y)q-;$ll!kE;xqkDo@1tVJv@s&jidD9LXV=CZ%w)O*W(Y7pp<%GEy}d z_NJ_~pa5%^3e4Et>p8viaS4YmdU}n!%k54AH*0GHKy{LnGxUOzg*1ru9?T6v{{REg z+Ijd}Nr|(c;8ifr>fkYU1}1pp>FX?%|Jh*NutJB`osLchxY$ScjfSez{Y0#3x0$7T z$Cbx7m+2oB8o?Zq6~<^yuckmpQopG4K>^sXVEmt%A>g~2BHkWd)S~ID31-5&k&y)Le7i1{lW`$*s@&(6e>I0%gWl* zFIMfzlkZ$L$_Rcjwrkg1w?K5j4lvXA?>*3z0pqn8wmJL>LWXNlL7YB)T9E)>#`+H~ zKn6MnOQ8M$+tx8V4^UHCS&E{fHDJ>A^w4a1T>=|=kcyY@?X`o1Dv&A~xXd+PH6IHK zh$JuWv0I>%lBIhvBcCU(2@3X(&wh%jHT}=LuFp&sE6b#(DY5YK0<I1RwE^ZGW-nX*SmTA2kUk2x5^TU8o zr^?kzUH!@Z=Jf|}vVsDusn3V5d_+ozoLL^y@M|L#>&q!#|B|UhwYq8uMom2!3_3R2 zM}A1@^btK!KTq$3d95z+P|tAT`V?8R>!FR_hw(s}BkizOg)MU;dd<)f2+oI`m}ATR z`LkO^MU7c5)_uAagdf?kb|Y1UFgFH!hF=3ED40|xA}U-rb|93f-?P~+>K z1u=-YavOf#_4luUcK{+2_)^WRt+i@A4evD(KTF2Pu0{tTg{J5}uh@+nQ$T^=ng}{? z+7zQ8m$;s*<%mF(z2?#lfBV)iC)zfbrVwdnHrJkTSXEG93K|s<6G0$<^YzqpqbAQ| z%B!bM2#n$AA}m2ZZ2>Fh(!iTG!UHW_oy<9b7^zRi0pDmuW{;|osTwim^$XsARA5U` zfl&cn{uVbk5@dd}wvR!M^k10V{uUJlh>FLVw8O8=imIO6_i%BU0{>*_1VMY!7N@3Y zS6(%|IvHvVl#oMST^RY!x;{f{sq`AJ>miO*b|8$iF?0ED`xr(@tkXGAmUuT=!KA9hvrM5bG5rPRYe(Wx* zTLdURj=5<2I+qk9bCy(4iNnaAmw)sDw#n6q!@$uV_2aW|gOf>mvP7qEcQ zg!y}Q@OJv|8WvCiEmysV$OpzTP!|mho}V2z06mtPYJ7=$PO?-m#J4or2NjSTfNl?3 z4p3F>tP7SRO^s=E5Ki!qPZ7KDB7v08$JZ8xV81rB@gu*6xayqXVlztLZrVJGhJ^Hg z3Vu*6D_J}JE7hFo<#xBcSiW|J&RcvlJY1F7PWQf+mX_l1j5mlcP}}=bhu{E6Q~%@k z*tJ2DaXy(^&VGdjqb&c0P9rSVauq~TV`D1-bB`A2C)XQ*x77ex$-(YiU)MMx{j38O zOi!b$=A2)?V-E9N<0*|q1kmw$7Am0@UH3k$d!{Qq$3T}iKd%c8+Tf1pGEz}B?33FM zsXswvcetJFWDPlvOY!O?3nR})6N}b^2VQqzlH+#WPG(NCsJ2!cqV$y*?s1)`2nm=m zmzk5WCxJuK?e@sw`aiLW3y`1sdGXTVnPzV}#~;V!z^PlxqRmzz!%Geadt44PZIdq8 z0$59PAWcp-&7SNUfnpofr2yZa_zXjxo44jN<-CV-5p=T+>B`H~0@m*sDB2NBy2tQL z`U43NQtAQBbb)c$G>9TQN`rFPQ@?_OLPb1>4yNAd&P?Zhw4c&++{hPR*4FVXPx=0( zK))2mC_0tu=g*u0f!#Ho%4?S|S5{ZwGevXi1RX@h4juORyoZJmAaPh+Z11FOUWsTaKu8!^z%do7 z+57`S9y_hSoEpBoh=8*bH-R}6&BK$D_24Ze;c+@1$#wxsjK2^s@o-l!_FGpwDbq;U z#w;!+i}Qy9_Nevvgz8iTZJnC212^|{tccqIG>ssif*hb;pk3UiNCh-75F-EHsS`8; zp%EC_cWy!9_7p++nbGpJKnn&B3xLFCp5+!q#Vn^{5tp1l>r|d4PtN!L02$9LXQSKZ z%>?Pr4qLBqx22Mp;5TEy;cKuaU^pov$Llln2GT4NSmWC zF&RQ1E2TBqou}qhYK)v7E5mJ#DO&+~5DcvD-UO7!`~^*( z?B3mu#0lt}pbo+nD&eZx+X30h;NW>vvzSJQ7a-B2QAKX0JjxaD1(|C+YX!($#9w<0 zMq`x?GOWkD!TRvIYi_Bk+qrsu@VNgIP50A{ke~zs0Z{}LBo{eDktvWQS%N^3K|xT0L=lxF65lH9KDW<3 z=e*Z<+&2b4T5YYb_P5vCYtJ?3TzM?ERsqt;N+Lg(%BMDTcthnkUe4fI3$Q-(IuzoP z`~hiPyb32+JJwMSXxr`~%~b?MFgWz0#RaFBJJ1#2roRW6)@Q)tV{uy5vSbVJ^iTTU zay$!*ORk=)P&#W8&Pt9bdpZ=|12po#9U7QT-+)7fRZTfg4)n*`{4@34=}C4Eek_2X z7cNQ+lZDq-Aer0Kn1n&yGF6TIoG%Txy%YcTNsNOpBpx3Lb&B@F!sdN+`n4Uz`_`X& z6XoDrpaA@Gq?OWKA`-9=(b#huB%=-2U z3-M(*QJE!KTqGGt>nRZClMamdgNzo||Bm#J!(jgbau>1nUi?@On6C~thvc<5X{)cx zEY60Jo8a7@P_TW{DAsJ_w=awsAOa;_4u?M>+K6&;+h#gz+nZ5xCCJ}uq$19;#A5og z>lhptJ(caS=YsB#aG9i3`A@?73h$TyN#OB^m|ZU%$o>1NU*|hsi-|sE@0KdH*;}1! zkheTPt$hAIfdDl~b_0Z3^fSp0aW;rEJxkJGc4ug0s}Fb&yAUr6@?NcLDV()HY223`PRlb?(;lOz0N;sx*WFX~hxA`J4j?KWP)%&scpWICqQ(KJO|y-YqS`)h4nQ2GiBMbvInDr7(DhU4dK`iZqfP7pS=(yKgZj13;+7c8| zsp2hBioyFUAZR3ahf&8C#7Q_s>md^j#bf|}5N4}KLa1vej(ngITZ4bR#jz%;_UUSl zU$OWKh3Iu*Qhh`cA@3j3p;~|FdLP%ZHTk`90J#1Z=|&e=;f*`UkK+z<`O1+{^>>Bt z*tpA;InQ&BB&_6Vt>n4mkaBX+4k6yk#R@+W29FMeR>qMD58NXFJ%gRqawPCe^|cxz zds?wl3(y3`Cad->9Ia?yBN5{K7JW_7^If(bt~*TG1ZesNY4%|g8HM6jT8Vev!z?K1 zr%@}e$-lU&HE>1T%Gf!EoxkseTHZ&VTBXg!dqs+6F~#I%IW4$eMc;zZ2)D$;(^G=S zCWZr4m^@7#PbC6&edHJ6U)(DW0WMH`z%{6n)y&Kne&8u#t%60@=6=QMwj#Zpb1dIA zm&YDw{Ng!sN+|5{F%vz4%BL^B&Z0ygw8fwH&A|zUuyTZGQr{L=0&UF+I?F@7)nl1P zps(dB)=qOXfv*^Mj3YuvWI*)gNq10R<$-h%TTQsTYD|D$6G9J$MvOU-A#^DT{J>W2^Hm4Z>-dQyT`0& zYI>>rWLDkl*D!ZGE;>4l1?NlQRI&vI*o?G8c>@H50egD^HM)tvLRJQ8$D>DWJHOWw zCvGo4c3CWQ*xEpWIN%b}glybm{#d3-`{uN+$~-o+W-R@|`_m^CaZ-VFGm(k}$Om?B z)w#cs16CK(op8=`0Ger!-@%zOx@5_x@R5rtJVIF~Tq21K2a}Ne{jMDLqWvrHUr*%a zKm8^zLa8)bd*94p)C{u%dqK=Y}43?nh4Q0jCtQ;6mP;4*F15=H39pEYRWUv;5) z6Qgmi?t#!!F{ee*EPLPV3w^39FRc=CUX0R-V=b-fq`Fq%O;X}SGN*+V3tZO+b!KEa zQcY6AqpZNJ0a)>0-hIPHMGg7vePq7kY6E5tm$b5<^1ik#G|1(87(}0ebA}=5q{#;y zr16P+L-z3zRIty)kYgF51kEm2`kJeQ{2eN7*c#Ntka=)NwVQLR!K-KMg-4q$KIbHp zDV4?O!mgV7AM7f(q5o1&wf4>EolXRofqJ2x)vjW4^7>Y_JyubtTcA;J^^`i;6>-3V zn{3|~g7dvXUbDf-tyd~d8Q4A-tZ6+}V|5>egzQm1kuP)AfL9CxhywQmDEiInz1>=Z z!u)@dxvmKosYy+^&#TS=j@-jprQsbp*YzYX>55J;qwK*7)b9rQ3A#3PMs=POz&K5_ z+24Qk$s4l!(QaRa6UAI8XXodAPqVPtV>~Xf!>qPS+pX_5wjMMt zHxkXhlJLC;;*DfkM@*(jP}q+OWAf;zKS1+{*t3 zcXUYOz-Z;OZ-L626yL5ShlL+p$5rnqD~zI4QY15~tLf@^*Oo_*Vvtr8ELYaSQycK3 z=PX=2+~bdX*I)>fm{>J)yNBD}MUN*)bjn`o#=-XO1*xfCU&_6C6K|>DiYCX|=)ey2 zUv&>wmfeMb9VaDmk+Ji=0-#!Q+h#WZv9nP}Y~6$Oc%Z*8X}7{H%#??lbtF|l)_xOU zn0H0P+$P)RU<{$Fs|t53z|&|e#H~7qbn;E#mj?x9!##b>;Uloy0GBx#y&S9(n`m{M zq<~>RY%nHOvcyh2^rJ}^o#p5|3&lh9o7Vj}EL=YDep(0aWOH^*D7dT<93p1_N$>X0 zJ^ldoKlEbPPm6Bxl&^PhyB{<*-3|M3+TvlYufN&++KSlo1e|0nl~wUcy*R(!d-01Z zduDJQ=ma(DC>%HpS0C`dLIU3MW9=xLFUS_4R1N2t$H#i-nK|ft0pk&f$AHQI)(Sw6P$`C6+;`tXN$CTu$n~&i z^B!JGN=$}A?m)Cnz%rc_b)L@I+WNWdo&xqbF7rOsu6f&2ExPbWMu>6fo&lhAZ-2KH zpuFd&c^JgX5DrM>X0^*nbsp%p#KZ_kH2^&ociH9UhEMbGu;=7dU3ees=?+$ zf8_RdHn=i;e+}cliqg{Zzyt?g*U(TokTj>KGeEdu{91pn^`tmJxZuELfDyp)UXQ6jkSTX2cDu5AJ>zdwFu+<=ye>F0NIP@H?L4jnf8#(?)(Z_`iBjIDZ; zuRvio8f&MiFa6*YTtE&KJ7WZGZbjb-8CBK`r!z!+zCTHcb=-M@J8iSvHT5Z0`e+BJ zvUOzWb=TkE{0bfcmFsWlR+$0~ZLstdm1apIOTGok_w1(@cDmZ=)k2hym+mW#oOd-n zaKT0e*MHmenhB$^?LXkTFlC0hz@il9iTTDf)C;QD!uA4&IsXyV#^PW*4C>r%QtG}& zmc~BiAx?jhz<%=?pB#=p>-ecY>s4rnyEEhEmJ zO)w4J(}LUDw8zeIsVGM<5}yAG z)<%Y45(Zpl=%~DZ|KOA3&)~2y7tt%YcCWT}cgmgUnEfi>gQ>W<_b@$CP*CuUeN4*1 zX#K+{f!F;9n=z&pOOJ(SJj5ln9I(54gkP3cz(c<$H#aVt3&DET1|p!*nL_n-dAfhd zmw+PCmucyQc=kcQ{pIWwjQSNU%6HKlj^Vw5ga(L&;jsIF<`Tw2jq+BNV@dyRWL4&0VCyM&HrM~BP zCHqZQ6X@JK?R?AU*nW*@gqYuqrlF^-)RMA?z!oV(`rkiS?|mhXU+a9=_v{-{PjwS< z2$!zB4HlfsVR&ssQc~C2CJcYL`)Ob7nSu8D-g6e>i(kL7z8~2X6cyE8cWOz2V z+`_N6H*V+ybP25QNjGi9=X_oN50GjXJQ`cvT@GUkd{kjEquJXKkTg- z9a#p0JQ%{rYitrnmAnBBRU2TDD8v5^tzY0%*SQyxyu)_H(a;XPeUo@R8ypDqgFBgl zUYSG~E<>2#ld(iR{6wCpXs4mr0hH05&#&r*gD4(;z$N7Id_DlOAF5oxux@~U`)2~x zg@T@YV|3OA9JrQ+YeF9Ry~_Ua*HDq1nzF>v`2U}TvN5)yGPr-df7T;!hjmvbZF(WQ z(?0g5_(G8SpD&{$K=fPo^6S=tUmj`lB^)&D=j-QYpPe@+$`fi4_yR`^xXD~|reqd< zZPg_lwLkSl)0l+4Ep#zT_W^NCd1I9`jR3;MXTO?xCrK-Zb9Hru{P-%!ft+2g-j#Bh z8wUfwz#{Sq=TVp&$-rO&200dQa*ZmlU=8nlWudiheb&9Y+IjRO8ZzA|yj@=2(?&eW zl`o)mQeRJ2S>-rEVu^j_FzWfaLnDz-nY7pN!TBv^=UpNDku)QN$HOX0x6!~xdcj?W ze~eMz>VpLj!e=ji*|(ZD@#+Yj<>Qg=D^~>41kK7}l%Kiwif>)i>$@%oGK2;YUEU_n zF+6+~k?9st6+XLnD{@8@SInOJ`^7AzJ3A~$d^J5gj@rje{#7QGdWf+h?ExvC)GhrgeG9$OK z9~a(^-i^e#$8VHN(BMi7FkZAw$JAxv7AfSTA%->=k3X{6oE_H#q&^$_r zm&MENW9|=0Ig<4+UcRI6q43)MIh@2deyvwXd~g5hL9%$Y?c)1aFWNqkZ07ik{8k0j zRPg9WlF=N9t*2zsC0P*jUHPK?^x36SBpI2Oo&6UMRpm(!FI81jP~p%L)t)zhG(J8I zg_^^wl|ly3qEYA>L&0Fxr8^I0?Z=bBhNIP(1pvO0 z$6sXjgB0&}TIXA?BlstrmYX9l_tYbQI&AV?2^Rek!rvYG~Ow9ju#ZSdy_uPN; z9J$;9y>4*0UV`HQ&9y{-kNE{A=nS{*>EA);JHa~=cu<c&D+cLF~3yWNOmWG(Ns00GbsPrrb z2qU=*UJlrO?@FYF#ems?Auv>S>b%0bV%!uZ5K@P9_oq|x%!8?qbjB$nKWt6y6|sdl z)?5qE6b3LgUbD^%V7k|Ls0!k9jj91`o_mJLjiaitIoXwfX7jpo5l{(&V}EO_A6l8f z3L<9hfW&=cMA<{|F?&)NXPFrJZQ0A0pvz}o7~f<58)rW@45xOuhchvB=6*M~Vhl0B zm;osMdp;Yj-=AE78hH8K0fWCu$q%$pct9v6=oN~zN|&N3i1!IX`#>wb{T`9@ou1(% zIB%P;-%z&NS;u5zaejPHScYz4Ys^MJ(?WB+#+rRt(=mDGb=9-w^Z$j$F{14;NX-O7 zfKk15k_hKTYiwca1=oi(^+#(_Ts#~iSrQfHAolrrEH2Y6Hq&IBKC&pR`} zE56$R>B6-|?ML~I|7t3@P98H9)T>VH5yU2f-0&yIX1^=<>fmJFX|*hLkHDBmHzCvwB*jlmj5z5FARzX zQAu+Guh{d*XrVHDEK(#Tv^ybVGuPAE12#nrhDx=PIPmf4&3ymmMSVaXt$V{s{#Y?Q zqW2)s0eY@JB8;f}$9nppUGC& zPz_iV%;v&B2y&UywF$hQLkSKZ5xjRQYCQP%${7evE0^vQ%5`OQzoX|{)*lbM zjU>_pecjh?a@yl+cn}0mwY<7j7{Blgq62&))@dEl{ssKmALt3~IT9uQ)YHM*+ zQU&)-+Os$bMp4JsXR<_0YHH*ljZm+4iX(#5NRQsP>1@`ZG76B&1)jNv}z%l;%YQk1Z+KlQFCC~%uR2E;O^O%UA(~S$eneAvpJMWL|)$Z zB03uH5keCT%+?Q9aq=%IUY!#Qh8U2$rD@6WeS#J>dGXWL9%u=4sOT7qS?ZWiVEA#h zFmuSO{S+Wo{UmrL@qk?7z2H>mW~T|@*^cL*vJDXc|4R0x zyt}j)$`C1i>E$#2$O}2S7mvwTPy-oyGC5b_)foTK4x@jAd7H@~q+Q6b&`2u_ zFReQ9(e;w&M=8ly@OUN2xS;$%ZtKxz=rldeSvHM==9}fz#V6cG6BvO*`%KEe1vDLY zkZ>zCcHTbd?!Yw^q0#ZZ;9#1_^&=WgXT&_R$2iqZ3GoT6m%F6n|6#VIo&-|5Dts`< zDRiKlO=4(?4GRFe{o6lZ(V@9G%qPHq-fRMHXOERBI7$BEph+~;R!XNp(j&_llzrH6 z<`A-KW?m~`SYnnPm0_3gxJMo>iuTXU?$OmbO>Td#kH=c>I!8iMkbxnzjh@^=`m zKms~BmlFHoRqks9S1ImSuuzU(WfE?;I?wzZI-%)s*q6Qq6f-B<6$`XAr=^AZW$8Oq z`WpI^+yoZmV5a`OkYB)wet=&Pi*qdl)zgd75#{&tT*nYCI=PfMH@nX>>NS zF_@C0GKNz4&_&xT7*a^J$A+`D932Eq5|R$zv(+Bv@Q6Mh5CO=(5-nsFD4MRuIRp_L z`THldA=QtZ3xy2PD*|py6r9f5#5dIrq;)(l7KGtBjkKxg@W4mw=;zWVPk0Lm+*P&w z$ZXK@pqhr>JhvY*WmaMPNz0IecsJM2z{s3Hv)bmGCZ6ju*!0FVGc;c2F_*oxaWoA& z?A8DMH4#h~JW61noi&)9bHDxxolhtSUSb$W_mI7=vfm{;f80LMu)u=Y3(kwFXh+Tz z_FG#6Y@$a-0K9Gvyw}lecp8-rMdAO%1a0GQl zAxHgM^H%RbfueOUKI%TqB~X0O8S_VK_@C}Ol95oKO3NI+0qY!&vrf8lO1BOC3Qi>Y z+cDyfO`Pw|#Z5L=pPt7ONAK?QbDt&UQw|Upl#kNpNxXKyNAVn~OH|Z}80+)cp{F?A z)54j$Hq^!>`H#~MC?_kg+270q|GzR1&~q)>EH7n_{g&5QoA2(+n1_P)Ow_1%k^5j7SCT0KYEuL437(9t^S@A8VAvKATE678V$Ms^1fRzIN{$>!^rXf=h=YYEG2We3>{2 zb42Y?o(!JoPd5BPR1ohSN;{mPBaqX0ptZ{3Q?>&0&hHcq=g>%Ass*?MN8GId8Ev%hzQg2AFx zzrb-HWc-MEQgAGnKX%E%vx7NrXGZ1qCVSBM_x0Qpnx z&bjny&W9_HBkQmBgieK=B?Wd+d^m|J*OSov2ae4u1;Av8B`F>{r*pL#9FuiRke)tV z(2idB8!$L2`h_U>^_5iJ9!0gAlLb$b(KUbYY_ib_mpHps&9nnsN@N>H`OEnT2|vej z=Vpf=$R2dcwoQ|S@B?wH*bfewxKs7G69xzj&+m1V4NGuE;*!WpU?u(ox4reJ1cQCK zM+^{D)`65aKLW{4TETq5bkgCXskxIhPo}vsqbRs4(#HRxG`z+V+{_{VFoJ-a<%}PO z@_}MLfn`>vc8uIlBxfm7Hh|C`t8L+vRkhl~Ef`%KNaZjYn@GOO_V&_~@uRnJ+8Dp^ zLIb#|36d6^&YQk;=m`PojOFEP;9dEJHuu#vj;g(L!vjSDw7UW`nLLi5NRSe+3UoX z*X8Rn*Wm`Vy>$tFES%0<(~Ptr-V^C*4|^Gusu*2L0~eYVtq9zbnqDc42UWm7e8B){ zhItX4in#zz-Mn$5T)zNk+8@2AVmQ~AW&!+OJt^1RDqw88u0I8UAKd6|J#1CAwT&Gd z#?$0r`s`2eb^kVsNtv&E+zLM6x<+2@7NtGOdrj@Ecfp+=Jw2liYd7(B0jDieXgUAp zNDq;y>wSE?nyV)Aggr$a>b<{BxAYN9CJ}{3+6yyU%n?bSJX&uooFwX4k9U_}#_iRN zuaqjlP^)w56n8n)0>2jY=Rbmj^c!tw!=34RRdAg`QT0;fhf3d4r>U1<@t+L0Z92LT zVj9)*N^!|urz|deVTZWF(jQHr2k_ZmEY2k_L+a+=_t~F^Q4FG#XD-9&OYlAU_VG?f zmywaa{`&gj8raC{>id8f6G*tq^b5=$>U;-}A8z`5^p?x>_w}HS#F*-XK*7h;8qlfmeZxiV!Lo( zSvBUFkWsl!UaJL*glA$)|BwA0jMT2vEa>uBo<9c*mhZ2BK0X@T=UMLdMN-ncY75xy zvAiaZjy~G@`hlSXzqJnaTNdpA6W7<{(}w`U_Wr%-lZU(=eS-UevSvGr&v$M%}ri2BhjIl-X%h zlFy~UOdZVX85vhTRcX}P>Ad>QZ8U(bH8v)$p9sn=aLa$7bqXJ@nE$)|U-u}YMhI#r zs2bj!w1lyelHX*GwUO(%>3-9cK@8-uh#prX` zgM%X+#esyR{selw&&r;r`=|R8sTcc`6U3@CJ+9(7ORBhhjf^Cg>Sp>4`Y?rvzMIN| zj+bTKnN+5!2jAtMsB4Q)-d8=?em98tg}skVoO_oo5ZyVtA~vs^R68^iqL-~Ytfi(? z_eD&|P0D0GG3w_1mE98WUz~Y%uWq-mQMQMHqyOnELyu2?HpXO+EWZAk@W}y8**8n$sDP(U415o6Xg<8;~h~BGToW^Y&#<#}oi^Z8m zrf05)?1smdp&2|s(vVqmzGO%g(jt@RkvHZcF!bXu!|w@gRGc8NyF^1T))-H$W010& znq8|#IIJw3INtgB0{ukuXLqZ~WntNOlz3(K3PIe)Xrz9XOib>HSZl1bkXHP$uT7y- z`48Z4&_8#F#QzWyzoW9%vbHwD)$_|(1rvC}l;{^27z`$$Ucr3MQpi^@=T$Wm#Uk$B zY*ET_LCxvPH@cItFvlR|WbyL&uk=<6m==>E_oOBhJsOX&{<-YYYEk<9)a9j-Lh*%y z(L$s9;c;#-I6FI=hZD_UCoeA~?lQN$GQpB5nEg^$6I|ZE*nlInT ze$1VuF`mUJ!djY#pm~8>6hXk{)^*&jW=BvRn_2qVky5I3f%nqSB|__;Tegsbh0F); zzA+T)vMIcom+yx`e(tt8K6mfBh=GRiF*LARzlK}o@91(&OvH7*&&x#`y>j5;DSR5D z1lbpX>|XlT0D_vIFbAIb`4`RMWf(kMsb6sO=Gz75p50y9^EGJ)K0hb0adGBThf;m81>D(5x*;TYq<#ORh1sWAES6`144ar3e=qy_(?<2&y63xm# z-z=lj%G`IFkd>GBiY1-gU!-J@;Th0y>?tg$$92-6wyGjaG00TZ&G~Cy`~h|xSvi-l zi(7_K){TbZ-fJej*L71wCOMD6>U(U$-la>C;emnLh$k~Zn+}<2&sq8Kb8;pw({5yB zb2^&q6R*B{^(`58&y{;q&GKi5m!tJIO{<(PUv}=E!(z9{R3hwx8c5WQj6RKQYWf}| zy?PaEse{2LB=DKl!#GWETvjEq@m19(@!_-2fW;We(Rf*HUOb}=sn7BRg&6IIUkFj^bJ`a-gP z$nob>7-U|a*5E*P{r)zSUh$Rfog|QpGGDf!9ck^r08vE?AY~XKOlNFY(FXMB@x|9q zB!&FM&n1LQ*axvAwB_YRA=20xP3WB~NUJiNdhkRsx4Bj+a^BGt8(^4uG_+=c*+hK|@byqPYU4&&rQMZ6-GR=FCqWadrxvHf47#D!&J2N4;pfHIj}ZK2B+{ zJ^Co;V5W-!`{D=*iQix*0`=W~fX3ZmPzpXZVf9@5MTWXo9fDpA2(j33@zq{PR>4B1iv; z5W7~@*GrFFr$|I=@vdc@VjE1oI766^{3Y>q(~2vTUSCM*BF3tGgpKekI=&Dimex)3 zsqcK#%cev@5w6Q3k=(7eVXW=tsyBJ}cZH0PQa{Bb8~qmFHxZUMNieLOSzrA+>Y?nQ z6^{d+_1}3t>%FKuAIDvn-~WK=G%(Mc9IN$kZ@Z{y1%BMVEM*~bEYB6PV>MQT)?~m9 zXP2+3SK?b!HDO2SWPU#Kv-5=(bi5FPSm|!4!jzHY+gm=n3HOEfXG13Ys1~UN(a6ut zHZ@M1nPk|XQRhgT+4fqi{_#q~9`jEq+t$G2LZe7d$aF|jxDzVz)tb?lhU+lcLm zIX^oNt7#Fj-SM^4i2{Dzq>}sBvX>1Ini0-gLPqa@=)CRuxE1QHK=|WKrD$A$oE+o9 zy6xbeQa_D?(#~;od{pozGlhW{-KUC7h>5AxA{3c_s)o{P6~ooEJb^ABtvADGCz}ii z|8iptx*&MWCA$C>u?I@J`rO=HsN^I!hB%@cY_$f7@kisupRiOw zGx!A3Bw2CYcYU`(IkZ7}UWLzEkc#zQNPrl>Xl|;#{cJU10MChm{YZhVa@zUuPL7^H zqM&X>`%YCyVwdnU5%xUJ%y z^3OKGOzPNj^;HcfpB0bes#MZQBG!E5tDd=+L53P!@ZvS!G^KRVj?O6M*~NVD=DfYp zb!JLnm8z)qra#48R1LY2HDytY!vHQK{yCx!9eMl45J(ZaB9MfbsX~7UrYK(w+ z1Yx3qdi2KI!-JgzJvyj%cV)$gOY}iF8|lJO+%V(qind+X5#_nMcbZH*VwTMBzv`6v zy3Emb&B<}#pyI{=q8Jl#&5dO(cQ2jMw-W5fD$%}ZIF zhCb$ZUFK&6eXxC{Vdjzy)u|*QIqOk)axtEV zi^s0#zGyzl0x3mPFKzviMwm60NXG94&y5zpKyIE540BEGWjfI%G{Y6g%|XQDkNYVe zYl;%X@+K_DW_IGXDuE(-QxFXtYo+>7?4n!V^j z74PhJQ+9Lb(I74d-pfPf4Q-%dVmop2(u*Srn?vCe!@O`r<*>PINZvgztDquxBLP8- z(_+;dQUXBsZ3s?xNIQx3WM zUU1OaBh96V9{>32k-wx5lHqQC>lb;*Ag_Y_v$@C`r(ZFb;d8rB!sjxuYIWKaM)(}_ zi?NIOG$?uh0KR?S`Ik);XoieNjxz|mX0LI7yX^bCS7A=3gCQTgSta0l-E26qfgW{$ z?F}m?|L~aB=ChH19K-!D6nrcqB*i68Kf;_gB)NlMCgX%2?&#%{RL2?$KkL4PMk3sO zUcPhI`<&9NBoG&2hr}w&Tnq{eW3^79le+zkjg*%)pmn`uJ=ld!1`;~sVM5=XD~LbR z(h>lBqri+F-_9lb(7Q_;@mtU|ZM~6-yJv*@JYA&;Ssz{eL7KSHCt!0l{FuL1AyQum z<0gDN?fNU?UDq?zuB%>SIT91ZXa);;63h)DBli_`$k5fjQV#~1bCG3fS(hUN3re4T zL-oTNJK+s06F+Y|TXa_I9E}3f4|PI6!U(|p<`rsn%xxu4lAiO&vTYAz=#ia*6!))S z`R(QP2Sh!dHVoy&9VAV8x-NMr^B+o7M5}d3Lv76UN})E@#78=q;1`v18$l<^7*h32IZXLfJieX6EqO;620&V?B#nK}v&bMCDHxaiM-9iYR{~eCMa?aku8tpELZep9>}q^3`OIm@s`ounG(kCAN&h24pi1~(uG%>rOjOOY z0STjPbD4Gur9rAou@uM%JNnZnwN@4= zUxWlF^LzHkKH(_jJ;o?_L-5jo5=6H0jgA3LsV-bC{)iAAPtF`BZ!Dj!z!BPwCv>jl zY$Xf_yY_B&`9W7=p!wEa_t)}S0A)UsDCdZIjS7{LyN?W>)i52cv;e(#*f# ziqt7P_4Mwq^8vV}?Kh0i=Ou)4!~$ zRbYG^I$C{w(MegFEb94A z#c02e(|+G!!B#&TNr7C~L>O5k7-ePSk7*+~F*jwir~49;aE}{G`|mgA(??w`Yg*z* zH8on_JZ)N7h!^5Mf7#-DMrp)I36+sC?RF5-)Sq2I@RY&xFmko9(-x^>x#_BSvW7{= zTQ%8-E^Con*Ey+-sc$_kZ3=ku@>kxAiV$r~_|v#FXBtqrlo&kdUO02EeMafU`Gu81 zX`u|MV8zlK4ea$21lBZjtZW#+ZbPAj+5@DInz*DeqyW@4fjBE2#QHqg+ zQR z^x*qrU#)IqD*((s8S;}Vr#uNNoRKWHW;43ni}C!iWo zw9K97N~4!ZV-UM?&eFzJ_9KlyoBjoCX@!`R+Ku6E0p1J6GmddWC*p}oNLrCGz1}PL zY(75?PwPj#@?}Z*wU$%q_x2ZRLs^gq7zE&+;3P=zOc&1> z&o4kY6x#S~95*DG5Rfhd=a+QVFVRTx>=#YNvOA?`epWNwrVu38i6WK0!4;8~TpVcV zIl_Iu8C6qod(I_x?aIpBt50nbXLK;BG65I39p!{0?XMg$Qh3fY8fx}D=AH9>hef1u z|7RkoVRDh{Ick^a7c1XXUY~Sm-X*l84}Ys>*_kYKZuV5&HymKZV5AYEsxjJ|REEs< zvkT&gfT(j~_uNTfBTYs1KXaiL;7Kw4@&pH_#Qn$3s;GmJ8tUdgy@s(vhJsE)kF!xD%Ds9NR z(e#INPQvE<_yR{eYP1nDR3%k2tMZ0gK5MQDFs-pXFVl{mo+V{);5y~`AN}c7mpm*f zkJ}2w{dh;+JYzv5(_vG1>AvfQsL>l^yz$}F*-E!nDR2R5`F{^kclzB`A-r8v4A%>* zhnn%p2mt{>(S@b50#>Gb2it@1g!{d)ANk^U`6@8=9^7BrIuoa+&ir$1vLHO_7wqvO zG6r+*)`pi(?Ip(4kJj!)c2!Q^wdrxFON8HooSLqM?Dd=)E5D$2wI=UE{^CqiOvEFZ zCV1`E4G$!C;?;8QO0ZJ@z)-6^WaDKSn8ZwBq1p46hLUw7$8nG%zl@O9qm0t0`5i+Yfe{R;fSP7^q{)DBJ7CH zK#k|rOW~yXR^zT38SouroW;(_Wt^M7Apb^<(SbqC!T;F7u~(tqPMr?wxmLTO-=}WB ztvT_-wuR%pzii7}<;cN50$1|C1ulo#CSN{^XL}C&d0@24wg5Fn#7oVN{3;&PZ+IK4 zT$&;tt^NL@4i-7IZ0ZK{AU_@aM2k2^vsM|s6IXvH&d=?f-tofB^koyb4i?Hgt))Pflzf-RLXbpSUZ&`0_Nu02=d4Yo-qN3Kqhw1N`0ck!v;e z@<&pa=T&FR*Ut=d*Dcv*K3DnVIn_VRXT|eL(xN%w-KauqSl5H;;qrD(ib2xsSKqQ8{vC^}^ zvU;%G50P3@0mzEm5l*nwk~8VgZaxl!pLAv4x;IUsS(kVH#nvAn73`1k4iMmLBQCL1 z7#M!`6~o;gyi4p9d^%YMev=wgLmE4lIc2E)1N?n<7l{wPn*j?B3aE#*Hq>K}LV4~C zxXF1`JKJsW9%eCZcQi>z)%NJE@DpKnjqxP%CnfR9j!FkTyIH}0l{b5N+$Vx5;z|&o zv#jL{-rE??J6~DfUrC_pbUIT>H$*aUH%WRCp;_c3d!zMQdn83Sb*l|M8W8ZHP>7UU(FmdC zkc4Ht4h;%g+8}P8)dpsDtoQVN0at3AIk+DwoJNnSZ{Xo8<7p$pn6%Zgg8l>MZNt4J zEXFno1lD8MtakA&87?|nGiYH;M$UckaadZf`S@s-^<(nJ+(@9nhu165;nz^Zs~OeG zj-0l-SA;STD;Vz&8xXxt%*abHOp(z@R-{Xzj-AXAJ;YO<{L_cN4%Sigv&-&jzi~-V ziYXK?UHgcr7D3(|30r>uWy7G}=YXuulBrl07oi@vhx+WU`$pgQ3GGD{O01b#vDS+q zozoOWAhWs{ZfjHQ=8+yj#I!jPXd?`1!p;odTfq8A3$3M@b@k~SvLthn`WGJ&Q8f}t z3k_Bc6Ade$yU2nx-gD?{di?5=Q?`nB8>@88qiOe;8aoc1c7Vm57hPN`eT8RBPqyjt zUPF(TC%KtxTpmm|OHfB@E%MEy8DzuuLudteyR*p!jft#r_5P$?E8Z}LPvD`sz4ueR zzkFy*g8l{onoHiHQTH}5O{8p!Zhi>%a63o5GDemRt4uqNheq+H!`vJmW6gSeg_0X) z!Gm)lnXEizs;2EG88hEYJw@XnrZoBFY}K*jiP{vSDpHafEZPXx?FtGLVgiVce|n`} zs{i?w_UmE==vkJ{$`1^^+pcCFnNpLp)4<*{+-^>vnK&JJT(q$%R0nb1ARVq;xyomP zlv@*G#qs~cHyf<_@Sm4cw>l$X{Y*_%vXGqF@y1^WhXAy;*+ z8$vC=Zi;2O)e*u^OS?6>KhMxe1c~9bt)Sm^^6LVe5B#isbBA`LhXOu z_s^#z)JANbpdi07Q=P>z1GNhE`&7S!1`BV_eGd;`@NvW=J&Knq+BPYkQ8{>sF z7<};zNS=K#I2&KEJiheMn_{%z6gnJ9M4p6h4><$E=3qyV*9U^pW#U*Uf|%$(R* zlo>p}hP*;T4f$Gb_7c^sA0f<4Azq_gjOUh%d47z-P532GSTk(R;FlYmBmE=liZc7W`c* z%h5V|Z7a`SC9oFbj9SAJuuAp!GgAwa2<(F68vX0{agjuNTqIB-DF#9sA-I0~gkD5m1lc)+ zG?uiWg9pa5n%^hO9W7;-Ahdo-GAQ`RN@TFc?7lhs(A^UrM53HxjKDHrGAh5i7-c1V znnma#nP%;^yXWYvp>R^^7@?S9Ot37;yH zI;LpL4S8R&kklV`;lD#90w>s0k?k?-6p1$(l?VUmyoUPd|0~v2lx-r!l%#XjZK^G> z`&aeduMb@1w2*s)E*~E@eH>eRvv{D&Wg;SgIX}M`xn^2@frw;!gd!KI@pcq8QOOZ0 z)yZ2N!$ol6ug+kCOt|*^cP3m|Y1399|6e~G$HP%yz`;klX^ja}0y{l+AHxAKhVkVR zKWLgsIdC4e7EzSiu^j%v`o;4g#`KrA;uPb$iXVR$sqsl|M5rvPu?a4<@Yf4aqgN8v zyv8b0gjx{ncS1feQ^X%3z#X1%fJN}{irtc0qM_yYFOwf3ZU)$*{sb$JEBy;t**T1B zX%61xDlGoOa`DsJ@cKjX81DzpQ>DEEhW4_}K^&I!*V;H#c+FmDBfh*pVm5H?09Mev z{|5jh2VWXf4)`dM%KB79nW!_v)VR8iRnGlW^iQu;+*OnBKU_84WBpJpMgs^Z4qJYe zH5iO535d$J`CBz^Ad0r1a~hHBO68FjCRMFQxes%5I_{2}d{PKgg$rW5HpZc|rX68u zUor!EF-KdFqt}%7^4SyCj2c)A1+`C|#s39~ogiu&88u#6;ON~mFDWAvwdZJU9)1R; zF~{Rz7s!PdDJ}q%*bRSR|{J?ulL~(%y;v1 zSy}s00Ri#a*s^5rg2nx;YA>Gv9fa$Oa#v%;J(;g6iVHuQ8n z+6)|nXO9bGU5!l_5`h%*&o=ooLWpiC>RT3Qxti}3KMk_OcglXI^^rk3W`0_sNZanx zSI=Z`LWvPq1#7z4<*#ZTobe+y!fD+cg%`gPU$M_t5@L!L&d=RTc)u>2CnD9=)D%hv zT{I7i`+!tunS6jdp1{N@0lK@UD{u0(!E*DK3>Cra9W(PO0yVcjr$Jvm9>U;nv|b*{ zo^n5d!*P|A`LF)=@MLgJM1-oAo*mzOW|K>gXN?w}O~SYI;C1}0*viK~?4a9X4+osi zPLZlta+)0nsjS^bQD(D(NZV&79cTVXw1IKY=j$TeN$QucQw2gJZudEYNA6{Px~dY- z!K_Jwx>W21lEv=mA%nWy{Hv0OSt_BK5U%ppCs(uF9 zP39!rhLz@#zJrH)#MwBCCwN=uk2e}$`amncRgD|1am$P(aTGDP(0`xiRys$=yO=a) z_TlCJ*$+#eN036x@;aGxRy(l0N8E6&KX_V49mLBsxybX!Zrc{cSm6MJ$)#^XN6E=I z$=(93a=XY**ww~TTH3FZS>^->be#OI-|-<3O=x5O(|oH3#V|*KjyHFGMmP0J@0wf% zo~TyqrEe5$T`tb&^f-Mk>`}cdI^7oMoxw>t2j|}CaVyyT zJGUka;-lz(m(jmls}!th0@dl?&`2O(!LeV-58WdL4(@J0#8GsaBz^@q{Zf2m{1r|1 zCx0?+E~SUguhE3GG4$t>CEBXHP7$)z;$=4ID+qnSuxAw!cr{;WEs4w06YBbAd%d_k zR`S?0qcb$W6O-k0vutPn+s*0b98tIiUAl5K4h?tWx3rBon8m?--!;VS>7lb5*~-^~ z{Z(T}(j2cCHGLM^NmuRGXn;^Y_b1W7X#^Td>R5^ltpd`@y0eUE;>LJRRS#=CgX9pV z>M5ICI-mA_qD#q;}s6u0tA5P#MLF6Krv#I5f?*e~5*rXm_j<|mM zGWYFx`al$l9eQt1(gv8GUt_oC`nCSTo4H%@ zkWbKqsup{9sw~0BJN*g{DG49$MF9&)e#d3a9`EqIL{qWjC{P`b5)w&zvhn}qvO5y`qg<3Iql*&PWyO=TOMjE(gL6WsLCe9>`BlJVj7}xI@*}sWcRXX zo`&M4Zc_W&&(7=&rbe|*bD<5qcWrXkk_SD$^(3+4NIyuG!h-H{VDJeW6}4JSRNg%> zy32Crj_CvQldFFF3GwkNF~DyFre3kx!AFbI&5ehn%9yD-*Q`FoO$h zNhdJ#(ZCCHxLc|ZqnEuG2Slo?U2;^q?C#tNdBGayasv?ymw~9}I8RBv5Ii@}F%|;r zuLa}fL_>}QL;5Vg8~cB(xQo>0o6}GM{^-Egk@RqUC+QeE!=#JFg&cCZfI;#d2WfL( z+}4>9QGHw^ZoW@y7Ft*Z{f^;w^&M72`-{1dI%=d0ssJx=i^~*@o zeDa5zS1Ju}CnOkA)3irLMTsZMIRl6j!p%J8O;xaa!Tcr~%*MzzaPQ0zYIma!GJ z&!$ed1WFN}m{a5+?Cr_Fx3@Ka+wnFL%cLDw4k!lVm{pzhZ6I;0{Nz)~&p-QrSbGboD%)-ETWOI-kdPJ- z0g+gAw*t~#N-VlTq!FY;Iu@yPOG}GLNw+iz(hbt_UJG@fz2E&j`#tA7-x!Q%JYzVN z`@Zfg<~8T<|DXG+gY$N*wY}MSANP`|F~)5qGud!vm3rcsT)?ITg+jp#fCiW9g&Nb7 zXF*_E0K2}oK|ul@r(%xbmKUe=N1KygYr~1yLK{xp#nqJ!2S;ziLiU%N@{5Y%1uW>;j!)OS-`Aa zopVwr+;`^YD=PAyEjIvXis}0Cw8-7uA(xW$bTo>WjEvge_U|(Sw!iJ30|ccnh41Qk z0b+ENX$Fpq?_WBX`@H=8%E2fn_qrWN9f0cE*9KuYSr&oF#eTmZd7bD1xhOWs9_MSJB zsSJ7>_qYx3Y~bBU!Ad<4S-UAfNOU%xl&;_)4| zp6@w7N-lOTzaCxQBj8Fs{U)%EgUU`Pdq~gU=A%+hELYI!9?OvnRpKTptLV70cUOcI zl+4aK&UAH_k_ma5uMQgX@UYuV*Or?OD222>O@-9iEsRc7uD{&g0>Eijj!BffspsK% z&B=ZUz%JLvgn-rV!o`I%AyY113M85YbZ*WqSg+>cDF=2PXVcDHz;Zj0U4ZW;|JAT^ zQi+~Z#CHIgN#u|7ZP%--RvLYc!N`mRfM0G!diqMDNvadKH3z4)JFsaa3nv}Kx6Rb1 z9l;Pl3Vy>*(1|f)k$rLb!yLCi+^?3_HalFUlPX<}T8xW=d+0~r_ps||H%7I5V`C$l zUI^fJl&a+!QnTubJR&%4VWeKiJ4qecb!SDThS8Oaxo6^8=`gc*iy55Pj1#4|ELmfr zdEC(D@aM5cO^@^0wmugO3%%pIIB=u;b-MUainqSif3-uwh;@zwB^L46NOcPI)l`F! z-Vx-sQ)^o!pbaX|G1~z5YbAwW0}Q`W94okp;-`$%zT9jBEG`qFh1%!=pm4Vv-8Mka z6pOK9{5)uTcnaWsJ3H~gq-t%=VyXXdTj(8df*T$^q!x62d9bppt~bu8)RTzIeF=MGo+VOM z+m6c8ig&_KJNr4h9MEv*)1QV%^Mzotg2CI_u?=YjGC36`xRbWw+J$zb`wwM#D^boQ zP6)Ghm5xk!t-K1jBN09oG)L?zxP^}zktY_X+5>J4Y;n~D4fe`7NN;nYK9qAARv@3* zDR1uoMyl<;iFl}VX832L0B(345-|m!A?TQ5afB!O;Wl~yg#w?S@tv7;CydN`E$_^4 z`Q5(Db8jpwe;hmt*xRF{q6QX{-_vtQy#HoV(okFsYdD-(kEWBeJ=pLJbN zIXy)gKPf2DpZ#{cSl(wnU6z#v_%FcvRW5ea;u-q@l;yj8{Phfjz-O3B8G~7Fr4uJT z0q2s_6lO@c6hPOElyH!AP2dUr(yeSj=7``#C=p&~TP&&{h%x_R0d&UM_nlKil6a-i zFqlkTnu?vDM>WLpqDPZ~B&++>21uhRdH?gYK8u;g&BEN=!jjqkSua#F zlNNo%+@d1Tqb9HE3RsfK06u;VG$MH`pTdhNP!5jfe`Ss_h5nT}{tqY7Qog3`PWt(| z&)l3yi$5kG*d)ixO=Ql3pT%hwv8q{g%bWD5nRZfL{NL!AH{^9$Yh_5S+pDaC`HHmN9~W64mBB*)+Ml}CBJjI_6W@;&`YK9g z(pm${_Kzu?-5M=heaBUfuLL8ukC5EeAt50${EoTnXWKRuf>RygKi zw`RR;ff}bQKXHVl&UyP1aMfYtBTNq-lxZ0P{&~xB=Imm-NxM8-?Y@1`qu5^9i%yL3 z0yL9!SY(}bEXVnQ>Ob%fbcKTbcdF>gpb6`0!1b=#4PW>>C;PVt&KoWcaSLGtckUGG zU2uOL+aqMp3+qb2LAu%dFT3Ri*7>ddI6zx6eRD{4eD65QROzbzOlI~=yY_$y@jxPr z3P@B%3r;+#Z8h5~Oe}6u_*KXN;UeBLKdV(IJHh5MJ0X|f8>!IVDxuoX!_k z40YLw3C4eF#xUvm)=ETH{7JoAE zl@2$gP_bWDRW=0jx$ZnS(VgaX*VJ#i1k@d9LPg#5Zj}vHUHvIYpozC(@4aw=yuiCz z#up(am#PaYbB$-s+K0PBc}Bje{jxC%-V0q#bYNwn5azsXRxA) zFa&?~=_8f~1&G7Ai6uOPdj?ovVH$iw=-*I|ifTPE*-|hsIENRAW+pfdY8oqh2M=xF z8_^f6i0Rj9&$U5qPM?ltMv^yjjR|nl4=t1HuIL8qe^EQ{gO_rlE9|i2mK38ju>kR` z*ijbV^S3+@GNVSDEW5`tIiGffAJno29WArdlh(?X>OO*#Lo``v%Y4oujCY6(d14v~gOEGeZR=RsHS{izr}i~ESk6O;El-QAT_MT=4^Nz6>1t~BB%Bb zf0P8#>VG=wz+H*@>nlNE{+)~)2K%pS;Mdlb%}eU6Cu(Jy6%A&c+q0qpUHzqOARm4d z@=kU$pLlS3XY5AXoyC}ji}P=viNB_JDBuw%#i-SSOrG&W=!$T1@N2HS@1Dn{Ahb-Q zjL5zTFCg#*^F}cAMpX9GPTwYf&-2{P) z=I_oZ4o;8TIWo&IMWnk7UPS()m%%KTq7f>UPD>$w`$-d<*|+M(I3>4;@WTYc5z9>z z8|%yM5XPoU^OamyfDG|SVFj7jd!h}4W4>kJ}^I!ZfOxAxBxlA+RUT}3Qh_= z;(-5V1!0`{{Z1JIFqm)t4zr*0w-XKppo!JK&M3a}6 z5~+W*CJ!PW=z!Gro#!?42;6lyquxn*04Hm8`B zcB40&^-l_5Y3=y_U)Ckc^wL38Z1y8;L{~^*@=bkrjg{PPk^_@mA$hgi{cSUTj`s!V> zQrF&}$rZ@r+=)c`i%iBf9sF0xoWw;uLNZW&r-)4^10k2nx4=uV1A;0Wxbb$ae@q&) zjfW7*@~0(*`@fa`VrO6P701gIz$ur0n*BL;o?l>X#UD-$NCH|GW?Hq9_q zT~L!SLv^drbD`8s=Y>Q*lpv`VBpnt12>g?6Gs%x0T^eh$*QAkKPAo>c*7LwI?Lm-I z`Y)RVIHvsN;H6G z@NBa5di)4@HOVqtCJv6nyM2?qNu6{c-v{yyD}_KpBT<42baS%N z^$g$mXud?pIKou{HUmN$tl~6EHQ{gK<0S`L)&zf*Dk?NH zO(rI`f1t&t|AH2ec2OBhFmV>(2|*Z?zJR)L|4r7tP$Fgt+1qO8=2x9R)^8siV6P zylOE-0(5}zEf#%N@dl^;DEJA>?&&bv?K?l;oUIBSWeEZOormDgIDlmT)!%uy{2w#q zUl{G5ltxOpEpNr1>QgUi!XK~@*5i?H@nhRMKG~IznSj;+GWzGz)9DO?w5lV~{{lIR!!^pI zrsJjb*DEMFz$*nrw(r8OIK-Wa@uGAyM5s2^?W6ASZJ-ZSfXbA(@-pPaaDX{V>M4rf zAS-tmYi@+^Is{64pfKfX;nGiz3^WaLqQsRYUV{S7%OczrFldEA{~Ecj5WI5c0i=Nc zpONj6+hCTax&|dQ+p!hfFI;G+dFkiV=muGA)z*(63k#<=PUug+CL9e!y0fbgbC z@Z%BPRS7b@7i65x{#Tz3C1ozKt3t(5#0=V>$Oy!W!8Mk|7cRr3AqQ@s0;J|-?NUpr z{b%#i0B;R~=B-hA5OFO!P0>;$9t>OjWC$NKQeTk;FHsDJILk&8;FEXbRmq{m;kEA+ zK>+|)Xo7(cE{-zGA3uO>f_Mo0;L_k3^DU?*nzp7FI1Kz;QvDd}Q%2v!d=_umvI zb>jh6{?kzG(dC))CR6O1w-z#1EA^Z#ZgwqWL#`oRpHvglfLD8*Wz;E!VC4hDmNRy3 zmYheEG|WF$uzsD7%ZfHkgWESL0HxdJ688|E7?05_NYiS#-Q+67Sx{y>4yABKlBdDh z?}-3;ra>;l+3>}z!x}lM_@<-fjo7|-sgxs3E}ekcicv`b65F$U;_18dw|yRaZk0`x zUzM*Nzl|;NidWMTGTp)1A2+wbEh2K9oh{+epK$wO5~su51^;4^e^Xrl^6AI?NgfDg za}5l>Ja;_kCLxR>Vm%)CCT*b+$ovT1LyVoJ8IPDGl&M|PFGtetr*-=cbemiRDys>m z6$vu-EZfoBW0}*pDOfpQ?I^Gb{VaV6L>Snx&!6LP0;Pa}(c&xjP7aY@U%7Me z&sSzb^C_f`o3XtQ4yI=^X)B=#(zu|4gcTC5j!o!Y*ehkofJa?w&S|sHS1I6?q4-02?RQIe9JRSYiRQ9jlI;{e`#A0RJ( zg%y1HM4NXFYe#=Q@Y3a~v$0zmG1fd=&bsrq^l}K6C=EHYxY!`hc<+OVShpm9zMwTp zVGbo{k_O$vO#hh@Zpa0xsOMb|CVOohs@z%E{VWV+l5DX-9Pr>iQ^VzEVv)L~Rk#$_ zk-tPcDhjf^&hA>_-Qw|=mx4~?sh`r*Voh~YTz#%<-fQ^xSHdJt1?E~Zyd2u>6HN3A zwr3;c>J-7$7_+prjYPD$a925#BuN3r5;k(t7y2u%rX~8D^-J{ezAC%QMcQb$yV)W4eSZS_ zi}$dLN%?Ese@jr%a4|x`LPPp$Z8d~G^l|?K6-DYBR7F5c+Nd`S5^yw9CT}ppo^`m1 z;vlVO@>Fr6Zc*jIG~0fFwToFU_{uNDX)qZ?58t}?68m(q!tUX+ak#b}mKd@#+Db^? z`lTU}kDdWoJE9=}=_8ZEz#u(p|K7kLtU#ATT7$;h;|10}fNlq>4mh#FE9vPGrP34g zOccZc7Ve_>7Dj=>;6enDQm^>v$glWGstl)JUaRj)rHC}kszX}v*ZdZKO2>n1AUp%R z^fy2(VEZ*k3b{Ljb)Kk}lMS`cslU7U7I8Ke%KFcQ^jOGWqVMEP9Z4esnqorg>cE7a-&CY3)jQP08qxO}K#Feen^agT)Ob5wE>z<9sN2lweRqlrFO*g3G$ZyS$m71WBH zJ||wg(l{HMZ%JJ5y{4UODBKLNT@4j=D2gj_AE%Y8;EHVz=lsNDwOZgD`Rd-0jn!A! z2EaKW9iSuNtj&P4EtLn=dqW=khZlQIGa&rNV!F8>?UbdA)=8N8f@2m%g&jY^ zq0#_eyY0sMo822RJ_WhS9rH%h*v3~w%`}zl<;1NKxsxX`z)%jfDOMBF^yJgS(IW@D&$~K4rO$OjQp)w~Q!l!6hN@Gs zgN`-dWeW;#L|iX+j5sY(8zv=-1a_%AJcc~oE^|v&UnzBQt^o0+-EhNc~bgU1Za_ z19K4~T`c#kYrV|c*s(9$DT1$j#9H>MJxskyZH8m`gRNI<%O(qQy|6(JFK8+7A6IUq zk-{zV13luhpv z1>IKwJuilGu_q_YZ1p-!2B=*g;SR9%hw$G2Y);IjJM8*g;e)ZxvyT21AfdqBjRwLW ztfc2HUcSsU6v=F)cg%@}XxxuA%B;oRCqCS&N>^YPq?EiqGkD%Q4uS>qtl->bpo5sv z01U#hfWdoq0}$1KpOs1jy~~cw&iz8%vd)IRnx!)J%F%1RclXtC?9RsoRGWSlOWB8( z;)qnL>{->;Kx_I4HI>_K!pD%M$ID51ctx?}+vqretB}uNYSa92ItO#*3U{tBN%$S- z3)BH#k*#vh{~);6tOj8ul<*0G(C?!~0E89gU&^5#X@-GYq~#4GeBvbZ zNfRz86PClx4rZJyLIZr5rv({f0f@>1XxQ`eY9F8?Z*ezEVQ0PJN%YA^XfCPY9>K)5xcoT_$ zPv!=h<}zi7#$t&OskB5lsUG!GNiwECS(5pUMC!38GPl!?m$9XmjZsjLfwFeNP_v6q zQ2(TE@(|xfGW<#RwpqN@fI2XpF%TK2dc{9vUGn~tgbi?TFtzLCr2FX+yCXRO+-Cp` zjpB-f)BgvoJo@=SGhTeX|RA0SE8WKUx`>+^D5q%V=Zvo&m7g~R@=)Vi_d+nMj!l*-9LpiIqmb(B@HM!m>hvhe6> zcZo}9)p95pURdJ$gFQB9u#LrVpT|6CeHU3=e@F$y+#O~HF-vf&ygSEBNNgF{EzgC4 zdRRKr&6}Ixx8qyIia_vhzOyx(=tC(COx(d^Mb|N{dmP5T8GaTe zfi5aoY90rwKohkl7m^T}Ze7j2uayJiFN0TdKZ0!($LQfq1pX^gWu)k9)Op6!zU3A8 zec*F~da}CgCp<8ubQ9m8Bj&{hlgS3WHzF+>nkhZ_L(`Dk`JffLpG>?E zX=ZRjaeSZzt%>qV)^?q3=lD^;$_efbqhJS0rJkMWP*AlUC{F}O4zyyF=nNRi=4*b7 z==?#Nf2gm&)7Eet>H_c-5Z@xBAS-_{{YG$mgy~&3)KU-Q?dL8=kk9fqZ=002CW5ak zK{+t3HiK_zAOL?GacyhxAK|H5aeECr!tSUtP^tq$PPZ`+zRb3#zUgwejr_Edm~QRTN?p$Rj6s5 zq57hVW#;(LmgBa=hj4xJy^jwBX-JXRQA30A;Ah76OBOX;cY^Qf&j7iZ40N^Ks2z7+ z1YZY{;G_hzrZpDgnGK^drJ9cvq4Lf6-Y!481O57$F!+8LIMBL@xO6LjN5tGs!v|a- zVS)Y4)fNpn9vDL}%Fq;&*o)v+4nCBB8WB4X41(D1vILg*0DcC8^kSuJ{6XcZt1pqY zTf>*oaYPftDaR3nj#}G1(hxmuf230ov4bm%@`&t#Y0fQ^9`Sp>cJ4s7o~8;G?a0}o zXVto~3$GnX_Xuo$pRUS)Jp8MR+h6eD`wsA=>mxxL{~sR!_?XRtyFgEvI5YSt1<@ov zaEj|j#}@k+^2~hAXEOTNIZy_V2s;h?&)*$Tzshfsvr&_{4Mh<}3cgz$E_Ok!)@Zxj zlhEB)E&s4`44f#K$)M)Sg9?nN$+<~Am?U_dS39GvO>Tzbr$}77Cvr}|ljubMeXoR* z8govQXRJdXqMdr>nZ9DpR>;m-BA9#2;+9E6LVEJML5f-B+lmr|RA6}^$CH0{Ctzmf zR)*70lg~aifTMx{djxJbl48?FbGn<1%P4ZUwxZUGb++gLR6jXGW_P1x7v zp!~1`x#R12S(f&1;^qG~hFuH;Px-Vr^=uE@jy;aL1Bi>W$^qMobbTiNl>azy6LG-u;cl_?s?*`Af?ueV-CDn>l)Z zJK?Pa(EeM=sm6RUX@N#6tl(ts3Ph+zC6*EqBdY8?UMTzg?_uxcDTp>$v&@;mk?X9; z@N+m|+V=MihxdOy9wd1%%%Gq&okqd!POi4TJjsX)Z9rM8a+cRfg$KL10Q;RlLHae% zNsz)N`ZZDD`Fnr~aD-;n1j`x*nvNiVzSa3>H+$FIAH*S;2R~NTxoJYc?HKZhJ)h=w zp9}jpEv61c2}09_y9iPgAR{-356f&wGJBg0Nzy#q!_T>NI6U=u(PA*Y(KNpX+<=tT zflm(5tB`I*U8#afB|Kcbh4SBF;EQ9>e4>a|ySuzb@fSlC6a2U_T2 z*0XxWWWIUpDA&AAH0Gd#>j{HDt*-j--3EW2D_>J_Ks5FR8}r`tA=&mHe!alriNAy5 zzgZmiw+9YHKqpG)KCQHxg!uaP>x^Y+YhSXtsqLeO60Kor4L^szF_p{1z+`XN7k#kl zO0L3wcYpCEt2(2D5uw=g+`aCw(RxSwvWRx{>~)1KbO!c^vuwE|;9< zC-!iz7nD4@IIOl=5F8mR*uQPU50@i+u$B^El&Rhh^Uoq&e& zwkREUsN8DIp>lE)=lc5Ui9hn6HTL!U+5!U)aAj1Nq{6=b7J_bbPr9Jj-#X%bR z`Z7tAZ$jWfk8+_@RPQ1q2}4@fes}=`zMk#*j>uiYkP+db$jG>e2!)m-glYo#$Z>c5 zvk`YXK*<4Y(bd~4=(6YQP|-50q}=u$H!c<7a2Txd@g*51*sB=ql>51$1w^3dk$e6#KXZ6mgLUV#hWw7wSD57 za?=*tuC_?55aRu~7U|m2a}O>^2`d9S5_0QOjqGPB!K&rIO~^Q2{KnsSUEss5Bs2(+ zicHyax_~sd8DC|T2!_w{c?A(3qyj#nD&;`G7R>y$7L@Lpo@C^gkR?vfJkSn#3I%tp z$h2=kLxyc~_{K^#_c*v62M0gb3VE@4bS-qe_{Uq^1(6&uy9!c_QM;~+d!G$P@sMNO zJGO5!+{*oh(kpV*)-V3Q-o&2{Km-6X<>C}P*(lepwlp)dbXNiqGkpBbaHN44CYo(G z>E5*~8L@CS+H999ur2^4Db4zm+w8J7+<39cVJy+v@ht8?4D8m&Uj*fGM;lP{Kl0~q zY?p>8D^W3EeF@O{y`6WL1)Ia+`lBpb2ma&SYq;L_DV!|NY~f-?lwc>m=vX6aW^0w3H=in@lUhc>;*2IKA>NC-f%nIM}Ee8GT6P7Db%Uro-` zfR%ogXs5SY<5MnB^aEI|8RY&?gH+w#(L`GZ% z7%>urY9-$=XO5YwzS+CQKD_e!H!;P&O8T`wl<*pq+V^FLKe-k);=fW-vbc@$z&m!$ zpArpB>THu9dvjhwvP-J0=f<|u36y9cY;&0u>YLq6M@w{!Re;j;!9ZcTQ%TE=AhiOv zL-i;BP+sUcilM^p#M9S9&jE4~z_w|DaDzqk2B zXD-!pfVw|h7RcG3N#PUH`Dkm`1Jlw>G;0rEwyM$r7Q>)jVD$_ybtUK}+%)7OpI18! z6adAV%5E2@M;`uSIf#_XZ%Xc~Q+1H5!(A)jA2NUZm}dl8JA7%Yp1*h_@d<(a`0Q~8C0bq< zDPhA&A8|+$g0Bz5AR*FCDhV2p-qpNTnLD~|)y@14!0A$g&8v*NMq5J)uZB*{^QK(As*gaAQwpK zbpz(eA6?45hI5hb7XxZNKWEu6VByrEZxa{^FSKR3?=)+wtRFR9S%WzRKYZftR}{u7 zEoP4nJ(Uy`ViLHs0l;dwU=!g`*RTP8U3)m$?AXx{AU-)+N}5$yd$hHFR#sEvw?Xt3 z^gSq06WHyfPt$K$`!pS$ugeX*Ta&o$_D=SJ3e9qB3L$5oyzz?@Ga)0$&YL>AbpB^Q zzg*~`hCNq6nucS1jN8xGo@={Q)5aG#sO~5G;&;4Nd#_~how((Aoc>ij$X=d zS*zJw)*_%$ey>Jtx#7)DTHLgU9^vKJ(r_DCf@Bp_H|0z>5OR#Dxnkc{g45|7E_Qtn z*f`Aqi3T9kh$LKPbg1A_tlJ}F^9)}oF#S->kSe}fXa~2hR=G54!(~0SS303Q_{(}+ za``>KZw6w3({PO0_}@8n12D9EaEKkud7)=3o|nb9i8v9$a%!<(4R-MJnkRe zfPFPqyHlR|;f&k1Hq&TkrqKbniKi#kXg$z50N9+Jj_xJsZPLNSY?Xhlzsb#6UN}6A zXC}0em9_M0!Y_c@*#&uB5sDc9FgS;JUwMc+&?sd;m0^6Ac4>Qltm57%s|#7*ybzb5 z!zTa&h-;!hJNHpC1mEv{oD4W4nT3hQZ(Msk->i(e6>@K`+b6$J{RP)i9$?^FzkFd$;^LA?>)%*AwK88D1}@(-o@crt zt-*~b{exu?!URM`Zbq%r)og#Uj0*S$6 ze4+d&qo1!OVgKjmRa{1ks!wH#pWV9RI(XW4wq^j#J`k$g?Vg*Ha@Mk`a( zFYwVgJ#BP9`7!n(!V$c9gWHiIdm$_$*U|qy~Ciit*vbnh_!&`t9P~Dd8vDxL77cDD^zwNd_Ed%Oajhlhtrpy^t==P zd~`_>sy!c!{PT0+Wc(9EC4m5dQj;MMDb01?VOhPW5Y6!|>-Dwy_8&d!9qBud07-@Z zVaoPq^0aqO39AJ4R|O=k3?x0EiLsHmCWMYjpw#dgU3OXPN9})LhqhH%bo#03o^2j z;Kl?VyYGt`Qg3DWfY9uz(O0<8tOM^cJwA6*3vn;(rU{6sM{$G-P=7<3F@JF_2m2vN z(1x35AM{8a)I4!Gr3{dgw&51t9f7V$lc~W8w9PKHj;eLf?7{BZZOxE?PoF-q);R3L z(vq|qltKiQNq;}N%PzqP6Qji`sf{BqXwayC<#~3fmWS{|AV0Cf<|z+In}}yy!adBH zZaRR@QanI?jo6!w`?!e8Qw{sirTs6mB~K^NefTTw%mz^F7S|Mem6hPnB&WP(6cBUm z#P~qlh!S5YFqk}z$=R{|HZ&^{(r<;RihzjNb>v)qzAkJ$rieJQs`dgk(OM|)RR+i< ztVWgOLZ5!d??oMvHIBQ;hIcgZEl2sMPC?zVII_P`6&$}w&@74vFvGu^Md9W~Re?0J zX4FD~mvipn7AnK-WhG0+(b!(?5>{gwqM-X$8>zq%~kf3D8vko5Z<5N`BK6CI5yAV*i2K z%?{U~yl;kNQbvYsMw(_AGG;5&C-2E&FNv?60s~;>07`JXST7%IvL@q$Ja`o`Blvs* zgeoBW1UT3Y?gbX?mCgGSXJFuh-|{i=ax4Xt85v%@(CPxVCcr8I0RfnTM*>su<97k% zr>NVP@o}6zz%qOC0GNUw?6hFF$n;=LiGVu+%5i@_!nGm@RiRE?ycOvGn{!)lU6+f` zbs&s_AEyD_I6I`bwbHw7)*gU`_tkP^n8VXRu`8vs7X>s_2oXRu3jKV6G-@YaEbGry z0DCdrUJhRl0N?>N0A!KCq^P>n<0c``Jt6e!6xoy}Eu@|wwB*u&)~w0kd)M6-&Yrzo zrXGsnA1VvV@3`6o)o`o}GcOq;23IhWA8vWEoAmxvLB-?qI&HQAaIQzcr}KNyK@(Mak#X^3R-Npl=GcC9+4QonSj&f=l^V^VG`+|} z4Mp*DwB#pr(#DUoir<5rM33x?yWPi`J21QgDM6wX-0iAK=6!K!Z}aaBKd z?*UVU54MLx8E^D1e))h4waB^gce@uOf^R^cO!m)SAApi}SOPf~gEu1_j(VR!H-&Xe zNs}S4GY(i}0_U}rSFk92HVSLED3m%5kjx@E?Qp*; zA2C$=CA~V}rhOLu7$RZVT|$=y>CwRwI1sv*$vC zSAex8_=(W-@69$-8Jeq7H+uR{))K5S1^1;BF%tO${MuhPA><7|vFzk!74-V)BX(6W z*CW&}pyfY7t^bmfi4|h~bf!=)379c!UA4>mhu>qEq}Yq>1Xe??TG3=Cnwe!}hB3s^ zk=ln_u@dcP8)Jb*#>hw$qMEjy#H&J3q+S+?LUG)#m-^{dq9t zAl%Wc3_KKQL<2>b*$%96%|(nxE(uuJD)=4lIPZa+#JSBLY7liGXgorNbmym@<-BB^_aA|CH!%M0yEOn%I@(pgMWmlb;(?Ic3B z*y*Zb0G!K>{7Sy;C>V^DDZ#dW-ZXcep`4S&HmQsvG8KDRNrN+}RH_h}Cz#url5=w; zw89k3OU}1R!)n;u>u=sZ3t8P3e?&cpKKP&^(7U2p7FBF*U!dem^-aoXG<^vbb~K;+ z0wOBxp6ca3sC*Bly9FPVo_}L`0iiHkC85l|_kF#7UtQ}|yse1b2T~cD6cEB90J~_m zJNl?(B|wu&mn|iWt?|MJ*|H*XHd2<*7kk@mliQmxZ{)noCtzUZkNH-a-+-KdaWglM4r3BE zd37G-*MJilLWNVNppZbv^(&s^w0@y}pFOW5eq)$}M-S1z{2rljG{dbM9m>?ydN-L1 z)3pb%H3#vNU+|UB8P??{8wG8A^=o^cH$KkDDaqN7<3#_~iNe zqknr7)O=#Bdp;881c^wA3M!cC*=S5{Omm?(MW<#ftuj{SRUm%GhzhsfAjAm7Q%@GLSCXprRjVx(^I?WA(Wa((Z@v9N`4uG&OawF<0dTQ z_9?sA_IPNF0^6%N)aWzC{pF33snVcPic8B|=v5+8Z!*=8;|H%lF<)JT7)WJU|(O#f-iohRn1w#cVc zGOj+0=_3@OR}X(C@D6Uy>fIE^~AMr0LT2Yt7{-;lV6f(3Dz+|+T0 zRA09qUI5%&8y6pIJQ#*A*8Q zTCUbBcjfQDK^&R3bJyib@qW~BeT3ih4@-V%dRYkAd9b@#+qTN;A1V4Urjg&vzGKhR zL-bp{?HO|?7H2jBzd4grC8c{f(%MsBHDB123n1U5Z#7ki=00O;oHU26*?O-3Y|_R)j=*gWi3tmfHq+~XKI!IVA)w!E zXP+D<2W z2mY`C%#MMZ0B&8lnOfTcf5ITegh)$oE3W01OtbM+_|;JxwWDbkeOcN48N1u95f*z7ZRPC3Zf?A68FB54N!b~g`3^+mg{NNB4JxRI(N9tzQX@`(Awnz+Yp;cM;Gr_>0? zF&v)i4zgbK=da&BQ@*eeP)*xUN>J#$^WKHZ{La$-5~bJeQg!S>OncPEh8a&qqh=Wx z!#(xA_{*j+iez7e4pMXy$p!Q*P_I`#Uc+uvC2;wY3w34cI^SYpJiF2!J}Q4M9OJfR z+gx-JRjl+ikx_a9eP*Mx6(`Qof9l@bL0a-G8JwgSax$>S6Poo;3}TE}QBUN3lsXPD zUcX|2ujW&b+xW|km%5z>biR98&K3p?eI}GAB!lE=*P=>I_q=>* zvg@{2-FMZ5z?jdso_ZNP|Hz;YVXr7l9jTRIa7Iu-C*Bv0y_5zUiwQT7*S;3}AYDWq zO^F|ze{=ZG>(&J1WLDwDSKl2cMS}57g6zcBl4|NGy0<9Zd3n*Srl(g3lx!vvMr?a zJ8VmS4=*C8#)*1u0sDy<@H|pWf=-0)jw)g<140PvZEi#Fj@<{U5E&AI}~B(#n8Ml*D@EK z!4VE)J4p@}@vx|=Z)g@89VEq_gf4lPZIZ?!bXSu~#DrZ)_24|6cZhP?<=z~tP37c9g~nF)lC8qgyP%V#8+;qgwj_Ms`)8ud>BNI2 zFyyMOGWiFRXNV#SUzX&1&S#Sg0~oSa6xBNJ)QG&*v^O#27DODuK*egNt_+s-k$sSl zqM(dKXh>+=b|Z2@Q3>LvhZ{9_tOZ$c2yI^q&>}U>p*6l#A|SW4;t#NJDW}BQTDi~+ z?`6{cv+xYvJ<@jyt0IxRcud(wy8GLP=<1PtC^1nSM$goF&Jnu2E7F(b=OTG&9}Tk8 z-)HfO$Y$pJ$DmQX2Sdia5m9M&Y);}{L?2F;Ont)rdufT^)~tYJE%~WLfKf}5FsfXw zD^(Qi;I@$~3f7tW7G`zcRlV0*Fsf)0@Pe`*n>YjZE|$%aWFcpb|Fe*4rd~{*)u-U` zCWfsHYtOF4W9;o%iZd&<@rOs_S=ooFW%e(_x|39Em6VT)@NMaRyVNM(2 zvx8iTEq8u&p(j#vt+2{~Ro4#g8}py2G&0p9pG4e}7`b5{{K=pjp_ho#p8sWQV-4mF zp0N!{&$$}erbIQ#3E8$DpHKP)EncU~RdUBV<&F2Zw;zfL^apb}&GIneB34ht;W{`RyFC`aXtuL1kum2mx1O9@R zvccz%8yIUpt4(8SxqFUAb!qvR_(`9a`bM-Ziy7qoiCOV{cPbfv21)RIW3Vrxr+(E; zN{T%4ou>NndVS`jy|wkXO>9xOyylGcl^^XP1h_P_+(jzCClYQ}mdD5sg1?OX^z~mu zYIEaLVWQ|+9ph6$?Dw;70+5U(MeecR*{pKOYn~KQGgP zniD6GOuPa=Yaa?_;c-=fF|D3)c3zJ?<|wo_m(6a0z2awoBDWdNAh&G?vl7Md zV($=u7p(6E7v%xg@C(*JeFTAqX>&oXKw+(brLI?VRb*{k$eVq>F|wa6ta8Ycmo{bz6QSg)ZA*{ZlV&_Gk-hb8pXlAZU1O%sp1)QOSG-KYAC z&R;=6;7O#4toz!tV@Ma`d-ux|T?_w>@Zsl=EzdJr^0`bbKdOD$u6Ztep9p6h^+gGq za1)xW>)}^Mdt9dfc^4&rk#p z%{~H`#J>#9-qcEY!+5Jqfk`tACClumh};%Ga8z9k_`ydULeyhXCZWf?d9$OjwG(S! z^x<7F8MhWnS5<60^1rm6^kh`XIAqLECf2S>5R4A>3tJDb9Dt+D=CckoSEY$ zAQus{m?M^K#!r>x*c}c7RLgS@Mc#fnNezl658A&cHkc?|zexc0S}kPgsp^rmhed*{ zs&da4lUh5=`rx~R?$LA?%>b2l6pZ2{ZP^7Kx43Q52=i2p@Qg$iA>J_R4@BkO>?mkX zZ*M@HEC=Flns8t~9r_bGc3{h>hDeC-C|!_}1~p*_gligz;JV|!Uej5BGIh18q&(u| zQfu3CTyyMkIg+N<-B2Ib`G$qVt0394gJ=ZfBCwopi#aFu^hODUrWi(?FgbB7W%6k1 zd+e;@s}+-G@7#^!ZJ(Qw;gPFG3$}gQ3#rwTW5ZDBWO}JXztKqliD{)Z+id^OC$jP{ zcbXbw#SCU21d+*^1z$c-9i&}*7y*AJ{QvPQ*&E^x)Mr{-zUliA@0t+5_(V^ce96!8 zwc}GkvOT6}ZJK-3J_(n-!;xDFm(PF>-aQzfV9Yg&i(9pngV;!Q*9LYxP|LKpCeH{X zx2aqrdRi+hWON_`CFdz^eT(H5pJ}=pV&e-V%qQLu(%KNqH5hfLI_a1>$s{mALVQ0+ zBXkQzJhhc#4aSpDv!hX^gQ6VWJS@H@Rp^y(VU7qKdef**xNi}t^dx9$)m(7Nu*@7v z_k7+>{z84~n<&0)43PjEX^yivBt+xtj{A+9!obiRX_4kR!Tn3s4x`7O7t7vWJNgOU zgktB|wQb@2C%Haq-rnYTV1&i4=O;J2Do;qi7boQ&K9lr7RjDG`vprD*W-r&Er8#dV}9WaCBOM+1e=QJc6;ziI`Ac>72mK(#yax1gRQkZKSdr5^MGj6!KCY zem@;fwQ4KP-CgX|r;V5sw%PEWZ<|PyhYeD>i-^+Dh>_A}Bk^?A$O1kPE&+X2VIiOW zwxb6JVkB3vml$;47bDb^v)&W>Kdik4P}SYnHjGM0gGh^%fOL0>q@>af(kb1gAT8a8 zl7>TfgMf5*r*wD6yYc?t&-2bZ?>FD~%zVz7GsuYN$KGr2wbpfAYc0@36vAq|giQkT zT$=GI$7j2CXhJs5Q11}z>3!`qsJ|GcjsI4y@B#*kKk!>#@{@sqceJLSTELR;^we|4 zgI2XwhwGJ{9fIN?2HwEVTViu?)Vxx_b)UoMDVNV-Ae5r)ll^;|L@3G2%~x@hP1x}R zN};oD!R~NG1=bGujavhELcBEBls0}fHJ(86udz*-;y(}>!#zt6Qo!mzLhzH`!#5IlDlus9mx9md3>IGN*T8$?0tUEz-kp zpF`lF)6sUyAD@6dhIukh3Mv^YRQ)o+@61Dr#77zd^RfT=fRT=h)S$G8BT?&4L-tt1 z%f+d9m3O1sc+;v4c9nVeCn&k5UD;Jm435!EB(pC)^;D9_Li0imq;tM(MXXh$$INp_ z+dDgK;Uchx+1F)>zJK7gLN)d$?+ATC{MiGkaqy)LyudMviwM;lZgVEQ34!HrqEh7B z_41P*%GTBeE32s`Vn%&7zOy{OMr(VX@^d%kTE8qR*t;uUD3yP2%eYn^5oJcD9Bt*y zz3&y&o1K)rmUaz&V=z+xk;n(cyEcx<(odd#g&}#z{^TcwR1%*D%oKq>L!VMX0)Ox3 z_$=T+Dx6J7_+fe~SJ{iB>s&yhu#j$G)0}_VBl)&y9URlklTfXlJi4w-u^>r25b(vF zb4oe*dUi2%e-344&EjF5S7ObRC*YHJ9}rUxeS|2V44aSl&0L`pqoU^g_H8mGo`&n- z^)*s+^W9ERZKA%@kz`i_XyMbIjp~r#^+osW7Tx6ndOhCqUPF+CM&WJn6O*+nDd`;8QGsi z<_jwM&`pvkrbk#JH5(e#63g|tsZe*(UMF zfBq{yYyHbN9(c;l8s>{75IDp=9#Bn<@09jDEAM zrFh2DRA^wD<`M_o_;y(pS%%qx?HSq1Q z0XD`&F|QO-TukY_>{1ua7+3ny=Vg*MQTFi4{UA4^2v$TYLxLJf6Yb@WSDyXB#j2`q zJ+%g|vQ`pv0B4_dncd{j%L7Sg5&NrWq6s#7C#pSG*~ldne=fKw#^=FX2~MRc%tU(| zCskT`5|20NT}Qn9o%l~kBdeV`69!n;j|kv-go6zw!0fJVj9W7vqnV`DriR~4>+InXh|z<_CIv|8y(*f>WV`p1IA)ivea;*uC~|L8J!Rv=HFKxf}&)7)@ZYNLIIUj z;Z)`qb|!u82nWY--G**j9n+xpgR6vXRlz1eD}4WMf4fvBx2!o$gDiuvI48w332 zuiV7!s^)taT>TgA4UA@WH)9v3J?d#?ZH-Kb`8Nn0Eaqy4>sR#B^fGxI-D?&GYh7gyldPpU-rOd}!imG2|0J z8FY{KkpN&HS5)d3cqcPR_~a4(7s7ZagAPU?y&f>b9s!~Bu1)g`tnzzw%i{2$P!Lzc zDPfl)dN#;jk-bzIg;}L0#VP>%a5ZLl)W)4y*UBuTh+1X_HTvgP1S2?ZkU)!tQ~FrE zF1EfG1#LchXvJZsyqYc#cFVu5T~nh8pS@`$qYy_Mr&Fvcu+)5Wdya1IRq!J}KnjY} z7xcrhr|J=Ks?(sqwbIj@;Qgf>C=93>F01oox~I8YE`^Ugl4(5O5C;bK!SnBImW&dA z5w$k#{#`&X$-(4<96;E=f#YlW9Nn-P!F^@Qhw&Ii;$+H;x7jMfmqWC)q4xk47fivQ zaZw63*j8lq-y{i3;=)qlJv|J}yql#3=+P82$eA8QIH25Zp{9f-$6*W5hdtY2s*OMt znIoQSWi(rhYBn2N#N2|x`qTLzF2Q~Cjqx+`vffc`oKVu<gww9e||!tMi={_-FN-QV6itXMt6i6QZnKMB+axz-}LM%CFi95GXt3f zcO64^Gpj=;;Q}#&)x&oyQW!Ic^Z}e2JV47$Ouft`EnYO9ph^%9NWTFz!BWn;+>0k{(4h=VAL^x2j}6$g!b|D zZK@eOFIJ_6@Lw7&Gt25-kQ>;uOcXN~a8w_VB;IW((Pn!xSO-n$c@2a>$4npXpZ@}OGU$LyT zeaxTPq4YB>OFy!OOx6D$?F>zJeI`86Dq6MKv#rSk0Sbj?!3!xV$yi!yneRl###YVD z)LotA2dw^-X+c<%yys+L5n{nwg#i)X8Z1)K3*?~4ne8a>a$=NQXyjYltOKc5bg)Ky zYQ|2a6QjRW6qFVVIVkg5AwDJGer#QgG}W-ZJbZ8+42YDI3n4t_F7Lv*nloV*7LbeE zA}4->B};NPNLMDhdpcH=R{yR}TbU2p22u89HdPVE@B6CMi(T@?h9C9@nupFB5C^IROqJU{i6>3xB>f|H%pV;^FWlH-_^B?Vbz| zW9_1$Z`!r`bP3(=f{$9=i!|RAJFtmjP$zQOP6N{io64V{`8q@IV0dg!UUWBCLjz!o zo7AY3U?R;U3o^bD2Ftw^J`4MkGH%0Q0%oJTD?A1LV^pXjROpu)4Gs;6O3Xy`Ju&ZplhDA*{nw6F{GQ=A z3iSGAeJdD5vejLp8=jH3EYF%TnnYUkaZ8U&eMu)sZE~4exypL9Wy*Y)%B-I~J~ZKAo7 z8ePrY$RM+PLw9{jWGA-BU<&nBSM)V^g3a~>q(wYT#uOMbp#p+-TUIBKGHi(rFd}QQ zSS@5^*ugvfeyyfGNG^Y@8Qv>cjiRK20;+->gdX@qum(;xdnb zF53U_5R8tF1_vjiHs9RcxM7Ubad1>+X1*MtvJFZwAR;>gkP%sQwkZ*C3~^_0X=`cS zrv=i2lwua|?cY{#IT%IIlc|#imoLFaqEihOVkpEjSfA37O}c`}=5>yQ&3Iprlb@HV1Hi*QHj0<}o>!cn zw{4*{2f%k-q7A7@K7zX^PM}qRMAvV8$}CaIUC4+{-cOs#Yto-0_g`5p4}p~9+B7k2vb62PjL|5yjAI!mX>zh+ZOP+?h7T-A|lEUFWs9OroiYEf1M9C3w_1yd3nfu zN(50@C+Vk>f&o|<%fY*Qw!5RC*oq)vVPd`Xk} z>?-u?otsi3p)XfKLA=mkS;*}y?WI=#=!&`=9^+F~a?v!a5yeW0(CKQnpIIUbI&pa! zA(wKJPyN0oK^Vs=3*yptekyT1oCg6f2cWu9<;gMCu>Mv9z1OBoprj9658wr$E=Mtq zMb*xa?)y!Ky6b&aEkhkXAn{*c*8uN&oh~(Kv%{Ug`r!ZY4PqmhR`>^$HzG2^=XW3eCQ&QIFXYmtOcaFBex!_W zCbBVWpbc}HLSGteOt&cMTuk0yez;0r(EW_)DiV~ljQZm+vb0+pDLvw-9s>9p3|LZb z3|JdDQ?M?PP<+fqlO>d1@5A{rc_r40)a&(7Uj*&v@FHuS_b*dDsvhcx&eMMEg-N;U z3?$XJ&QQ&bc;M9C8<>_oDs^np&mMDcPnPr+T@$8-g6O0I^KN$n5-CS$!K)bgemoQa zXCLI9Pwq}{_1K1|6oITq=X;r+JodFxKJimYSTwFtc;cVg2<(pnB9J@wtC>O*=3e)X zfvA9GS$?g@PE_ku)^JCtBK58_AA%M!{ ztMnW`)Eo-*)md4V2EX-6r)chNoGz4N{mUFJ4Gqat^TovI=;w&aPrS7%hf0j{B=-3!6Loluq1rPj`t(H>UicT z*jLzXJ!xLv-qL9~W*(D>9pcIZ>`X(A{wZHi6MwR6F8RjDc>pT%vUWJ~lK`invCMC* zD&aVzi-=a_F}&ZCF7Z!f8-Yp<>70M#QewZbJ`>cbX=D(?EJY;j9t0%XF%4>^B;cq!7y*2y785( zvSlWYy8nAA_02-xO)dXTckdjNG)}&B&i6ePB2o@qQN+SD`k#nMG8oAYt%}2;+1*Z< zLfOPv)0xzH5RV%E^A4Y$_~`*CVnY9)j#H;dyuhwchJaT3=poe4X}litI{#)%?@lfb zqK%5JJ)M_b2nW%u5x$SMYP`*<`XPKQRAEhT;zljfd%Y| zeO_9^j_>XxqbuWB>YE1UuYRsv9;^#6LwK&g`4V%%Hwz5O#2F3@z4+rz^2k`y*D=+& zqbg{GrR8B)>E>IB@oVa6EXL=Z867Ys1>UMi>7=UAbX{GJ&FwJW;S`h|C%5PZRo`RY zraxg3k~7H_dvqY5-N$zuFBYPpSb4hH1tZvj!r_(XK(&Vx4C0|AXL!*}jF0)mRhbx3 zv(MkrbMhn*JRJZBIkW6vx=u7*)>Xs1R6=B=l`bV+Xb>7Q29X|l&r9dAdb<8%Rk~Q= zK;3E^s2CxS*5S2OX(~U?R^^`|Y#+Xca`CdMJucbNo6D{{sv${Jx|zWIRicTd8bMlF zI7N(r=nA0+UxMmRAWF&U20UHYSwdn`jg{Ipm4@yglSb;hL&#h?@%Y`P=^Q1gCV^@k zV-g;YEXwTnF84ul)TBo)8(#kO?W*7U3k8mYQ?GVhI1A6{$_>JnuW##jdYAl!?c6$P zLLZis)ibh;yT(S2-fFFUs)HR3hWANm*!Bj&Os&ZQSQPJX4>)J{FKcvluEm{@(cO2F z>)!da=rXa%5H%Edw1yNUv)^-LBU_3r_K@0a1XkI6ns3}l`buJtDGPz`;sOFuk;k=m zc?H(mt4TsJ?kzh-zP)HVYnfwVjahb8Yl8Y6#;>cYth(pc*$^f@;-6w4KSE%VsM~`% zivFX!L?IF_9yM|40+FLOim!4sWmLsdvQHNcy#Y0js|U`y;$?TN$>+Lv7oq1!dnM6F z4H;_xBu>vsq*}%EP34QvejI2Z((2>YT`hepwGez}9eV&!;nR zwvU8e?QO|5G9kZy#cH5MNDv)Sm<3*0sfoVDgqn-={46dPBSYvk^zNF&I02caOaCBH z%Z9GD2SXwqtw(`x=N@F1?>7c+r56i2GDFMLkR6>j80hz1n2R3{dhVM6O22f>-ZXBG zF)l4E?Eby^TWw+O&W}@~L6+);!3SrHg}lHdejPZKfjw{y>dE=@vF6jZAL#yA zaj8Ix2>XmoMvMA})A*$do$I;XUAbWE;sIOPIupCj!G#E4wDb^+TA703+9O|u57%UN zp89a>FX9j=uTUnb@`i`pd=d$2^l&xk1!NakBR?fJtDcfiUslw!9r-?VvFG(lG zc(ebF^edb+*)?=lga%`UPhZeqM!SlPzkfpyC;W=76XRp*flJ@7EqB;(!*o;~5sU>j zgnXmvXFD8%Gj&$E^adYX&d@hj1l0R%SAU|ma+F!!X0zJzU7Q&*Y7qU|GyYH# zSTG*WA5+E=txoKb+iU>0+97?p%i4_;v@wLyw+nMoaDy-#_Q0a!4IKTDg25|78TdI41Im?89E_yh#hH9`Ciae(>HitO``s6<-S;9@p&MuRYhm zl4onW=t&KGN{3)vB#BM(-#r?#Ipr22sy7Vq+YZPgPYsOjK@J z|61-16Uc@T9{90!tn|>v<^z(0ETYWJB10117d&2#lX1-D--mN)jg&=>@jY2j!@?Y! zV@1QsyizL5^Qn*DvVYPNG`C210IzAt&~0r~R?HLmeCqfQE0i*;@m_fhEu2P?`J+9FrER=W{q7^Ck;_Nr{ zb*8CLW;^(cehZ45tg}2bMWzC-ECN*v(}DMPm|{SiJO%e6GFeJSafUs zjrZYx@IK2Cv08Um8NNDqTM0_vnaQpso*8t@#>5DN1v7pL%#DNNJR@UJOgxp}<#;WI zR(NeK=8`Ph*Y|87xt>_S9aJd)P_P?Fa-X8RUA#Imx29}9pA_#7mmFdsT*9chcnqgNp_rnUm6g4{&h9QQ_%KFNh z z+!GTez_w0Dgkg(u+lAS<^RADc(=KeG(BqoMVdwQtI8$btfwx&#BtH%F-|*v|#HW6B z_1M+bRm<6`ljSz5{e5k`Qp7tv;#4-C>w*&NA|D@Q9^xoaySckNcY4})dMeGxs3Q&6 zEe?W}3OH4EWRxU_hx-iuPT7HEjVj~8rr}}Zt*wMjvxNoE%1Tr^oEsnX8NsVnvfy?< zI}q4F%^Wv}mUDAe?VUb-DlL)lA22AL()Fj5qw_s)HgrsKH0g2-kitq_AJs&CqkMmxLNas za9b?W2zqhh-7*%Vkl20Y{2{y&K(t#LeY&Z5!1e@F6-_>{Y;qD?F+#oEM5VoQ0C`J> z_NO9?C;m)2WWHR#Ev|o%r%D?L#9iF^Ls{S80MXaVfT=Z#?8c>7kMTrbd+HzDtlqLA z*3t@eNtCzFf>4`goq?4l2oXr?+PhW2`~1d=48-ye+2o}`l6J1&k5TqJ(MP=emP(X8 zk(Da7UGKzM2%NLXNO=T=;{DmlldBCK1Ecl6WDUptxg1Wb&;p(2;g%MBH6hUM1sfaN z?eegc*J+=aZ)#(um7cM<0SvyozaviNvLR6Jx3Jj7YkjuP2fhJOCyL_GGF!x_U0Iq)@_ za`w2IMH^rck+|5tqkRFen$S=sVPVdKLd#{@>M!%dV`D$Q$ZKo694xc}OLBfWS-kk& zZf@Q~OS6X8ahCwU*6sQ<;qdV7Z_7NyJZOTy|HWGGB*row4UMDyDgJRl6_~*O|2bp> z3ahO@IkdRe+27c{N+YexWO0D7pxEusVp=^CUQj@u9neg*J7*)AvFBft5mwwnDZb3% zPu>cN82pI(_N~ewU;Z0H!6Wf$RMed7vhHIAjV%l~V2sauz015J$@DR>vI7p^txNwhn%F^jaC~LM8$N zMEJW{LD;2CY8ua22{p8nYBiSnBG;yQ#R>KU%)~@}a|E4|lYftmJFFOkYa241h|RODU{Wf=t#E zLqj3ozZ^LeYl-Jj*$PK1ZNX5p+sQ?q%*;EAv7CfD{bwF%=vfdR- zuaS|i=kNE$-oC(FsT_=;G&1ZzIzJ~Q@`z1JDs|ku2G@g(On*WGH#)IEX-SE8v*(|c z_Cj#&Yz*pbZmNdt>$UmGkdt44v5Skfb{D|jj*ojhZd}jz`M*x&sM2xNAr?fWO+rW? zVRa8&1{ClhuX!EIsxO{*JYW9=HS5qT#au=aXL)AcZ|#l$GD7rSq32X2oQwc3Or`sr zfh8PQucY+;^}qS(Dq8IaLZ}>_Y|z)(r(hmX9}Ek1_{n=2Vn?3BzVZ@-54FA}RcVKZ z@5~gF_~#~LBBW_Uo3O>bPH*7!u?iV=Dodp8DdtT~(Iq5Y=3%{mUcRZuZALus(n3Bs zjV@r+2~DXD1OOvQ=QpAXeSNgX7g0!FsBUrQ1id~FME`PgB|mIDPkl4vlPER4A4(^V z#!248(Wj%J;-6P&*lxFBB;s48^!tnIY6?+?;ZM#4s(qKE5*sYz+3xMPk#ZVLJBbP3 zt<+zZRf%C*1y*OtvJ2lVy@=4Lxln7ndjRT|ZQ_vG&X?F19$I#4(Ni`I&{AI8;f0kH z+zyVS3L&wZ>;ksG&`@|fq{Y1W%VJZtLAN|kFh7?wwk+7h_c(DOp#mCZ2Gr>gLj!}1 z;OlDucd7`uU&7+VGx4(7*&YLb-y$^#yohEE)AM{IM8t0EJ6aV1b2GE81HpPLoxvs# zeyKPHLEBZX;C4qx$NZwAd9brxUGs$+PCx56!9V&k?@xy_+uC{!JP;QYe`VYNn$YYu z^rutq*CPt+o{7a2eGe}YS zKVrdbiLnn>`2wYHJ#OvSo(`~u!-sc!YZB%u5mI{~D(UGdzYn?t9N}|s^_gek*ajO# zF`+UQU|Q|Rk2{6M#aYG0Yx|}PReZK8a$=4toODHL^BT<*%N=d<_Mt1LsHh>U`-ULf zg;)&+sD#gRE1vWxltuWo27azSmB@z)QwwRga6)M3^(N`6XTntOr!%gJ(!0MQvs|hK{Mie#lVm<6flgUesggk4z0!lj@CJU1dY-M2mHRM=0jttiD@R`UFr< z*_2)JVDSL6*o1_WOVC3fu>rcX(CBY%*oP*gswb>iE)`ew3#w*e!Kdlf8*Axyan9#_ z$VEqY9L%`?if`)C5?EP#^Huz>Zw6yx`givDpXM7$;NIOC^YU_*s9H!#4P=OCfG97X z2r^r>_Yz41BMW7Z)V2s;lSjX!4;=N{TDZ-Kl}sDJyv5+V=4WRT0rv1W>svBknB?YR z2^;5Yf+v9Sb$Voc$IUcg{;8xPPG6c$anBy1dpaL?ueJgwYN64ZO)%ScI6#6LJSB3N!=_;E|J zdev`^UkAi%qWp{CT_BHLyZfYH%S8e0GensL6sSD=_HrCWU=n562_XCo49mdg-95jD zg*_OjYRSpU3WK%sH?p*BtgPHyZtH|3&JJC_6l1ruwr&Dfu#8M{NXXr6Meyq7TANUEO(!>Mj_H`dYvpMTqnf z*YdOI6R9$jv;-wZ|9Ie^C!7S-XmUE_0FOFA`xiI{x{lgmnHdcCS-Bv;F>s?QCXIwV z=^Cl+N-=L6!7E5r1f+vW=l!Okxe+OH{}8{yhD+0>%;lL}u==d=+TdBOd%63rQg3xL zTRTNvdOjE#60X;rNiLzH(j)LuT87;{lRIAjn)i(B4Q;&qB0(|~f9A=}Qc1#iD_odY zzT2foU)lKUyo5Tss2^qPxBEFD0i@?5rxCo*0bqw_?UOHtTbR2qQuNn%2gjnDHeXn} zt7if^EP&e>)yhsmSE0kt1{PHm>5x*@-M+r0dMDz!CJ(EPf%&4M{Q$~#y;5c}~ zN89dxTfkuZWgSR&;Pm^0GQ3V~)U@;iyuLbY#C|`ukRu}c z`Loea#E$8t9x~0xunza>D3|W>u*4<{8(l0zco!L5?~OsCokE-vA8)wcdLjuS=JC>( z?@ZvXPkCeIvZ7Hj!CUF4a}x1aSdFj2tI+bzGx2ZSt72VQ|8AB`yX)k{$FwE}$o`>*SZCYhIfG9fTyrwv?>iWl9>@;($#owHy{&FO z42P@3a76_v)G@=T@!;0lPG*`*@z2mPGY9MIpHA=ffV#&@}%2SIY zXhm@c!50Ar;zW#twIuxa-QCynZYoF85TCzwRwE|5k=D+Ni$O^lYpaK`dp+z<7j)ci zg_fGpPFwW_6R1tK?L{`ljNiv~nNIt5^#nF?Q385o1L9J^{a{!3+AT+swKB7_`??5#Pg`ukSE5zn6j2I?@}O`%LJ&yGsJO{pOAp+M|#?|pZI(63{xU* z7paAuWcx2Qj@fzzUe3WHtbuL*7`F@Bmi8SG;EB6BCrITL8mX}%uv+3ieXGLm3C$OL>+C(aCu65S;U7gx6M#wqQa0O+#0t{Os??XL zd2nA@^SH7IpM_`HMbJ8F)SfxViEBmrHVtUpu;uw^YPVw*bGeOQpZfbII#Kf025>q< z_i!DOvdFUb-ot>IsTfZwW?g}#4!QZ7HD}|y*C+hQus1-&KkE1pz$d!{`>~%gDwnc1 zPN@#k7y9a|%n=nl!(HQt6FDG}PTB|A=gb1hzse~fT1$Ln#>6;Hg%2$SsR`tfVT|Q% zBst+pq&CP@_V3}&L2^+f#+knz zEyKKPkscg2qRE0}t;Zfo(%=dZ;TCi8+E|L&_{M|@QfeAip>9LuZDnY($)UtR^OWxj z&{T?ikECnxl;DPK_Sk&*KwF(ZT1UDhJ-hQPcS{JBYqafg{p^60ac+~+@_Ex9m8h`U zlO<^y*{u#3gDuU~p5#4ack^1AW$9f?87OIeP3fN~t-V($VM#K04N7x%Y*Z9&MEKw0 z!bCro6eiR*MFj``e zk6KtDlRVGnaHd;)3w8UE>=V648h8f{Fr#dW{nnm>Lkw*C1p~ zLc*s56^#P_XZL`RFC%_Sv!3#7nK&>5nJdv>P(Y2m=)xQcf|y($LpHl*IMJ(+BF|4p z$4XL-Ot433P1FWprK1Les&|8k6HSO3K7D%`5qWrAT2O#!f1!BaVpd@zRx@l@kpOTi z4A=xNE(*&zCGOj|54RcK`HO%OWT|tN-X$V^Zab)T+{i*+vf1#(heD;j| z1v_PsbT^Gw#HL??R&OB$9}mye)U^3xaPl<7Qj{;}fvS*fL3XhUu+j%;|2seU|7_Hp zlYUvus>N)N%AL9QsZyLl@~p{cXO&Ar7K@229K$@<9>OOhKdn4X>mC{;H9$ttAMFre zw0<3{3{OBX)Q2%!mmR)+oxps=kg|Yt6P6h!Q_*x;xOH0@nLY6loe_DetSBWrr789@ zBPT2CE?b;PEBo8B`QlS=u3M)1lDGchOl}o|b#)Liv6;Q-QC$4m$aGGB{F)!N;{>0eKl$niv)RD-X5V!K~jXD=kZ0X6ncW18t)0JI2tPVEp zHrC$6YK~|U1bIKp5WyKFdF~22>D`|IboyrT;eN9D{_G*|JT7h$#gJP^BZ|lW2{6HvE`$tLd0E88Xe`&69In)2tFp9KP^Iu74slg^?eIn&Xe@BV` z7WOQ9+vvJ6>%%Z&Ld($76yBVxd}&DnI{Mz>yP28(QlXYA2;|4o$NCGEW5z|AB6Ih0 zybzLav00d%gE%50v)WBc=f_IHmpI;m6ZJYV+Ce@al$*%jvY6+G(|v>S%V#WJ@2ksY3ostQ9iu1vBvqj4;bJccWhMB zKr&_*v(Xk8ccMrKm&etgbYUu6jaaexgdJgw)8+Hu3b;YBbYWahl@$(ZB5is{n>DYE zv=CUQ33j>!K#jR^F`mvd$oI(2I1%QJ4L=!p?1q8(Q9(#)gmQ_l`+NGU|EfYl$h`hd zcC~9z`kbrxz0^P*Oj*(rr4?oEhbW`2yoA?`o>-pj6V|5|=XV>GH>E3>5c1JtM2 z>l+<6I*e}_E-sGB-ycW7`lNj}9)#fQZ7g+-3=fYDe;!zw%`$Z1^ZdlgG3>f;rak#B z^T_K>IjFp35rUbsc8uJq+8?8>?&+XZCw5o3+w|21=Y3WO*W==51ti>^;M>i)+;$YW zO-ESDa}1iPC-VovUYF%A;GZSx5a`@eHZW;6*m;dqHzPIQn+{Kj(9ZR?S(z6t4&I&P z+6=@o?sRHbHeG66-Vp~EhrZ-<)&}s<_4rgeqnD=r!_Lf&4m$@uqS{p7Vhz7bn>b?d zBD#}3M{;|0zSc|mS-eP<-NfV_qPWc$*gFBS~- zTdA#Bt6bKL$56=6ulR8T1rwhtT?-BmclR8JtryuWtn=!KoLE&jrEC~HgH56$7j^~O zUmp29_$S7Bs<~bN#s^}aYyjwp9F)Zg{#s5~v08vk(u?@z*yxb5pTwUt9i>0qS z#K`55wz(#em0j5BM&afUOonAI;4(Kubq8DedLSeZFar(SzuFJ{RTxngMy#XH z{?!6Z#yRZRoHaH5NWO{EzOA~vKbhIP89}?)@Ukt;tmL1Y=`d+&U80~Ro1Gz_w36W) zNnLcK;iOtDPv=?MOlas_oM?8Z(7psRbJumr{gc#o;hTM@=DV?`#8;p2Wr znLjIi?Tl3F%RR3&$or<&+%m7#zs{#bSmDb6aJGc=nW!z*cbO&<^e{;u z+NtO9zS1QJ>(v4on3Mvlg|trULKavOOK76TCODpbq_wQ6DYdi+`4Q62%l6r8Xd~`!Q2gk@;p{i< z(hzs~rDFq$tgwD*Lak?;-hF7J>CH`9XewjL%4TyjnL9X{=O*lB0%#MHx6cZOaaN^3 zAyCsvc@XiL3oeBkf-5)Gp^VmEC5pBV1$C;7G_9HvYj#m+L~>SBR$ftH={M}z)-ZFz zm5s<3`$@kz%SxFp9FORw)~aLt>9L)$-*EM#g{pocsxM_O%pt!S^S3MBqpodgcFyHB z_~5JHDzcrXWa#83_2n#jj_>2qp#;!3yl{<;)1KD2_>1ngJv~|>)hqHM()$S0I z3@Ip`Jie-bv1eh2O)_lFk}&-D@(mJ6KCJ>=)<~zE3d550GgFuZq^|zFUBUC435rGm zT%B%yWOkCWS!4g*%C2ny`YPxxDX#Ff7SLba4kv<`Z@P3nl{9{oJ zzW*f0{nd2e`iqB|xemdialQla6UYy*ft~?ZYf*)3j6Mmkh_LxBc)#2>xKDmgwa+(wgML zJn;#(TUVERm7Ly#vwk=GOvl^z3+?p~9;^9{uav)Z&CCc1u8dz+G+bh7+fQ&XKpGnb z9rud-s06Rq;(pT7GRr&?nj{B(>Z}y^1q%OXWx}}GXEaNREJxV)>i?~k1@buJ5U@9^ zcJP-}W6zg=S8v%^`)fv1l9n(|dLW4-_p zgF(V30bYYP1?SaUrPA+Q#((P<4*lBsj5&AT^Qj@MN8;5j)!QA_FAj1w$#OBzfj&`y znCgzoD7{NwcAM=XCbqmiDZi4F`|UBsm2b+RAfCwD+gVIEU(VFR>Qp6Vo0}E_r>+;Z zJO7V#JMv224#koVq8+GIGPle>t>P54R^s*A7fTpGm@T7NTU3+CRDLIn`zCnJc*pmA z)w@%^&DsOJ%?<-wA*Ki_V#RsE>KeEYpI8%jV1&X)cR|b#3!u-%NJUmTs(cu^%VOl6 zcXA4xZo-7fM6qdTy&o0iI=d!Dc)EgH3|QC_I?yJleKdSjl2{$lBDdf0lxLdU`TY^B z2nmLM33YurhVA8AY^hnXxg~d$&*?fy{qeev^-;zAO@9Prk)R(!#AS8!s}3P7wkKZ+ zWPIw_mZXU^ei(iwc=kPA{8s{5Nu<%gjd?zFM5c3ttqF>hFW_G)z!Q95_{89Syg^=O zR$c^G+vR{{eU_S$H80qjrQb7?aqQQ3tq6ndak@qGm#%`zbmYo0bu2bjE1NoXpDtJH zuYc?w4X!$s5CuT+DRZ zzZ@Jr<}cxn%!=by9@cecwAm`0_J_1<;_zb<(1wM8fR;`?RDV~RoL4G4G*-zPr2xWQ zSs6cLm6za}-EnP1SE}6p@<3($PrT@^R^)y6`qH}m25gglUtf4^+2NWJDN>0dX|SlWoDb|4j94G{5aZ*nT0eUu9fh(`L#N{ttY_vzot{Wo<6`56F=C&Vo? ze>Px%6{Cm#F7~NtES;SUYSnt8&qG8uPg83J!+J?k0_9J5XGiv5$1mDG-UB7JTex>{-_0joL z{nK40K*Hb!8ys{Q3nusIDqIPPD@xQ>k{9MIIzS>1U|Zf10hs@~43TuHDjs0_Wj3Vs z981z1pQu6KJ?f9ojT20nL+i@bEi#ktU$6e!q#pOB&m&lsF?6IowqVLwttUidu)9g? z)fnrYl~4Qbz>nv(>Kq+oXOrO7I}(7MTq<}wefuZ1|B(L4q1QIcWENKyDhCgyie~;ofu*R|gop|iv^Z&;hv)i1CwyZOH zpb}1@e2dY}ERB8g=M0{cFt$Utkp^lM$ok`fZ|TA|Zt$2-b_GSBUIsNKN?Y{Dbb0?i zfRf7TUxW9vwezF<4uNy|v3DOhK+gXFPp>9n9_rpwP}pyfxK-;0$xtc}3!@Ue%-&wh zk3|^7)x6Www$u+N9{gdM9T#=bXm4n*oKjt#N-^1{d9+M=L))%F*Dr%H9h{e8X<=;G zdl9@)W2AiZT1G3m#L#;Rov>{-9|&0X$e)qLs$8iU^u?#{#Sjx$*?aa~i#9-CwcjO- zJjLv?c+GjV!VDUcDX42q^KnsmkQPAbi?}~>S_NCTgu1lTu8^H5V9{dfuR}_?m1EJi z9C}%C3MF+av2g}oOzeYPv&?-T+maV)!C9!Lm5;0XS#>|Md7GdyxPu^UdrM7RJn7z#!)7Ku|bJ`Df7? z2uUgvVFUIlOeI-vjs*ycHqQrDK7X!FOiWBji1rFJC{?v4l9KABM(t5-86U^4LV1`R zA6Lp=7ojmXH?Oa)O@KfkiHR9EESyxIV*_kp_FHFMgEW=`zgOhv=QlM8bO_vb=`zjD z&9SkwXT_2wpuo4auAHr@h%Eff%gX~FQ&sieKb@qp4e>8>(Y8df93ep$ITSLJ2$N*` z{0WR2W*4n=TJcLA**LZ|H;!t1P3d<7dwIg1u_gv(lUS5x?v-bi z!wSj~Y77RfuN%QDQ+oy!Rzn&MK85e))EZV|KhKh$9spb)|Jy6`(BYZ>az;(Y`YV%r z75!jYov!IIYyo?#feWwaAPW%m`gnD$)Z4N5AqW<|rRp599mo)@X@cd*Mu&#R#*Jh> zjVWa9)Pe)vP5gZ&TotK&5|IJlsT zfT|Lsegee}!ij=}>&26y{v2sQYIAUK;NakxnVTCM8KvJy0z$5=RvP=guy7oMmb!)p zA+caP{%?`@uKf`9FJHdM6AevHE|z@u$=iB)VJIptUaVC=F{OID0TaQQn~|ZQso7vM zoKAhlm8boo|IOcymU-dWJ#StFBRRkjJP5;BXOm*U7>JUja^89&RX*rE2R;7n!L@=V zl${|UhRp4H7{m&y$c@Ax7HyLcVu}QYRa&}>;TDb+3)jWZniegV)e}uTRLy{e5EWE* zr=%g88_1X{Ga<2NOmfRjSaaZ%NM=m6WA6S)CA>7WXm8Stzm<`whynoejOiL18zD0TavWiOf^e zcYcelTB_N(^ltY)TFU9Y`&D#qnaQ1aezDe-F)~%dn>}DU5%c<;=l9NAtBV`go`hVd zC+jhjVZ;xWZXy*Yp}AHP6`r#hF~a&%p0`%B@wsRI&B$nT-{3$?RO4X}%ny&8`4+H` z$Bs3aKcu}U-m$-W3d4@}>P0Y&ar@`}AiIMg2mie({Ow;-gHv_tj($8(V#PqGKmqYv z1TP>Pcx(}Pb2bf;?Qq@CHn@MS;!ZL;;~f?@2vy2|^ME+r+WKC6qtHdaMNnt61w?kX zT(3^~MU9M%l$DD&@;4!uTj^d+OFV}`RGgfg&z?O~P*8Ala|2%(8ynY*_4muDT%5^b zl6i=Vh>T4J8~bb)=jP_d(5is{sx@K#!99)krybX;(5x&9|280}TD8t(3^FdC2BtO+ zE;4Lv@I{w8YX#A14GSys?%v)oKba}K&u3%foQw<~<^CaZ2W#uEz5}4eH28cDjoX7Y z4A`{AxY*d(#6%5UU0ru~zUw{dD|k@%13vP-@R6d$ZzZySt}*gf%_^QHPS7b~wGfr7 zS4!5;5vB~KWTA+l<970U_rtdnzSH=$4t4RlA0jR5=yPW%2`e5D^Rkcux+>IVf+57| zY01uBS*Hzm|4{b<6!FG8ZvH_;)%`Js%dZN-}wAw$;M;XHW32XMIjdqX<1$5}EsJGYn z)%44ATHWn^PO5CpjeDz&1g%EX{{+4sDAGI+K~Q=W?s!_xjA48J`P>eqf=m68du~x6 zo*dLkbgL0uoIdLp9IE{bY_#8~uBiv-h>T0i$tmbOoQ2>3_#NkAF8u4e^TV=|3XXsf zCn0nkmwwQB$#Vm7^^=*mrCE8I3x-x^enxS=ImQw$5w2P|o%5 zOFk9?!$YOzsL%e1ppzFVRNDb#9yj8zeCPGIzs>b9fj`{wF|!4c;eSbD*lw_?>OMyu4tR&(6VvUP>^#j;?MQP?DpgD&FEj1<(&4&L0Ue z!L^^CG=Mx0!Z7%~ox&ovS`Na_p}{dh(>x=6HT#VWk!t=Q_5gSCvD@$*D3C z9UH4=W3wBAN&coxuhxD^5J-G~EEsDBE@wMiX|nP?xc1S{YYTo_`sd}N$mpC;#;Mq7 z^I@|PeGQJ>A5?``pAeqWl$yRuWe#{E{UkpuEI67b>lp*;1g<1LfkgK!8EFFai)cFu zTm$h=$+SntBn#IU{v?v9G;Sm5=|XP4^*#r~>m&U2SJx>iDOEGX2@2<@r|3_gesA!v zc{xK-75z2sDnj{A0$kl>af%occqlbLm8@1od-a3or_75{`O z9HiK1xPRJ8*i(@28DLGPn1br0hy!El@hHchKYJp@G&i1qcUJ$UQwWzyJ!*cEZ0%~E zIr4r3-cU(4ZZ>&jmcOGvpem5ow@cOA5x)7!&twY{7Ne$ZDi6o(*l+_jJ1Z{-%j;mi zB>Se?%aA&6Q>B%bM`uOnI`enmuFr3&hN%&EslABP9iM(SqGy-VtI_Oo7(YMK4-6=uBw}< za+|6($m5_J#SM$a4^u z4^FJ^?mf6uQ)0rFmVae*hhqmcGW2W8SJwnAY6_%Cyt(A!Gcs+RwJX&hD)_L18ld|8 zw}If*0{L1Dv*z&2j+K+djlT1lF`5IQsFu4ppbM z;`RM{`<&BzbQ*jv6Kd|YbCU9snr{s9OeQHTzWVc}$!5g&Qy>mvDD(00@ual8?L_FY z^zpd|w92=RN2w8si9}Jdh=^a$&d-B`f6s*2s~U zhrimbr&+jOKdne*U}Utmv~)}%N>Iojx4gOzK|-KqX8xn{0Xxss-X73}WuR2!_8e^8^sis-?Ch#4_k!nZtm0v(9Z&1_j(-oD+wA_JqL9D=yR5R3idShwt%M3N~8b!Ur7i5&mOmY$KzL5vgII#Spp4wk%sH>-MAWeWlfN4 zeTa``8I$%M)vP>jFz=7B)(Np9TwfDx4hiws+j2RB31cGj#<2I~bu3FSw58bBFICn? zKl-6SN)pC9QcYn~^3>Zs4iKm{3-{T#?xtskbMw@v7`vtghx)lKPX4z3nVzc!#iwV; zvf;3;MA+wyPgfL_Xb!B`9SI7FH*rYW?F52WJZgR=4WBT~*e2d-41-M*pA?g55;+o| z5T9HxZdaFb;;e6{Vhls%EuGEV7}>>)R9dH)vHImRLXzy*ZB)b9`!}n!l>|LgNDvoPO7KdrnQaH z@jkhPm7+Dqud1-?r4RbM_H)`*(8Q zE7Y?tp~ju)(QmvA<$h3%2~8o(wmj|N!3j`GVkqOb9byyc=S*gX)vZo* z+73%lV=p=8TlsUCY3;J!ObR1(KtnSxagK|isTp$_Lq0m&n%lJ0Z?(~G>!Y>n>fX)- zq!dWCkmuvX_YB_aW%obh%j7o=PF`LP%&qV9XI$}P;%mvF^MXoZ@7+D-)j}ty$=Qe@ zMuh1zqqDeN{PfNeZ#qi|qni$lA|F32y(k;x(5ZDAWW(1mPq6y(?K{IMv2yo7gh0oO z)O0OK#9VaR&m#{*BQ`{Dr=xoQ9$LkJOXu3r?x^_)Q#z?MdooL4`m2!?Lq0Fq2EWZY z{IqR3PyUkEGC_4wV3}Fr=lpJhfz<6!ZU#ycOd`$3EMn{LF2r8A|jn`OIWNB*pEJ3hRs++dZNW@6#>dplP zA>F}=U=U+tJzPv?S@%d0defoaG29~4h%RkrKpa*hfVfiqNCNU2%Gw5ZD{)S2FASr9 z;9G<#oG@{W3R&m44kJa#MyECz9Td51K*VJFche-yBMl2D4%G;DrIs(_uJ zN{jmJ89lFt`x!`M@v zf8aJqrCb~y*ZpHXl66C%>x6IF~{M%qN}d`8u4<(gOGKbOShTKr;+q%_ip zotLaMp&Q)FN($361p-2br|9@$(joK;&%*zZ_BovJh6^{)KVQ1&EZyhSb7eHw`Py9F z*2sFaY4&*nauL~S*_!8GV%Vv?35>dvhtdy`p z*l)d~X+`Q}i$g0fDIIWEuiR;FE?@2J7d2a5rOPfsL>r6TRG8L!_QGqsikR9qx7c#0 z&lmmFzl1adp#)0uG=p-{jl}&m0x}X3jdf=YQlH0qIxOj-mK?6Ge6H-gfS4j`C?QWe zJV=P6z;J<0jLU5SX*ZR!{_FUvz~P1GWUu|^$>V)KxyNfp7mU4<6d*{_(m|P-#_sO! z-rg;Nfrxd8X7AtEWM*b&XRl;N1XBvTkNx^3Mnglx%p9hWkB1smre6=H#v!Ai94<8z z>)VL&^1dJAYdIP^E+MUZ&{kEA0q<>UYO1Vsl#}brbYWjxTeIRN)0wdyt2uH)s832F zuFK6YEM&rTySlnUMMuBAxuJSXwYDFl=gOhL4Zm*#zBVV8B5&F)^71zn%w|*?{@lzqht5%*?u! zF$3`CW@o{z{Z{11RK zubS7V{YoSK>UHLsbI6y#$fA&!!r-+ouQ(++Zn%}Mhi&qC!6u?Uqd++y*P|WiG4Y(n_Mu9{h7OyfAb)|%-F=eZ z=J8m;!uIB!F?Uo1fm66BhOqKTq&jlOx=_Ztc*X!4 zp4DgQg$iCmFMnT-@y0Q6rn9hjQrwQ>(YCDnf6U9HC-Dt-m_S01P*jXiVl3stW=SXs z6zcfH4!%MF0d18?<-lm(V&DRrvqteW5|tf^V5V4f zbo8fBPkphK6Kjx_0~GS9UOK`J@gsFsIfIDCM;hxPL~4q|+;895SXoOugx{H)qb$kB zOJ=_v2Oi|x+=U~6FE@5}xTn6G%u=P!&(GuGAu9G(CJ}V>UW~mf=Bx~5Ok^-OH|MZX zm5^BJ@JCcvS9d3)b2_chy!Z-mfco8~Y7U0Ojbg>4;vGNAqkH$Q6hAeZc%OHWfTbpjX0|@9bL#337DVd>HtWot zQ9nyGZbsW3H6zCD?0m*d^hgXXc_P7gia!0MC3!XM8iR;6i^TmLU!N2H$@;@%ENdvC zq@iY;QT{G0cY?!9#)?O(QX`)DE9k2Kj8&2zH9P_}w)?7mx+ANbbP6AUVpGz_!gOJ) zl}-dL8-7Yd0cyX~rB0W!{o2Caek+xvuU2`?>>|ht41@Sm^17>6a~pw)@9kYH{oVd# z)qmV7;0tAgx{yWo{C~+sJq-?@!(s@zHSf!a@Ng?jOALOdyV`o77hGI!3!Hm@NelG8 zAS)6m>>D;@V8{o9?o(5o^o|qL+>c%WZ`Eh3WN!Y~CN@hOMw0@6#);hG8jL+UPkIf) zw$Ap^(?8}3-1Uy={baLdPU3*q4$p&Of8`e$#F~YGFJe@0_BnYUYMk_Pgjezken@DY zIH*6fKQZMl!Q(htosbTg{V{ibH!b?G_kk;J=Zb?IW@Q;&a^3+|qVw;nsN9RGSa!Q} zX?<~VwFN~P#ZorA?DfvF4$1xq)Np_Wir3(}01HVB1U@VkLhZX0oPI5Hl6EXU^-KB( zF=Af|CrM-YyOXSt5>ozslMd+hh&xzpa0r+wCTd8^M{Q8ZwooWDspzkVnM^rIj^sv@ zNYK247cYPc7W`E;uFttI#-lPd;)iw7r!ZD+L%El$Qp}0qy1n0c4G& z(jv|wi$CB5T%^J|U*PZ=bNNE>`mWQ$uFSbSRR6shV2Yv+WT|FksW6)S)QuX^S;sYNJ^Z%rPeb_-_=d%kHI@u2U-8964x^tej=SU?VyrRej@m~@ zWKpq{fvFNy5xy8(o`S?ba@|UFUaYZixC2pXwicPVeKjGpo3tDp9E^;}Co0*hmFJW} zDJVz8`1o|Oa$qQ)0jNm)S*FkLu)GIM)%to~Zmv|es<`kXJ_*C1Q&B1!EouVuJ2qNd z7WtaIFyyP9-u-kZ)r9n{*U@SWGbE}D(80Yl#%$Y?<_5N4#6-l-+G-xeQeQ_d@-BVg z_Bb)47NoMvc|TE+gTwoXL<7H!%A8UrixVVA^PZddDm2qf?rl^w!bNGJEG~+G+LyO? z@N68Ag!&r>REXdZR6*TLr~L5www)}Z57*$;R?BqXY{-P6TUh&j&7lDw`HDT<9#3LW z99hJCcTRAzUh^CZ!x{smb$G>H8Pf$;oe-FingrzZbzHHbAv}{I zki?OWqV< zkk_kY3Tm(D?A_!#a^TxlBqb%KpQ)*<<4x9m^e-x6F7Q@1p*lG^nJiF-@bK{PQO=St zg8;$Q)O+BL+5G$_|N8aJ*ceb6=$M$8XlMeCzu^LC00IHA=!b`g+l&2(u&|=?@;2Xl zPe(^C5CS_o>KhqZhFM?_xU%Wj+1}rrtEs82uCAunLwW~z6r#aGIQIl{){7}Tua;(| zQ;K24!;q8K*u<(VEz_n_O=#eY|D@N~p2;4l*Ov)XYf*22<2-jA&B;?H)Oayn+w7#| zZE1)B(3o{mYLwk5c>xxhIKnR!_=Ib6(vp0@24X4a%Q_8U!WMtu)D}vL7Oc<4e2_*`e_{+W_d<- zii7rA)?t|jKL{uGk;OETAodACZ2V+?v{kGc`n3wd_FMP^zfBUd;HQV^dh1mXZLOikU{ z+NuZsHh6RIXKFEzlRRLbSy)P%nri)J2c*Mj z)4$lHBdfig`m~e6(9kfM+@;0q@?f%1h4~Z6v%PuqhC{b{ee}t8UVc7^02AWkuI%hI zfl!uh#j%>*B!cR25RNpe#8h_}QQUX0FAS!d{ez@-^30kts}0fX9)`t&J_AyS#f)?V z@`RGRL+w$)((LP_scO5NY=^u_LWP43A0$I}2WC@r0 z=(Z01k||CQwIIvOS#O#D(GN#Rz;k&eZyuy3F>MVJGG`9ton|lsryv|xyFYBcKkPt*$b!x9MQ9Z&nKdnn?2T>wm4I2QPHp)Vom2|m zSAU;H<)h#!DC@m{1D=gevYIYM55uq0IwY9Nf z<>ag=F9+!;dpo=PyK5IIsV)#y1U~SmhzO8G=QLh`A{JbgwsLkVev;X^-=3d-qthQ1p5)$oG+EVjwrM~a;tJ2s{oS1a0LS=bI zBS{c@JFdHRc7pd*?nCg3g1zg}@@JF^y2B={pZR!_9nU%-*Ltd7>&0~r(6s@?C;)7}xw(1z^eGa8$;I9r*m#VLj4oRvwe|I2?_NKJtPv20#$eji zHN)AK0w{v+(5b# z`&3-5rnq?f=6v_-*RO1n(^*O4Tuq+mJEvQt+=1jnh|S(W{WpRd_R$P>c6K1F8yg!k zx#K|B+b_3(XfdElY_F`qLsbKwCvps#JwDtY92@|(0QNojq#$NYMMc%@x(|%c$>n9C zdNFFiH@(_{rKP2b3Foz*C?LAvm2lPIxw^Ux_v$~-wWe-wY^7MEq^c>G6CcY$2WbwS8u*yNwsIDw{IUdGl%fwzSBe7^G zA^avmX(sNe7G8uJQ8QHBiYL#TFW#$&{X$b2Rbznb)h~)`5<(}w@0|WDc-MOBR8Loh z3SxZY@v2P9kFjYWn|N!{F-3DAzTij|Mz)d!;x?O56q|CBp(GQrcjlT{NHf*9pIND2 z{*0_A)+TxS7q9qp)52-NxWByBi35lhJ;uo$O$(AJy49dM3{<|0v$MnqSU!-=)pZW1 zLFIq3mkkuy$a?efimVL@c5wF(p_V{$+Rx^D3t8yvRoJ;^nUKL@M`2f5?8pcGl#6=v z6P@ZG3yv+>Mkl>e=Eow~K(I6llvXJARoOjm&bEP?rm+Q8{UAC4w0?g)lN!MjJi=tP z2Iq|da(;V|YHRViz1W^CWd2lFQv(N4DwkBNsjt5rPUBz76oUxg-i~M3pGn<*lu&W| z1w$!x=AoW3Y}D63ZF!L0jj-4srTRKJ{NCAF@J-C)&BIrX;4_Wu#@^FY5HuSY96UWa z`N<8U#gTV+cc6()US3{Dhq#ZA4-hv8hr`OsN}ws&bt>Nir~+gOEmo+ULYLoVQ|W0i zf`at3*dN{9{X-zkrmLpr#H=8mNw&jr`5@)ADT?f>nAn1bjy{?$a5|I%0nLltfuS=p zq9UOPeg63^%b)w(OR%=zySjiLgOkk%tr2X0wNXmM6l)X$(FKv~^^Fa?`5M3|aqEqy zq%j0$8CdK@VT_X0;?hz;5DK+D74+q8wsiQNYu3olgkB;2gaiLx?0qP{C(q^Cz!PG& z^Ras&Rk2r7qiBXgDCae|wsHqNIImjw^eC0|v{Syq#v;`SJWSG3WP`V$ySP^PVV~XkU9IqXM4`x zf?3ut7!O;y)c!GW?sPz$j_HQQr)}@1x>W^$x1qS@Vo(GHx*Y69c!32N(^z+R0z(pN zfTLL#m3414oT5LqzyAt?N_-O1G~-p3Reb$##g643#vE*;_O3-XNh;MJdx3Gs6C@GV zJzXt&G&@do>s9ChfsDOz4BqzsY54}f@mTpBkLvMSFM^0O+T5AhIoB*Z2kY5q4n#$r zE=;=)heuQO>_t7P$UZn)^tGY$Qya9+s5)(X%Go;iH#VWq92)r?tzLES!nW=<>!vS< z5`>Cn2(gV?_g{^38aMxHwShl*!)lL3R}d!Reb&%iNX%?$+)S!LBP8Zi-sVXO5m!6w z9OPe+GL!C>fAvX4ds@u%xS;mnAil2;Q8~(iETh%){efxJ>8B)UiMBe7gNbpPsnCd} ztR*%5P5Z)Q$@fRm_r`uvi3XIJ1~Q851?E)IqkVGha-YIZU6(8EzIsW{cGl1Z7ax-r zKYvnn8_!?H?^e(0a926TZC^iOi*p*`$K1DISg&>6%=2; zR#K9aS5zDwR8e>x9U=0&%2jKz^M1SW4Ag{t%{U;_NWdzj=x1Y7Pk`G5idrrDd`lK8DUZO#eUY5jIXz`9x zC^ptOIng*KKDN#Cdg4l|3cP5$(Sw&PN|Ldx3Z?))gph&?=*7j%HYDi8(tM}%+m$+Z zdmbhCnZ^XQ50O$-1iE^sG$pXRrq6dO|N=O~%Dfo}O5R#=oD|A|>Yw zL-lGZmlt(j|I;|1PVBe-%g3Ll4_cVOzq_j#f&Q9;)pKES@#=#8Q;vDJ41f?TD=Wsy z&lq)H1C&u%RaMn|%c?Q`?px*0$;pL3I1)-O>o=vx2q*2+A?f7!HUQM_9%RERL z1p9|Odp7p(YH%FqmX>Cgmag{t<54cX+I{b*#>eNzk1HQ!&9D1^Zf=P;Q7wCiS~k(mjt!WF188uzvL3Ej z$TC7ee1MIWXR+^h-?TYWTas35k-6CFy1Vm))6D{yiNXxyV(-iKgN3b`jODiDljWrz z3pXRNrjtKC2XL9EANcJ>9ZLLsPt|sFd)cu^HK|>eDs%QE527q;FP9g~{d6z0HXfpg zwMB%}P4NQ`AB-Na?sJNzkREy)>f^R@G}cPVV~uP zyTt?^hs<-WdX|H>a=*o!%7ZzE$vN0lh0lw=9%XqvHVvcjMXARZt52mQ5MtoyxflU5 ze(^>0nY$nG$uq7UHG+WVs8>x`GckkRpd`ZOIst6DyH!ue%PUMzKQ!vln-$O7SY#2|^Ql4@rz)bkNJhCY_$ zE&`mk#n&04L*c$R1Ifd^F`UVncKei>?GJXx6{Gje_cy0a6_&$=zN1ss@tI>nOMmw5 z&vvwHE?D*M(3Tvpu2M!GB8P6dF$tw>-OJCyD$>0Ll92C(k6Uw64 z%ZDT4Oggcwn$MP|Hkk`myIw+Lm99Nym78rMDTQ5vV|_jLJq(kOyG{LH`>~OOSnnNN zIM-nLejROyBJKwdoh3}k$ICw-DzV-hkND+K)dbR*P+I;;@iIxcB*lW#ulRA<=1|oK z8#fGs8CwkB5HV5Ujs36$To7RxyW;zFlcZL)+!_wsW=P%>R6Q7B-J?TC+8}s<>)=I# z+jbWu({SwQG`04Et)50s)x+@Jq;?13}yG?<+&%WTKYS?<}($9k26**J%7qXJomo+w9@9*xUw?bGivxJ zY1GmI`{o>dej&ow_R!%mH_2n}*RN1l*R%Wallp=%hEO-S{))`GJh2b1zR(5X?9#Qa z!t&2oG|ZoTucx#O4euK>i+8#6o0t6^;O3Wp&73??Mu>dMp%xNqO1bb+QaqY_IQCI_ z?0AE%6I0P@?5Wmc>T*AH+7*hmMhJWfy4o2WLsd|69p~^UKJo#O zt3Lm|W%_GhKYj%bcmrl~H12NmmrggK(5iGJwO6_$QB~I-ZmBn_XNJ^d>ZeV6_zHeA z@!pR<=fr1s7H3BMsk37r?)H=FM9)01Z>p$89`-(07mP-={)w=!DJI5bZu#O`f33x- zH#cE;(spUY?|DVHldLlmW2SfeHIDQ<(crfCx|p~-!i&h!P0?ScnC?%WA~}VzWBPRB zl#mFjxn!zr7^iAqH9ri?%#c1rVO*nyDZ|%4R*I=&>~*D*HWAX%W>tU4k|53uHSmoh z4HUK&a&LJlGr{?%?6H0G5~xrF$Gr4L#iB9q+iTBDf+eBv9{$YHetl^y5NO0< z)9Q)b>Wti~IZv{edq0vwfEU`E&l5#m|v-PCVYMJ(jxWFuEny;6~W0b$+ZS z;<+&uA1{!Uxg+qNN{*Ji5@8el9 zKKo#+=}=_qWwX-X=v>`}f8| zH_gA!d~U{m-Q({2j^9?@Ei<%?JXs92LAMdiyvV+9zl2D(?jPQKbj|d3eo&g}s<1p< zcu?`N^ive0i-H{5FCKWD;_iFQJ+6H4v*>f0uYGuvRh`Y8>U1jYLC*F{H|?tfL^s6W`4v0mDK zUUX-6n9LTAH;;anV_LVovP(DlqI*-|s+WYlvVNJYWm`2vQF>?S`i1kr&IoNdt6R^v zvtNg=SzVth=&V;O>qx)wpqAsm}HFSGInPpDIm|sx1vBAVh?OLtd z)VIjXEs(;U&BMpe&Yzi>?j0r|b&BH>0-gV5VL6cu;k9Ce@Iod8M#rX73-;bYxTGK= zpRu5p_U{dFsV`?Rl&zlgypTFQiftbrsiZ6s3~}c411%Gl}lb;nHx98YkUw9 z5TSb*gGZ8BocX4ZHy)i)JW$@4@Zs*=#kA0?iY4v1B$%;nN_s36KB>K+l$c|vgQu^A zwY~3aU70ui;mW&j^oP@Zg`eFGbt3w|cCCX!8}vERoLmIf+aVoxT}BmUYQSd@a=R&W zNu&>77=|4gxA&NII{YMpwvCitdU%C4IV`s?xm9ct4eCOMEdCrgxr84rlC8E`#kZbU z3&4ERDG>P^Z<@bhF0A5iu4E7pytmvMdBO<6MM9ge-u~E5%})Okx)m+u?-lo5a;kv& z#`Zm^dFzW3dhhO_qj%QUg}bt_^z#t=R3IbCy^c3G=a1a}TpTQ4_h*FCgF*S3v|V@q z?$(HCtC8!z?e%_1wa|2Ld8UZ(dE`RDG$})IxAx$t3S#PJV}p}QX2EX1wvEWHv({If z1q4e4#KvNJ&h1yT=Zn{;n2U96qs?(ndnTRZshT&#>FFhTVeXl^7FK79R=o#=6lDtI zW69Mv75Xdp0jsZMxVIllDYhufl=5csh?o0W8{%jgiqUjD>* z6nwe_0394!qzi%qw3M`&1s64!oxkI%UH+d;h)pTTgj{GabmJi5CZ!DMOX1-W$>97@ zlHW-ymO`{gwS}m|e|Sgwm|jmtx}e&$i*;O|`GP|ef+XSX?L9ca@;aTlS7lV#`z6Ek z=cKplnAU+ylvCV{hj(Uif3mtC+eBzB;HNl)Flf;~etaAEF-jLMor&<>!$r@_DXbSi zBLjzn8u&tn%`oCKaJ0I5m9(DNSi`~LGRaFzXi83b2ldmk^rawDJS+zkT@Shx&C+aT z_Z=5rL~Q!&8ioOO7BEQKFJc62ZZD(!-O(;9jb*^!-f? z6;T$YI(_WgD3hP&X?(|{0L3|H_BRhFa)!e5cqZYT|EcQk#SmDtiY)Q^=j4FZc;}E9 zU&wxvx#VaXlpZP-@k$>x<&|R>8Zs?*DA8eE4CHXH0ZX8m<`g9D{ki`|5dE2FonpX4 z$^Z%T{L)b^jV=I#BW+a-^E{hfH48~U=9Qfm`6rKutOa@CAMBqbx*oNE$t^k3mEE1o z9yCPPu!0mA1Fq$!-SD<9yaQ=N*pPXaUf5q@K!e`I9>N+ zw^6*2>uGIFaNg?xJIWL?5_T zGyD3GkWjhQ>whiITMR7-mSaN;U$Guzl9FscMh+E2B^y{wHgVZQyc{Z#x~pVb3*jiW z4=Pks{~|X(4m+RJ$UMNvUq{BAI$9oEqSKzD+Q00iSqIlzB68mRN;yOO?{At5#IpV- zIa4*+;}scm-yy4g#?wi(q_rTT$#p|h`%Ie@47@6P&zj7o8NKM z1T{c>DnJ5L06pY1&w3(2ZJ2<7-~qH=e>m#GR^`LjArC?O$4B}b436peK?5QzD?cYm z+6bcl&ojjH3sO@7#S(qGT(DKcoY|(VDCP2tg zQ6XALKmfKFkhB*1vATL-4z}g+t1Z+#e2MABi-WUmn7KJ2I9xV$-X@N9GQdAmd&$ne z23jM6${CulloVZA*`NLKh0zr9AY1hcM_NmZ4+0SqYv+PMyZ|NHpa*042x&Ic5FXw? zKc7iKQKg~L4k@}VEZ~aU^))}ff33A82Fdra5l6X_$T3%fIBx-KS2VL~)r5nso zMb{HO>$61R>R8kV{Vb5fbtgbOB zBGSqOfpPzBF=;YDVE$M8);&Q+zQ0zMS5o5PGq9(El4j)D{aq&-~eZeYj{ zXO424ih~+dQlgj)|K3sl?F9`Q}`d^`RXcP1#>GL zva+*vb=)!BLEQ`3KbZVzmSR)*f#NhjdU_h0y=YKZx9Yn_Mtr|ltsaSV{}l@`Y#9EG zyN!I>_B$VPvdO=>1>h+;3+bvkE@bCzZ;jPt&utUbeR`<3cV^V#m8GS-oLq9|FDx#3 z2#>R|8m?IgR9bpnl~0*bQC3=dx%KufkW{HT@kb{~xQbMNwiO?7nZkVYAKeTwa(Pa1 zTWCp`uG31gvKK$X<3TnAHDK!J&*#j{Q`1W^2?@>K*8@SUOE3Gm9_E70`=>uh+1gSh z8yXq;vT(;HW*YxlgJD8GMz7#?h*9Gp9iN<(mzABKE`YGyH(kc}o}DKfKf%Tt8XBsG zea7N4I^@o%W|{`<|x#@=>dD1)^C z(IJ05tRa^FXfD80yVC-^*YS$>hZ~?RT?`J9KOT+;R6+maL{m__fxdee+aLF?`W+Nn z*Lwd+Vh~LjWC?9-DgyjQ1d==T%@jXwP$rw7r8Hhs$jHb5%>nS=XhuiX@XinAuN#y) zQc|#{n`%%o$Bav2Q&3i>OoPK=^ev^Kn_?k-Ez^*Oso4$X@1D!{mm@F&osMIf!Av7V zT@U+*yKA$wh43`NTYM>X_3DO(b(JcNNIraA-0`t7kX=zyP{{b_EHGj2k9(kF7nW?{ z&d$#E&JHbVwQuWS5V=JEgUAybp@3zCKW1aw?>qllhPr)e&Mwk`P4K=YDlg&dAo zSJ-R<7L{;%l^#w2jn}2@$MeDD>_`kuO#gPUwR8?6#c4pcYG->J(eFw?6s@uyam~lp zZoi{RM#%L_YC3X@>_v_!P>*TgZmJ2h>}rQta|a>|3k$OTd8YI;kV}2C z%E`7f*GR-^&ILz*xC21t3M44 zxPYdn%&eueP1Es>ZyOzSb-TmyH{N;{L`K>IRrva~Jvd}#6C)$pIy|o&pT77euCL!* zvNHixK2TzAhgesPZkAkckepq%ft66n;Qf-I1v=e6L;ZdL^KughQzGp-?*EJn19peb zhHF3pIR%B@n>S9k7p)-YoRpviHpt0|#@48||NS92Vdu+;@88Y9nSz769kG?ESW@rj z%*|`>9_~YDqf=a6FD51?1Oz;Q`2a7zJQVAbV&Os^JeAPX6J%xG5_@bs+JamE@n}_U3|%6;9+r?jmB^)L#Bzg1*+Sy=&52GN2y5xaO!*SvQDje|AKeIH z_oMNk-d=ubt%tiIc`s>N-w zK5_yAVNLnZ6!IV?%Klv7V0~Tq3nu?VMurhKl`wJy34#|9HG+RkjCk%Oy0f!0F`G`G za{5@v*Z(ruOQx6MQBguZH*8lHmLnACLwJHAG9UBdS(%yX>Bj~JSpJ+LjD0kTry?57 zj+`T}%w5~b$uVP6$(XO_BpX8esO-t?xp<2o{@u!o6m)>+hCmRrJ3hf`Y7;qbHA!&MiySaM1V`wvQZczEVd4_sLCooxSsULZ;x0;1F*e*wiD5G_!1_^BAW z7XckgH4m_}$;dQDCF*@!{F0PZw=y~8Uk0~w9U%gfKZ%M;Uqf9TP-CLO8$>xc%7K-m zMU~7}9T-$`0*Jwc_OGqTEC*WQ$L4p~?VUWpFH%}s2@Gs46h!bB$a_0Yo#Re?f2mP0 z1%+rjRD`s%$`~x`q2-U^(knwlww0S~1(jWlBFwEO9t8yjUY?#S+U|}{b6EuiAvAp# z`@VqijT+FwXA%O8W!j(JR5;5NR^e@QxT1#I_!JBk@R?- z`FOoumUz%a3}SRWT70+kZ!;~5|j z*#5ruTlhIjFnPDOR=e{p(nYhuO{F4+J2e;AD=8@rfJ6W^;^X_kksA!%Lz-nkZu$IQ z1{e3hVdV8^FKTZ@SsZC4B{s2kocIuv_|1%Ue)eNb5{Vdqb+xn%zh}r*%KPEdBq6hv zkOt6z+CyGm@55zO+iw8Fz3>-raIXp(e3?xu1b>3w}MM#BOoCm{bwl-rapX?xhBa`agg|Li?hUI^(}Z0fgNpJ9?nC zQJ7{7xyK&Ag(~)TQ?IXV(XlOBP=T>_b3oE*2XoS;r0Ariq<&Wv6d?C_+%1O&eF1QcsSz%3K^Tdcc7nd70PJ<&tj${ufu- BN0R^m literal 0 HcmV?d00001 diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 450dd5254cc..67067ca4ced 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -17,6 +17,7 @@ nav2_common nav2_core nav2_costmap_2d + nav2_msgs nav2_util nav_msgs pluginlib diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index d6acd320b05..305140e2e13 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -37,6 +37,7 @@ void CriticManager::getParams() auto node = parent_.lock(); auto getParam = parameters_handler_->getParamGetter(name_); getParam(critic_names_, "critics", std::vector{}, ParameterType::Static); + getParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static); } void CriticManager::loadCritics() @@ -46,6 +47,13 @@ void CriticManager::loadCritics() "nav2_mppi_controller", "mppi::critics::CriticFunction"); } + auto node = parent_.lock(); + if (publish_critics_stats_) { + critics_effect_pub_ = node->create_publisher( + "~/critics_stats"); + critics_effect_pub_->on_activate(); + } + critics_.clear(); for (auto name : critic_names_) { std::string fullname = getFullName(name); @@ -67,11 +75,44 @@ std::string CriticManager::getFullName(const std::string & name) void CriticManager::evalTrajectoriesScores( CriticData & data) const { - for (const auto & critic : critics_) { + std::unique_ptr stats_msg; + if (publish_critics_stats_) { + stats_msg = std::make_unique(); + stats_msg->critics.reserve(critics_.size()); + stats_msg->changed.reserve(critics_.size()); + stats_msg->costs_sum.reserve(critics_.size()); + } + + for (size_t i = 0; i < critics_.size(); ++i) { if (data.fail_flag) { break; } - critic->score(data); + + // Store costs before critic evaluation + Eigen::ArrayXf costs_before; + if (publish_critics_stats_) { + costs_before = data.costs; + } + + critics_[i]->score(data); + + // Calculate statistics if publishing is enabled + if (publish_critics_stats_) { + stats_msg->critics.push_back(critic_names_[i]); + + // Calculate sum of costs added by this individual critic + Eigen::ArrayXf cost_diff = data.costs - costs_before; + float costs_sum = cost_diff.sum(); + stats_msg->costs_sum.push_back(costs_sum); + stats_msg->changed.push_back(costs_sum != 0.0f); + } + } + + // Publish statistics if enabled + if (critics_effect_pub_) { + auto node = parent_.lock(); + stats_msg->stamp = node->get_clock()->now(); + critics_effect_pub_->publish(std::move(stats_msg)); } } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 93f7998c56a..14d1327508b 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" + "msg/CriticsStats.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/WaypointStatus.msg" diff --git a/nav2_msgs/msg/CriticsStats.msg b/nav2_msgs/msg/CriticsStats.msg new file mode 100644 index 00000000000..f02406228df --- /dev/null +++ b/nav2_msgs/msg/CriticsStats.msg @@ -0,0 +1,5 @@ +# Critics statistics message +builtin_interfaces/Time stamp +string[] critics +bool[] changed +float32[] costs_sum diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index b81cef7ce56..01fe9656d5b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -76,6 +76,7 @@ controller_server: temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" + publish_critics_stats: true visualize: true regenerate_noises: true TrajectoryVisualizer: From ab4aaf863aea6a7d1e67e0c2ad536ec1f0c8ccf7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Penha=20Lopes?= <123633705+JPLDevMaster@users.noreply.github.com> Date: Sun, 7 Sep 2025 21:57:05 +0100 Subject: [PATCH 2/7] fix(nav2_theta_star_planner): Correct typo in CMakeLists ament_export_dependencies (#5514) Signed-off-by: JPLDevMaster --- nav2_theta_star_planner/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 0a478f2fb9b..09813efc4d7 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -87,7 +87,8 @@ ament_export_libraries(${library_name}) ament_export_dependencies( geometry_msgs nav2_core - nav2_costmap_2dnav2_util + nav2_costmap_2d + nav2_util nav_msgs rclcpp rclcpp_lifecycle From 8aea1a895f23ca505da3629819b6d16f4d126a0f Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 10 Sep 2025 03:21:22 +0100 Subject: [PATCH 3/7] default empty constructor (#5518) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- .../include/nav2_behavior_tree/bt_action_server.hpp | 4 ++-- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 4 ++-- nav2_core/include/nav2_core/behavior_tree_navigator.hpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 8e345abe0d7..924d1ebc28a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -53,11 +53,11 @@ class BtActionServer const std::string & action_name, const std::vector & plugin_lib_names, const std::string & default_bt_xml_filename, - const std::vector & search_directories, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, - OnCompletionCallback on_completion_callback); + OnCompletionCallback on_completion_callback, + const std::vector & search_directories = std::vector{}); /** * @brief A destructor for nav2_behavior_tree::BtActionServer class diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index b8fb983f91c..10a8289d5f4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -39,11 +39,11 @@ BtActionServer::BtActionServer( const std::string & action_name, const std::vector & plugin_lib_names, const std::string & default_bt_xml_filename, - const std::vector & search_directories, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, - OnCompletionCallback on_completion_callback) + OnCompletionCallback on_completion_callback, + const std::vector & search_directories) : action_name_(action_name), default_bt_xml_filename_(default_bt_xml_filename), search_directories_(search_directories), diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 9e984bc6550..fdabbf28133 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -214,13 +214,13 @@ class BehaviorTreeNavigator : public NavigatorBase getName(), plugin_lib_names, default_bt_xml_filename, - search_directories, std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1), std::bind(&BehaviorTreeNavigator::onLoop, this), std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1), std::bind( &BehaviorTreeNavigator::onCompletion, this, - std::placeholders::_1, std::placeholders::_2)); + std::placeholders::_1, std::placeholders::_2), + search_directories); bool ok = true; if (!bt_action_server_->on_configure()) { From 97de6f21c067f6d514c3940b6f5d44afb2ceaf34 Mon Sep 17 00:00:00 2001 From: Sushant Chavan Date: Thu, 11 Sep 2025 18:20:02 +0200 Subject: [PATCH 4/7] Feature/vector object server (#5479) * Add Vector Object server Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Meet review comments Signed-off-by: Alexey Merzlyakov * Simplify shapes param configuring Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Rename getROSParameter() to getParameter() Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Return back getMaskData() to nav2_costmap_2d Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Add composition node support Signed-off-by: Alexey Merzlyakov * Remove redundant methods Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Update nav2_map_server/src/vo_server/vector_object_server.cpp Co-authored-by: Steve Macenski Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Avoid shapes clearing Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Optimize switchMapUpdate() method Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Switch to vector of shapes Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Minor fixes Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Meet review comments Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Move isPointInside algorithm to nav2_util Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Testcases covering new functionality Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Fix linting issues Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Adjust for Vector Objects demonstration Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Code clean-up * Corrected headers * Functions ordering * Comment fixes Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela * Additional code facelift * Correct licensing years * Fix Vector Object server dependencies * Funcion rename for better readability * Improve/fix comments Signed-off-by: Alexey Merzlyakov * Minor fixing after rebase Signed-off-by: Alberto Tudela * Rename vector object server Signed-off-by: Alberto Tudela * Minor changes Signed-off-by: Alberto Tudela * Update tests Signed-off-by: Alberto Tudela * Merge branch 'main' into feature/vector_object_server Signed-off-by: Sushant Chavan * Fix merge issues and pre-commit checks Signed-off-by: Sushant Chavan * Change tf2_ros headers from `.h` to `.hpp` Signed-off-by: Sushant Chavan * Fix race condition in pub-sub of VO map Signed-off-by: Sushant Chavan * Cleanup Signed-off-by: Sushant Chavan * Remove use of ament_target_dependencies Signed-off-by: Sushant Chavan * Fix review comments Signed-off-by: Sushant Chavan * Fix linter errors Signed-off-by: Sushant Chavan * Fix exception handling Signed-off-by: Sushant Chavan * Update nav2_map_server/include/nav2_map_server/vector_object_utils.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Steve Macenski * Update nav2_util/include/nav2_util/raytrace_line_2d.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Steve Macenski * Add error logs Signed-off-by: Sushant Chavan * Fix cpplint Signed-off-by: Sushant Chavan --------- Signed-off-by: Alexey Merzlyakov Signed-off-by: Alberto Tudela Signed-off-by: Sushant Chavan Signed-off-by: Steve Macenski Co-authored-by: Alexey Merzlyakov Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Steve Macenski Co-authored-by: Alberto Tudela Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- .../nav2_collision_monitor/polygon.hpp | 7 - nav2_collision_monitor/src/polygon.cpp | 35 +- .../include/nav2_costmap_2d/costmap_2d.hpp | 98 -- .../costmap_filters/costmap_filter.hpp | 14 - .../plugins/costmap_filters/binary_filter.cpp | 5 +- .../costmap_filters/costmap_filter.cpp | 23 - .../costmap_filters/keepout_filter.cpp | 10 +- .../plugins/costmap_filters/speed_filter.cpp | 5 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 5 +- nav2_costmap_2d/src/costmap_2d.cpp | 8 +- .../test/regression/CMakeLists.txt | 6 - .../test/regression/costmap_bresenham_2d.cpp | 159 -- .../test/unit/costmap_filter_test.cpp | 20 +- nav2_map_server/CMakeLists.txt | 56 +- .../nav2_map_server/vector_object_server.hpp | 228 +++ .../nav2_map_server/vector_object_shapes.hpp | 413 +++++ .../nav2_map_server/vector_object_utils.hpp | 142 ++ .../launch/vector_object_server.launch.py | 169 +++ nav2_map_server/package.xml | 3 + .../params/vector_object_server_params.yaml | 30 + nav2_map_server/src/vo_server/main.cpp | 28 + .../src/vo_server/vector_object_server.cpp | 558 +++++++ .../src/vo_server/vector_object_shapes.cpp | 559 +++++++ nav2_map_server/test/unit/CMakeLists.txt | 22 + .../test/unit/test_vector_object_server.cpp | 1349 +++++++++++++++++ .../test/unit/test_vector_object_shapes.cpp | 805 ++++++++++ nav2_msgs/CMakeLists.txt | 8 +- nav2_msgs/msg/CircleObject.msg | 6 + nav2_msgs/msg/PolygonObject.msg | 5 + nav2_msgs/package.xml | 1 + nav2_msgs/srv/AddShapes.srv | 6 + nav2_msgs/srv/GetShapes.srv | 5 + nav2_msgs/srv/RemoveShapes.srv | 6 + .../include/nav2_util/geometry_utils.hpp | 43 + .../include/nav2_util/occ_grid_utils.hpp | 100 ++ .../include/nav2_util/raytrace_line_2d.hpp | 146 ++ nav2_util/test/CMakeLists.txt | 2 + nav2_util/test/regression/CMakeLists.txt | 3 + .../test/regression/map_bresenham_2d.cpp | 170 +++ 39 files changed, 4890 insertions(+), 368 deletions(-) delete mode 100644 nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_server.hpp create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_utils.hpp create mode 100644 nav2_map_server/launch/vector_object_server.launch.py create mode 100644 nav2_map_server/params/vector_object_server_params.yaml create mode 100644 nav2_map_server/src/vo_server/main.cpp create mode 100644 nav2_map_server/src/vo_server/vector_object_server.cpp create mode 100644 nav2_map_server/src/vo_server/vector_object_shapes.cpp create mode 100644 nav2_map_server/test/unit/test_vector_object_server.cpp create mode 100644 nav2_map_server/test/unit/test_vector_object_shapes.cpp create mode 100644 nav2_msgs/msg/CircleObject.msg create mode 100644 nav2_msgs/msg/PolygonObject.msg create mode 100644 nav2_msgs/srv/AddShapes.srv create mode 100644 nav2_msgs/srv/GetShapes.srv create mode 100644 nav2_msgs/srv/RemoveShapes.srv create mode 100644 nav2_util/include/nav2_util/occ_grid_utils.hpp create mode 100644 nav2_util/include/nav2_util/raytrace_line_2d.hpp create mode 100644 nav2_util/test/regression/CMakeLists.txt create mode 100644 nav2_util/test/regression/map_bresenham_2d.cpp diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 3191de1727e..02fc680afe5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -239,13 +239,6 @@ class Polygon rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( std::vector parameters); - /** - * @brief Checks if point is inside polygon - * @param point Given point to check - * @return True if given point is inside polygon, otherwise false - */ - bool isPointInside(const Point & point) const; - /** * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] * @param poly_string Input String containing the verteceis of the polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 5e13f5d5ef8..b37f677fb57 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -22,6 +22,7 @@ #include "tf2/transform_datatypes.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/array_parser.hpp" @@ -229,7 +230,7 @@ int Polygon::getPointsInside(const std::vector & points) const { int num = 0; for (const Point & point : points) { - if (isPointInside(point)) { + if (nav2_util::geometry_utils::isPointInsidePolygon(point.x, point.y, poly_)) { num++; } } @@ -602,38 +603,6 @@ void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr updatePolygon(msg); } -inline bool Polygon::isPointInside(const Point & point) const -{ - // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." - // Communications of the ACM 5.8 (1962): 434. - // Implementation of ray crossings algorithm for point in polygon task solving. - // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. - // Odd number of intersections with polygon boundaries means the point is inside polygon. - const int poly_size = poly_.size(); - int i, j; // Polygon vertex iterators - bool res = false; // Final result, initialized with already inverted value - - // Starting from the edge where the last point of polygon is connected to the first - i = poly_size - 1; - for (j = 0; j < poly_size; j++) { - // Checking the edge only if given point is between edge boundaries by Y coordinates. - // One of the condition should contain equality in order to exclude the edges - // parallel to X+ ray. - if ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) { - // Calculating the intersection coordinate of X+ ray - const double x_inter = poly_[i].x + - (point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) / - (poly_[j].y - poly_[i].y); - // If intersection with checked edge is greater than point.x coordinate, inverting the result - if (x_inter > point.x) { - res = !res; - } - } - i = j; - } - return res; -} - bool Polygon::getPolygonFromString( std::string & poly_string, std::vector & polygon) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index fbacf15aa24..fe3764c443a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -461,105 +461,7 @@ class Costmap2D */ virtual void initMaps(unsigned int size_x, unsigned int size_y); - /** - * @brief Raytrace a line and apply some action at each step - * @param at The action to take... a functor - * @param x0 The starting x coordinate - * @param y0 The starting y coordinate - * @param x1 The ending x coordinate - * @param y1 The ending y coordinate - * @param max_length The maximum desired length of the segment... - * allows you to not go all the way to the endpoint - * @param min_length The minimum desired length of the segment - */ - template - inline void raytraceLine( - ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - int dx_full = x1 - x0; - int dy_full = y1 - y0; - - // we need to chose how much to scale our dominant dimension, - // based on the maximum length of the line - double dist = std::hypot(dx_full, dy_full); - if (dist < min_length) { - return; - } - - unsigned int min_x0, min_y0; - if (dist > 0.0) { - // Adjust starting point and offset to start from min_length distance - min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); - min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); - } else { - // dist can be 0 if [x0, y0]==[x1, y1]. - // In this case only this cell should be processed. - min_x0 = x0; - min_y0 = y0; - } - unsigned int offset = min_y0 * size_x_ + min_x0; - - int dx = x1 - min_x0; - int dy = y1 - min_y0; - - unsigned int abs_dx = abs(dx); - unsigned int abs_dy = abs(dy); - - int offset_dx = sign(dx); - int offset_dy = sign(dy) * size_x_; - - double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); - // if x is dominant - if (abs_dx >= abs_dy) { - int error_y = abs_dx / 2; - - bresenham2D( - at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); - return; - } - - // otherwise y is dominant - int error_x = abs_dy / 2; - - bresenham2D( - at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); - } - private: - /** - * @brief A 2D implementation of Bresenham's raytracing algorithm... - * applies an action at each step - */ - template - inline void bresenham2D( - ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, - int offset_a, - int offset_b, unsigned int offset, - unsigned int max_length) - { - unsigned int end = std::min(max_length, abs_da); - for (unsigned int i = 0; i < end; ++i) { - at(offset); - offset += offset_a; - error_b += abs_db; - if ((unsigned int)error_b >= abs_da) { - offset += offset_b; - error_b -= abs_da; - } - } - at(offset); - } - - /** - * @brief get the sign of an int - */ - inline int sign(int x) - { - return x > 0 ? 1.0 : -1.0; - } - mutex_t * access_; protected: diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 25238cf8a35..da4720ff798 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -185,20 +185,6 @@ class CostmapFilter : public Layer const std::string mask_frame, geometry_msgs::msg::Pose & mask_pose) const; - /** - * @brief: Convert from world coordinates to mask coordinates. - Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. - * @param filter_mask Filter mask on which to convert - * @param wx The x world coordinate - * @param wy The y world coordinate - * @param mx Will be set to the associated mask x coordinate - * @param my Will be set to the associated mask y coordinate - * @return True if the conversion was successful (legal bounds) false otherwise - */ - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const; - /** * @brief Get the data of a cell in the filter mask * @param filter_mask Filter mask to get the data from diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index 097537d40c6..c805a537396 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -44,6 +44,7 @@ #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -190,8 +191,8 @@ void BinaryFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { // Robot went out of mask range. Set "false" state by-default RCLCPP_WARN( diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 06d7ed2d025..58b89740b8b 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -193,29 +193,6 @@ bool CostmapFilter::transformPose( return true; } -bool CostmapFilter::worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const -{ - const double origin_x = filter_mask->info.origin.position.x; - const double origin_y = filter_mask->info.origin.position.y; - const double resolution = filter_mask->info.resolution; - const unsigned int size_x = filter_mask->info.width; - const unsigned int size_y = filter_mask->info.height; - - if (wx < origin_x || wy < origin_y) { - return false; - } - - mx = static_cast((wx - origin_x) / resolution); - my = static_cast((wy - origin_y) / resolution); - if (mx >= size_x || my >= size_y) { - return false; - } - - return true; -} - unsigned char CostmapFilter::getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 48cd6c74e42..c432a917e59 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -35,15 +35,17 @@ * Author: Alexey Merzlyakov *********************************************************************/ +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + #include #include #include #include "tf2/convert.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -200,8 +202,8 @@ void KeepoutFilter::updateBounds( geometry_msgs::msg::Pose mask_pose; if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { unsigned int mask_robot_i, mask_robot_j; - if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); is_pose_lethal_ = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE); @@ -366,7 +368,7 @@ void KeepoutFilter::process( msk_wy = gl_wy; } // Get mask coordinates corresponding to (i, j) point at filter_mask_ - if (worldToMask(filter_mask_, msk_wx, msk_wy, mx, my)) { + if (nav2_util::worldToMap(filter_mask_, msk_wx, msk_wy, mx, my)) { data = getMaskCost(filter_mask_, mx, my); // Update if mask_ data is valid and greater than existing master_grid's one if (data == NO_INFORMATION) { diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index a66886bc6d0..c1e822cb03a 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -198,8 +199,8 @@ void SpeedFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { return; } diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index b897ac515be..c09d60da61b 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -45,7 +45,9 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "nav2_util/raytrace_line_2d.hpp" #include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "rclcpp/version.h" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -768,7 +770,8 @@ ObstacleLayer::raytraceFreespace( unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); MarkCell marker(costmap_, FREE_SPACE); // and finally... we can execute our trace to clear obstacles along that line - raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range); + nav2_util::raytraceLine( + marker, x0, y0, x1, y1, size_x_, cell_raytrace_max_range, cell_raytrace_min_range); updateRaytraceBounds( ox, oy, wx, wy, clearing_observation.raytrace_max_range_, diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index f3903308cff..28798b0f8a3 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/raytrace_line_2d.hpp" namespace nav2_costmap_2d { @@ -462,14 +463,15 @@ void Costmap2D::polygonOutlineCells( { PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); for (unsigned int i = 0; i < polygon.size() - 1; ++i) { - raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); + nav2_util::raytraceLine( + cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y, size_x_); } if (!polygon.empty()) { unsigned int last_index = polygon.size() - 1; // we also need to close the polygon by going from the last point to the first - raytraceLine( + nav2_util::raytraceLine( cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, - polygon[0].y); + polygon[0].y, size_x_); } } diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index d468dd37987..435d9072d03 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,9 +1,3 @@ -# Bresenham2D corner cases test -ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) -target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core -) - # OrderLayer for checking Costmap2D plugins API calling order add_library(order_layer SHARED order_layer.cpp) target_link_libraries(order_layer PUBLIC diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp deleted file mode 100644 index c4afee34e00..00000000000 --- a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2022 Samsung Research Russia -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Alexey Merzlyakov -*********************************************************************/ -#include -#include - -class CostmapAction -{ -public: - explicit CostmapAction( - unsigned char * costmap, unsigned int size, unsigned char mark_val = 128) - : costmap_(costmap), size_(size), mark_val_(mark_val) - { - } - - inline void operator()(unsigned int off) - { - ASSERT_TRUE(off < size_); - costmap_[off] = mark_val_; - } - - inline unsigned int get(unsigned int off) - { - return costmap_[off]; - } - -private: - unsigned char * costmap_; - unsigned int size_; - unsigned char mark_val_; -}; - -class CostmapTest : public nav2_costmap_2d::Costmap2D -{ -public: - CostmapTest( - unsigned int size_x, unsigned int size_y, double resolution, - double origin_x, double origin_y, unsigned char default_val = 0) - : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val) - { - } - - unsigned char * getCostmap() - { - return costmap_; - } - - unsigned int getSize() - { - return size_x_ * size_y_; - } - - void raytraceLine( - CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -}; - -TEST(costmap_2d, bresenham2DBoundariesCheck) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 6; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - some asymmetrically standing point in order to cover most corner cases - const unsigned int x0 = 2; - const unsigned int y0 = 4; - // (x1, y1) point will move - unsigned int x1, y1; - - // Running on (x, 0) edge - y1 = 0; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (x, sz_y) edge - y1 = sz_y - 1; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (0, y) edge - x1 = 0; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (sz_x, y) edge - x1 = sz_x - 1; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -} - -TEST(costmap_2d, bresenham2DSamePoint) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 0; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - const double x0 = 2; - const double y0 = 4; - - unsigned int offset = y0 * sz_x + x0; - unsigned char val_before = ca.get(offset); - // Same point to check - ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length); - unsigned char val_after = ca.get(offset); - ASSERT_FALSE(val_before == val_after); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp index 320eeaa1653..8cf4583d15d 100644 --- a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp @@ -19,6 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" @@ -30,13 +31,6 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter public: CostmapFilterWrapper() {} - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const - { - return nav2_costmap_2d::CostmapFilter::worldToMask(filter_mask, wx, wy, mx, my); - } - unsigned char getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const @@ -52,7 +46,7 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter void resetFilter() {} }; -TEST(CostmapFilter, testWorldToMask) +TEST(CostmapFilter, testWorldToMap) { // Create occupancy grid for test as follows: // @@ -82,19 +76,19 @@ TEST(CostmapFilter, testWorldToMask) CostmapFilterWrapper cf; unsigned int mx, my; // Point inside mask - ASSERT_TRUE(cf.worldToMask(mask, 4.0, 5.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 4.0, 5.0, mx, my)); ASSERT_EQ(mx, 1u); ASSERT_EQ(my, 2u); // Corner cases - ASSERT_TRUE(cf.worldToMask(mask, 3.0, 3.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 3.0, 3.0, mx, my)); ASSERT_EQ(mx, 0u); ASSERT_EQ(my, 0u); - ASSERT_TRUE(cf.worldToMask(mask, 5.9, 5.9, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 5.9, 5.9, mx, my)); ASSERT_EQ(mx, 2u); ASSERT_EQ(my, 2u); // Point outside mask - ASSERT_FALSE(cf.worldToMask(mask, 2.9, 2.9, mx, my)); - ASSERT_FALSE(cf.worldToMask(mask, 6.0, 6.0, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 2.9, 2.9, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 6.0, 6.0, mx, my)); } TEST(CostmapFilter, testGetMaskCost) diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index a003a8fb087..6459b42ed2c 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -5,21 +5,26 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(backward_ros REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(PkgConfig REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) +pkg_search_module(UUID REQUIRED uuid) + nav2_package() set(map_server_executable map_server) @@ -28,12 +33,16 @@ set(library_name ${map_server_executable}_core) set(map_io_library_name map_io) +set(vo_library_name vector_object_core) + set(map_saver_cli_executable map_saver_cli) set(map_saver_server_executable map_saver_server) set(costmap_filter_info_server_executable costmap_filter_info_server) +set(vo_server_executable vector_object_server) + add_library(${map_io_library_name} SHARED src/map_mode.cpp src/map_io.cpp) @@ -78,6 +87,31 @@ target_link_libraries(${library_name} PRIVATE yaml-cpp::yaml-cpp ) +add_library(${vo_library_name} SHARED + src/vo_server/vector_object_shapes.cpp + src/vo_server/vector_object_server.cpp) +target_include_directories(${vo_library_name} + PUBLIC + "$" + "$" + "$") +target_link_libraries(${vo_library_name} PUBLIC + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rclcpp_components::component + tf2_ros::tf2_ros + ${UUID_LIBRARIES} +) +target_link_libraries(${library_name} PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + yaml-cpp::yaml-cpp +) + add_executable(${map_server_executable} src/map_server/main.cpp) target_include_directories(${map_server_executable} @@ -142,12 +176,28 @@ target_link_libraries(${costmap_filter_info_server_executable} PRIVATE rclcpp_lifecycle::rclcpp_lifecycle ) +add_executable(${vo_server_executable} + src/vo_server/main.cpp) +target_include_directories(${vo_server_executable} + PRIVATE + "$" + "$") +target_link_libraries(${vo_server_executable} PRIVATE + ${vo_library_name} + ${nav_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) + rclcpp_components_register_nodes(${library_name} "nav2_map_server::CostmapFilterInfoServer") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer") +rclcpp_components_register_nodes(${vo_library_name} "nav2_map_server::VectorObjectServer") install(TARGETS - ${library_name} ${map_io_library_name} + ${library_name} ${map_io_library_name} ${vo_library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -155,13 +205,14 @@ install(TARGETS install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} - ${costmap_filter_info_server_executable} + ${costmap_filter_info_server_executable} ${vo_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -178,6 +229,7 @@ ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries( ${library_name} ${map_io_library_name} + ${vo_library_name} ) ament_export_dependencies(nav_msgs nav2_msgs nav2_util rclcpp rclcpp_lifecycle nav2_ros_common) ament_export_targets(${library_name}) diff --git a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp new file mode 100644 index 00000000000..ed4186722bc --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -0,0 +1,228 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" +#include "nav2_map_server/vector_object_shapes.hpp" + +namespace nav2_map_server +{ + +/// @brief Vector Object server node +class VectorObjectServer : public nav2::LifecycleNode +{ +public: + /** + * @brief Constructor for the VectorObjectServer + * @param options Additional options to control creation of the node. + */ + explicit VectorObjectServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + /** + * @brief: Initializes TF buffer/listener, obtains ROS-parameters, creates incoming services, + * and output map publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates output map publisher and creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates map publisher and timer (if any), destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all services, publishers, map and TF-s + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in the failure case + */ + bool obtainParams(); + + /** + * @brief Finds the shape with given UUID + * @param uuid Given UUID to search with + * @return Iterator to the shape, if found. Otherwise past-the-end iterator. + */ + std::vector>::iterator findShape(const unsigned char * uuid); + + /** + * @brief Transform all vector shapes from their local frame to output map frame + * @return Whether all vector objects were transformed successfully + */ + bool transformVectorObjects(); + + /** + * @brief Obtains map boundaries to place all vector objects inside + * @param min_x output min X-boundary of required map + * @param min_y output min Y-boundary of required map + * @param max_x output max X-boundary of required map + * @param max_y output max Y-boundary of required map + * @throw std::exception if can not obtain one of the map boundaries + */ + void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const; + + /** + * @brief Creates or updates existing map with required sizes and fills it with default value + * @param min_x min X-boundary of new map + * @param min_y min Y-boundary of new map + * @param max_x max X-boundary of new map + * @param max_y max Y-boundary of new map + * @throw std::exception if map has negative X or Y size + */ + void updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y); + + /** + * @brief Processes all vector objects on raster output map + */ + void putVectorObjectsOnMap(); + + /** + * @brief Publishes output map + */ + void publishMap(); + + /** + * @brief Calculates new map sizes, updates map, processes all vector objects on it + * and publishes output map one time + */ + void processMap(); + + /** + * @brief If map to be update dynamically, creates map processing timer, + * otherwise process map once + */ + void switchMapUpdate(); + + /** + * @brief Callback for AddShapes service call. + * Reads all input vector objects from service request, + * creates them or updates their shape in case of existing objects + * and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void addShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for GetShapes service call. + * Gets all shapes and returns them to the service response + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void getShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for RemoveShapes service call. + * Try to remove requested vector objects and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void removeShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +protected: + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief All shapes vector + std::vector> shapes_; + + /// @brief Output map resolution + double resolution_; + /// @brief Default value the output map to be filled with + int8_t default_value_; + /// @brief @Overlay Type of overlay of vector objects on the map + OverlayType overlay_type_; + + /// @brief Output map with vector objects on it + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Whether to process and publish map + double process_map_; + + /// @brief Frame of output map + std::string global_frame_id_; + /// @brief Transform tolerance + double transform_tolerance_; + + /// @brief Frequency to dynamically update/publish the map (if necessary) + double update_frequency_; + /// @brief Map update timer + rclcpp::TimerBase::SharedPtr map_timer_; + + /// @brief AddShapes service + nav2::ServiceServer::SharedPtr add_shapes_service_; + /// @brief GetShapes service + nav2::ServiceServer::SharedPtr get_shapes_service_; + /// @brief RemoveShapes service + nav2::ServiceServer::SharedPtr remove_shapes_service_; + + /// @beirf Topic name where the output map to be published to + std::string map_topic_; + /// @brief Output map publisher + nav2::Publisher::SharedPtr map_pub_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp new file mode 100644 index 00000000000..dd5cae95b62 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -0,0 +1,413 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" + +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" + +namespace nav2_map_server +{ + +/// @brief Possible vector object shape types +enum ShapeType +{ + UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2 +}; + +/// @brief Basic class, other vector objects to be inherited from +class Shape +{ +public: + /** + * @brief Shape basic class constructor + * @param node Vector Object server node pointer + */ + explicit Shape(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Shape destructor + */ + virtual ~Shape(); + + /** + * @brief Returns type of the shape + * @return Type of the shape + */ + ShapeType getType(); + + /** + * @brief Supporting routine obtaining shape UUID from ROS-parameters + * for the given shape object + * @return True if UUID was obtained or false in failure case + */ + bool obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid); + + /** + * @brief Gets the value of the shape. + * Empty virtual method intended to be used in child implementations + * @return OccupancyGrid value of the shape + */ + virtual int8_t getValue() const = 0; + + /** + * @brief Gets frame ID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Frame ID where the shape is + */ + virtual std::string getFrameID() const = 0; + + /** + * @brief Gets UUID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Shape UUID string + */ + virtual std::string getUUID() const = 0; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * Empty virtual method intended to be used in child implementations + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + virtual bool isUUID(const unsigned char * uuid) const = 0; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * Empty virtual method intended to be used in child implementations + * @return True if shape to be filled + */ + virtual bool isFill() const = 0; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * Empty virtual method intended to be used in child implementations + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + virtual bool obtainParams(const std::string & shape_name) = 0; + + /** + * @brief Transforms shape coordinates to a new frame. + * Empty virtual method intended to be used in child implementations + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + virtual bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) = 0; + + /** + * @brief Gets shape box-boundaries. + * Empty virtual method intended to be used in child implementations + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0; + + /** + * @brief Is the point inside the shape. + * Empty virtual method intended to be used in child implementations + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + virtual bool isPointInside(const double px, const double py) const = 0; + + /** + * @brief Puts shape borders on map. + * Empty virtual method intended to be used in child implementations + * @param map Output map pointer + * @param overlay_type Overlay type + */ + virtual void putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0; + +protected: + /// @brief Type of shape + ShapeType type_; + + /// @brief VectorObjectServer node + nav2::LifecycleNode::WeakPtr node_; +}; + +/// @brief Polygon shape class +class Polygon : public Shape +{ +public: + /* + * @brief Polygon class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape + */ + explicit Polygon(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Polygon parameters + * @return Polygon parameters + */ + nav2_msgs::msg::PolygonObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Polygon parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + + /** + * @brief Gets shape box-boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape + */ + bool checkConsistency(); + + /// @brief Input polygon parameters (could be in any frame) + nav2_msgs::msg::PolygonObject::SharedPtr params_; + /// @brief Polygon in the map's frame + geometry_msgs::msg::Polygon::SharedPtr polygon_; +}; + +/// @brief Circle shape class +class Circle : public Shape +{ +public: + /* + * @brief Circle class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape + */ + explicit Circle(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Circle parameters + * @return Circle parameters + */ + nav2_msgs::msg::CircleObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Circle parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + + /** + * @brief Gets shape box-boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape + */ + bool checkConsistency(); + + /** + * @brief Converts circle center to map coordinates + * considering FP-accuracy losing on small values when using conventional + * nav2_util::worldToMap() approach + * @param map Map pointer + * @param mcx Output X-coordinate of associated circle center on map + * @param mcy Output Y-coordinate of associated circle center on map + * @return True if the conversion was successful + */ + bool centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy); + + /** + * @brief Put Circle's point on map + * @param mx X-coordinate of given point + * @param my Y-coordinate of given point + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type); + + /// @brief Input circle parameters (could be in any frame) + nav2_msgs::msg::CircleObject::SharedPtr params_; + /// @brief Circle center in the map's frame + geometry_msgs::msg::Point32::SharedPtr center_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp new file mode 100644 index 00000000000..f478f71b3ea --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -0,0 +1,142 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" + +namespace nav2_map_server +{ + +// ---------- Working with UUID-s ---------- + +/** + * @brief Converts UUID from input array to unparsed string + * @param uuid Input UUID in array format + * @return Unparsed UUID string + */ +inline std::string unparseUUID(const unsigned char * uuid) +{ + char uuid_str[37]; + uuid_unparse(uuid, uuid_str); + return std::string(uuid_str); +} + +// ---------- Working with shapes' overlays ---------- + +/// @brief Type of overlay between different vector objects and map +enum class OverlayType : uint8_t +{ + OVERLAY_SEQ = 0, // Vector objects are superimposed in the order in which they have arrived + OVERLAY_MAX = 1, // Maximum value from vector objects and map is being chosen + OVERLAY_MIN = 2 // Minimum value from vector objects and map is being chosen +}; + +/** + * @brief Updates map value with shape's one according to the given overlay type + * @param map_val Map value. To be updated with new value if overlay is involved + * @param shape_val Vector object value to be overlaid on map + * @param overlay_type Type of overlay + * @throw std::exception in case of unknown overlay type + */ +inline void processVal( + int8_t & map_val, const int8_t shape_val, + const OverlayType overlay_type) +{ + switch (overlay_type) { + case OverlayType::OVERLAY_SEQ: + map_val = shape_val; + return; + case OverlayType::OVERLAY_MAX: + if (shape_val > map_val) { + map_val = shape_val; + } + return; + case OverlayType::OVERLAY_MIN: + if ((map_val == nav2_util::OCC_GRID_UNKNOWN || shape_val < map_val) && + shape_val != nav2_util::OCC_GRID_UNKNOWN) + { + map_val = shape_val; + } + return; + default: + throw std::runtime_error{"Unknown overlay type"}; + } +} + +/** + * @brief Updates the cell on the map with given shape value according to the given overlay type + * @param map Output map to be updated with + * @param offset Offset to the cell to be updated + * @param shape_val Vector object value to be updated map with + * @param overlay_type Type of overlay + */ +inline void processCell( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const unsigned int offset, + const int8_t shape_val, + const OverlayType overlay_type) +{ + int8_t map_val = map->data[offset]; + processVal(map_val, shape_val, overlay_type); + map->data[offset] = map_val; +} + +/// @brief Functor class used in raytraceLine algorithm +class MapAction +{ +public: + /** + * @brief MapAction constructor + * @param map Pointer to output map + * @param value Value to put on map + * @param overlay_type Overlay type + */ + MapAction( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + int8_t value, OverlayType overlay_type) + : map_(map), value_(value), overlay_type_(overlay_type) + {} + + /** + * @brief Map' cell updating operator + * @param offset Offset on the map where the cell to be changed + */ + inline void operator()(unsigned int offset) + { + processCell(map_, offset, value_, overlay_type_); + } + +protected: + /// @brief Output map pointer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Value to put on map + int8_t value_; + /// @brief Overlay type + OverlayType overlay_type_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ diff --git a/nav2_map_server/launch/vector_object_server.launch.py b/nav2_map_server/launch/vector_object_server.launch.py new file mode 100644 index 00000000000..f3c2a20ff1b --- /dev/null +++ b/nav2_map_server/launch/vector_object_server.launch.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023 Samsung R&D Institute Russia +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + # Environment + package_dir = get_package_share_directory('nav2_map_server') + + # Constant parameters + lifecycle_nodes = ['vector_object_server', 'costmap_filter_info_server'] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch arguments + # 1. Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + + # 2. Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(package_dir, 'params', 'vector_object_server_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='True', + description='Automatically startup Vector Object server') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of container that nodes will load in if use composition') + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + # Declare node launching commands + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + Node( + package='nav2_map_server', + executable='vector_object_server', + name='vector_object_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_vo_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings) + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::VectorObjectServer', + name='vector_object_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='costmap_filter_info_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_vo_server', + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings) + ], + ) + ] + ) + + ld = LaunchDescription() + + # Launch arguments + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + + # Node launching commands + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index ffb3d3d1fce..44045314fa3 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -22,12 +22,15 @@ yaml_cpp_vendor launch_ros launch_testing + geometry_msgs tf2 + tf2_ros nav2_msgs nav2_util graphicsmagick lifecycle_msgs nav2_ros_common + uuid ament_lint_common ament_lint_auto diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml new file mode 100644 index 00000000000..6a6a5fc2b68 --- /dev/null +++ b/nav2_map_server/params/vector_object_server_params.yaml @@ -0,0 +1,30 @@ +vector_object_server: + ros__parameters: + map_topic: "vo_map" + global_frame_id: "map" + resolution: 0.05 + default_value: -1 + overlay_type: 0 + update_frequency: 1.0 + transform_tolerance: 0.1 + shapes: ["Poly", "Circle"] + Poly: + type: "polygon" + frame_id: "map" + closed: True + value: 100 + points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2] + Circle: + type: "circle" + frame_id: "map" + fill: True + value: 100 + center: [1.5, 0.5] + radius: 0.7 +costmap_filter_info_server: + ros__parameters: + type: 0 + filter_info_topic: "vo_costmap_filter_info" + mask_topic: "vo_map" + base: 0.0 + multiplier: 1.0 diff --git a/nav2_map_server/src/vo_server/main.cpp b/nav2_map_server/src/vo_server/main.cpp new file mode 100644 index 00000000000..d2352b7aad3 --- /dev/null +++ b/nav2_map_server/src/vo_server/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 + +#include "nav2_map_server/vector_object_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp new file mode 100644 index 00000000000..89d7c3c825f --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -0,0 +1,558 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 "nav2_map_server/vector_object_server.hpp" + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/create_timer.hpp" + +#include "tf2_ros/create_timer_ros.hpp" + +#include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" + +using namespace std::placeholders; + +namespace nav2_map_server +{ + +VectorObjectServer::VectorObjectServer(const rclcpp::NodeOptions & options) +: nav2::LifecycleNode("vector_object_server", "", options), process_map_(false) +{} + +nav2::CallbackReturn +VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Obtaining ROS parameters + if (!obtainParams()) { + return nav2::CallbackReturn::FAILURE; + } + + map_pub_ = create_publisher( + map_topic_, + nav2::qos::LatchedPublisherQoS()); + + add_shapes_service_ = create_service( + "~/add_shapes", + std::bind(&VectorObjectServer::addShapesCallback, this, _1, _2, _3)); + + get_shapes_service_ = create_service( + "~/get_shapes", + std::bind(&VectorObjectServer::getShapesCallback, this, _1, _2, _3)); + + remove_shapes_service_ = create_service( + "~/remove_shapes", + std::bind(&VectorObjectServer::removeShapesCallback, this, _1, _2, _3)); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + map_pub_->on_activate(); + + // Trigger map to be published + process_map_ = true; + switchMapUpdate(); + + // Creating bond connection + createBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + process_map_ = false; + + map_pub_->on_deactivate(); + + // Destroying bond connection + destroyBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + add_shapes_service_.reset(); + get_shapes_service_.reset(); + remove_shapes_service_.reset(); + + map_pub_.reset(); + map_.reset(); + + shapes_.clear(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2::CallbackReturn::SUCCESS; +} + +bool VectorObjectServer::obtainParams() +{ + auto node = shared_from_this(); + + // Main ROS-parameters + map_topic_ = nav2::declare_or_get_parameter(node, "map_topic", std::string{"vo_map"}); + global_frame_id_ = nav2::declare_or_get_parameter(node, "global_frame_id", std::string{"map"}); + resolution_ = nav2::declare_or_get_parameter(node, "resolution", 0.05); + default_value_ = nav2::declare_or_get_parameter(node, "default_value", + static_cast(nav2_util::OCC_GRID_UNKNOWN)); + overlay_type_ = static_cast(nav2::declare_or_get_parameter(node, "overlay_type", + static_cast(OverlayType::OVERLAY_SEQ))); + update_frequency_ = nav2::declare_or_get_parameter(node, "update_frequency", 1.0); + transform_tolerance_ = nav2::declare_or_get_parameter(node, "transform_tolerance", 0.1); + + // Shapes + auto shape_names = nav2::declare_or_get_parameter(node, "shapes", std::vector()); + for (std::string shape_name : shape_names) { + std::string shape_type; + try { + shape_type = nav2::declare_or_get_parameter(node, shape_name + ".type", + rclcpp::PARAMETER_STRING); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + get_logger(), "Error while getting shape %s type: %s", shape_name.c_str(), ex.what()); + return false; + } + + if (shape_type == "polygon") { + auto polygon = std::make_shared(node); + if (!polygon->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(polygon); + } else if (shape_type == "circle") { + auto circle = std::make_shared(node); + if (!circle->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(circle); + } else { + RCLCPP_ERROR(get_logger(), + "Please specify the correct type for shape %s. Supported types are 'polygon' and 'circle'", + shape_name.c_str()); + return false; + } + } + + return true; +} + +std::vector>::iterator +VectorObjectServer::findShape(const unsigned char * uuid) +{ + for (auto it = shapes_.begin(); it != shapes_.end(); it++) { + if ((*it)->isUUID(uuid)) { + return it; + } + } + return shapes_.end(); +} + +bool VectorObjectServer::transformVectorObjects() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { + // Shape to be updated dynamically + if (!shape->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) { + RCLCPP_ERROR( + get_logger(), "Can not transform vector object from %s to %s frame", + shape->getFrameID().c_str(), global_frame_id_.c_str()); + return false; + } + } + } + + return true; +} + +void VectorObjectServer::getMapBoundaries( + double & min_x, double & min_y, double & max_x, double & max_y) const +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + double min_p_x, min_p_y, max_p_x, max_p_y; + for (auto shape : shapes_) { + shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y); + min_x = std::min(min_x, min_p_x); + min_y = std::min(min_y, min_p_y); + max_x = std::max(max_x, max_p_x); + max_y = std::max(max_y, max_p_y); + } + + if ( + min_x == std::numeric_limits::max() || + min_y == std::numeric_limits::max() || + max_x == std::numeric_limits::lowest() || + max_y == std::numeric_limits::lowest()) + { + throw std::runtime_error("Can not obtain map boundaries"); + } +} + +void VectorObjectServer::updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y) +{ + // Calculate size of update map + int size_x = static_cast((max_x - min_x) / resolution_) + 1; + int size_y = static_cast((max_y - min_y) / resolution_) + 1; + + if (size_x < 0) { + throw std::runtime_error("Incorrect map x-size"); + } + + if (size_y < 0) { + throw std::runtime_error("Incorrect map y-size"); + } + + if (!map_) { + map_ = std::make_shared(); + } + + if ( + map_->info.width != static_cast(size_x) || + map_->info.height != static_cast(size_y)) + { + // Map size was changed + map_->data = std::vector(size_x * size_y, default_value_); + map_->info.width = size_x; + map_->info.height = size_y; + } else if (size_x > 0 && size_y > 0) { + // Map size was not changed + memset(map_->data.data(), default_value_, size_x * size_y * sizeof(int8_t)); + } + + map_->header.frame_id = global_frame_id_; + map_->info.resolution = resolution_; + map_->info.origin.position.x = min_x; + map_->info.origin.position.y = min_y; +} + +void VectorObjectServer::putVectorObjectsOnMap() +{ + // Filling the shapes + for (auto shape : shapes_) { + if (shape->isFill()) { + // Put filled shape on map + double wx1 = std::numeric_limits::max(); + double wy1 = std::numeric_limits::max(); + double wx2 = std::numeric_limits::lowest(); + double wy2 = std::numeric_limits::lowest(); + unsigned int mx1 = 0; + unsigned int my1 = 0; + unsigned int mx2 = 0; + unsigned int my2 = 0; + + shape->getBoundaries(wx1, wy1, wx2, wy2); + if ( + !nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) || + !nav2_util::worldToMap(map_, wx2, wy2, mx2, my2)) + { + RCLCPP_ERROR( + get_logger(), + "Error to get shape boundaries on map (UUID: %s)", shape->getUUID().c_str()); + return; + } + + unsigned int it; + for (unsigned int my = my1; my <= my2; my++) { + for (unsigned int mx = mx1; mx <= mx2; mx++) { + it = my * map_->info.width + mx; + double wx, wy; + nav2_util::mapToWorld(map_, mx, my, wx, wy); + if (shape->isPointInside(wx, wy)) { + processVal(map_->data[it], shape->getValue(), overlay_type_); + } + } + } + } else { + // Put shape borders on map + shape->putBorders(map_, overlay_type_); + } + } +} + +void VectorObjectServer::publishMap() +{ + if (map_) { + auto map = std::make_unique(*map_); + map_pub_->publish(std::move(map)); + } +} + +void VectorObjectServer::processMap() +{ + if (!process_map_) { + return; + } + + try { + if (shapes_.size() > 0) { + if (!transformVectorObjects()) { + return; + } + double min_x, min_y, max_x, max_y; + + getMapBoundaries(min_x, min_y, max_x, max_y); + updateMap(min_x, min_y, max_x, max_y); + putVectorObjectsOnMap(); + } else { + updateMap(0.0, 0.0, 0.0, 0.0); + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not update map: %s", ex.what()); + return; + } + + publishMap(); +} + +void VectorObjectServer::switchMapUpdate() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { + if (!map_timer_) { + map_timer_ = this->create_timer( + std::chrono::duration(1.0 / update_frequency_), + std::bind(&VectorObjectServer::processMap, this)); + } + RCLCPP_INFO(get_logger(), "Publishing map dynamically at %f Hz rate", update_frequency_); + return; + } + } + + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + RCLCPP_INFO(get_logger(), "Publishing map once"); + processMap(); +} + +void VectorObjectServer::addShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not added properly, + // set it to false. + response->success = true; + + auto node = shared_from_this(); + + // Process polygons + for (auto req_poly : request->polygons) { + nav2_msgs::msg::PolygonObject::SharedPtr new_params = + std::make_shared(req_poly); + + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { + // Vector Object with given UUID was found: updating it + // Check that found shape has correct type + if ((*it)->getType() != POLYGON) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a polygon type for a polygon update. Not adding shape.", + (*it)->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } + + std::shared_ptr polygon = std::static_pointer_cast(*it); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::PolygonObject::SharedPtr old_params = polygon->getParams(); + if (!polygon->setParams(new_params)) { + RCLCPP_ERROR( + get_logger(), + "Failed to update existing polygon object (UUID: %s) with new params. " + "Reverting to old polygon params.", + (*it)->getUUID().c_str()); + // Restore old parameters + polygon->setParams(old_params); + // ... and set the failure to return + response->success = false; + } + } else { + // Vector Object with given UUID was not found: creating a new one + std::shared_ptr polygon = std::make_shared(node); + if (polygon->setParams(new_params)) { + shapes_.push_back(polygon); + } else { + RCLCPP_ERROR( + get_logger(), "Failed to create a new polygon object using the provided params."); + response->success = false; + } + } + } + + // Process circles + for (auto req_crcl : request->circles) { + nav2_msgs::msg::CircleObject::SharedPtr new_params = + std::make_shared(req_crcl); + + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { + // Vector object with given UUID was found: updating it + // Check that found shape has correct type + if ((*it)->getType() != CIRCLE) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a circle type for a circle update. Not adding shape.", + (*it)->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } + + std::shared_ptr circle = std::static_pointer_cast(*it); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::CircleObject::SharedPtr old_params = circle->getParams(); + if (!circle->setParams(new_params)) { + RCLCPP_ERROR( + get_logger(), + "Failed to update existing circle object (UUID: %s) with new params. " + "Reverting to old circle params.", + (*it)->getUUID().c_str()); + // Restore old parameters + circle->setParams(old_params); + // ... and set the failure to return + response->success = false; + } + } else { + // Vector Object with given UUID was not found: creating a new one + std::shared_ptr circle = std::make_shared(node); + if (circle->setParams(new_params)) { + shapes_.push_back(circle); + } else { + RCLCPP_ERROR( + get_logger(), "Failed to create a new circle object using the provided params."); + response->success = false; + } + } + } + + switchMapUpdate(); +} + +void VectorObjectServer::getShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + std::shared_ptr polygon; + std::shared_ptr circle; + + for (auto shape : shapes_) { + switch (shape->getType()) { + case POLYGON: + polygon = std::static_pointer_cast(shape); + response->polygons.push_back(*(polygon->getParams())); + break; + case CIRCLE: + circle = std::static_pointer_cast(shape); + response->circles.push_back(*(circle->getParams())); + break; + default: + RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str()); + } + } +} + +void VectorObjectServer::removeShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not found, + // set it to false. + response->success = true; + + if (request->all_objects) { + // Clear all objects + shapes_.clear(); + } else { + // Find objects to remove + for (auto req_uuid : request->uuids) { + auto it = findShape(req_uuid.uuid.data()); + if (it != shapes_.end()) { + // Shape with given UUID was found: remove it + (*it).reset(); + shapes_.erase(it); + } else { + // Required vector object was not found + RCLCPP_ERROR( + get_logger(), + "Can not find shape to remove with UUID: %s", + unparseUUID(req_uuid.uuid.data()).c_str()); + response->success = false; + } + } + } + + switchMapUpdate(); +} + +} // namespace nav2_map_server + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::VectorObjectServer) diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp new file mode 100644 index 00000000000..72466486c01 --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -0,0 +1,559 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 "nav2_map_server/vector_object_shapes.hpp" + +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/raytrace_line_2d.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace nav2_map_server +{ + +// ---------- Shape ---------- + +Shape::Shape(const nav2::LifecycleNode::WeakPtr & node) +: type_(UNKNOWN), node_(node) +{} + +Shape::~Shape() +{} + +ShapeType Shape::getType() +{ + return type_; +} + +bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + try { + // Try to get shape UUID from ROS-parameters + std::string uuid_str = nav2::declare_or_get_parameter( + node, shape_name + ".uuid", rclcpp::PARAMETER_STRING); + if (uuid_parse(uuid_str.c_str(), out_uuid) != 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Can not parse UUID string for shape: %s", + shape_name.c_str(), uuid_str.c_str()); + return false; + } + } catch (const std::exception &) { + // If no UUID was specified, generate a new one + uuid_generate(out_uuid); + + char uuid_str[37]; + uuid_unparse(out_uuid, uuid_str); + RCLCPP_INFO( + node->get_logger(), + "[%s] No UUID is specified for shape. Generating a new one: %s", + shape_name.c_str(), uuid_str); + } + + return true; +} + +// ---------- Polygon ---------- + +Polygon::Polygon( + const nav2::LifecycleNode::WeakPtr & node) +: Shape::Shape(node) +{ + type_ = POLYGON; +} + +int8_t Polygon::getValue() const +{ + return params_->value; +} + +std::string Polygon::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Polygon::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Polygon::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Polygon::isFill() const +{ + return params_->closed; +} + +bool Polygon::obtainParams(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!params_) { + params_ = std::make_shared(); + } + if (!polygon_) { + polygon_ = std::make_shared(); + } + + params_->header.frame_id = nav2::declare_or_get_parameter( + node, shape_name + ".frame_id", std::string{"map"}); + params_->value = nav2::declare_or_get_parameter( + node, shape_name + ".value", static_cast(nav2_util::OCC_GRID_OCCUPIED)); + params_->closed = nav2::declare_or_get_parameter( + node, shape_name + ".closed", true); + + std::vector poly_row; + try { + poly_row = nav2::declare_or_get_parameter>( + node, shape_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Error while getting polygon parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (poly_row.size() < 6 || poly_row.size() % 2 != 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Polygon has incorrect points description", + shape_name.c_str()); + return false; + } + + // Obtain polygon vertices + geometry_msgs::msg::Point32 point; + bool first = true; + for (double val : poly_row) { + if (first) { + point.x = val; + } else { + point.y = val; + params_->points.push_back(point); + } + first = !first; + } + + // Filling the polygon_ with obtained points in map's frame + polygon_->points = params_->points; + + // Getting shape UUID + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const +{ + return params_; +} + +bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) +{ + params_ = params; + + if (!polygon_) { + polygon_ = std::make_shared(); + } + polygon_->points = params_->points; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); +} + +bool Polygon::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + for (unsigned int i = 0; i < params_->points.size(); i++) { + from_pose.pose.position.x = params_->points[i].x; + from_pose.pose.position.y = params_->points[i].y; + from_pose.pose.position.z = params_->points[i].z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + polygon_->points[i].x = to_pose.pose.position.x; + polygon_->points[i].y = to_pose.pose.position.y; + polygon_->points[i].z = to_pose.pose.position.z; + } else { + return false; + } + } + + return true; +} + +void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + for (auto point : polygon_->points) { + min_x = std::min(min_x, static_cast(point.x)); + min_y = std::min(min_y, static_cast(point.y)); + max_x = std::max(max_x, static_cast(point.x)); + max_y = std::max(max_y, static_cast(point.y)); + } +} + +bool Polygon::isPointInside(const double px, const double py) const +{ + return nav2_util::geometry_utils::isPointInsidePolygon(px, py, polygon_->points); +} + +void Polygon::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mx0, my0, mx1, my1; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!nav2_util::worldToMap(map, polygon_->points[0].x, polygon_->points[0].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[0].x, polygon_->points[0].y); + return; + } + + MapAction ma(map, params_->value, overlay_type); + for (unsigned int i = 1; i < polygon_->points.size(); i++) { + mx0 = mx1; + my0 = my1; + if (!nav2_util::worldToMap(map, polygon_->points[i].x, polygon_->points[i].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[i].x, polygon_->points[i].y); + return; + } + nav2_util::raytraceLine(ma, mx0, my0, mx1, my1, map->info.width); + } +} + +bool Polygon::checkConsistency() +{ + if (params_->points.size() < 3) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Polygon has incorrect number of vertices: %li", + getUUID().c_str(), params_->points.size()); + return false; + } + + return true; +} + +// ---------- Circle ---------- + +Circle::Circle( + const nav2::LifecycleNode::WeakPtr & node) +: Shape::Shape(node) +{ + type_ = CIRCLE; +} + +int8_t Circle::getValue() const +{ + return params_->value; +} + +std::string Circle::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Circle::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Circle::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Circle::isFill() const +{ + return params_->fill; +} + +bool Circle::obtainParams(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!params_) { + params_ = std::make_shared(); + } + if (!center_) { + center_ = std::make_shared(); + } + + params_->header.frame_id = nav2::declare_or_get_parameter( + node, shape_name + ".frame_id", std::string{"map"}); + params_->value = nav2::declare_or_get_parameter( + node, shape_name + ".value", static_cast(nav2_util::OCC_GRID_OCCUPIED)); + params_->fill = nav2::declare_or_get_parameter( + node, shape_name + ".fill", true); + + std::vector center_row; + try { + center_row = nav2::declare_or_get_parameter>( + node, shape_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY); + params_->radius = nav2::declare_or_get_parameter( + node, shape_name + ".radius", rclcpp::PARAMETER_DOUBLE); + if (params_->radius < 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Circle has incorrect radius less than zero", + shape_name.c_str()); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Error while getting circle parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (center_row.size() != 2) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Circle has incorrect center description", + shape_name.c_str()); + return false; + } + + // Obtain circle center + params_->center.x = center_row[0]; + params_->center.y = center_row[1]; + // Setting the center_ with obtained circle center in map's frame + *center_ = params_->center; + + // Getting shape UUID + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const +{ + return params_; +} + +bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) +{ + params_ = params; + + if (!center_) { + center_ = std::make_shared(); + } + *center_ = params_->center; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); +} + +bool Circle::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + from_pose.pose.position.x = params_->center.x; + from_pose.pose.position.y = params_->center.y; + from_pose.pose.position.z = params_->center.z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + center_->x = to_pose.pose.position.x; + center_->y = to_pose.pose.position.y; + center_->z = to_pose.pose.position.z; + } else { + return false; + } + + return true; +} + +void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = center_->x - params_->radius; + min_y = center_->y - params_->radius; + max_x = center_->x + params_->radius; + max_y = center_->y + params_->radius; +} + +bool Circle::isPointInside(const double px, const double py) const +{ + return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <= + params_->radius * params_->radius; +} + +void Circle::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mcx, mcy; + if (!centerToMap(map, mcx, mcy)) { + return; + } + + // Implementation of the circle generation algorithm, based on the following work: + // Berthold K.P. Horn "Circle generators for display devices" + // Computer Graphics and Image Processing 5.2 (1976): 280-288. + + // Inputs initialization + const int r = static_cast(std::round(params_->radius / map->info.resolution)); + int x = r; + int y = 1; + + // Error initialization + int s = -r; + + // Calculation algorithm + while (x > y) { // Calculating only first circle octant + // Put 8 points in each octant reflecting symmetrically + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy + x, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy - x + 1, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy - x + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy + x, map, overlay_type); + + s = s + 2 * y + 1; + y++; + if (s > 0) { + s = s - 2 * x + 2; + x--; + } + } + + // Corner case for x == y: do not put end points twice + if (x == y) { + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + } +} + +bool Circle::checkConsistency() +{ + if (params_->radius < 0.0) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Circle has incorrect radius less than zero", + getUUID().c_str()); + return false; + } + return true; +} + +bool Circle::centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Get center of circle in map coordinates + if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) circle center to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + // We need the circle center to be always shifted one cell less its logical center + // and to avoid any FP-accuracy losing on small values, so we are using another + // than nav2_util::worldToMap() approach + mcx = static_cast( + std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1; + mcy = static_cast( + std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1; + if (mcx >= map->info.width || mcy >= map->info.height) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + + return true; +} + +inline void Circle::putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type) +{ + processCell(map, my * map->info.width + mx, params_->value, overlay_type); +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index 4140b14d6f0..06a4b2228a0 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -18,3 +18,25 @@ target_link_libraries(test_costmap_filter_info_server ${library_name} ${map_io_library_name} ) + +# test_vector_object_shapes unit test +ament_add_gtest(test_vector_object_shapes + test_vector_object_shapes.cpp +) + +target_link_libraries(test_vector_object_shapes ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_shapes + ${vo_library_name} +) + +# test_vector_object_server unit test +ament_add_gtest(test_vector_object_server + test_vector_object_server.cpp +) + +target_link_libraries(test_vector_object_server ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_server + ${vo_library_name} +) diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp new file mode 100644 index 00000000000..88bfd697d0a --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -0,0 +1,1349 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "unique_identifier_msgs/msg/uuid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_server.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static const double UPDATE_FREQUENCY{10.0}; +static const int8_t POLYGON_VAL{nav2_util::OCC_GRID_OCCUPIED}; +static const int8_t CIRCLE_VAL{nav2_util::OCC_GRID_OCCUPIED / 2}; + +class VOServerWrapper : public nav2_map_server::VectorObjectServer +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void configureFail() + { + ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::FAILURE); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void cleanup() + { + ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void setProcessMap(const bool process_map) + { + process_map_ = process_map; + } + + void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const + { + VectorObjectServer::getMapBoundaries(min_x, min_y, max_x, max_y); + } + + void updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y) + { + VectorObjectServer::updateMap(min_x, min_y, max_x, max_y); + } + + void putVectorObjectsOnMap() + { + VectorObjectServer::putVectorObjectsOnMap(); + } +}; // VOServerWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + void setVOServerParams(); + void setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid); + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(const double frame_shift); + + template + typename T::Response::SharedPtr sendRequest( + typename nav2::ServiceClient::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout); + + void mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map); + bool waitMap(const std::chrono::nanoseconds & timeout); + void verifyMap(bool is_circle, double poly_x_end = 1.0, double circle_cx = 3.0); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Service clients for calling AddShapes.srv, GetShapes.srv, RemoveShapes.srv + nav2::ServiceClient::SharedPtr add_shapes_client_; + nav2::ServiceClient::SharedPtr get_shapes_client_; + nav2::ServiceClient::SharedPtr remove_shapes_client_; + + // Output map subscriber + rclcpp::Subscription::SharedPtr vo_map_sub_; + // Output map published by VectorObjectServer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + + // Vector Object server node + std::shared_ptr vo_server_; +}; // Tester + +Tester::Tester() +: map_(nullptr) +{ + vo_server_ = std::make_shared(); + + add_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/add_shapes"); + + get_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/get_shapes"); + + remove_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/remove_shapes"); + + vo_map_sub_ = vo_server_->create_subscription( + "vo_map", std::bind(&Tester::mapCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS()); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(vo_server_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + vo_map_sub_.reset(); + + add_shapes_client_.reset(); + get_shapes_client_.reset(); + remove_shapes_client_.reset(); + + vo_server_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setVOServerParams() +{ + vo_server_->declare_parameter( + "map_topic", rclcpp::ParameterValue("vo_map")); + vo_server_->set_parameter( + rclcpp::Parameter("map_topic", "vo_map")); + + vo_server_->declare_parameter( + "global_frame_id", rclcpp::ParameterValue("map")); + vo_server_->set_parameter( + rclcpp::Parameter("global_frame_id", "map")); + + vo_server_->declare_parameter( + "resolution", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("resolution", 0.1)); + + vo_server_->declare_parameter( + "default_value", rclcpp::ParameterValue(nav2_util::OCC_GRID_UNKNOWN)); + vo_server_->set_parameter( + rclcpp::Parameter("default_value", nav2_util::OCC_GRID_UNKNOWN)); + + vo_server_->declare_parameter( + "overlay_type", + rclcpp::ParameterValue(static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + + vo_server_->declare_parameter( + "update_frequency", rclcpp::ParameterValue(UPDATE_FREQUENCY)); + vo_server_->set_parameter( + rclcpp::Parameter("update_frequency", UPDATE_FREQUENCY)); + + vo_server_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("transform_tolerance", 0.1)); +} + +void Tester::setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid) +{ + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + setPolygonParams(poly_uuid); + setCircleParams(circle_uuid); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + vo_server_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + vo_server_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + vo_server_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(POLYGON_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", POLYGON_VAL)); + + std::vector points{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; + vo_server_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + vo_server_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + vo_server_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + vo_server_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(CIRCLE_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".value", CIRCLE_VAL)); + + vo_server_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(std::vector{3.0, 0.0})); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{3.0, 0.0})); + + vo_server_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +void Tester::sendTransform(const double frame_shift) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(vo_server_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = vo_server_->now(); + transform.transform.translation.x = frame_shift; + transform.transform.translation.y = 0.0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = POLYGON_VAL; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 3.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = CIRCLE_VAL; + + return co; +} + +template +typename T::Response::SharedPtr Tester::sendRequest( + typename nav2::ServiceClient::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout) +{ + auto result_future = client->async_call(request); + + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return result_future.get(); + } + rclcpp::spin_some(vo_server_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + +void Tester::mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + map_ = map; +} + +bool Tester::waitMap(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + if (map_) { + return true; + } + rclcpp::spin_some(vo_server_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::verifyMap(bool is_circle, double poly_x_end, double circle_cx) +{ + // Wait for map to be received + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Map should contain polygon and circle and will look like: + // + // polygon {x1, y1} (and map's origin): should be always {-1.0, -1.0} + // V + // xxxxxx....xxx. + // xxxxxx...xxxxx < circle is optionally placed on map + // xxxxxx...xxxxx + // xxxxxx....xxx. + // ^ + // polygon {x2, y2}. x2 = poly_x_end; y2 - is always == 1.0 + + // Polygon {x2, y2} in map coordinates + const unsigned int m_poly_x2 = 9 + poly_x_end * 10; + const unsigned int m_poly_y2 = 19; + + // Center of the circle in map coordinates (expressed in floating-point for best precision) + const double m_circle_cx = 9 + circle_cx * 10 + 0.5; // cell's origin + 0.5 center of cell + const double m_circle_cy = 9 + 0.5; // cell's origin + 0.5 center of cell + + // Radius of the circle in map coordinates + const double m_circle_rad = 10.0; + + for (unsigned int my = 0; my < map_->info.height; my++) { + for (unsigned int mx = 0; mx < map_->info.width; mx++) { + if (mx <= m_poly_x2 && my <= m_poly_y2) { + // Point belongs to the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + } else if (is_circle && std::hypot(m_circle_cx - mx, m_circle_cy - my) <= m_circle_rad) { + // Point belongs to the circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + } else { + // Point does not belong to any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + } + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +// ---------- ROS-parameters tests ---------- +TEST_F(Tester, testObtainParams) +{ + setVOServerParams(); + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "01010101-0101-0101-0101-010101010102"); + vo_server_->start(); + + verifyMap(true); + + vo_server_->stop(); +} + +TEST_F(Tester, testObtainNoShapes) +{ + setVOServerParams(); + // Set shapes array, but does not set shapes themselves + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectShapeType) +{ + setVOServerParams(); + setShapesParams("", ""); + // Setting incorrect type of circle + vo_server_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".type", "incorrect_type")); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectPolygonParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "incorrect_polygon_uuid", + "01010101-0101-0101-0101-010101010102"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectCircleParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "incorrect_circle_uuid"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +// ---------- Service call tests ---------- +TEST_F(Tester, testAddShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now move X-coordinate of polygon's border and + // add a new circle fully placed inside first one + // through AddShapes.srv: + // Update polygon x2-coordinate to 1.5 + po_msg->points[0].x = 1.5; + po_msg->points[3].x = 1.5; + // Prepare second circle object (fully placed inside first circle) + auto co2_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3}); + co2_msg->radius = 0.5; + + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_msg->circles.push_back(*co2_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true, 1.5); + + // Check that getShapes.srv will return correct shapes + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circles + ASSERT_EQ(get_shapes_result->circles.size(), 2u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + c_check = std::make_shared(get_shapes_result->circles[1]); + compareCircleObjects(c_check, co2_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now remove circle from map + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that there is only polygon remained on map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + // Then call RemoveShapes.srv with enabled all_objects parameter + remove_shapes_msg->all_objects = true; + remove_shapes_msg->uuids.clear(); + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + // getShapes.srv should return empty vectors + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + ASSERT_EQ(get_shapes_result->polygons.size(), 0u); + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +TEST_F(Tester, testAddIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to update polygon with circle's uuid + po_msg->uuid.uuid[15] = 2; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + po_msg->uuid.uuid[15] = 1; // Restoring back for further usage + + // Then try to update circle with polygon's uuid + co_msg->uuid.uuid[15] = 1; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Try to update polygon with incorrect number of points + po_msg->points.resize(2); + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect polygon + po_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + // Restoring back for further usage + po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + + // Try to update circle with incorrect radius + co_msg->radius = -1.0; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect circle + co_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->radius = 1.0; // Restoring back for further usage + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Verify that map did not corrupted after all false-manipulations + verifyMap(true); + + // Check that getShapes.srv will return correct shapes after all false-manipulations + get_shapes_msg = std::make_shared(); + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to remove two shapes: non-existing shape and circle + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 100}; + remove_shapes_msg->uuids.push_back(uuid); + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_FALSE(remove_shapes_result->success); + + // Check that despite on the error, the circle was removed from map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +// ---------- Overlay tests ---------- +TEST_F(Tester, testOverlayMax) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MAX))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayMin) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MIN))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlaySeq) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->start(); + + // Sequentially add polygon and then circle overlapped with it on map + auto add_shapes_msg = std::make_shared(); + + // Prepare first polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + add_shapes_msg->polygons.push_back(*po_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Then prepare circle object to add over the polygon + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + // Also check that putBorders part works correctly + co_msg->fill = false; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 10; // on the circle border laying over the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 14; // inside circle and polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 24; // inside circle only + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + mx = 29; // on the circle border laying outside the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayUnknown) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter("overlay_type", static_cast(1000))); + vo_server_->start(); + + // Try to add polygon on map + auto add_shapes_msg = std::make_shared(); + auto po_msg = makePolygonObject(std::vector()); + add_shapes_msg->polygons.push_back(*po_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Check that polygon was not added on map: map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +// ---------- Transform/dynamic tests ---------- +TEST_F(Tester, testShapesMove) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // No shift between polygon and map + sendTransform(0.0); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon and circle to be in another moving frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true); + + // Move frame with polygon + sendTransform(0.5); + + // Map is being published dynamically. Wait for the map to be published one more time + map_.reset(); + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then try to publish polygon in incorrect frame + add_shapes_msg->polygons[0].header.frame_id = "incorrect_frame"; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Having incorrect transform, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + vo_server_->stop(); +} + +TEST_F(Tester, testSwitchDynamicStatic) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // 0.5m shift between polygon and map + sendTransform(0.5); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon to be in different frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then return to the static model and ensure the everything works correctly + add_shapes_msg->polygons[0].header.frame_id = GLOBAL_FRAME_ID; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + verifyMap(true); + + vo_server_->stop(); +} + +// ---------- Corner cases ---------- +TEST_F(Tester, switchProcessMap) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Check that received default map is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + // Disable map processing and trying to obtain the map + vo_server_->setProcessMap(false); + + // Switching map update by calling remove service + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = true; + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Having process_map_ disabled, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + // Then enable map processing and trying to obtain the map + vo_server_->setProcessMap(true); + + // Switching map update by calling remove service + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Ensure that map is being updated + ASSERT_TRUE(waitMap(timeout)); + + // ... and it is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + vo_server_->stop(); +} + +TEST_F(Tester, testIncorrectMapBoundaries) { + setVOServerParams(); + vo_server_->start(); + + // Set min_x > max_x + EXPECT_THROW(vo_server_->updateMap(1.0, 0.1, 0.1, 1.0), std::runtime_error); + + // Set min_y > max_y + EXPECT_THROW(vo_server_->updateMap(0.1, 1.0, 1.0, 0.1), std::runtime_error); +} + +TEST_F(Tester, testIncorrectMapBoundariesWhenNoShapes) { + setVOServerParams(); + vo_server_->start(); + double min_x, min_y, max_x, max_y; + EXPECT_THROW(vo_server_->getMapBoundaries(min_x, min_y, max_x, max_y), std::runtime_error); +} + +TEST_F(Tester, testShapeOutsideMap) { + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true); + + // Modify the map to have a shape outside the map + vo_server_->updateMap(2.0, 2.0, 4.0, 4.0); + + // Try to put the shapes back in the map + vo_server_->putVectorObjectsOnMap(); + + // Verify that map did not corrupted after all false-manipulations + verifyMap(true); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_map_server/test/unit/test_vector_object_shapes.cpp b/nav2_map_server/test/unit/test_vector_object_shapes.cpp new file mode 100644 index 00000000000..43a6f43a58d --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_shapes.cpp @@ -0,0 +1,805 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_shapes.hpp" +#include "nav2_map_server/vector_object_utils.hpp" + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static double FRAME_SHIFT = 0.1; +static std::vector POINTS{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; +static std::vector CENTER{0.0, 0.0}; + +class PolygonWrapper : public nav2_map_server::Polygon +{ +public: + explicit PolygonWrapper(const nav2::LifecycleNode::WeakPtr & node) + : Polygon(node) + {} + + geometry_msgs::msg::Polygon::SharedPtr getPoly() + { + return polygon_; + } +}; // PolygonWrapper + +class CircleWrapper : public nav2_map_server::Circle +{ +public: + explicit CircleWrapper(const nav2::LifecycleNode::WeakPtr & node) + : Circle(node) + {} + + geometry_msgs::msg::Point32::SharedPtr getCenter() + { + return center_; + } +}; // CircleWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(); + + nav_msgs::msg::OccupancyGrid::SharedPtr makeMap(); + void verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::shared_ptr polygon_; + std::shared_ptr circle_; + + nav2::LifecycleNode::SharedPtr node_; +}; // Tester + +Tester::Tester() +{ + node_ = std::make_shared("test_node"); + + // Create shapes + polygon_ = std::make_shared(node_); + circle_ = std::make_shared(node_); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + polygon_.reset(); + circle_.reset(); + + node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(POINTS)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + if (!uuid.empty()) { + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(CENTER)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + node_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = nav2_util::OCC_GRID_OCCUPIED; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 0.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = nav2_util::OCC_GRID_OCCUPIED; + + return co; +} + +void Tester::sendTransform() +{ + std::shared_ptr tf_broadcaster = + std::make_shared(node_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = node_->now(); + transform.transform.translation.x = FRAME_SHIFT; + transform.transform.translation.y = FRAME_SHIFT; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav_msgs::msg::OccupancyGrid::SharedPtr Tester::makeMap() +{ + nav_msgs::msg::OccupancyGrid::SharedPtr map = std::make_shared(); + map->header.frame_id = GLOBAL_FRAME_ID; + map->info.resolution = 0.1; + map->info.width = 40; + map->info.height = 40; + map->info.origin.position.x = -2.0; + map->info.origin.position.y = -2.0; + map->data = std::vector(400 * 400, nav2_util::OCC_GRID_FREE); + + return map; +} + +void Tester::verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Map is expected to be as follows: + // 0 0 0 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 0 0 0 0 0 0 + const unsigned int map_center_x = 19; + const unsigned int map_center_y = 19; + + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (my == map_center_y - 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 1st border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (mx == map_center_x - 10 && my >= map_center_y - 10 && my <= map_center_y + 10) { + // 2nd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (my == map_center_y + 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 3rd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else { + // Does not belong to any border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } + } +} + +void Tester::verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Circle center in cell coordinates + const double circle_center_x = 19.5; // 19 cell's origin + 0.5 center of cell + const double circle_center_y = 19.5; // 19 cell's origin + 0.5 center of cell + + double radius; + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (map->data[my * map->info.width + mx] == nav2_util::OCC_GRID_OCCUPIED) { + radius = std::hypot(circle_center_x - mx, circle_center_y - my); + ASSERT_NEAR(radius, 10.0, 1.0); // Border drift no more than once cell + } + } + } +} + +void Tester::verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +//---------- Polygon testcases ---------- + +TEST_F(Tester, testPolygonObtainParams) +{ + setPolygonParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testPolygonObtainIncorrectParams) +{ + // Setting polygon parameters w/o points + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + + // Check that points is mandatory parameter for the polygon + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + + // Setting incorrect number of points + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", std::vector{1.0, 2.0, 3.0})); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + // Setting incorrect UUID + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); +} + +TEST_F(Tester, testPolygonSetParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(polygon_->setParams(po)); + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::PolygonObject::SharedPtr params = polygon_->getParams(); + comparePolygonObjects(params, po); +} + +TEST_F(Tester, testPolygonSetIncorrectParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector()); + // Setting incorrect number of vertices + po->points.resize(2); + + // And check that it is failed to set these parameters + ASSERT_FALSE(polygon_->setParams(po)); +} + +TEST_F(Tester, testPolygonBoundaries) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + double min_x, min_y, max_x, max_y; + polygon_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testPolygonPoints) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_TRUE(polygon_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(polygon_->isPointInside(-2.0, -2.0)); + ASSERT_FALSE(polygon_->isPointInside(2.0, 2.0)); +} + +TEST_F(Tester, testPolygonBorders) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyPolygonBorders(map); +} + +TEST_F(Tester, testPolygonBordersOutOfBoundaries) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + // Set polygon to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".points", + std::vector{5.0, 5.0, 6.0, 5.0, 5.0, 6.0})); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testPolygonBordersOnePointInsideBoundaries) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + // Now, set the first point inside the map and the others out of the map + node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".points", + std::vector{0.5, 0.5, 6.0, 5.0, 5.0, 6.0})); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testPolygonDifferentFrame) +{ + setPolygonParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Polygon::SharedPtr poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0, EPSILON); + + // Transform shape coordinates to global frame + ASSERT_TRUE(polygon_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0 + FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(polygon_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +//---------- Circles testcases ---------- + +TEST_F(Tester, testCircleObtainParams) +{ + setCircleParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testCircleObtainIncorrectParams) +{ + const std::string circle_name(CIRCLE_NAME); + + // Setting circle parameters w/o center and radius + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter( + circle_name + ".radius", rclcpp::PARAMETER_DOUBLE); + + // Check that center and radius are mandatory parameter for the circle + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect radius + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", -1.0)); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + // Setting incorrect center format + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{0.0})); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect UUID + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(circle_->obtainParams(circle_name)); +} + +TEST_F(Tester, testCircleSetParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(circle_->setParams(co)); + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::CircleObject::SharedPtr params = circle_->getParams(); + compareCircleObjects(params, co); +} + +TEST_F(Tester, testCircleSetIncorrectParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector()); + // Setting incorrect radius + co->radius = -1.0; + + // And check that it is failed to set these parameters + ASSERT_FALSE(circle_->setParams(co)); +} + +TEST_F(Tester, testCircleBoundaries) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + double min_x, min_y, max_x, max_y; + circle_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testCirclePoints) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_TRUE(circle_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(circle_->isPointInside(-1.0, -1.0)); + ASSERT_FALSE(circle_->isPointInside(1.0, 1.0)); +} + +TEST_F(Tester, testCircleBorders) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyCircleBorders(map); +} + +TEST_F(Tester, testCircleBordersOutOfBoundaries) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + // Set circle to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(CIRCLE_NAME) + ".center", + std::vector{5.0, 5.0})); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testCircleBordersOutsideMap) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + // Set circle to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(CIRCLE_NAME) + ".center", + std::vector{-3.0, -3.0})); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testCircleDifferentFrame) +{ + setCircleParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Point32::SharedPtr center = circle_->getCenter(); + ASSERT_NEAR(center->x, 0.0, EPSILON); + ASSERT_NEAR(center->y, 0.0, EPSILON); + // Transform shape coordinates to global frame + ASSERT_TRUE(circle_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + center = circle_->getCenter(); + ASSERT_NEAR(center->x, FRAME_SHIFT, EPSILON); + ASSERT_NEAR(center->y, FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(circle_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 14d1327508b..41311173486 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(geographic_msgs) find_package(action_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) nav2_package() @@ -41,6 +42,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Trajectory.msg" "msg/TrajectoryPoint.msg" "srv/GetCosts.srv" + "msg/PolygonObject.msg" + "msg/CircleObject.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" @@ -52,6 +55,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "srv/SetInitialPose.srv" "srv/ReloadDockDatabase.srv" + "srv/AddShapes.srv" + "srv/RemoveShapes.srv" + "srv/GetShapes.srv" "srv/SetRouteGraph.srv" "srv/DynamicEdges.srv" "action/AssistedTeleop.action" @@ -72,7 +78,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/UndockRobot.action" "action/ComputeRoute.action" "action/ComputeAndTrackRoute.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs unique_identifier_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/msg/CircleObject.msg b/nav2_msgs/msg/CircleObject.msg new file mode 100644 index 00000000000..2aa64adab62 --- /dev/null +++ b/nav2_msgs/msg/CircleObject.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +unique_identifier_msgs/UUID uuid +geometry_msgs/Point32 center +float32 radius +bool fill +int8 value diff --git a/nav2_msgs/msg/PolygonObject.msg b/nav2_msgs/msg/PolygonObject.msg new file mode 100644 index 00000000000..b5c9721c629 --- /dev/null +++ b/nav2_msgs/msg/PolygonObject.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +unique_identifier_msgs/UUID uuid +geometry_msgs/Point32[] points +bool closed +int8 value diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 61e84addc37..33de880662e 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -21,6 +21,7 @@ action_msgs nav_msgs geographic_msgs + unique_identifier_msgs rosidl_interface_packages diff --git a/nav2_msgs/srv/AddShapes.srv b/nav2_msgs/srv/AddShapes.srv new file mode 100644 index 00000000000..1b3b5753874 --- /dev/null +++ b/nav2_msgs/srv/AddShapes.srv @@ -0,0 +1,6 @@ +# Add/update vector objects on map + +CircleObject[] circles +PolygonObject[] polygons +--- +bool success diff --git a/nav2_msgs/srv/GetShapes.srv b/nav2_msgs/srv/GetShapes.srv new file mode 100644 index 00000000000..79b7d6c431c --- /dev/null +++ b/nav2_msgs/srv/GetShapes.srv @@ -0,0 +1,5 @@ +# Get vector objects which are now being applied on map + +--- +CircleObject[] circles +PolygonObject[] polygons diff --git a/nav2_msgs/srv/RemoveShapes.srv b/nav2_msgs/srv/RemoveShapes.srv new file mode 100644 index 00000000000..a001f9360ef --- /dev/null +++ b/nav2_msgs/srv/RemoveShapes.srv @@ -0,0 +1,6 @@ +# Remove vector objects from map + +bool all_objects +unique_identifier_msgs/UUID[] uuids +--- +bool success diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 3a56cb5feea..d67f17e3c7c 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -188,6 +188,49 @@ inline int find_next_matching_goal_in_waypoint_statuses( return itr - waypoint_statuses.begin(); } +/** + * @brief Checks if point is inside the polygon + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @param polygon Polygon to check if the point is inside + * @return True if given point is inside polygon, otherwise false + */ +template +inline bool isPointInsidePolygon( + const double px, const double py, const std::vector & polygon) +{ + // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." + // Communications of the ACM 5.8 (1962): 434. + // Implementation of ray crossings algorithm for point in polygon task solving. + // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. + // Odd number of intersections with polygon boundaries means the point is inside polygon. + const int points_num = polygon.size(); + int i, j; // Polygon vertex iterators + bool res = false; // Final result, initialized with already inverted value + + // Starting from the edge where the last point of polygon is connected to the first + i = points_num - 1; + for (j = 0; j < points_num; j++) { + // Checking the edge only if given point is between edge boundaries by Y coordinates. + // One of the condition should contain equality in order to exclude the edges + // parallel to X+ ray. + if ((py <= polygon[i].y) == (py > polygon[j].y)) { + // Calculating the intersection coordinate of X+ ray + const double x_inter = polygon[i].x + + (py - polygon[i].y) * + (polygon[j].x - polygon[i].x) / + (polygon[j].y - polygon[i].y); + // If intersection with checked edge is greater than point x coordinate, + // inverting the result + if (x_inter > px) { + res = !res; + } + } + i = j; + } + return res; +} + } // namespace geometry_utils } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/occ_grid_utils.hpp b/nav2_util/include/nav2_util/occ_grid_utils.hpp new file mode 100644 index 00000000000..26bb8740f2b --- /dev/null +++ b/nav2_util/include/nav2_util/occ_grid_utils.hpp @@ -0,0 +1,100 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// Copyright (c) 2023 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! +// Alexey Merzlyakov + +#ifndef NAV2_UTIL__OCC_GRID_UTILS_HPP_ +#define NAV2_UTIL__OCC_GRID_UTILS_HPP_ + +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace nav2_util +{ + +/** + * @brief: Convert from world coordinates to map coordinates. + Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. + * @param map OccupancyGrid map on which to convert + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ +inline bool worldToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const double wx, const double wy, unsigned int & mx, unsigned int & my) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + const unsigned int size_x = map->info.width; + const unsigned int size_y = map->info.height; + + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = static_cast((wx - origin_x) / resolution); + my = static_cast((wy - origin_y) / resolution); + if (mx >= size_x || my >= size_y) { + return false; + } + + return true; +} + +/** + * @brief Convert from map coordinates to world coordinates + * @param mx The x map coordinate + * @param my The y map coordinate + * @param wx Will be set to the associated world x coordinate + * @param wy Will be set to the associated world y coordinate + */ +inline void mapToWorld( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const unsigned int mx, const unsigned int my, double & wx, double & wy) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + + wx = origin_x + (mx + 0.5) * resolution; + wy = origin_y + (my + 0.5) * resolution; +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__OCC_GRID_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/raytrace_line_2d.hpp b/nav2_util/include/nav2_util/raytrace_line_2d.hpp new file mode 100644 index 00000000000..b44d1e79a8d --- /dev/null +++ b/nav2_util/include/nav2_util/raytrace_line_2d.hpp @@ -0,0 +1,146 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! + +#ifndef NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ +#define NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ + +#include +#include +#include + +namespace nav2_util +{ + +/** + * @brief get the sign of an int + */ +inline int sign(int x) +{ + return x > 0 ? 1 : -1; +} + +/** + * @brief A 2D implementation of Bresenham's raytracing algorithm... + * applies an action at each step + */ +template +inline void bresenham2D( + ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, + int offset_a, int offset_b, unsigned int offset, + unsigned int max_length) +{ + unsigned int end = std::min(max_length, abs_da); + for (unsigned int i = 0; i < end; ++i) { + at(offset); + offset += offset_a; + error_b += abs_db; + if ((unsigned int)error_b >= abs_da) { + offset += offset_b; + error_b -= abs_da; + } + } + at(offset); +} + +/** + * @brief Raytrace a line and apply some action at each step + * @param at The action to take... a functor + * @param x0 The starting x coordinate + * @param y0 The starting y coordinate + * @param x1 The ending x coordinate + * @param y1 The ending y coordinate + * @param step_x OX-step on map + * @param max_length The maximum desired length of the segment... + * allows you to not go all the way to the endpoint + * @param min_length The minimum desired length of the segment + */ +template +inline void raytraceLine( + ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, unsigned int step_x, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) +{ + int dx_full = x1 - x0; + int dy_full = y1 - y0; + + // we need to chose how much to scale our dominant dimension, + // based on the maximum length of the line + double dist = std::hypot(dx_full, dy_full); + if (dist < min_length) { + return; + } + + unsigned int min_x0, min_y0; + if (dist > 0.0) { + // Adjust starting point and offset to start from min_length distance + min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); + min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); + } else { + // dist can be 0 if [x0, y0]==[x1, y1]. + // In this case only this cell should be processed. + min_x0 = x0; + min_y0 = y0; + } + unsigned int offset = min_y0 * step_x + min_x0; + + int dx = x1 - min_x0; + int dy = y1 - min_y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * step_x; + + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + // if x is dominant + if (abs_dx >= abs_dy) { + int error_y = abs_dx / 2; + + bresenham2D( + at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); + return; + } + + // otherwise y is dominant + int error_x = abs_dy / 2; + + bresenham2D( + at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 09d7d4a38c0..c8447eaab6e 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -1,3 +1,5 @@ +add_subdirectory(regression) + ament_add_gtest(test_execution_timer test_execution_timer.cpp) target_link_libraries(test_execution_timer ${library_name}) diff --git a/nav2_util/test/regression/CMakeLists.txt b/nav2_util/test/regression/CMakeLists.txt new file mode 100644 index 00000000000..f216fbe7751 --- /dev/null +++ b/nav2_util/test/regression/CMakeLists.txt @@ -0,0 +1,3 @@ +# Bresenham2D corner cases test +ament_add_gtest(map_bresenham_2d map_bresenham_2d.cpp) +target_link_libraries(map_bresenham_2d ${library_name}) diff --git a/nav2_util/test/regression/map_bresenham_2d.cpp b/nav2_util/test/regression/map_bresenham_2d.cpp new file mode 100644 index 00000000000..220962948e5 --- /dev/null +++ b/nav2_util/test/regression/map_bresenham_2d.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Alexey Merzlyakov + +#include + +#include + +#include "nav2_util/raytrace_line_2d.hpp" + +// MapAction - is a functor class used to cover raytraceLine algorithm. +// It contains char map inside, which is an abstract one and not related +// to any concrete representation (like Costmap2D or OccupancyGrid). +class MapAction +{ +public: + explicit MapAction( + char * map, unsigned int size, char mark_val = 100) + : map_(map), size_(size), mark_val_(mark_val) + { + } + + inline void operator()(unsigned int off) + { + ASSERT_TRUE(off < size_); + map_[off] = mark_val_; + } + + inline unsigned int get(unsigned int off) + { + return map_[off]; + } + +private: + char * map_; + unsigned int size_; + char mark_val_; +}; + +class MapTest +{ +public: + MapTest( + unsigned int size_x, unsigned int size_y, + char default_val = 0) + : size_x_(size_x), size_y_(size_y) + { + map_ = new char[size_x * size_y]; + memset(map_, default_val, size_x * size_y); + } + + char * getMap() + { + return map_; + } + + unsigned int getSize() + { + return size_x_ * size_y_; + } + + void raytraceLine( + MapAction ma, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) + { + nav2_util::raytraceLine(ma, x0, y0, x1, y1, size_x_, max_length, min_length); + } + +private: + char * map_; + unsigned int size_x_, size_y_; +}; + +TEST(map_2d, bresenham2DBoundariesCheck) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + MapTest mt(sz_x, sz_y); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point - some asymmetrically standing point in order to cover most corner cases + const unsigned int x0 = 2; + const unsigned int y0 = 4; + // (x1, y1) point will move + unsigned int x1, y1; + + // Running on (x, 0) edge + y1 = 0; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - 1; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x - 1; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } +} + +TEST(map_2d, bresenham2DSamePoint) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + MapTest mt(sz_x, sz_y, 0.1); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point + const double x0 = 2; + const double y0 = 4; + + unsigned int offset = y0 * sz_x + x0; + char val_before = ma.get(offset); + // Same point to check + mt.raytraceLine(ma, x0, y0, x0, y0, max_length, min_length); + char val_after = ma.get(offset); + ASSERT_FALSE(val_before == val_after); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From b45fca27e418238bb2a786d590be5da2834d2a35 Mon Sep 17 00:00:00 2001 From: Sushant Chavan Date: Mon, 15 Sep 2025 23:31:48 +0200 Subject: [PATCH 5/7] Fix/dependency and header (#5520) * Add missing dependency declaration Signed-off-by: Sushant Chavan * Remove unused header Signed-off-by: Sushant Chavan --------- Signed-off-by: Sushant Chavan --- nav2_mppi_controller/package.xml | 1 + nav2_smac_planner/src/a_star.cpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 67067ca4ced..d6301408998 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -13,6 +13,7 @@ angles backward_ros + benchmark geometry_msgs nav2_common nav2_core diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 98d040bd9cc..b8d12e7941f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -13,7 +13,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include #include #include #include From 2a803c2527baa0b04ead03e8ac66e733864e4bbd Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 17 Sep 2025 02:01:23 +0800 Subject: [PATCH 6/7] Replace rclcpp::spin_some and rclcpp::spin_all (#5521) * Get rid of rclcpp spin all and spin some Signed-off-by: mini-1235 * Linting Signed-off-by: mini-1235 * Vector object server Signed-off-by: mini-1235 * Linting Signed-off-by: mini-1235 * Update nav2_rviz_plugins/src/utils.cpp Signed-off-by: Steve Macenski * Error log Signed-off-by: Maurice --------- Signed-off-by: mini-1235 Signed-off-by: Steve Macenski Signed-off-by: Maurice Co-authored-by: Steve Macenski --- .../plugins/action/test_bt_action_node.cpp | 6 ++- .../action/test_controller_selector_node.cpp | 10 ++++- .../test_goal_checker_selector_node.cpp | 10 ++++- .../action/test_planner_selector_node.cpp | 10 ++++- .../test_progress_checker_selector_node.cpp | 10 ++++- .../action/test_smoother_selector_node.cpp | 10 ++++- .../condition/test_is_battery_charging.cpp | 16 ++++--- .../plugins/condition/test_is_battery_low.cpp | 22 +++++---- .../test/collision_detector_node_test.cpp | 10 +++-- .../test/collision_monitor_node_test.cpp | 14 +++--- nav2_collision_monitor/test/polygons_test.cpp | 45 ++++++++++++------- nav2_collision_monitor/test/sources_test.cpp | 11 +++-- .../test/velocity_polygons_test.cpp | 19 ++------ .../test/integration/dyn_params_tests.cpp | 8 +++- .../test/integration/inflation_tests.cpp | 4 +- .../integration/plugin_container_tests.cpp | 4 +- .../integration/test_costmap_subscriber.cpp | 9 +++- .../test_costmap_topic_collision_checker.cpp | 5 ++- .../test/unit/binary_filter_test.cpp | 14 +++--- .../test/unit/keepout_filter_test.cpp | 8 +++- .../test/unit/speed_filter_test.cpp | 14 +++--- .../opennav_docking/test/test_controller.cpp | 44 ++++++++++-------- .../test/test_simple_charging_dock.cpp | 28 ++++++++---- .../test/test_simple_non_charging_dock.cpp | 21 ++++++--- .../unit/test_costmap_filter_info_server.cpp | 8 +++- .../test/unit/test_vector_object_server.cpp | 6 ++- .../test/trajectory_visualizer_tests.cpp | 23 +++++++--- nav2_mppi_controller/test/utils/utils.hpp | 4 +- nav2_ros_common/test/test_actions.cpp | 4 +- nav2_ros_common/test/test_service_client.cpp | 12 +++-- nav2_ros_common/test/test_service_server.cpp | 9 ++-- nav2_route/test/test_graph_loader.cpp | 4 +- nav2_route/test/test_graph_saver.cpp | 4 +- .../nav2_rviz_plugins/docking_panel.hpp | 1 + .../include/nav2_rviz_plugins/nav2_panel.hpp | 2 +- .../include/nav2_rviz_plugins/utils.hpp | 4 +- nav2_rviz_plugins/src/docking_panel.cpp | 14 +++--- nav2_rviz_plugins/src/nav2_panel.cpp | 20 +++++---- nav2_rviz_plugins/src/utils.cpp | 22 +++++++-- .../assisted_teleop_behavior_tester.cpp | 9 ++-- .../assisted_teleop_behavior_tester.hpp | 1 + .../behaviors/wait/wait_behavior_tester.cpp | 5 ++- .../localization/test_localization_node.cpp | 6 ++- .../test/test_base_footprint_publisher.cpp | 8 ++-- nav2_util/test/test_odometry_utils.cpp | 13 +++--- nav2_util/test/test_twist_publisher.cpp | 10 +++-- nav2_util/test/test_twist_subscriber.cpp | 10 +++-- .../test/test_velocity_smoother.cpp | 16 +++++-- .../test/test_task_executors.cpp | 8 +++- 49 files changed, 384 insertions(+), 191 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index 3b2a3b382c1..96e9a594447 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -202,8 +202,10 @@ class BTActionNodeTestFixture : public ::testing::Test action_server_ = std::make_shared(); server_thread_ = std::make_shared( []() { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(action_server_); while (rclcpp::ok() && BTActionNodeTestFixture::action_server_ != nullptr) { - rclcpp::spin_some(BTActionNodeTestFixture::action_server_); + executor.spin_some(); std::this_thread::sleep_for(100ns); } }); @@ -211,6 +213,8 @@ class BTActionNodeTestFixture : public ::testing::Test void TearDown() override { + // Sleep for some time to avoid race condition + std::this_thread::sleep_for(std::chrono::milliseconds(80)); action_server_.reset(); tree_.reset(); server_thread_->join(); diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 9f686ad2ed4..8de6cb2b77b 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -31,6 +31,8 @@ class ControllerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("controller_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -67,6 +69,7 @@ class ControllerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -76,12 +79,15 @@ class ControllerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr ControllerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr ControllerSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr; std::shared_ptr ControllerSelectorTestFixture::factory_ = nullptr; @@ -126,7 +132,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); controller_selector_pub->publish(selected_controller_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check controller updated @@ -173,7 +179,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); controller_selector_pub->publish(selected_controller_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check controller updated diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index 0067a5bd7ed..c0d0ee35daa 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -31,6 +31,8 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("goal_checker_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -67,6 +69,7 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -76,12 +79,15 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr GoalCheckerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr GoalCheckerSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * GoalCheckerSelectorTestFixture::config_ = nullptr; std::shared_ptr GoalCheckerSelectorTestFixture::factory_ = nullptr; @@ -127,7 +133,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); goal_checker_selector_pub->publish(selected_goal_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check goal_checker updated @@ -175,7 +181,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); goal_checker_selector_pub->publish(selected_goal_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check goal_checker updated diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index 7043d193789..82ea0635f27 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -31,6 +31,8 @@ class PlannerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("planner_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -65,6 +67,7 @@ class PlannerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -74,12 +77,15 @@ class PlannerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr PlannerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr PlannerSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * PlannerSelectorTestFixture::config_ = nullptr; std::shared_ptr PlannerSelectorTestFixture::factory_ = nullptr; @@ -125,7 +131,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); planner_selector_pub->publish(selected_planner_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check planner updated @@ -173,7 +179,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); planner_selector_pub->publish(selected_planner_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check planner updated diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp index eebb6c193b4..c305471f412 100644 --- a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -30,6 +30,8 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("progress_checker_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -66,6 +68,7 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -75,12 +78,15 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr ProgressCheckerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr ProgressCheckerSelectorTestFixture:: +executor_ = nullptr; BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr; std::shared_ptr ProgressCheckerSelectorTestFixture::factory_ = nullptr; @@ -129,7 +135,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); progress_checker_selector_pub->publish(selected_progress_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check progress_checker updated @@ -179,7 +185,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); progress_checker_selector_pub->publish(selected_progress_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check goal_checker updated diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index 50238346741..e099477de84 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -31,6 +31,8 @@ class SmootherSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("smoother_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -67,6 +69,7 @@ class SmootherSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -76,12 +79,15 @@ class SmootherSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr SmootherSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr SmootherSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr; std::shared_ptr SmootherSelectorTestFixture::factory_ = nullptr; @@ -127,7 +133,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); smoother_selector_pub->publish(selected_smoother_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check smoother updated @@ -175,7 +181,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); smoother_selector_pub->publish(selected_smoother_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check smoother updated diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index fb27568c275..9733012e952 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -29,6 +29,8 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("test_is_battery_charging"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); factory_ = std::make_shared(); config_ = new BT::NodeConfiguration(); @@ -58,10 +60,12 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test battery_pub_.reset(); node_.reset(); factory_.reset(); + executor_.reset(); } protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static nav2::Publisher::SharedPtr @@ -69,6 +73,8 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test }; nav2::LifecycleNode::SharedPtr IsBatteryChargingConditionTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr IsBatteryChargingConditionTestFixture:: +executor_ = nullptr; BT::NodeConfiguration * IsBatteryChargingConditionTestFixture::config_ = nullptr; std::shared_ptr IsBatteryChargingConditionTestFixture::factory_ = nullptr; nav2::Publisher::SharedPtr @@ -90,32 +96,32 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index a5080593117..86affc16424 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -30,6 +30,8 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("test_is_battery_low"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); factory_ = std::make_shared(); config_ = new BT::NodeConfiguration(); @@ -59,10 +61,12 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test battery_pub_.reset(); node_.reset(); factory_.reset(); + executor_.reset(); } protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static nav2::Publisher::SharedPtr @@ -70,6 +74,8 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test }; nav2::LifecycleNode::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr IsBatteryLowConditionTestFixture:: +executor_ = nullptr; BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr; std::shared_ptr IsBatteryLowConditionTestFixture::factory_ = nullptr; nav2::Publisher::SharedPtr @@ -91,25 +97,25 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) battery_msg.percentage = 1.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.49; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.percentage = 0.51; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } @@ -129,25 +135,25 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) battery_msg.voltage = 10.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 4.9; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.voltage = 5.1; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 4bbd37bc4e9..a49a28008f3 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -155,6 +155,7 @@ class Tester : public ::testing::Test protected: // CollisionDetector node std::shared_ptr cd_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // Data source publishers nav2::Publisher::SharedPtr @@ -178,6 +179,8 @@ class Tester : public ::testing::Test Tester::Tester() { cd_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(cd_->get_node_base_interface()); scan_pub_ = cd_->create_publisher( SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -213,6 +216,7 @@ Tester::~Tester() collision_points_marker_sub_.reset(); cd_.reset(); + executor_.reset(); } bool Tester::waitState(const std::chrono::nanoseconds & timeout) @@ -222,7 +226,7 @@ bool Tester::waitState(const std::chrono::nanoseconds & timeout) if (state_msg_) { return true; } - rclcpp::spin_some(cd_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -236,7 +240,7 @@ bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) if (collision_points_marker_msg_) { return true; } - rclcpp::spin_some(cd_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -544,7 +548,7 @@ bool Tester::waitData( if (cd_->correctDataReceived(expected_dist, stamp)) { return true; } - rclcpp::spin_some(cd_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index ea0213f468c..92dd5e6751a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -188,6 +188,7 @@ class Tester : public ::testing::Test // CollisionMonitor node std::shared_ptr cm_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // Footprint publisher nav2::Publisher::SharedPtr @@ -222,6 +223,8 @@ class Tester : public ::testing::Test Tester::Tester() { cm_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(cm_->get_node_base_interface()); cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); footprint_pub_ = cm_->create_publisher( @@ -281,6 +284,7 @@ Tester::~Tester() collision_points_marker_sub_.reset(); cm_.reset(); + executor_.reset(); } void Tester::setCommonParameters() @@ -724,7 +728,7 @@ bool Tester::waitData( if (cm_->correctDataReceived(expected_dist, stamp)) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -737,7 +741,7 @@ bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) if (cmd_vel_out_) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -753,7 +757,7 @@ bool Tester::waitFuture( if (status == std::future_status::ready) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -766,7 +770,7 @@ bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) if (action_state_) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -779,7 +783,7 @@ bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) if (collision_points_marker_msg_) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 86f393febfa..bfe9d38f8a7 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -162,18 +162,9 @@ class TestNode : public nav2::LifecycleNode polygon_received_ = msg; } - geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( - const std::chrono::nanoseconds & timeout) + geometry_msgs::msg::PolygonStamped::SharedPtr getPolygonReceived() { - rclcpp::Time start_time = this->now(); - while (rclcpp::ok() && this->now() - start_time <= rclcpp::Duration(timeout)) { - if (polygon_received_) { - return polygon_received_; - } - rclcpp::spin_some(this->get_node_base_interface()); - std::this_thread::sleep_for(10ms); - } - return nullptr; + return polygon_received_; } private: @@ -265,6 +256,9 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, std::vector & poly); + geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( + const std::chrono::nanoseconds & timeout); + // Wait until circle polygon radius will be received bool waitRadius(const std::chrono::nanoseconds & timeout); @@ -274,6 +268,7 @@ class Tester : public ::testing::Test std::vector & footprint); std::shared_ptr test_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::shared_ptr polygon_; std::shared_ptr circle_; @@ -285,6 +280,8 @@ class Tester : public ::testing::Test Tester::Tester() { test_node_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(test_node_->get_node_base_interface()); test_node_->configure(); test_node_->activate(); @@ -471,6 +468,21 @@ void Tester::sendTransforms(double shift) tf_broadcaster->sendTransform(transform); } +geometry_msgs::msg::PolygonStamped::SharedPtr Tester::waitPolygonReceived( + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + auto polygon = test_node_->getPolygonReceived(); + if (polygon) { + return polygon; + } + executor_->spin_some(); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + bool Tester::waitPolygon( const std::chrono::nanoseconds & timeout, std::vector & poly) @@ -481,7 +493,7 @@ bool Tester::waitPolygon( if (poly.size() > 0) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -494,7 +506,7 @@ bool Tester::waitRadius(const std::chrono::nanoseconds & timeout) if (circle_->isShapeSet()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -512,7 +524,7 @@ bool Tester::waitFootprint( if (footprint.size() > 0) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -956,8 +968,7 @@ TEST_F(Tester, testPolygonPublish) { createPolygon("stop", true); polygon_->publish(); - geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = - test_node_->waitPolygonReceived(500ms); + geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = waitPolygonReceived(500ms); ASSERT_NE(polygon_received, nullptr); ASSERT_EQ(polygon_received->polygon.points.size(), 4u); @@ -996,7 +1007,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) polygon_->publish(); // Wait for polygon: it should not be published - ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr); + ASSERT_EQ(waitPolygonReceived(100ms), nullptr); } TEST_F(Tester, testPolygonInvalidPointsString) diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 034cdd40d96..60a9f896761 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -375,6 +375,7 @@ class Tester : public ::testing::Test void checkPolygon(const std::vector & data); std::shared_ptr test_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::shared_ptr scan_; std::shared_ptr pointcloud_; std::shared_ptr range_; @@ -384,6 +385,8 @@ class Tester : public ::testing::Test Tester::Tester() { test_node_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(test_node_->get_node_base_interface()); tf_buffer_ = std::make_shared(test_node_->get_clock()); tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model @@ -506,7 +509,7 @@ bool Tester::waitScan(const std::chrono::nanoseconds & timeout) if (scan_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -519,7 +522,7 @@ bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout) if (pointcloud_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -532,7 +535,7 @@ bool Tester::waitRange(const std::chrono::nanoseconds & timeout) if (range_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -545,7 +548,7 @@ bool Tester::waitPolygon(const std::chrono::nanoseconds & timeout) if (polygon_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 88024dd723b..64f318dbdbc 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -92,20 +92,6 @@ class TestNode : public nav2::LifecycleNode polygon_received_ = msg; } - geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( - const std::chrono::nanoseconds & timeout) - { - rclcpp::Time start_time = this->now(); - while (rclcpp::ok() && this->now() - start_time <= rclcpp::Duration(timeout)) { - if (polygon_received_) { - return polygon_received_; - } - rclcpp::spin_some(this->get_node_base_interface()); - std::this_thread::sleep_for(10ms); - } - return nullptr; - } - private: nav2::Subscription::SharedPtr polygon_sub_; geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received_; @@ -167,6 +153,7 @@ class Tester : public ::testing::Test std::vector & poly); std::shared_ptr test_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::shared_ptr velocity_polygon_; @@ -177,6 +164,8 @@ class Tester : public ::testing::Test Tester::Tester() { test_node_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(test_node_->get_node_base_interface()); test_node_->configure(); test_node_->activate(); @@ -382,7 +371,7 @@ bool Tester::waitPolygon( if (poly.size() > 0) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index 94e0f72ea77..684aaa58a12 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -32,6 +32,10 @@ TEST(DynParamTestNode, testDynParamsSet) { auto node = std::make_shared("dyn_param_tester"); auto costmap = std::make_shared("test_costmap"); + rclcpp::executors::SingleThreadedExecutor node_executor; + node_executor.add_node(node->get_node_base_interface()); + rclcpp::executors::SingleThreadedExecutor costmap_executor; + costmap_executor.add_node(costmap->get_node_base_interface()); costmap->on_configure(rclcpp_lifecycle::State()); // Set tf between default global_frame and robot_base_frame in order not to block in on_activate @@ -76,8 +80,8 @@ TEST(DynParamTestNode, testDynParamsSet) rclcpp::Parameter("robot_base_frame", "wrong_test_frame"), }); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); - rclcpp::spin_all(costmap->get_node_base_interface(), std::chrono::milliseconds(50)); + node_executor.spin_all(std::chrono::milliseconds(50)); + costmap_executor.spin_all(std::chrono::milliseconds(50)); EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234); EXPECT_EQ(costmap->get_parameter("footprint_padding").as_double(), 2.345); diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 19446c00e38..658791beeb5 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -102,8 +102,10 @@ std::vector TestNode::setRadii( void TestNode::waitForMap(std::shared_ptr & slayer) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (!slayer->isCurrent()) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); } } diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp index 13969ae9a55..dc8a4f4b3a2 100644 --- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -93,8 +93,10 @@ class TestNode : public ::testing::Test void waitForMap(std::shared_ptr & slayer) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (!slayer->isCurrent()) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); } } diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 85468400224..4921fa38bb5 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -205,6 +205,8 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) auto costmapPublisher = std::make_shared( node, costmapToSend.get(), "", topicName, always_send_full_costmap); costmapPublisher->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { @@ -220,7 +222,7 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) expectedGrids.emplace_back(grid); costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); costmapPublisher->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); receivedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); } @@ -252,6 +254,9 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) std::uint32_t yn = costmapToSend->getSizeInCellsY(); bool first_iteration = true; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { costmapToSend->setCost(observation.x, observation.y, observation.cost); @@ -272,7 +277,7 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) expectedGrids.emplace_back(grid); costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); costmapPublisher->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); receivedCostmaps.emplace_back(getUpdatedCharMapFromSubscriber(x0, xn, y0, yn)); first_iteration = false; } diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index e5b73e4b5cc..d7da1e104c5 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -133,9 +133,10 @@ class TestCollisionChecker : public nav2::LifecycleNode // Add Static Layer std::shared_ptr slayer = nullptr; addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer, callback_group_); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(this->get_node_base_interface()); while (!slayer->isCurrent()) { - rclcpp::spin_some(this->get_node_base_interface()); + executor.spin_some(); } // Add Inflation Layer std::shared_ptr ilayer = nullptr; diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index e0061e91e88..bfce85d2106 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -251,6 +251,7 @@ class TestNode : public ::testing::Test const double resolution_ = 1.0; nav2::LifecycleNode::SharedPtr node_; + rclcpp::executors::SingleThreadedExecutor node_executor_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -262,6 +263,7 @@ class TestNode : public ::testing::Test std::shared_ptr info_publisher_; std::shared_ptr mask_publisher_; std::shared_ptr binary_state_subscriber_; + rclcpp::executors::SingleThreadedExecutor binary_state_subscriber_executor_; }; void TestNode::createMaps(const std::string & mask_frame) @@ -318,8 +320,8 @@ void TestNode::waitSome(const std::chrono::nanoseconds & duration) { rclcpp::Time start_time = node_->now(); while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node_->get_node_base_interface()); - rclcpp::spin_some(binary_state_subscriber_); + node_executor_.spin_some(); + binary_state_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } } @@ -327,7 +329,7 @@ void TestNode::waitSome(const std::chrono::nanoseconds & duration) std_msgs::msg::Bool::SharedPtr TestNode::getBinaryState() { std::this_thread::sleep_for(100ms); - rclcpp::spin_some(binary_state_subscriber_); + binary_state_subscriber_executor_.spin_some(); return binary_state_subscriber_->getBinaryState(); } @@ -342,7 +344,7 @@ std_msgs::msg::Bool::SharedPtr TestNode::waitBinaryState() binary_state_subscriber_->resetBinaryStateIndicator(); return binary_state_subscriber_->getBinaryState(); } - rclcpp::spin_some(binary_state_subscriber_); + binary_state_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return nullptr; @@ -389,6 +391,8 @@ bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_ binary_state_subscriber_ = std::make_shared(BINARY_STATE_TOPIC, default_state_); + binary_state_subscriber_executor_.add_node(binary_state_subscriber_); + node_executor_.add_node(node_->get_node_base_interface()); // Wait until mask will be received by BinaryFilter const std::chrono::nanoseconds timeout = 500ms; @@ -397,7 +401,7 @@ bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_ if (node_->now() - start_time > rclcpp::Duration(timeout)) { return false; } - rclcpp::spin_some(node_->get_node_base_interface()); + node_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return true; diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index 37a5a683299..f6078c52cc0 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -200,8 +200,10 @@ void TestNode::rePublishMask() void TestNode::waitSome(const std::chrono::nanoseconds & duration) { rclcpp::Time start_time = node_->now(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(10ms); } } @@ -229,8 +231,10 @@ void TestNode::createKeepoutFilter(const std::string & global_frame) keepout_filter_->initializeFilter(INFO_TOPIC); // Wait until mask will be received by KeepoutFilter + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (!keepout_filter_->isActive()) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(10ms); } } diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 5d3eebb6db5..aabe7ce03b7 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -244,6 +244,7 @@ class TestNode : public ::testing::Test const double resolution_ = 1.0; nav2::LifecycleNode::SharedPtr node_; + rclcpp::executors::SingleThreadedExecutor node_executor_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -255,6 +256,7 @@ class TestNode : public ::testing::Test std::shared_ptr info_publisher_; std::shared_ptr mask_publisher_; std::shared_ptr speed_limit_subscriber_; + rclcpp::executors::SingleThreadedExecutor speed_limit_subscriber_executor_; }; void TestNode::createMaps(const std::string & mask_frame) @@ -308,7 +310,7 @@ void TestNode::rePublishMask() nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::getSpeedLimit() { std::this_thread::sleep_for(100ms); - rclcpp::spin_some(speed_limit_subscriber_); + speed_limit_subscriber_executor_.spin_some(); return speed_limit_subscriber_->getSpeedLimit(); } @@ -323,7 +325,7 @@ nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::waitSpeedLimit() speed_limit_subscriber_->resetSpeedLimitIndicator(); return speed_limit_subscriber_->getSpeedLimit(); } - rclcpp::spin_some(speed_limit_subscriber_); + speed_limit_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return nullptr; @@ -333,8 +335,8 @@ void TestNode::waitSome(const std::chrono::nanoseconds & duration) { rclcpp::Time start_time = node_->now(); while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node_->get_node_base_interface()); - rclcpp::spin_some(speed_limit_subscriber_); + node_executor_.spin_some(); + speed_limit_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } } @@ -366,6 +368,8 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) speed_filter_->initializeFilter(INFO_TOPIC); speed_limit_subscriber_ = std::make_shared(SPEED_LIMIT_TOPIC); + speed_limit_subscriber_executor_.add_node(speed_limit_subscriber_); + node_executor_.add_node(node_->get_node_base_interface()); // Wait until mask will be received by SpeedFilter const std::chrono::nanoseconds timeout = 500ms; @@ -374,7 +378,7 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) if (node_->now() - start_time > rclcpp::Duration(timeout)) { return false; } - rclcpp::spin_some(node_->get_node_base_interface()); + node_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return true; diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index 8708b31428d..e8de87d99c6 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -291,6 +291,8 @@ TEST(ControllerTests, CollisionCheckerDockForward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Set the pose of the dock at 1.75m in front of the robot auto dock_pose = collision_tester->setPose(1.75, 0.0, 0.0); @@ -302,7 +304,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot @@ -310,7 +312,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { // But it does not hit because the collision tolerance is 0.3m collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); // Set an object between the robot and the dock @@ -318,7 +320,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 0.2, 1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -329,7 +331,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); collision_tester->deactivate(); @@ -356,6 +358,8 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Set the pose of the dock at 1.75m behind the robot auto dock_pose = collision_tester->setPose(-1.75, 0.0, 0.0); @@ -367,7 +371,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot @@ -375,7 +379,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { // But it does not hit because the collision tolerance is 0.3m collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); // Set an object between the robot and the dock @@ -383,7 +387,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 0.2, -1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -394,7 +398,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); collision_tester->deactivate(); @@ -421,6 +425,8 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Set the staging pose at 1.75m behind the robot auto staging_pose = collision_tester->setPose(-1.75, 0.0, 0.0); @@ -432,7 +438,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked @@ -440,7 +446,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { // But it does not hit because the collision tolerance is 0.3m collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); // Set an object beyond the staging pose @@ -448,7 +454,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 0.2, -1.75 - 0.5, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); // Set an object between the robot and the staging pose @@ -456,7 +462,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 0.2, -1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -467,7 +473,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); collision_tester->deactivate(); @@ -494,6 +500,8 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Set the staging pose at 1.75m in the front of the robot auto staging_pose = collision_tester->setPose(1.75, 0.0, 0.0); @@ -505,14 +513,14 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked // It should not hit anything because the robot is docked and the trajectory is backward collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); // Set an object beyond the staging pose @@ -520,7 +528,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 0.3, 1.75 + 0.5, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); // Set an object between the robot and the staging pose @@ -528,7 +536,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 0.2, 1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -539,7 +547,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { collision_tester->clearCostmap(); collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); collision_tester->deactivate(); diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 293cfcc8933..3b96f59149f 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -71,6 +71,9 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -80,7 +83,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); @@ -91,7 +94,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(dock->isCharging()); EXPECT_FALSE(dock->hasStoppedCharging()); @@ -124,6 +127,9 @@ TEST(SimpleChargingDockTests, StallDetection) rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names))); dock->configure(node, "my_dock", nullptr); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + EXPECT_EQ(dock->getStallJointNames(), names); // Stopped, but below effort threshold @@ -134,7 +140,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -146,7 +152,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -158,7 +164,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(dock->isDocked()); @@ -229,6 +235,8 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped pose; @@ -242,7 +250,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -269,6 +277,8 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -276,7 +286,7 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -306,6 +316,8 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -313,7 +325,7 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 61d3207e150..82839040c18 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -84,6 +84,9 @@ TEST(SimpleNonChargingDockTests, StallDetection) EXPECT_EQ(dock->getStallJointNames(), names); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -95,7 +98,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -107,7 +110,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -119,7 +122,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(dock->isDocked()); @@ -190,6 +193,8 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped pose; @@ -203,7 +208,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -230,6 +235,8 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -237,7 +244,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -267,6 +274,8 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -274,7 +283,7 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp index 4fcd8aec35e..0af3f159969 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -134,8 +134,10 @@ class InfoServerTester : public ::testing::Test TEST_F(InfoServerTester, testCostmapFilterInfoPublish) { rclcpp::Time start_time = info_server_->now(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(info_server_->get_node_base_interface()); while (!isReceived()) { - rclcpp::spin_some(info_server_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); // Waiting no more than 5 seconds ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); @@ -155,8 +157,10 @@ TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate) info_server_->activate(); rclcpp::Time start_time = info_server_->now(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(info_server_->get_node_base_interface()); while (!isReceived()) { - rclcpp::spin_some(info_server_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); // Waiting no more than 5 seconds ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp index 88bfd697d0a..26ea6ea7d95 100644 --- a/nav2_map_server/test/unit/test_vector_object_server.cpp +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -150,6 +150,7 @@ class Tester : public ::testing::Test // Vector Object server node std::shared_ptr vo_server_; + rclcpp::executors::SingleThreadedExecutor executor_; }; // Tester Tester::Tester() @@ -174,6 +175,7 @@ Tester::Tester() tf_buffer_ = std::make_shared(vo_server_->get_clock()); tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model tf_listener_ = std::make_shared(*tf_buffer_); + executor_.add_node(vo_server_->get_node_base_interface()); } Tester::~Tester() @@ -406,7 +408,7 @@ typename T::Response::SharedPtr Tester::sendRequest( if (status == std::future_status::ready) { return result_future.get(); } - rclcpp::spin_some(vo_server_->get_node_base_interface()); + executor_.spin_some(); std::this_thread::sleep_for(10ms); } return nullptr; @@ -424,7 +426,7 @@ bool Tester::waitMap(const std::chrono::nanoseconds & timeout) if (map_) { return true; } - rclcpp::spin_some(vo_server_->get_node_base_interface()); + executor_.spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7e710fc7375..1228e81d60a 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -50,12 +50,15 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) "~/transformed_global_plan", [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); vis.on_activate(); vis.visualize(pub_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_path.poses.size(), 5u); EXPECT_EQ(received_path.header.frame_id, "fake_frame"); } @@ -70,6 +73,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) auto my_sub = node->create_subscription( "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // optimal_trajectory empty, should fail to publish Eigen::ArrayXXf optimal_trajectory; @@ -81,7 +86,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_msg.markers.size(), 0u); // Now populated with content, should publish @@ -89,7 +94,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Should have 20 trajectory points in the map frame EXPECT_EQ(received_msg.markers.size(), 20u); @@ -134,6 +139,9 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + models::Trajectories candidate_trajectories; candidate_trajectories.x = Eigen::ArrayXXf::Ones(200, 12); candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12); @@ -146,7 +154,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // 40 * 4, for 5 trajectory steps + 3 point steps EXPECT_EQ(received_msg.markers.size(), 160u); } @@ -165,6 +173,9 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) "~/optimal_path", [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + // optimal_trajectory empty, should fail to publish Eigen::ArrayXXf optimal_trajectory; TrajectoryVisualizer vis; @@ -174,7 +185,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_path.poses.size(), 0u); // Now populated with content, should publish @@ -187,7 +198,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Should have a 20 points path in the map frame and with same stamp than velocity command EXPECT_EQ(received_path.poses.size(), 20u); diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp index 2772ea9a858..04e18bb191d 100644 --- a/nav2_mppi_controller/test/utils/utils.hpp +++ b/nav2_mppi_controller/test/utils/utils.hpp @@ -35,9 +35,11 @@ using namespace std::chrono_literals; // NOLINT template void waitSome(const std::chrono::nanoseconds & duration, TNode & node) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); rclcpp::Time start_time = node->now(); while (rclcpp::ok() && node->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(3ms); } } diff --git a/nav2_ros_common/test/test_actions.cpp b/nav2_ros_common/test/test_actions.cpp index 7da319e4e2e..5bda2e7b891 100644 --- a/nav2_ros_common/test/test_actions.cpp +++ b/nav2_ros_common/test/test_actions.cpp @@ -173,8 +173,10 @@ class RclCppFixture { auto node = std::make_shared(); node->on_init(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); while (rclcpp::ok() && !stop_.load()) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(10ms); } node->on_term(); diff --git a/nav2_ros_common/test/test_service_client.cpp b/nav2_ros_common/test/test_service_client.cpp index 99d6576292d..ebb5d6705ba 100644 --- a/nav2_ros_common/test/test_service_client.cpp +++ b/nav2_ros_common/test/test_service_client.cpp @@ -85,7 +85,9 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) }); pub->publish(std_msgs::msg::Empty()); - rclcpp::spin_some(sub_node); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); + executor.spin_some(); rclcpp::shutdown(); srv_thread.join(); @@ -98,7 +100,9 @@ TEST(ServiceClient, can_ServiceClient_timeout) rclcpp::init(0, nullptr); auto node = rclcpp::Node::make_shared("test_node"); TestServiceClient t("bar", node, true); - rclcpp::spin_some(node); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin_some(); bool ready = t.wait_for_service(std::chrono::milliseconds(10)); rclcpp::shutdown(); ASSERT_EQ(ready, false); @@ -128,7 +132,9 @@ TEST(ServiceClient, can_ServiceClient_async_call) { // Test async_call client.async_call(req, callback); std::this_thread::sleep_for(std::chrono::seconds(1)); - rclcpp::spin_some(node); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin_some(); rclcpp::shutdown(); srv_thread.join(); diff --git a/nav2_ros_common/test/test_service_server.cpp b/nav2_ros_common/test/test_service_server.cpp index 7365e34afe7..9e42d2a21d1 100644 --- a/nav2_ros_common/test/test_service_server.cpp +++ b/nav2_ros_common/test/test_service_server.cpp @@ -51,7 +51,6 @@ TEST(ServiceServer, can_handle_all_introspection_modes) for (const auto & mode : introspection_modes) { int a = 0; auto node = rclcpp::Node::make_shared("test_node_" + mode); - node->declare_parameter("introspection_mode", mode); auto callback = [&a](const std::shared_ptr, @@ -64,13 +63,17 @@ TEST(ServiceServer, can_handle_all_introspection_modes) auto client_node = rclcpp::Node::make_shared("client_node_" + mode); auto client = client_node->create_client("empty_srv_" + mode); + rclcpp::executors::SingleThreadedExecutor node_executor; + node_executor.add_node(node); + rclcpp::executors::SingleThreadedExecutor client_node_executor; + client_node_executor.add_node(client_node); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto req = std::make_shared(); auto result = client->async_send_request(req); - rclcpp::spin_some(node); - rclcpp::spin_some(client_node); + node_executor.spin_some(); + client_node_executor.spin_some(); ASSERT_EQ(a, 1); } } diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp index 84b544a1be5..7255ae33904 100644 --- a/nav2_route/test/test_graph_loader.cpp +++ b/nav2_route/test/test_graph_loader.cpp @@ -74,6 +74,8 @@ TEST(GraphLoader, test_api) TEST(GraphLoader, test_transformation_api) { auto node = std::make_shared("graph_loader_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); auto tf = std::make_shared(node->get_clock()); tf->setUsingDedicatedThread(true); auto tf_listener = std::make_shared(*tf); @@ -107,7 +109,7 @@ TEST(GraphLoader, test_transformation_api) tf_broadcaster->sendTransform(transform); rclcpp::Rate(1).sleep(); tf_broadcaster->sendTransform(transform); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + executor.spin_all(std::chrono::milliseconds(50)); graph[0].coords.frame_id = "map_test"; EXPECT_EQ(graph[0].coords.frame_id, "map_test"); diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp index bd69a9bc112..49c3747ebac 100644 --- a/nav2_route/test/test_graph_saver.cpp +++ b/nav2_route/test/test_graph_saver.cpp @@ -110,6 +110,8 @@ TEST(GraphSaver, test_api) TEST(GraphSaver, test_transformation_api) { auto node = std::make_shared("graph_saver_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); auto tf = std::make_shared(node->get_clock()); tf->setUsingDedicatedThread(true); auto tf_listener = std::make_shared(*tf); @@ -143,7 +145,7 @@ TEST(GraphSaver, test_transformation_api) tf_broadcaster->sendTransform(transform); rclcpp::Rate(1).sleep(); tf_broadcaster->sendTransform(transform); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + executor.spin_all(std::chrono::milliseconds(50)); GraphSaver graph_saver(node, tf, frame); std::string file_path = "test.geojson"; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index c96551ae826..fcbc1d1b11a 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -88,6 +88,7 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // The Node pointer that we need to keep alive for the duration of this plugin. std::shared_ptr node_ptr_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 44ad28d279e..8c99a270d64 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -112,7 +112,7 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; - + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // Timeout value when waiting for action servers to respond std::chrono::milliseconds server_timeout_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp index ba52b009372..86ef9670348 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp @@ -33,10 +33,12 @@ namespace nav2_rviz_plugins * @param server_name The name of the server to load plugins for * @param plugin_type The type of plugin to load * @param combo_box The combo box to add the loaded plugins to + * @param executor The executor to pass to the AsyncParameterClient */ void pluginLoader( rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, - const std::string & plugin_type, QComboBox * combo_box); + const std::string & plugin_type, QComboBox * combo_box, + rclcpp::Executor::SharedPtr executor = nullptr); // Create label string from goal status msg QString getGoalStatusLabel( diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 2d40b3ae2fd..9f1f33c3e72 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -170,6 +170,8 @@ DockingPanel::DockingPanel(QWidget * parent) undocking_->addTransition(undockingTransition); client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); + executor_ = std::make_shared(); + executor_->add_node(client_node_); state_machine_.addState(pre_initial_); state_machine_.addState(idle_); @@ -252,7 +254,7 @@ DockingPanel::DockingPanel(QWidget * parent) if (!plugins_loaded_) { RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_, executor_); plugins_loaded_ = true; } }); @@ -451,7 +453,7 @@ void DockingPanel::onUndockingButtonPressed() }; auto future_goal_handle = undock_client_->async_send_goal(goal_msg, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -490,7 +492,7 @@ void DockingPanel::onCancelDocking() if (dock_goal_handle_) { auto future_cancel = dock_client_->async_cancel_goal(dock_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -507,7 +509,7 @@ void DockingPanel::onCancelUndocking() if (undock_goal_handle_) { auto future_cancel = undock_client_->async_cancel_goal(undock_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -530,7 +532,7 @@ void DockingPanel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = dock_goal_handle_->get_status(); // Check if the goal is still executing @@ -549,7 +551,7 @@ void DockingPanel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = undock_goal_handle_->get_status(); // Check if the goal is still executing diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index e6780574604..8da22379965 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -455,6 +455,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) auto options = rclcpp::NodeOptions().arguments( {"--ros-args", "--remap", "__node:=rviz_navigation_dialog_action_client", "--"}); client_node_ = std::make_shared("_", options); + executor_ = std::make_shared(); + executor_->add_node(client_node_); client_nav_ = std::make_shared( "lifecycle_manager_navigation", client_node_); @@ -989,7 +991,7 @@ Nav2Panel::onCancelButtonPressed() if (navigation_goal_handle_) { auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -1002,7 +1004,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); @@ -1015,7 +1017,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = nav_through_poses_action_client_->async_cancel_goal(nav_through_poses_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel nav through pose action"); @@ -1137,7 +1139,7 @@ Nav2Panel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = waypoint_follower_goal_handle_->get_status(); // Check if the goal is still executing @@ -1158,7 +1160,7 @@ Nav2Panel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = nav_through_poses_goal_handle_->get_status(); // Check if the goal is still executing @@ -1179,7 +1181,7 @@ Nav2Panel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = navigation_goal_handle_->get_status(); // Check if the goal is still executing @@ -1236,7 +1238,7 @@ Nav2Panel::startWaypointFollowing(std::vector p auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -1288,7 +1290,7 @@ Nav2Panel::startNavThroughPoses(nav_msgs::msg::Goals poses) auto future_goal_handle = nav_through_poses_action_client_->async_send_goal(nav_through_poses_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -1334,7 +1336,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index ee291c5c5e3..f833c56f32f 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -22,14 +22,14 @@ namespace nav2_rviz_plugins void pluginLoader( rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, - const std::string & plugin_type, QComboBox * combo_box) + const std::string & plugin_type, QComboBox * combo_box, rclcpp::Executor::SharedPtr executor) { // Do not load the plugins if the combo box is already populated if (combo_box->count() > 0) { return; } - auto parameter_client = std::make_shared(node, server_name); + auto parameter_client = std::make_shared(node, server_name); // Wait for the service to be available before calling it bool server_unavailable = false; @@ -50,7 +50,23 @@ void pluginLoader( return; } auto parameters = parameter_client->get_parameters({plugin_type}); - auto str_arr = parameters[0].as_string_array(); + if (executor) { + if (executor->spin_until_future_complete(parameters) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), + "Failed to get parameter '%s' from server '%s'", + plugin_type.c_str(), server_name.c_str()); + return; + } + } else { + if (rclcpp::spin_until_future_complete(node, parameters) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), + "Failed to get parameter '%s' from server '%s'", + plugin_type.c_str(), server_name.c_str()); + return; + } + } + + auto str_arr = parameters.get()[0].as_string_array(); combo_box->addItem("Default"); for (auto str : str_arr) { combo_box->addItem(QString::fromStdString(str)); diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp index dfedf68072d..b9a3a428d20 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp @@ -83,6 +83,7 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() ); stamp_ = node_->now(); + executor_.add_node(node_); } AssistedTeleopBehaviorTester::~AssistedTeleopBehaviorTester() @@ -103,7 +104,7 @@ void AssistedTeleopBehaviorTester::activate() RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); sendInitialPose(); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); + executor_.spin_some(); } // Wait for lifecycle_manager_navigation to activate behavior_server @@ -146,7 +147,7 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( auto goal_handle_future = client_ptr_->async_send_goal(nav2_msgs::action::AssistedTeleop::Goal()); - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + if (executor_.spin_until_future_complete(goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); @@ -182,7 +183,7 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( return false; } - rclcpp::spin_some(node_); + executor_.spin_some(); r.sleep(); } @@ -190,7 +191,7 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( preempt_pub_->publish(preempt_msg); RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - if (rclcpp::spin_until_future_complete(node_, result_future) != + if (executor_.spin_until_future_complete(result_future) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp index 40e2aa9a754..a35b3a59c6c 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -79,6 +79,7 @@ class AssistedTeleopBehaviorTester std::shared_ptr tf_listener_; rclcpp::Node::SharedPtr node_; + rclcpp::executors::SingleThreadedExecutor executor_; // Publishers rclcpp::Publisher::SharedPtr initial_pose_pub_; diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index dffd52f9115..68fb6d9a4a8 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -68,12 +68,13 @@ void WaitBehaviorTester::activate() throw std::runtime_error("Trying to activate while already active"); return; } - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_); while (!initial_pose_received_) { RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); sendInitialPose(); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); + executor.spin_some(); } // Wait for lifecycle_manager_navigation to activate behavior_server diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 768fd06c449..aabc414a544 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -32,10 +32,11 @@ class TestAmclPose : public ::testing::Test tol_ = 0.25; node = rclcpp::Node::make_shared("localization_test"); + executor_.add_node(node); while (node->count_subscribers("scan") < 1) { std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node); + executor_.spin_some(); } initial_pose_pub_ = node->create_publisher( @@ -50,6 +51,7 @@ class TestAmclPose : public ::testing::Test protected: std::shared_ptr node; + rclcpp::executors::SingleThreadedExecutor executor_; void initTestPose(); private: @@ -76,7 +78,7 @@ bool TestAmclPose::defaultAmclTest() // TODO(mhpanah): Initial pose should only be published once. initial_pose_pub_->publish(testPose_); std::this_thread::sleep_for(1s); - rclcpp::spin_some(node); + executor_.spin_some(); } if (std::abs(amcl_pose_x - testPose_.pose.pose.position.x) < tol_ && std::abs(amcl_pose_y - testPose_.pose.pose.position.y) < tol_) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp index a3cdda664f0..641e13ec98f 100644 --- a/nav2_util/test/test_base_footprint_publisher.cpp +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -22,7 +22,9 @@ TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) { auto node = std::make_shared(); - rclcpp::spin_some(node->get_node_base_interface()); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin_some(); auto tf_broadcaster = std::make_shared(node); auto buffer = std::make_shared(node->get_clock()); @@ -41,7 +43,7 @@ TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) transform.header.frame_id = "test1_1"; transform.child_frame_id = "test1"; tf_broadcaster->sendTransform(transform); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_THROW( buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero), tf2::TransformException); @@ -56,7 +58,7 @@ TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) tf_broadcaster->sendTransform(transform); rclcpp::Rate r(1.0); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); auto t = buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero); EXPECT_EQ(t.transform.translation.x, 1.0); EXPECT_EQ(t.transform.translation.y, 1.0); diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp index 5b8087e097e..91f9f4a5756 100644 --- a/nav2_util/test/test_odometry_utils.cpp +++ b/nav2_util/test/test_odometry_utils.cpp @@ -66,7 +66,8 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->on_activate(); nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); nav_msgs::msg::Odometry odom_msg; geometry_msgs::msg::Twist twist_msg; geometry_msgs::msg::Twist twist_raw_msg; @@ -79,7 +80,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_msg.twist.twist.angular.z = 1.0; odom_pub->publish(odom_msg); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); EXPECT_EQ(twist_msg.linear.x, 1.0); @@ -93,7 +94,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); @@ -111,7 +112,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); @@ -129,7 +130,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); @@ -147,7 +148,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index dc3b307984f..0b716d20623 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -47,9 +47,10 @@ TEST(TwistPublisher, Unstamped) auto my_sub = sub_node->create_subscription( "cmd_vel", [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); vel_publisher->publish(std::move(pub_msg)); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(pub_msg_copy.linear.x, sub_msg.linear.x); EXPECT_EQ(vel_publisher->get_subscription_count(), 1); @@ -86,9 +87,10 @@ TEST(TwistPublisher, Stamped) auto my_sub = sub_node->create_subscription( "cmd_vel", [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); vel_publisher->publish(std::move(pub_msg)); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); ASSERT_EQ(vel_publisher->get_subscription_count(), 1); EXPECT_EQ(pub_msg_copy, sub_msg); pub_node->deactivate(); diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp index 8e30b6eec21..f4043f899d9 100644 --- a/nav2_util/test/test_twist_subscriber.cpp +++ b/nav2_util/test/test_twist_subscriber.cpp @@ -47,12 +47,13 @@ TEST(TwistSubscriber, Unstamped) auto vel_pub = pub_node->create_publisher("cmd_vel"); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); pub_node->activate(); vel_pub->on_activate(); vel_pub->publish(pub_msg.twist); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); ASSERT_EQ(vel_pub->get_subscription_count(), 1); EXPECT_EQ(pub_msg, sub_msg); @@ -87,9 +88,10 @@ TEST(TwistSubscriber, Stamped) pub_node->activate(); vel_pub->on_activate(); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); vel_pub->publish(pub_msg); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); ASSERT_EQ(vel_pub->get_subscription_count(), 1); EXPECT_EQ(pub_msg, sub_msg); diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index ae3f36aa1b7..8de8bd7e9a7 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -55,6 +55,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer6dof) { auto smoother = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); std::vector deadbands{0.2, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector min_velocity{-0.5, -0.5, -0.5, -2.5, -2.5, -2.5}; std::vector max_velocity{0.5, 0.5, 0.5, 2.5, 2.5, 2.5}; @@ -96,7 +98,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer6dof) // Process velocity smoothing and send updated odometry based on commands auto start = smoother->now(); while (smoother->now() - start < 1.5s) { - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); } // Sanity check we have the approximately right number of messages for the timespan and timeout @@ -125,6 +127,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer) { auto smoother = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); std::vector deadbands{0.2, 0.0, 0.0}; smoother->declare_parameter("scale_velocities", rclcpp::ParameterValue(true)); smoother->set_parameter(rclcpp::Parameter("scale_velocities", true)); @@ -152,7 +156,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer) // Process velocity smoothing and send updated odometry based on commands auto start = smoother->now(); while (smoother->now() - start < 1.5s) { - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); } // Sanity check we have the approximately right number of messages for the timespan and timeout @@ -181,6 +185,8 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) { auto smoother = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); smoother->declare_parameter("feedback", rclcpp::ParameterValue(std::string("CLOSED_LOOP"))); smoother->set_parameter(rclcpp::Parameter("feedback", std::string("CLOSED_LOOP"))); rclcpp_lifecycle::State state; @@ -222,7 +228,7 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) odom_msg.twist.twist.linear.x = linear_vels.back(); } odom_pub->publish(odom_msg); - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); } // Sanity check we have the approximately right number of messages for the timespan and timeout @@ -633,10 +639,12 @@ TEST(VelocitySmootherTest, testCommandCallback) auto pub = smoother->create_publisher("cmd_vel"); pub->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); auto msg = std::make_unique(); msg->twist.linear.x = 100.0; pub->publish(std::move(msg)); - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(smoother->hasCommandMsg()); EXPECT_EQ(smoother->lastCommandMsg()->twist.linear.x, 100.0); diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp index 726851be5d4..becf874d4c1 100644 --- a/nav2_waypoint_follower/test/test_task_executors.cpp +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -61,19 +61,21 @@ TEST(WaypointFollowerTest, InputAtWaypoint) auto node = std::make_shared("testWaypointNode"); auto pub = node->create_publisher("input_at_waypoint/input"); pub->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; auto publish_message = [&]() -> void { rclcpp::Rate(5).sleep(); auto msg = std::make_unique(); pub->publish(std::move(msg)); - rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); + executor.spin_some(); }; std::unique_ptr iaw( new nav2_waypoint_follower::InputAtWaypoint ); iaw->initialize(node, std::string("IAW")); + executor.add_node(node->shared_from_this()->get_node_base_interface()); auto start_time = node->now(); @@ -103,6 +105,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) auto node = std::make_shared("testWaypointNode"); auto pub = node->create_publisher("/camera/color/image_raw"); pub->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; std::condition_variable cv; std::mutex mtx; std::unique_lock lck(mtx, std::defer_lock); @@ -124,7 +127,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) msg->data.push_back(fake_data++); } pub->publish(std::move(msg)); - rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); + executor.spin_some(); lck.lock(); data_published = true; cv.notify_one(); @@ -135,6 +138,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) new nav2_waypoint_follower::PhotoAtWaypoint ); paw->initialize(node, std::string("PAW")); + executor.add_node(node->shared_from_this()->get_node_base_interface()); // no images, throws because can't write geometry_msgs::msg::PoseStamped pose; From 4d05d23c1bf396371a7021b004a447b6dd4b09be Mon Sep 17 00:00:00 2001 From: Koensgen Benjamin Date: Tue, 16 Sep 2025 23:21:11 +0200 Subject: [PATCH 7/7] feat(opennav_docking): add on-demand detector client & unit tests for dock plugins (#5015) (#5218) * feat(opennav_docking): Add dynamic lifecycle for external detectors This change introduces an API to dynamically enable and disable external docking detectors, such as those based on AI, to conserve system resources when not actively docking. Key changes: - Added `startDetectionProcess()` and `stopDetectionProcess()` to the docking plugin interface (`ChargingDock` and `NonChargingDock`). - Implemented this logic in `SimpleChargingDock` and `SimpleNonChargingDock` using a `std_srvs/Trigger` service call and dynamic topic subscription to manage the detector's lifecycle. - The `DockingServer` now ensures `stopDetectionProcess()` is called on all exit paths, including success, failure, and cancellation, to prevent dangling resources. - Added new parameters (`detector_service_name`, `subscribe_toggle`, etc.) to configure this behavior and updated the README documentation. - Added comprehensive C++ unit tests to verify the new detector lifecycle logic, specifically covering service failure cases and proper cleanup on action termination. Closes #5015 Signed-off-by: Koensgen Benjamin - updtae Signed-off-by: Koensgen Benjamin Signed-off-by: bkoensgen * fix: Address review feedback and fix unit tests Signed-off-by: Koensgen Benjamin fix(docking): Change subscribe_toggle default to false Signed-off-by: Koensgen Benjamin - Signed-off-by: bkoensgen * refactor(docking): Improve plugin lifecycle and resource management This commit addresses several code review suggestions to improve the design and robustness of the docking plugins. - Replaced the `DetectorState` enum with a simpler `bool detector_enabled_` for clarity. - Encapsulated the detection process lifecycle within the plugin itself. The `deactivate` method now ensures `stopDetectionProcess` is called, improving encapsulation and simplifying the `DockDatabase`. - Refactored the `configure` method in plugins to group related logic, improving readability. - Cleaned up redundant checks and calls in both the plugins and the `DockingServer` for more robust and intentional code. Signed-off-by: Koensgen Benjamin Signed-off-by: bkoensgen * fix(plugins): Resolve race condition by setting enabled state in callback The previous refactoring introduced a race condition where the detector was marked as 'enabled' in startDetection() before a message was actually received. This caused tests to fail because getRefinedPose() would be called with an enabled state but no valid/recent data. This commit fixes the issue by moving the `detector_enabled_ = true` assignment into the subscription callback. This ensures the plugin's state accurately reflects that it has received at least one valid data point before being considered active. Signed-off-by: Koensgen Benjamin Signed-off-by: bkoensgen * refactor(docking): migrate to nav2_ros_common (node_utils, LifecycleNode, QoS) Signed-off-by: bkoensgen * build(opennav_docking): update deps (std_srvs, package.xml + CMakeLists) Signed-off-by: bkoensgen * refactor(Docking): migrate to nav2::LifecycleNode Signed-off-by: bkoensgen * refactor(docking): use nav2::qos::StandardTopicQoS for subscriptions Signed-off-by: bkoensgen * refactor(opennav_docking): replace raw queue size with rclcpp::QoS(1) in pubs/subs Signed-off-by: bkoensgen * refactor(tests): migrate nav2_util::NodeThread to nav2::NodeThread Signed-off-by: bkoensgen * refactor(tests): migrate to 3-args service callbacks Signed-off-by: bkoensgen * style(test): apply ament_uncrustify Signed-off-by: bkoensgen * refactor(opennav_docking) migrate detector Trigger service to node_->create_client() Signed-off-by: bkoensgen * docking: use nav2 types for pubs/subs in SimpleChargingDock and add explicit detection state flags Signed-off-by: bkoensgen * docking: use nav2 types for pubs/subs in SimpleNonChargingDock and add explicit detection state flags Signed-off-by: bkoensgen * docking: split detection state in SimpleChargingDock (detection_started_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen * docking: split detection state in SimpleNonChargingDock (detection_started_ vs initial_pose_received_) and warn before first pose Signed-off-by: bkoensgen * tests: adapt SimpleChargingDockTestable to initial_pose_received_ state Signed-off-by: bkoensgen * tests: adapt SimpleNonChargingDockTestable to initial_pose_received_ state Signed-off-by: bkoensgen * docs(docking): clarify external detection gating and subscribe_toggle behavior (default=false) Signed-off-by: bkoensgen * fix(docking): keep SimpleNonChargingDock registered as ChargingDock (stay in-scope) Signed-off-by: bkoensgen * docs(docking): revert README note to pre-e881de19 state Signed-off-by: bkoensgen * fix(docking_server): remove redundant null-check before stopDetectionProcess() in terminal cleanup Signed-off-by: bkoensgen * style(docking_server): unify terminal order (stash->publishZeroVelocity->stopDetection->terminate) Signed-off-by: bkoensgen * lint Signed-off-by: bkoensgen * fix(docking): inline detection process Signed-off-by: bkoensgen * chore(docking): drop redundant detector comment Signed-off-by: bkoensgen * chore(docking): clarify detector logs Signed-off-by: bkoensgen * fix(docking): activate lifecycle publishers Signed-off-by: bkoensgen * chore(docking): reuse dock pose subscription Signed-off-by: bkoensgen * lint Signed-off-by: bkoensgen * fix(docking_server): drop redundant DB deactivate on cleanup Signed-off-by: bkoensgen * refactor(docking): rename detection state flag to detection_active Signed-off-by: bkoensgen * fix(docking): reset detection flags on cleanup Signed-off-by: bkoensgen --------- Signed-off-by: Koensgen Benjamin Signed-off-by: bkoensgen --- nav2_docking/README.md | 18 +- nav2_docking/opennav_docking/CMakeLists.txt | 4 + .../opennav_docking/simple_charging_dock.hpp | 35 ++- .../simple_non_charging_dock.hpp | 47 ++- nav2_docking/opennav_docking/package.xml | 1 + .../opennav_docking/src/docking_server.cpp | 12 +- .../src/simple_charging_dock.cpp | 167 ++++++++++- .../src/simple_non_charging_dock.cpp | 161 ++++++++++- .../test/test_docking_server.py | 36 ++- .../test/test_docking_server_unit.cpp | 164 +++++++++++ .../test/test_simple_charging_dock.cpp | 270 ++++++++++++++++-- .../test/test_simple_non_charging_dock.cpp | 266 +++++++++++++++-- .../opennav_docking/test/testing_dock.cpp | 12 + .../opennav_docking_core/charging_dock.hpp | 10 + .../non_charging_dock.hpp | 10 + 15 files changed, 1118 insertions(+), 95 deletions(-) diff --git a/nav2_docking/README.md b/nav2_docking/README.md index f246c0c2493..2d2d92d8f97 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -36,12 +36,14 @@ The `ChargingDock` and `NonChargingDock` plugins are the heart of the customizab The docking procedure is as follows: 1. Take action request and obtain the dock's plugin and its pose 2. If the robot is not within the prestaging tolerance of the dock's staging pose, navigate to the staging pose -3. Use the dock's plugin to initially detect the dock and return the docking pose -4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system -5. Exit the vision-control loop once contact has been detected or charging has started -6. Wait until charging starts (if applicable) and return success. +3. Call the dock's plugin `startDetectionProcess()` method to activate any external detection mechanisms. +4. Use the dock's plugin to initially detect the dock (`getRefinedPose`) and return the docking pose. +5. Enter a vision-control loop where the robot attempts to reach the docking pose while it's actively being refined by the vision system. +6. Exit the vision-control loop once contact has been detected or charging has started (if applicable). +7. Wait until charging starts (if applicable) and return success. +8. Call the dock's plugin `stopDetectionProcess()` method to deactivate any external detection mechanisms. -If anywhere this procedure is unsuccessful, `N` retries may be made by driving back to the dock's staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client. +If anywhere this procedure is unsuccessful (before step 8), `N` retries may be made, driving back to the dock's staging pose, and then restarting the process from step 3. If still unsuccessful after retries, it will return a failure code to indicate what kind of failure occurred to the client. Undocking works more simply: 1. If previously docked, use the known dock information to get the dock type. If not, use the undock action request's indicated dock type @@ -175,8 +177,9 @@ some robots. `getStagingPose` applies a parameterized translational and rotational offset to the dock pose to obtain the staging pose. `getRefinedPose` can be used in two ways. -1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation. +1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation. The `start/stopDetectionProcess` methods would typically do nothing in this case. 2. The more realistic use case is to use an AR marker, dock pose detection algorithm, etc. The plugin will subscribe to a `geometry_msgs/PoseStamped` topic `detected_dock_pose`. This can be used with the `image_proc/TrackMarkerNode` for Apriltags or other custom detectors for your dock. It is unlikely the detected pose is actually the pose you want to dock with, so several parameters are supplied to represent your docked pose relative to the detected feature's pose. +The subscription to `detected_dock_pose` can be managed dynamically (see `subscribe_toggle` parameter). Additionally, an external detector process can be triggered via a service call (see `detector_service_name`). The `DockingServer` calls `startDetectionProcess()` before `getRefinedPose` (which is called in a loop) and `stopDetectionProcess()` after the docking action concludes (successfully or not). This allows plugins to manage resources like GPU usage by only running detection when needed. During the docking approach, there are two options for detecting `isDocked`: 1. We can check the joint states of the wheels if the current has spiked above a set threshold to indicate that the robot has made contact with the dock or other physical object. @@ -257,6 +260,9 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required. | staging_yaw_offset | Staging pose angle relative to dock pose (rad) | double | 0.0 | | dock_direction | Whether the robot is docking with the dock forward or backward in motion | string | "forward" or "backward" | | rotate_to_dock | Enables backward docking without requiring a sensor for detection during the final approach. When enabled, the robot approaches the staging pose facing forward with sensor coverage for dock detection; after detection, it rotates and backs into the dock using only the initially detected pose for dead reckoning. | bool | false | +| detector_service_name | Trigger service name to start/stop the detector (e.g., a camera node or an AprilTag detector node). Leave empty to disable service calls. | string | "" | +| detector_service_timeout | Timeout (seconds) to wait for the detector service (if `detector_service_name` is set) to become available or respond. | double | 5.0 | +| subscribe_toggle | If true, dynamically subscribe/unsubscribe to `detected_dock_pose` topic when `start/stopDetectionProcess` are called. If false, the subscription is kept alive throughout the plugin's lifecycle if `use_external_detection_pose` is true. This can be useful if the detector node is always running and publishing. | bool | false | Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in `image_proc` and `isaac_ros`. diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 8801b18d5a8..cbb068aa504 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -127,6 +128,7 @@ target_link_libraries(simple_charging_dock PUBLIC rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ${sensor_msgs_TARGETS} + ${std_srvs_TARGETS} tf2_geometry_msgs::tf2_geometry_msgs tf2_ros::tf2_ros nav2_ros_common::nav2_ros_common @@ -155,6 +157,7 @@ target_link_libraries(simple_non_charging_dock PUBLIC rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ${sensor_msgs_TARGETS} + ${std_srvs_TARGETS} tf2_geometry_msgs::tf2_geometry_msgs tf2_ros::tf2_ros ) @@ -211,6 +214,7 @@ ament_export_dependencies( rclcpp_action rclcpp_lifecycle rcl_interfaces + std_srvs tf2_ros yaml-cpp ) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index 719e6e5cf7d..7cc33d34c4e 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -19,6 +19,7 @@ #include #include +#include "std_srvs/srv/trigger.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -54,17 +55,17 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock /** * @brief Method to cleanup resources used on shutdown. */ - virtual void cleanup() {} + void cleanup() override; /** * @brief Method to active Behavior and any threads involved in execution. */ - virtual void activate() {} + void activate() override; /** * @brief Method to deactivate Behavior and any threads involved in execution. */ - virtual void deactivate() {} + void deactivate() override; /** * @brief Method to obtain the dock's staging pose. This method should likely @@ -104,14 +105,24 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock */ virtual bool hasStoppedCharging(); + /** + * @brief Start external detection process (service call + subscribe). + */ + bool startDetectionProcess() override; + + /** + * @brief Stop external detection process. + */ + bool stopDetectionProcess() override; + protected: void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); // Optionally subscribe to a detected dock pose topic nav2::Subscription::SharedPtr dock_pose_sub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; - rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; + nav2::Publisher::SharedPtr dock_pose_pub_; + nav2::Publisher::SharedPtr filtered_dock_pose_pub_; + nav2::Publisher::SharedPtr staging_pose_pub_; // If subscribed to a detected pose topic, will contain latest message geometry_msgs::msg::PoseStamped detected_dock_pose_; // This is the actual dock pose once it has the specified translation/rotation applied @@ -150,6 +161,18 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock nav2::LifecycleNode::SharedPtr node_; std::shared_ptr tf2_buffer_; + + // Detector control parameters + std::string detector_service_name_; + double detector_service_timeout_{5.0}; + bool subscribe_toggle_{false}; + + // Client used to call the Trigger service + nav2::ServiceClient::SharedPtr detector_client_; + + // Detection state flags + bool detection_active_{false}; + bool initial_pose_received_{false}; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp index a0b3a46531e..ef2c7082e2c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -19,6 +19,7 @@ #include #include +#include "std_srvs/srv/trigger.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -54,17 +55,17 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock /** * @brief Method to cleanup resources used on shutdown. */ - virtual void cleanup() {} + void cleanup() override; - /** - * @brief Method to active Behavior and any threads involved in execution. - */ - virtual void activate() {} + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + void activate() override; - /** - * @brief Method to deactivate Behavior and any threads involved in execution. - */ - virtual void deactivate() {} + /** + * @brief Method to deactivate Behavior and any threads involved in execution. + */ + void deactivate() override; /** * @brief Method to obtain the dock's staging pose. This method should likely @@ -89,14 +90,24 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock */ virtual bool isDocked(); + /** + * @brief Start external detection process (service call + subscribe). + */ + bool startDetectionProcess() override; + + /** + * @brief Stop external detection process. + */ + bool stopDetectionProcess() override; + protected: void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); // Optionally subscribe to a detected dock pose topic nav2::Subscription::SharedPtr dock_pose_sub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; - rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; + nav2::Publisher::SharedPtr dock_pose_pub_; + nav2::Publisher::SharedPtr filtered_dock_pose_pub_; + nav2::Publisher::SharedPtr staging_pose_pub_; // If subscribed to a detected pose topic, will contain latest message geometry_msgs::msg::PoseStamped detected_dock_pose_; // This is the actual dock pose once it has the specified translation/rotation applied @@ -128,6 +139,18 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock nav2::LifecycleNode::SharedPtr node_; std::shared_ptr tf2_buffer_; + + // Detector control parameters + std::string detector_service_name_; + double detector_service_timeout_{5.0}; + bool subscribe_toggle_{false}; + + // Client used to call the Trigger service + nav2::ServiceClient::SharedPtr detector_client_; + + // Detection state flags + bool detection_active_{false}; + bool initial_pose_received_{false}; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index b65cb91ee56..97edbafb954 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -25,6 +25,7 @@ rclcpp_lifecycle rcl_interfaces sensor_msgs + std_srvs tf2 tf2_geometry_msgs tf2_ros diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 1208ade5521..ec7a7fc5e81 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -331,6 +331,7 @@ void DockingServer::dockRobot() result->num_retries = num_retries_; stashDockData(goal->use_dock_id, dock, true); publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->succeeded_current(result); return; } @@ -339,6 +340,7 @@ void DockingServer::dockRobot() // Cancelled, preempted, or shutting down (recoverable errors throw DockingException) stashDockData(goal->use_dock_id, dock, false); publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->terminate_all(result); return; } catch (opennav_docking_core::DockingException & e) { @@ -354,6 +356,7 @@ void DockingServer::dockRobot() // Cancelled, preempted, or shutting down stashDockData(goal->use_dock_id, dock, false); publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->terminate_all(result); return; } @@ -401,6 +404,7 @@ void DockingServer::dockRobot() stashDockData(goal->use_dock_id, dock, false); result->num_retries = num_retries_; publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->terminate_current(result); } @@ -429,12 +433,18 @@ Dock * DockingServer::generateGoalDock(std::shared_ptr go void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose) { publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION); + + if (!dock->plugin->startDetectionProcess()) { + throw opennav_docking_core::FailedToDetectDock("Failed to start the detection process."); + } + rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(initial_perception_timeout_); while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) { if (this->now() - start > timeout) { - throw opennav_docking_core::FailedToDetectDock("Failed initial dock detection"); + throw opennav_docking_core::FailedToDetectDock( + "Failed initial dock detection: Timeout exceeded"); } if (checkAndWarnIfCancelled(docking_action_server_, "dock_robot") || diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index ff8be965603..3be3428e4eb 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -13,11 +13,14 @@ // limitations under the License. #include +#include #include "nav2_ros_common/node_utils.hpp" #include "opennav_docking/simple_charging_dock.hpp" #include "opennav_docking/utils.hpp" +using namespace std::chrono_literals; + namespace opennav_docking { @@ -37,6 +40,14 @@ void SimpleChargingDock::configure( nav2::declare_parameter_if_not_declared( node_, name + ".use_battery_status", rclcpp::ParameterValue(true)); + // Parameters for optional detector control + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_name", rclcpp::ParameterValue("")); + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_timeout", rclcpp::ParameterValue(5.0)); + nav2::declare_parameter_if_not_declared( + node_, name + ".subscribe_toggle", rclcpp::ParameterValue(false)); + // Parameters for optional external detection of dock pose nav2::declare_parameter_if_not_declared( node_, name + ".use_external_detection_pose", rclcpp::ParameterValue(false)); @@ -105,6 +116,26 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + node_->get_parameter(name + ".detector_service_name", detector_service_name_); + node_->get_parameter(name + ".detector_service_timeout", detector_service_timeout_); + node_->get_parameter(name + ".subscribe_toggle", subscribe_toggle_); + + // Initialize detection state + detection_active_ = false; + initial_pose_received_ = false; + + // Create persistent subscription if toggling is disabled. + if (use_external_detection_pose_ && !subscribe_toggle_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + std::string dock_direction; node_->get_parameter(name + ".dock_direction", dock_direction); dock_direction_ = utils::getDockDirectionFromString(dock_direction); @@ -123,21 +154,17 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".filter_coef", filter_coef); filter_ = std::make_unique(filter_coef, external_detection_timeout_); + if (!detector_service_name_.empty()) { + detector_client_ = node_->create_client( + detector_service_name_, false); + } + if (use_battery_status_) { battery_sub_ = node_->create_subscription( "battery_state", [this](const sensor_msgs::msg::BatteryState::SharedPtr state) { is_charging_ = state->current > charging_threshold_; - }, nav2::qos::StandardTopicQoS()); - } - - if (use_external_detection_pose_) { - dock_pose_.header.stamp = rclcpp::Time(0); - dock_pose_sub_ = node_->create_subscription( - "detected_dock_pose", - [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { - detected_dock_pose_ = *pose; - }, nav2::qos::StandardTopicQoS(1)); // Only want the most recent pose + }); } bool use_stall_detection; @@ -197,6 +224,12 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, return true; } + // Guard against using pose data before the first detection has arrived. + if (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet"); + return false; + } + // If using detections, get current detections, transform to frame, and apply offsets geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; @@ -323,6 +356,120 @@ void SimpleChargingDock::jointStateCallback(const sensor_msgs::msg::JointState:: is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); } +bool SimpleChargingDock::startDetectionProcess() +{ + // Skip if already active + if (detection_active_) { + return true; + } + + // 1. Service START request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to start.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Subscription toggle + // Only subscribe once; will set state to ON on first message + if (subscribe_toggle_ && !dock_pose_sub_) { + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + + detection_active_ = true; + RCLCPP_INFO(node_->get_logger(), "External detector activation requested."); + return true; +} + +bool SimpleChargingDock::stopDetectionProcess() +{ + // Skip if already OFF + if (!detection_active_) { + return true; + } + + // 1. Service STOP request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to stop.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Unsubscribe to release resources + // reset() will tear down the topic subscription immediately + if (subscribe_toggle_ && dock_pose_sub_) { + dock_pose_sub_.reset(); + } + + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested."); + return true; +} + +void SimpleChargingDock::activate() +{ + dock_pose_pub_->on_activate(); + filtered_dock_pose_pub_->on_activate(); + staging_pose_pub_->on_activate(); +} + +void SimpleChargingDock::deactivate() +{ + stopDetectionProcess(); + dock_pose_pub_->on_deactivate(); + filtered_dock_pose_pub_->on_deactivate(); + staging_pose_pub_->on_deactivate(); + RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock deactivated"); +} + +void SimpleChargingDock::cleanup() +{ + detector_client_.reset(); + dock_pose_sub_.reset(); + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock cleaned up"); +} + } // namespace opennav_docking #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp index 2a845559c6d..7e2aa26cf85 100644 --- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -13,11 +13,14 @@ // limitations under the License. #include +#include #include "nav2_ros_common/node_utils.hpp" #include "opennav_docking/simple_non_charging_dock.hpp" #include "opennav_docking/utils.hpp" +using namespace std::chrono_literals; + namespace opennav_docking { @@ -50,6 +53,14 @@ void SimpleNonChargingDock::configure( nav2::declare_parameter_if_not_declared( node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + // Parameters for optional detector control + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_name", rclcpp::ParameterValue("")); + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_timeout", rclcpp::ParameterValue(5.0)); + nav2::declare_parameter_if_not_declared( + node_, name + ".subscribe_toggle", rclcpp::ParameterValue(false)); + // Optionally determine if docked via stall detection using joint_states nav2::declare_parameter_if_not_declared( node_, name + ".use_stall_detection", rclcpp::ParameterValue(false)); @@ -94,6 +105,26 @@ void SimpleNonChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + node_->get_parameter(name + ".detector_service_name", detector_service_name_); + node_->get_parameter(name + ".detector_service_timeout", detector_service_timeout_); + node_->get_parameter(name + ".subscribe_toggle", subscribe_toggle_); + + // Initialize detection state + detection_active_ = false; + initial_pose_received_ = false; + + // Create persistent subscription if toggling is disabled. + if (use_external_detection_pose_ && !subscribe_toggle_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + std::string dock_direction; node_->get_parameter(name + ".dock_direction", dock_direction); dock_direction_ = utils::getDockDirectionFromString(dock_direction); @@ -112,13 +143,9 @@ void SimpleNonChargingDock::configure( node_->get_parameter(name + ".filter_coef", filter_coef); filter_ = std::make_unique(filter_coef, external_detection_timeout_); - if (use_external_detection_pose_) { - dock_pose_.header.stamp = rclcpp::Time(0); - dock_pose_sub_ = node_->create_subscription( - "detected_dock_pose", - [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { - detected_dock_pose_ = *pose; - }, nav2::qos::StandardTopicQoS(1)); // Only want the most recent one + if (!detector_service_name_.empty()) { + detector_client_ = node_->create_client( + detector_service_name_, false); } bool use_stall_detection; @@ -178,6 +205,12 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos return true; } + // Guard against using pose data before the first detection has arrived. + if (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet"); + return false; + } + // If using detections, get current detections, transform to frame, and apply offsets geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; @@ -289,6 +322,120 @@ void SimpleNonChargingDock::jointStateCallback(const sensor_msgs::msg::JointStat is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); } +bool SimpleNonChargingDock::startDetectionProcess() +{ + // Skip if already active + if (detection_active_) { + return true; + } + + // 1. Service START request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to start.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Subscription toggle + // Only subscribe once; will set state to ON on first message + if (subscribe_toggle_ && !dock_pose_sub_) { + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + + detection_active_ = true; + RCLCPP_INFO(node_->get_logger(), "External detector activation requested."); + return true; +} + +bool SimpleNonChargingDock::stopDetectionProcess() +{ + // Skip if already OFF + if (!detection_active_) { + return true; + } + + // 1. Service STOP request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to stop.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Unsubscribe to release resources + // reset() will tear down the topic subscription immediately + if (subscribe_toggle_ && dock_pose_sub_) { + dock_pose_sub_.reset(); + } + + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested."); + return true; +} + +void SimpleNonChargingDock::activate() +{ + dock_pose_pub_->on_activate(); + filtered_dock_pose_pub_->on_activate(); + staging_pose_pub_->on_activate(); +} + +void SimpleNonChargingDock::deactivate() +{ + stopDetectionProcess(); + dock_pose_pub_->on_deactivate(); + filtered_dock_pose_pub_->on_deactivate(); + staging_pose_pub_->on_deactivate(); + RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock deactivated"); +} + +void SimpleNonChargingDock::cleanup() +{ + detector_client_.reset(); + dock_pose_sub_.reset(); + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock cleaned up"); +} + } // namespace opennav_docking #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 8974c236ab0..46d69b0e40c 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -28,6 +28,7 @@ import launch_testing.asserts import launch_testing.markers import launch_testing.util +from lifecycle_msgs.srv import GetState from nav2_common.launch import RewrittenYaml from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot from nav_msgs.msg import Odometry @@ -36,6 +37,7 @@ from rclpy.action.client import ActionClient from rclpy.action.server import ActionServer, ServerGoalHandle from sensor_msgs.msg import BatteryState +import tf2_ros from tf2_ros import TransformBroadcaster # This test can be run standalone with: @@ -127,8 +129,28 @@ def setUp(self) -> None: self.command = Twist() self.node = rclpy.create_node('test_docking_server') # Publish odometry + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) self.odom_pub = self.node.create_publisher(Odometry, 'odom', 10) + def wait_for_node_to_be_active(self, node_name, timeout_sec=10.0): + """Wait for a managed node to become active.""" + client = self.node.create_client(GetState, f'{node_name}/get_state') + if not client.wait_for_service(timeout_sec=2.0): + self.fail(f'Service get_state for {node_name} not available.') + + start_time = time.time() + while time.time() - start_time < timeout_sec: + req = GetState.Request() + future = client.call_async(req) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=1.0) + if future.result() and future.result().current_state.id == 3: # 3 = ACTIVE + self.node.get_logger().info(f'Node {node_name} is active.') + return + time.sleep(0.5) + # raises AssertionError + self.fail(f'Node {node_name} did not become active within {timeout_sec} seconds.') + def tearDown(self) -> None: self.node.destroy_node() @@ -206,6 +228,7 @@ def nav_execute_callback( def test_docking_server(self) -> None: # Publish TF for odometry self.tf_broadcaster = TransformBroadcaster(self.node) + time.sleep(0.5) # Create a timer to run "control loop" at 20hz self.timer = self.node.create_timer(0.05, self.timer_callback) @@ -245,10 +268,19 @@ def test_docking_server(self) -> None: # Publish transform self.publish() - # Run for 1 seconds to allow tf to propagate - for _ in range(10): + # Wait until the transform is available. + self.node.get_logger().info('Waiting for TF odom->base_link to be available...') + start_time = time.time() + timeout = 10.0 + while not self.tf_buffer.can_transform('odom', 'base_link', rclpy.time.Time()): + if time.time() - start_time > timeout: + self.fail('TF transform odom->base_link not available after 10s') rclpy.spin_once(self.node, timeout_sec=0.1) time.sleep(0.1) + self.node.get_logger().info('TF is ready, proceeding with test.') + + # Wait until the docking server is active. + self.wait_for_node_to_be_active('docking_server') # Test docking action self.action_result = [] diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 7cc1f2fe8ab..22d676bf60a 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -46,6 +46,15 @@ class DockingServerShim : public DockingServer } }; +// Test shim to expose the TF2 buffer for injection +class DockingServerTFShim : public DockingServerShim +{ +public: + DockingServerTFShim() + : DockingServerShim() {} + std::shared_ptr getTfBuffer() {return tf2_buffer_;} +}; + TEST(DockingServerTests, ObjectLifecycle) { auto node = std::make_shared(); @@ -311,6 +320,161 @@ TEST(DockingServerTests, testDockBackward) node.reset(); } +TEST(DockingServerTests, ExceptionHandlingDuringDocking) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client"); + + // Configure docking server + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + node->declare_parameter("exception_to_throw", ""); + node->declare_parameter("dock_action_called", false); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + // Test multiple exception scenarios to ensure coverage + std::vector exceptions = {"FailedToDetectDock", "FailedToControl"}; + + for (const auto & exception_type : exceptions) { + node->set_parameter(rclcpp::Parameter("exception_to_throw", exception_type)); + node->set_parameter(rclcpp::Parameter("dock_action_called", false)); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + auto goal = DockRobot::Goal(); + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + auto status = rclcpp::spin_until_future_complete(client_node, future_result, 5s); + ASSERT_EQ(status, rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + } + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); +} + +TEST(DockingServerTests, StopDetectionOnSuccess) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client_success"); + + // Configure the server with the test plugin + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + + // Configure TestFailureDock to report success + node->declare_parameter("exception_to_throw", ""); + node->declare_parameter("dock_action_called", true); + // Note: isCharging() in TestFailureDock returns false, so it will wait for charge + // which will succeed because the plugin is a charger. We'll set the timeout low. + node->set_parameter(rclcpp::Parameter("wait_charge_timeout", 0.1)); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + DockRobot::Goal goal; + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + ASSERT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result, 5s), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_TRUE(result.result->success); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->shutdown(); +} + +TEST(DockingServerTests, HandlesPluginStartFailure) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client_start_failure"); + + // Configure the server with the TestFailureDock plugin. + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("test_dock.frame", "odom"); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + node->declare_parameter("exception_to_throw", ""); + + // Configure the TestFailureDock to fail its startup process. + node->declare_parameter("fail_start_detection", true); + node->declare_parameter("dock_action_called", false); + + node->on_configure(rclcpp_lifecycle::State()); + + // Mock the necessary TF transform to prevent a premature failure. + geometry_msgs::msg::TransformStamped identity_transform; + identity_transform.header.frame_id = "odom"; + identity_transform.child_frame_id = "odom"; + identity_transform.transform.rotation.w = 1.0; + node->getTfBuffer()->setTransform(identity_transform, "test_authority", true); + + node->on_activate(rclcpp_lifecycle::State()); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + DockRobot::Goal goal; + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + ASSERT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result, 5s), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + EXPECT_EQ(result.result->error_code, DockRobot::Result::FAILED_TO_DETECT_DOCK); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->shutdown(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 3b96f59149f..a95bd36eda3 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "gtest/gtest.h" +#include "std_srvs/srv/trigger.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -24,6 +26,8 @@ // Testing the simple charging dock plugin +using namespace std::chrono_literals; + namespace opennav_docking { @@ -39,6 +43,15 @@ class SimpleChargingDockShim : public opennav_docking::SimpleChargingDock } }; +class SimpleChargingDockTestable : public opennav_docking::SimpleChargingDock +{ +public: + using opennav_docking::SimpleChargingDock::SimpleChargingDock; + + // Expose detector state for test verification + bool isDetectorActive() const {return initial_pose_received_;} +}; + TEST(SimpleChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -47,6 +60,8 @@ TEST(SimpleChargingDockTests, ObjectLifecycle) auto dock = std::make_unique(); dock->configure(node, "my_dock", nullptr); dock->activate(); + EXPECT_TRUE(dock->startDetectionProcess()); + EXPECT_TRUE(dock->stopDetectionProcess()); // Check initial states EXPECT_FALSE(dock->isCharging()); @@ -71,9 +86,6 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -83,7 +95,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); @@ -94,7 +106,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_TRUE(dock->isCharging()); EXPECT_FALSE(dock->hasStoppedCharging()); @@ -127,9 +139,6 @@ TEST(SimpleChargingDockTests, StallDetection) rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names))); dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - EXPECT_EQ(dock->getStallJointNames(), names); // Stopped, but below effort threshold @@ -140,7 +149,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -152,7 +161,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -164,7 +173,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_TRUE(dock->isDocked()); @@ -235,8 +244,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); + dock->startDetectionProcess(); geometry_msgs::msg::PoseStamped pose; @@ -250,7 +258,8 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -258,6 +267,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -277,8 +287,6 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -286,7 +294,7 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -316,16 +324,7 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - geometry_msgs::msg::PoseStamped detected_pose; - detected_pose.header.stamp = node->now(); - detected_pose.header.frame_id = "my_frame"; - detected_pose.pose.position.x = 1.0; - detected_pose.pose.position.y = 1.0; - pub->publish(detected_pose); - executor.spin_some(); + dock->startDetectionProcess(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -338,11 +337,24 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) transform.child_frame_id = "other_frame"; tf_buffer->setTransform(transform, "test", true); - // It can find a transform between the two frames but it throws an exception in isDocked + // First call to getRefinedPose starts detection + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + // Now publish the detection after subscription is created + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); EXPECT_FALSE(dock->isDocked()); dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -413,6 +425,208 @@ TEST(SimpleChargingDockTests, ShouldRotateToDock) dock.reset(); } +TEST(SimpleChargingDockTests, DetectorLifecycle) +{ + auto node = std::make_shared("test"); + + // Test with detector service configured + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + node->declare_parameter("my_dock.detector_service_name", + rclcpp::ParameterValue("test_detector_service")); + node->declare_parameter("my_dock.subscribe_toggle", rclcpp::ParameterValue(true)); + + // Create a mock service to prevent timeout + bool service_called = false; + auto service = node->create_service( + "test_detector_service", + [&service_called](std::shared_ptr, + std::shared_ptr, + std::shared_ptr response) + { + service_called = true; + response->success = true; + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + + dock->activate(); + dock->startDetectionProcess(); + // Spin to process async service call + auto start_time = std::chrono::steady_clock::now(); + while (!service_called && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) + { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_TRUE(service_called); + dock->stopDetectionProcess(); + dock->deactivate(); + + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleChargingDockTests, DetectorServiceConfiguration) +{ + auto node = std::make_shared("test_detector_config"); + + // Configure with detector service + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "detector_service"); + node->declare_parameter("my_dock.detector_service_timeout", 1.0); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_NO_THROW(dock->activate()); + + EXPECT_NO_THROW(dock->deactivate()); + EXPECT_NO_THROW(dock->cleanup()); +} + +TEST(SimpleChargingDockTests, SubscriptionCallback) +{ + auto node = std::make_shared("test_subscription_reliable"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.subscribe_toggle", true); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + // A LifecyclePublisher must be activated to publish. + publisher->on_activate(); + + dock->startDetectionProcess(); + + // Wait for the publisher and subscriber to connect. + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + // Publish a message to trigger the subscription callback. + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state was updated, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleChargingDockTests, DetectorServiceTimeout) +{ + auto node = std::make_shared("test_detector_timeout"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "slow_service"); + node->declare_parameter("my_dock.detector_service_timeout", 0.1); + + // Create a mock service that never responds in time + auto mock_service = node->create_service( + "slow_service", + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The call to invoke() should timeout and be caught + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); + node->shutdown(); +} + +TEST(SimpleChargingDockTests, DetectorServiceFailure) +{ + const char * service_name = "charging_dock_slow_service"; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.detector_service_name", std::string(service_name)}, + // The client will time out after 100ms + {"my_dock.detector_service_timeout", 0.1} + }); + auto node = std::make_shared("test_detector_failure", options); + + // Create a service that responds slower than the client's timeout. + auto slow_service = node->create_service( + service_name, + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The invoke() call should time out, but the exception must be caught + // within the startDetectionProcess method. The test passes if no + // uncaught exception is thrown. + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleChargingDockTests, SubscriptionPersistent) +{ + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.subscribe_toggle", false} // The key parameter to test. + }); + auto node = std::make_shared("test_sub_persistent", options); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The subscription should be active immediately after configuration. + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + publisher->on_activate(); + + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state changed, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 82839040c18..77951bd6cd5 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "gtest/gtest.h" +#include "std_srvs/srv/trigger.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -24,6 +26,8 @@ // Testing the simple non-charging dock plugin +using namespace std::chrono_literals; + namespace opennav_docking { @@ -39,6 +43,15 @@ class SimpleNonChargingDockShim : public opennav_docking::SimpleNonChargingDock } }; +class SimpleNonChargingDockTestable : public opennav_docking::SimpleNonChargingDock +{ +public: + using opennav_docking::SimpleNonChargingDock::SimpleNonChargingDock; + + // Expose detector state for test verification + bool isDetectorActive() const {return initial_pose_received_;} +}; + TEST(SimpleNonChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -47,6 +60,8 @@ TEST(SimpleNonChargingDockTests, ObjectLifecycle) auto dock = std::make_unique(); dock->configure(node, "my_dock", nullptr); dock->activate(); + EXPECT_TRUE(dock->startDetectionProcess()); + EXPECT_TRUE(dock->stopDetectionProcess()); // Check initial states EXPECT_THROW(dock->isCharging(), std::runtime_error); @@ -84,9 +99,6 @@ TEST(SimpleNonChargingDockTests, StallDetection) EXPECT_EQ(dock->getStallJointNames(), names); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -98,7 +110,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -110,7 +122,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_FALSE(dock->isDocked()); @@ -122,7 +134,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); EXPECT_TRUE(dock->isDocked()); @@ -193,8 +205,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); + dock->startDetectionProcess(); geometry_msgs::msg::PoseStamped pose; @@ -208,7 +219,8 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -216,6 +228,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -235,8 +248,6 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -244,7 +255,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - executor.spin_some(); + rclcpp::spin_some(node->get_node_base_interface()); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -274,16 +285,7 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - geometry_msgs::msg::PoseStamped detected_pose; - detected_pose.header.stamp = node->now(); - detected_pose.header.frame_id = "my_frame"; - detected_pose.pose.position.x = 1.0; - detected_pose.pose.position.y = 1.0; - pub->publish(detected_pose); - executor.spin_some(); + dock->startDetectionProcess(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -296,11 +298,24 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) transform.child_frame_id = "other_frame"; tf_buffer->setTransform(transform, "test", true); - // It can find a transform between the two frames but it throws an exception in isDocked + // First call to getRefinedPose starts detection + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + // Now publish the detection after subscription is created + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); EXPECT_FALSE(dock->isDocked()); dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -371,6 +386,211 @@ TEST(SimpleChargingDockTests, ShouldRotateToDock) dock.reset(); } +TEST(SimpleNonChargingDockTests, DetectorLifecycle) +{ + auto node = std::make_shared("test"); + + // Test with detector service configured + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + node->declare_parameter("my_dock.detector_service_name", + rclcpp::ParameterValue("test_detector_service")); + node->declare_parameter("my_dock.subscribe_toggle", rclcpp::ParameterValue(true)); + + // Create a mock service to prevent timeout + bool service_called = false; + auto service = node->create_service( + "test_detector_service", + [&service_called](std::shared_ptr, + std::shared_ptr, + std::shared_ptr response) + { + service_called = true; + response->success = true; + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + + dock->activate(); + dock->startDetectionProcess(); + // Spin to process async service call + auto start_time = std::chrono::steady_clock::now(); + while (!service_called && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) + { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_TRUE(service_called); + dock->stopDetectionProcess(); + dock->deactivate(); + + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceConfiguration) +{ + auto node = std::make_shared("test_detector_config"); + + // Configure with detector service + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "detector_service"); + node->declare_parameter("my_dock.detector_service_timeout", 1.0); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_NO_THROW(dock->activate()); + + EXPECT_NO_THROW(dock->deactivate()); + EXPECT_NO_THROW(dock->cleanup()); +} + +TEST(SimpleNonChargingDockTests, SubscriptionCallback) +{ + auto node = std::make_shared( + "test_subscription_reliable_non_charging"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.subscribe_toggle", true); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + // A LifecyclePublisher must be activated to publish. + publisher->on_activate(); + + dock->startDetectionProcess(); + + // Wait for the publisher and subscriber to connect. + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + // Publish a message to trigger the subscription callback. + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state was updated, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceTimeout) +{ + auto node = std::make_shared("test_detector_timeout"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "slow_service"); + node->declare_parameter("my_dock.detector_service_timeout", 0.1); + + // Create a mock service that never responds in time + auto mock_service = node->create_service( + "slow_service", + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The call to invoke() should timeout and be caught + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); + node->shutdown(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceFailure) +{ + const char * service_name = "non_charging_dock_slow_service"; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.detector_service_name", std::string(service_name)}, + // The client will time out after 100ms + {"my_dock.detector_service_timeout", 0.1} + }); + auto node = std::make_shared( + "test_detector_failure_non_charging", options); + + // Create a service that responds slower than the client's timeout. + auto slow_service = node->create_service( + service_name, + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The invoke() call should time out, but the exception must be caught + // within the startDetectionProcess method. The test passes if no + // uncaught exception is thrown. + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleNonChargingDockTests, SubscriptionPersistent) +{ + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.subscribe_toggle", false} // The key parameter to test. + }); + auto node = std::make_shared( + "test_sub_persistent_non_charging", options); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The subscription should be active immediately after configuration. + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + publisher->on_activate(); + + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state changed, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp index 8a33c3b3304..a5ee4adaff5 100644 --- a/nav2_docking/opennav_docking/test/testing_dock.cpp +++ b/nav2_docking/opennav_docking/test/testing_dock.cpp @@ -102,6 +102,18 @@ class TestFailureDock : public opennav_docking_core::ChargingDock return true; } + virtual bool startDetectionProcess() + { + bool should_fail; + node_->get_parameter_or("fail_start_detection", should_fail, false); + return !should_fail; + } + + virtual bool stopDetectionProcess() + { + return true; + } + protected: nav2::LifecycleNode::SharedPtr node_; }; diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index 67e599405d7..111dc1c03ef 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -124,6 +124,16 @@ class ChargingDock */ virtual bool hasStoppedCharging() = 0; + /** + * @brief Start any detection pipelines required for pose refinement. + */ + virtual bool startDetectionProcess() = 0; + + /** + * @brief Stop any detection pipelines running for pose refinement. + */ + virtual bool stopDetectionProcess() = 0; + /** * @brief Gets if this is a charging-typed dock */ diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp index 9422ee19f13..97b9dd06e61 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp @@ -125,6 +125,16 @@ class NonChargingDock : public ChargingDock throw std::runtime_error("This dock is not a charging dock!"); } + /** + * @brief Start any detection pipelines required for pose refinement. + */ + virtual bool startDetectionProcess() = 0; + + /** + * @brief Stop any detection pipelines running for pose refinement. + */ + virtual bool stopDetectionProcess() = 0; + /** * @brief Gets if this is a charging-typed dock */