From b8f51ae1d1923209f94fa38875e89822ff49b143 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 6 Feb 2024 18:46:52 +0100 Subject: [PATCH] New Graceful Motion Controller (#4021) * Initial commit Signed-off-by: Alberto Tudela * Fix egopolar and add compute next pose Signed-off-by: Alberto Tudela * Added simulated trajectory Signed-off-by: Alberto Tudela * Added slowdown publisher Signed-off-by: Alberto Tudela * Added first tests Signed-off-by: Alberto Tudela * Added basic tests and smoth control Signed-off-by: Alberto Tudela * Added initial rotation Signed-off-by: Alberto Tudela * Added final rotation Signed-off-by: Alberto Tudela * Added collision Signed-off-by: Alberto Tudela * Improve last motion target Signed-off-by: Alberto Tudela * Added new tests Signed-off-by: Alberto Tudela * Set min velocity Signed-off-by: Alberto Tudela * Added EOL Signed-off-by: Alberto Tudela * Path test Signed-off-by: Alberto Tudela * Add utils and minor fixes Signed-off-by: Alberto Tudela * Update footprint calculation Signed-off-by: Alberto Tudela * Added more tests Signed-off-by: Alberto Tudela * Added nav2_controller to package test Signed-off-by: Alberto Tudela * Improve comments Signed-off-by: Alberto Tudela * Added backward motion Signed-off-by: Alberto Tudela * Split in two libraries Signed-off-by: Alberto Tudela * Improve rotation velocity Signed-off-by: Alberto Tudela * Update documentation Signed-off-by: Alberto Tudela * Minor fixes Signed-off-by: Alberto Tudela * Revert collision_checker Signed-off-by: Alberto Tudela * Pass costmap transform to simulate trajectory Signed-off-by: Alberto Tudela * Retain old headers Signed-off-by: Alberto Tudela * Update dyn parameters of control law Signed-off-by: Alberto Tudela * Better comment in test Signed-off-by: Alberto Tudela * Update setSpeedLimits with angular vel max Signed-off-by: Alberto Tudela * Fix SpeedLimits Signed-off-by: Alberto Tudela * Fixes in vel and collision Signed-off-by: Alberto Tudela * Fix backward motion Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela --- .../CMakeLists.txt | 80 ++ nav2_graceful_motion_controller/README.md | 42 + .../doc/trajectories.png | Bin 0 -> 38333 bytes .../graceful_controller_plugin.xml | 7 + .../ego_polar_coords.hpp | 91 ++ .../graceful_motion_controller.hpp | 180 +++ .../parameter_handler.hpp | 97 ++ .../path_handler.hpp | 83 ++ .../smooth_control_law.hpp | 193 +++ .../nav2_graceful_motion_controller/utils.hpp | 47 + nav2_graceful_motion_controller/package.xml | 35 + .../src/graceful_motion_controller.cpp | 365 +++++ .../src/parameter_handler.cpp | 171 +++ .../src/path_handler.cpp | 126 ++ .../src/smooth_control_law.cpp | 124 ++ nav2_graceful_motion_controller/src/utils.cpp | 51 + .../test/CMakeLists.txt | 21 + .../test/test_egopolar.cpp | 83 ++ .../test/test_graceful_motion_controller.cpp | 1214 +++++++++++++++++ 19 files changed, 3010 insertions(+) create mode 100644 nav2_graceful_motion_controller/CMakeLists.txt create mode 100644 nav2_graceful_motion_controller/README.md create mode 100644 nav2_graceful_motion_controller/doc/trajectories.png create mode 100644 nav2_graceful_motion_controller/graceful_controller_plugin.xml create mode 100644 nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp create mode 100644 nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp create mode 100644 nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp create mode 100644 nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/path_handler.hpp create mode 100644 nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp create mode 100644 nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp create mode 100644 nav2_graceful_motion_controller/package.xml create mode 100644 nav2_graceful_motion_controller/src/graceful_motion_controller.cpp create mode 100644 nav2_graceful_motion_controller/src/parameter_handler.cpp create mode 100644 nav2_graceful_motion_controller/src/path_handler.cpp create mode 100644 nav2_graceful_motion_controller/src/smooth_control_law.cpp create mode 100644 nav2_graceful_motion_controller/src/utils.cpp create mode 100644 nav2_graceful_motion_controller/test/CMakeLists.txt create mode 100644 nav2_graceful_motion_controller/test/test_egopolar.cpp create mode 100644 nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp diff --git a/nav2_graceful_motion_controller/CMakeLists.txt b/nav2_graceful_motion_controller/CMakeLists.txt new file mode 100644 index 0000000000..3625eeba5d --- /dev/null +++ b/nav2_graceful_motion_controller/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_graceful_motion_controller) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_util REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(nav_2d_utils REQUIRED) +find_package(angles REQUIRED) + +nav2_package() + +include_directories( + include +) + +set(dependencies + rclcpp + geometry_msgs + nav2_costmap_2d + pluginlib + nav_msgs + nav2_util + nav2_core + tf2 + tf2_geometry_msgs + nav_2d_utils + angles +) + +# Add Smooth Control Law as library +add_library(smooth_control_law SHARED src/smooth_control_law.cpp) +ament_target_dependencies(smooth_control_law ${dependencies}) + +# Add Graceful Motion Controller +set(library_name nav2_graceful_motion_controller) + +add_library(${library_name} SHARED + src/graceful_motion_controller.cpp + src/parameter_handler.cpp + src/path_handler.cpp + src/utils.cpp +) + +target_link_libraries(${library_name} smooth_control_law) +ament_target_dependencies(${library_name} ${dependencies}) + +install(TARGETS smooth_control_law ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(smooth_control_law ${library_name}) +ament_export_dependencies(${dependencies}) + +pluginlib_export_plugin_description_file(nav2_core graceful_controller_plugin.xml) + +ament_package() diff --git a/nav2_graceful_motion_controller/README.md b/nav2_graceful_motion_controller/README.md new file mode 100644 index 0000000000..322485cec8 --- /dev/null +++ b/nav2_graceful_motion_controller/README.md @@ -0,0 +1,42 @@ +# Graceful Motion Controller +The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion.html) for additional parameter descriptions. + +## Smooth control law +The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: +* The egocentric polar coordinates of the motion target (r, phi, delta) with respect to the robot frame. +* A slow subsystem which describes the position of the robot. +* A fast subsystem which describes the steering angle of the robot. + +![Trajectories](./doc/trajectories.png) + +## Parameters + +| Parameter | Description | +|-----|----| +| `transform_tolerance` | The TF transform tolerance. | +| `motion_target_dist` | The lookahead distance to use to find the motion_target point. This distance should be a value around 1.0m but not much farther away. Greater values will cause the robot to generate smoother paths but not necessarily follow the path as closely. | +| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | +| `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. | +| `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. | +| `beta` | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases. | +| `lambda` | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves. | +| `v_linear_min` | Minimum linear velocity. Units: meters/sec. | +| `v_linear_max` | Maximum linear velocity. Units: meters/sec. | +| `v_angular_max` | Maximum angular velocity produced by the control law. Units: radians/sec. | +| `slowdown_radius` | Radius around the goal pose in which the robot will start to slow down. Units: meters. | +| `initial_rotation` | Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and k_phi, k_delta. | +| `initial_rotation_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `initial_rotation` is enabled. | +| `final_rotation` | Similar to `initial_rotation`, the control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the final pose will be ignored and the robot will follow the orientation of he path and will make a final rotation in place to the goal orientation. | +| `rotation_scaling_factor` | The scaling factor applied to the rotation in place velocity. | +| `allow_backward` | Whether to allow the robot to move backward. | + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `transformed_global_plan` | `nav_msgs/Path` | The global plan transformed into the robot frame. | +| `local_plan` | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | +| `motion_target` | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | +| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | \ No newline at end of file diff --git a/nav2_graceful_motion_controller/doc/trajectories.png b/nav2_graceful_motion_controller/doc/trajectories.png new file mode 100644 index 0000000000000000000000000000000000000000..b8ae25da0949b367ff95c3900f9c94a3f339dd20 GIT binary patch literal 38333 zcmd?Rg;$l|7e07FK|(@FNdZZb?v_TnQCd>EyOeGL3F!vuknZld(%s$NFz0^0znL}v z!mME}my7ql_tf6|+0Twcu!5Wf8VUgl1Oh>mk`z^fK;V5K5V&O|MDTG7<8~0! za8R~3c5v3WGlIzKJJ?uQJ6L@C`qs(F&iGxk5OPla4-lEX`@TZgAQFTN5xk$LvXFSkI3jZn_L6Sx4@-ZbQ zX5X;i+t}IUDCPIooA68;6k#MXKg@P3(+Tvqewg5w#5io9|DHLCqX>bIgpqhD5Yl0v z_@zX_EByENup}G}>=S*HYzQ{&lQLE%2ny`w={K|xf7r|WVgLUdhK)8{T)8w&wNNc( z$}ZLYkeiJ_w!=>#*l}<2WZr&6QA=yybbyOgUhsi!Wo2di!i|rgpV0Ah&7#};l9H0} z`mm>vurR~H1ZEQ=>xlwYv{N@8&7;OMbafq9N<~FQQ#Jxb3=$8uv7{m6T=_IPL&JCC zSy7h5RnYY#2A`OdU>dyAArmr=M?Jwv+tS7W`jMsjy^ zlQCf?kN>C}8ym~T%RA-Hosp5@7Z8xDEY6;C;FdagxX=LSvi$wU@zs@ekOo?`G}__e zp^1kjdq9)>Rp!K`S4%`>q+sEroOAd_ET@e}DhR>($VC&#MI& zcXv(~W82H!JHC+wW?yh>l~xNpw-Z2!wajTE@0A>_2;JFr`HSwlNLJNsN? zMf~d3D~Og&8^T}9xj2(F&p2-TO}0@2WOs52h*%}7%dy_-+S<)tY0LIfi{S9^@VwXE z0c7wuYhVssHvs?G~XW0aM+(d`8CJD!V;aA=0)HGe%9m$QmjiV%bgCs zczAlgVKp8^9vS*W>5)zVmhcVgpt*tbN#I8`hu_kxt~r-8x@q|C`&M%E-v*B@x$8pk9;VHHLk!5z&bxzkDIEZo0q?#r?3l zlb1%t$%&WG-{kT1aP^GIda)5pPIJkAgrDzn+5l#*y50}5csefMVTTBG9t8zuxHc6- z;QDPK2C4Do{!CAJgY%J&T8UO)wp5HUpOTluM0t3V(b)=SM=4p1@bFxdLtE_S=XK(1#) zoHq|wOLgnf(l9$s5b(UEjknFcn!4G4CX}+}dUd$qII>h8fj(DhGD4eITu`t&njw;x z++0u)lAWE6I6$`_MD7>T9Z6bRTU$%SX)&7q5ha~oxB33>*sKLdmj%cBLiJKLh*3`j zvEx$`hh?JO{s8ZX53k3H`$X?^UDfY0Xi0Op30&UdpObMa;5qc0+@c7qeY)inRrd{%8k!+W3t zd4oALPbKbNUS7UiJiiUnI@EPNRiQf${pO`jYNg-gbR8?~T=Dc@aa8zF?i+!(7=Tl*~(Lct(%gy~6Zui1I*Y!ai>?jsY zKH+}xD>kwElMf#}4)z)xl3NTygQ zwvg-9f^#I{BhX`79~R5mzxDcA;^9idA|kQT(a~$&O-Idl*b)*FIr;h9wHCUb7pe)a;p;ycA6-U8X@l zZ#VFv$8*5h!NCy7(=jjpH5L}P!!{<+^HwPIq{zNhtL}Ba3Pzjq=n=lohf48xHG3zm4TGyI)$6c%DNmhh4^NZPy~) zJr1g-GN<RTNkmy`ZL!nU#3HVCN$|G*S+uGVJ78~ny1o_?0xoU8DQS;2( zyabBpYAol(!IF`Ykn|2_t9nwDiXyW{+Pu@U2(24WYI`OoCV&zq11AV?VzRZiXpg7s z!3-W+OqZ=BPp)WP!KELbc{GoFGiajndx#c)oB0`7I*-1Ft1CAExK6MH@D129l2cNo zs>?EX6Ab-S%rFV&%bc!HX{M(CJ3m7qzYprXqE7Ubf(_FG*yT!YgBh$|{C ze!3D!%A(f_0ldMSj*d>w{kDeIF6EP~ESml1V1_|Y1T{ag3&*4Tu5#(r+S;0O4Z6h5 z^>s2+G1QdrU~gH4h=^z#9A#7Y#Eydr|5N44+FA!VUib z<>Hz}p1i~FU>$p@kp>S}-0$h>JHM76!zfL~UE-6DlLF~hB?jaobt|UH3oaxi1R{5r z?B?p40W}?Vbt97#LlJ_~r|c6Rgy6jI?eG5zh?C>e^>;T%oRz&j)K-944zpP#5)F3O z)*V5&JhyELDJh=9uYSkIs>ZM7D|^~cnN`UovISG8c|>zAdBgyJ90X{_D?-9Tvl3A|)k7Mn_kmvsR*2 zcUp9J!AqFJh+eIdaJhGY^p@Nb*6?+V3Hz|uRTK~a5utl2`H-xdsAfOh*Vl6 z@Of+9XQ!vad1JN!+tN;@r9Eq|%7;xzA-nt|V5|c7#|YK5t?|CT;O*^gWx1pEbx{CI z1n=_77?mI)xND}0U)czNWdcu;YC8*-oH4Zr(|vVq#|he=(BR+<08bt_8}TwJJQsdO z#EuTv-jQ5H{FXfClSOM099D~szX4XAj4R8hFz+d1QOm8@ZKWE_*V^tcwWLkyxEC*u zJ*tar^27r3EC<9c0E2WG5UkJD7Bk$AB4-QEOT^B%mWT6oz%z<}sE5LER<)UA-MmuXLsCfi2)x7E`g-R!G;73l@k?bRnNdcXv0x+5-@swCj4tK`*gWM&zrpPu(0);t+cH_d8y>Iw879UiM5RlF)J$u zAcgj>uACAhn7Dg$;&na&`~J;hgaTN8c1}(?aOTZ-hrH>Rg#4{w;DPItHD>&mo!#B9 z2?+;MoEQ55Yl&1V+{g@NAg84CH%WC3C@W*imHDl7ba8Qhz8P$PwmBqHI>q|66AAeH4%QJqk$48}+RIsU z!gp(ZTwGkEz!xGWEyaD%l$4T!Gb>$E!S+w{x<$@!dbzT#xNY6?5I1Mtk^%^H#i6Md z@DslQ#~aI)70DZe7OFGGF=+P#I@%q|*z7XD(+mO~X2V|0T-hWiHRlx}ITl%yOy~Y1sB}B=0^r3fw^yj$E4`F6VBJJ22`t6U|*NCe%-^uC}rw1-?E# zK2ST13s_7usg>$@YE3Z!O2-7_4$;yKD?nI1rfjFXg{2@A@RW1beFgthtyNe%&kSK6 zNHiv*1vsG`-4?Hq%F4>st*tx|&M3}QTh1XDBR$QbT<71MZM_G|rZ~T_fRhEy7Znwy z0q(f!bSur9gmV$F_}EZ}ddZXHV}&sBtbgE(SYPr4PCBtZ+1Qk2PMAfSvzu0Zi3OG; z8yp<0_!7}~b#Jc_#wtI#xw$n>o#lfQr1gO!sH3Wuwl)qYC+9{0p_PK2y?rDhCpoz=j0DLg^rV0EjW7@XyrkBO zW(oLko`tsC{@!_EX)}+0LM9D>?{MI+*(mw=v}|o`6uy4_`s)e^%WnflW>L~TPW!0y zhU_igFB=eBTHftfj!e2QgoK910*%lXssF+Y)X&2{0KfdVF>re4 zM@zb{CtY~*y_ZXtHU(Geo>y?P8OIN=g%M%gyIu;y+97zX%*LubFw{n}&2zJ+GYOu!4 zs{;39;YG_tla34H*dV1za;$`BL z&%qxbuXH%3@G*KH4Jl@mL1fdi25OLCjkS)#o z@#fbUA22fD#i|Z%rKFGn%7XZg#MxWU_Hxje^}TwDVed~e{FjJTYkxH=hzTPs1_CJN zD)f7uEV#tP#5y}Wmp3$ zQCk~bJJMe|R;^@YlhwWbaW#M(g{2VcUi(_LBE^iTE>!-Fl-EJ$&CO`m3kbpT6f!FQ z@ixTctH-z|8CZloEM55}W!^XE{{9nrUm*XCdv9+^3W}JE^7nw*1)Vg5ZuCD7s_=wC z&-CyI-gGIYS8qQn%QS*Bm|-{IZ9eead9r!R{Xz`ZZS&tyjs2heIZM8b#PKui4_{Cg zQ(+SF%kz}5;Zj;Mmt2ot>bTskw=}qLsDXj3!(Q zZeeIHSC>K}BZvj5(;(b$J*)HrE;Rn(w2$vp0t_~7|X3l;K8@zz?Fj2d8ssD*YL7v3#gwKbm~EH=@q$$}wM{u|PuM?U!eG|S&L zRC`*RAVAn=;c1>Grm6n;v2@n$h_DrH&O<5GmaO{+GA2yQcYef8 z?qjZFRBXS0S9SPfX2$*msbFZ)gvxlCy$EHgeChgBO9%6c##M_@#SU@Y5UZeLh6JNC zTjFq!%s0Logn$6yPbg~P0pnt!+t(R_yS-VWH!x~L7MgQV1J@?;$>u? z7hNy+5;~*>tMWYW-oDLW!LwldF{5=?BmN8>9j)ahnV&)wY&*s|T*ZEoq|-fks3MXq zfpAp|#SRX#8xz0DUXm>Pgj&-R4DD=D_+hS&|2GGr=H!IUfsb(X4yq42sxL)!-O(@s zgLmUIf&-|P7BJIfjF}h|GL@QdRxDRK>YW_*8Ka2AxNf}r%X_lKK|BBC4qN~??3Ys?ii6k*-?wJEh%nZICGY z5=j@r&1dIHpxm)0*DqpOM(VSgT+h~xRb#j$B0{GGEwE2_m+N`;fEdO>i^iQ6Q6Cgap|pI@LsZ7yZ<^brT4?6ivp_+ z60cf~{6z9t&N#ufBKAspiVJElR5V)-3Irv6uY|yY^Jz=<%>wsL9WN(hLrPn;i~#HmMeV2fW1_9D z(5n3lYL#SX)ftLkl?QbW<{1%Gvf`eZy~VM37Rs{^e7NuPU(Rt~#t^mtn3p z7TvRbkM{~fT#in7Ci9q{(jN@dlwjCaljEs%9db}i;QKbckNTWHflVO8G%Xp+KK3{7 z4=)pYo#zA=_vKw_msy$P5n#|vl57ayDoA{O%z`qxo*GtVAwkJi)Xfu;STTJ*MkH-7 zisadm1DA0@iSV|}^~@ONM|hkMi=42@zC0IGOt3Se%W+x$$gKN7De(x#qb$FJFKcW} zwq5H!0c;n@X*>w1f5?D9+7*Oxay&zQ5s(0X|EQFd@W@DcM4-09Dj4QdC2Ii-PIH!jt(Q`JNZj}R zfI__i5D<8*)n{N}08AvANec;%-H6@e z#_<5!JiOo8_b2jyYFRkxc_@wq)uZQm@j4hzZ&!MeF7K5vlC8l0CDL+V(U+=E2S~zf z(jl40`$3Kx_&(Hub(g_TW)iQ#J0vjsTMe@?CiE_GMtB?Hsh5`W0*OqtkPKMYYS>>y z;=b|4L(6|jil%=F{-;o+FxNM1I{83I(?Lv(%h0eOf4~J;%Ww>~Pv*r;1wsIN@xup# z&egQ{5G2ZH%5vrP`hKQThTiWIX=x#1duInVWpl!RerWGWgW{%huBx)JS20QLcFfwx+5jZXIgvkWL2{Jr8IMYJRs~# z6)mVb?3yZbhhq+<9VoEOu9AJ2B)ewVnEpg{?#iQJF$+XV*tLHMvl%YuM^Cid|D z#26l#@Ke6s3E6`XJ5^|eDY}wBR>w`upLMwO^^J@jsBhK~jv8N8@^^Xk&Sm-<2oIsE z*u?q@hEO^O#!m)4(Y;(66oR1^v=pa7`ONY4AgbQ9dk-}4=JN12A*fj(jT?koX=};KA_?cl*B*g-VraHgdJOisG71T z&m!t)UZz@eHcn`6unDs@9te_>vn!A2r(p0-sg}7pQRTQOgz!?#80v?q<}2!*HRtPgpNp7iwfCk5W_CFrwArpR24ShbD2yD7M5wz=>r zVsPt71*V@PNT8J(dPh3`BS-V?&7#DgsYqGg^V8-WFdH<$!%+`*2%!zDtmYQBL%&2+ zmLRquQNxp`a5b_Lb6P)p_3+~u-JZ;{>tnkXZ|gs-O6;dH`~msEH{;N>k{Tz=B30bg zFmQ$ibbJcE?eb#@Hq@bQAhu};(K#yJ%9 z<~UqTh)t{XDkmODZd#&70o^H9c^~6kd&zy0{#^O@t0^IMp!K$Hn{=#dF|mzG6ov7c z*UOidhOcDMziTkrtiIy2$sp3zW`-WLROX@7c;MsRY~RE~+9dlSc)RV51Ia@G<*We6FTsp60@n z7HwXo=4<)kFoSI*Vnj=Px1KDGR*!j7L#=Yg`+ek^d1@2%)HwD7qUD-}4E6TY=1s%ktF&lcMLv z7*#fbrz3lFd4@zc%EQR(2tj+B{fqB;@9rp|gCG)4{FT`{hG)4kKuNZ)pebWo25nn` zs0%9gu0>w8L(9*Ym6OHf#WFH}M#H0{R}~)}He#8XwVGOtI+js~WR~4m6}K||RozG^ z9K=rs?o}ae$Si9%dt)R=O{Ynp8hPRsQFrWYWYw%^5F#PjEgz2`_83{wFWa%XG0!sI zFP85xT?glhBJbJW&X5Kh1099FqdR?n?X>IkcqPIm6_+Y>z>fz!})oAInS`A4|O1^50+JPvJ8(SScV->$IY~S>W5_9Khy5oNI4Mz$j zLd2+m(q{kMBME=^jsX)%g}P`zQk0bn)W98#DAh{#^BiNcHL zk8@lyNVYscv&8k;=Uh&!=^ty%UvfCqDs>N%z6@)UyKOX6WJ`A*^?74qI!b61Pevdw z<%XQZsd2OHJwd;??z@wa$&es(8<>02Id!ljK<^XdI6>~0FF2DFiWpz^?l++XsWESr zcBzAt%=*9)(YCEoc70pt=P^^Z^OVsjv7_(9Q>fw9B*mouT+*tl$CC!!9@ov`7LS(S z4*T?PUr?Rg{ws0V$NoXIft9%edP@jY6s^$kbf5P*Nce^~NW42)iN@a@Gyh?l$`XHw zz>uBaDlYW}nPlbP?`l(g=_UDP@3Hrvm8Vk+DA3p)G`3fgx=}01@j2$PigO!0PB1fH z!Nwd+E4A9Zq=uKIMtLFA$YdoLBlwN6OBZ#Ev>@<#e&-t#P_JAI!X4K7X1rE_lIorv zjpJ^h5aDpK{*jq8^25gMK*6@Rxy-}uES7LGD*#8|d>ZxIi{Q5`$X!u%o8G!Z`Fp%+ zuENlL?FTnfoqhD2+AQa2?Eo(4_g=-zkA-JkLNAw*h2HheyBX2Sj~x7v+xlb7N46j0 zuuC~Scj$_Ie##?BmBFRhhbK?;b-gOptmidaww)$CM3w98s$ou5j^ayV^xxObo_2yG zf=lsGb$1flN9=lclCNus{)Utx-vphxp9F;bF-^ppheOXeRtjEULSOJ3d)*IrQgU_| z5=*hAr<>vt{mCSt?oVFlP|JLDI#8;uw?_{iKg9YwUCYE0`+`cvX2!sF`=wo?I#N~d zDJf^MuN?|Fg6|Un(x-x42iMRL1vH&q&N!B)#n^Z?Us+;nEOhD4NGnYZ9-gs zJXm%}Cv*Hp%ai8$#e+6Mi<9kSL1<*}dn|t@tG;LVUpRRY;J}Ften!os{UbTiyCif9G+9zp?dqq|v zAH~I_;P4phtKt;*Z#?1!%xkT8Q;&e1Ltp;FOT;g^iIF=YaN2y4)zO~NT5R!9_(Su^ zDeh6w-*gikl!;h5EuWhu3Qp~+*elk_u0KF_aA_Q=elduxWYXDyM(tJ?vA^2W?X(L3 z`{FeqaDVM^AGhGU{k_d3^*g4pBe3?$iu;N1`&wFfpBGjFGD1c1{lTKjHW-du-BOE? zqU*Vj1V5(#{7e26@J(%W3!eXT^;@DptIf?xGYQLb7DYs-SecZd;VU_oa_-rmP6TU| zI-?)Sw^h_Bd}DBu9D!x0LzaMD${w_Op=y(dd~Q>)*xp``lTnJpIx0Q#SI^FyJ@Lad zkwOttwC3iu|9zuuX})wvMf>*+c8MK1^cSKQmLKEI^k{hl`KJS}%wUaz5A(~n?^9+q zCI29IOR`^GEe+~6G$NxY@upy|Y60)Fd(dEV86*AHZBT2xK>_w11Se#}eK9JCfP2=j zM7wWj^Ruez9UGqZ*BPrUGF-1SMhuV0D2|E`{7w7m9@Q#NlobY>$;pSpm8 z9hlWJQ4#t)blzvA;Ke>CEOD=G7lNwcX}b(hgFW}M%eG;X0&hb zurqPMX^u;`Uw%400kFmZ$8Fa0itZ%*Vk7nL0^)HJ8!@l#3X~s>*4de)=3;gTXNp@P z94jN6^=JsYTkD$&Zz71FVdp?uURI6zQ^*dVC8rRmLP3DjV3!b@|MlsC^fxdjT6Qj? zxO4yd-iH$!(>Dv;F}@S}VTrSf2c{sv2Rq=(>TmzF+30}eV-u*w_N5N7H}!%5PntCH z0WzqyT53lC#Z+>R8|`9@@!k$VBvijCS&cks!{2C4!z>0-_-YstKXG5ia zTQ=N(uIl@z)BkP7bjpV8Q=!O4ZyKhPC@^tx_DOGDsuk@A6&doxB+VC;aAkomPWw^r zBAm{E9WL?JMYcT~1fZl9GyLQ8#`EWrE7X2XFjIog*3`hKcL=xNd%|Y!dnSlstY^vs z!7BNXQ$L<*TnupZ8N<1AuXyeGybY<2(3P6wffu@q>rM?g|(45a@nZBql2B{L{H`y0)wMp z^``O*OLLnVKD{KRf-NE$@8g>U!KeIQ(lZK(MZ8W+xy% zO$Syri2YAwt?L3@ipV}!=`OKjL7Tk`bFF_H%#L0)k@H3z>B^~bM?tub)p7YnD!9Wq zQ;9b7NB1jIF65Ie0+YgIGZ}i-hNh`M&SHJ+a8#4 zm_#c_tHv9*Q2&rge>p}%jGNkIidtU&Zz2?PRTqFL>uJbo=7^pN1l?Wq5BR!CYATcC zH3NiWEGf-cwUDqET@_E=zG+UlSsV9dYmc@1*>yXrDmZmhD-gMRfON?AL?ND)l@(~6 zV+AEHP;4l5Dl#s#w>Wrl^QeWv!eIzol}V(>RBTjK)fKYW@*8KTB6=0>D9_JN>S%(o zk znE`Iu<>V`@2G8TPF9|Bg*Pt>fDJ^ZVH(6{ZVIIfel_5x$XaR|%ubhhy&!^6MrBlcH zhGHTcYxu!=BA@ur#DF}Fp~Dql$Vq!lM^T@dh@Odel>stYBJIsP!OZOJYFpo$nwoy8 zMCxKWY3dONNV8|5(5)+$iNl%1!_E58EXA&1G1}^|++IFibVMiN0I&5NU`;&qjW%mK zjg8}S@BB=M*R@E}X;75&wV<-#ce(Ag>mJZ%9;6X?g>%+Z}W-sr~l&26xu;Ig64+?0lypw{KQ+ zrz#e>giC_)t>$Tt&zFOF&uY&ebcX1hrDx)XQY?BvB?*%)By-x+L_3B`b~RG)QEw=b z-7g^ZHt{#;T7b2{rZIVd>O1HvY4MXF!dMitXQ8C}y7O=g)oNoQt4jj|`en z&3Y957w~*Q2b$p7*2q?A%Og46?zwhy@;;KDh6eu3%nZm8u{n(!uT=O9(F@F{(O9x* zD)ZZ-nI_-{7+3x_8i->C4FWF_MSKUxNZ6+iG!)@do`p(sA}klvuE#Ul?Ks=>h!;IP~iQC{H4Fz;?R(nBe}g@_{WbQ`}L5&bIPqDnKqy= z@oa#xrOa%ifECvpBA)ds3+mEtm><=SO^BEOg_~L!80o>Giu(-&FT-4O4$B+?(|=WFGC^Mx9%?jw3oy- zRPv^@YK){Q=1%VWE-Tl>-vU{Dohbq~ax)7T(X zmkT1UR&{^`Ni%PkXlxD@bOwK_K*~sRrfa|>QsGq#HDdX8?ci=LB%)qzKXF0r#f>8z zqzo&ng650d>}eL>scoz%xb|6<+3b2=RoI`Dg5d?EkgY#G0b(iS!UIUH>7L_$w5zv(eh|FqzrW zF2G}>??DzD>$E`ov8j=j`}ez9@U%#5(M8PlKm34)0e!HwyRN|FB+du5*FkGpJ<8>0 zB>tV;F5fN?JRa>x+y*TMUS?-WqiAluDUR`g-~LfO}KFV5R0_XoXj5Gu{r$Iynd z&z1n#I4(^#grEc)o80EIr{q2`L zRGwpjmdL{{b_wK-Z?0qraYuj~`BG^+RrapHraO}F(6dPYu$98E4CX}6oNg}U&RMMx zmLD$o`}$y1AZc0yO|Use>g|lmYP-?eysM=W%I2p#E3)LI!m_AE$owqlt0p1~AQ);` zsCjJ~N7%fs96{)7V4qLJlHy8ECinH)_vBsN^MAa2gzw!*o@k<@7|hFNc4n4$SIw+; z>akeS{kYdpXX{Z%O>>@P$vSVqylg=w>OEy{H5lj#lJpqH*(10xJQ^I*?wA$%{&LhB zqK6|=n{LU`<*ybS!`>t-NL9%~jqVeiH3pSDp!PF`fZZUDWn{(KetZwn=z%PHHH&VD zW>SvrvD5eMcAhyKrFXR7{(Ae>1NG+HEIP1~Kc!oB2HoHFSn9SWB&*ht$i5)MHgrq> z?fF`u3FP>gO+9tLM(c}Ri2dg3Ry%8&AIs4hxfU%2skfDzK-}-Nxt`)R4zH^CBRuzK z$G3Qk*IiOA-oilvAOJK~aFgKI{eYH}v?gG&?PA^mnJkUogX_Afv&a-YTa=>gPY6bq zu&NvL|Bg;5xs;sc^jDny;gG(-wmlF&;F3m_Hv;oNTF%B5j%fU-_cj_^hKuEEZF~F3 zzjaBVLlh)oCf=Ws;+^>*tpxVEw^tl7$@4s#DM>>pduDF%He0FJxUaG7s+=6%3xE&*C4MHXJ={6= zZ7BztBR^^3^T>1*W~mntOaFyeR3zZA;$)e2#ex*F7zEk8H|&0xV~(xb-;;ZqF{6r= zWSe%e|H%8F;K+dBvO@Ql`!}CEw@YRbP^W&jgIn^Zlz{KLLEYc(=nq+>za0uA%)zD; zn7{dGa#;5cEmZ8lVJnxuA*{ju>6wtpm5Q4-Zno81P*(s|JJZ*2uOjZtTZJWR*y!;= zD>-$^rLOE}wcoQ)(f3SlVc)By`-2S_IUGDLoOx&Kj8A$rkv|-A*ij-FJB_~aiGl`# z=)upl7#x*s?wFC~bskM}TiYERn&1}3YXA3}gfEvfJWv@2f;WR!(nO@qir=46Dao{H zohT5dHqzqcAh2UU-k3101W^aN8tV)Gw@FxKBIe)TN)JwR7}^&Mn)~150U9+jam`hl z2d0@gqq}VLcV(yp-n|sjhJ#$kExMV(SCU&tzxy-_lCE4=_77$n`J1X+TFUWoS8$gz zVNhZYyFg>tTrO}QJrPOTQqmbkr!+*S_2k0qw12G3n`lbOP|FbJNlC`XS%=SWd>QA@ zyc+j5cPVkaC=&5-#<;-zp1Iz9?{NJKQU(Jk^qQ5B9JNnH9=4@v_1kXLZQa^hPWirj zNGWk*gQ9TN^D-S+%HxjoQ~14<;W}MS#&<)=J{@VNFJf1#qDWo7OFM^j;*ZtdhBpH) zE&SIQRB=?bufE7D(LKW-<9{XwW%9Nr!qdsuUurh?ND%)^)6wl;_I|>bb`5p(tC6|a z^Jfev%T>t1W)i$|tC_y{jngX+}j{Y;%7Ppq)}jR_rsMC-sK!B%cUenmEo|f zw{a=p-aDOK?8FB}aq(|BnVf3AKXD|1TUQ81l^+T*uu(zj(+pKeY;#>{QKS2{92{ka z*om7@EP({=2c9J{{PCf3CN@7KMjlt~+@tCb#e2RhaKEFUvG4J`jlPx-d zMUh-^wz)iU8egsaf$5;O9wc4Yh3esOr@#syXdN{Wg&H}z<9!TaXYEf_EHXwkF7X#uoXjITs4#S$Ual187Nhw9NT1cK2iVr7IG!9 z$#C_6Bh1DF^rbRu;#)#2b3&(x+pV8*X1bB$7d||#!wx)B74|)~4 z@??ZMoXyF<9qIO-zUh-q-MA7Xkl|XDLq2H}lWq0@ZE7%()_i5@U?GXIa)LUw`jI)u z#ir*$PIQ~aWiQbY^l%jm?T=#O9=u$c;Kf*x#QE%J)7SY{z*QhmsucC!(VrI*f6ho{ zWnmHXK^;lI1xRCP@qOWopV~WpzH6sU3L^Ed`$YsV@~jOx<~XPFiRJ&oM|$9Q9i{u(oO?TFqGwbSflz&Lxnw_gDpRj>9iDH{iRd7t^$f+Q$-rcS5z5< z;P&hJq;dGM9_%jQ=dYxB-|rBpc7>f35*oRJ;)0=sy*s*OqGLcfe>Kn<(B~gq<(*Xj>R*uNIOt)d|CFSs z5Z^(XG-hQ!v@Y1#FXMXgfXI8=O<0ILX{;QKO2G&{Q@D<#sSda^J{kDL2${8os9UP~ z4yl7;DM%?m>9kzRd6Q1bXR33f%RMVb4X6U&8oztlIrk^}eFIhba#hp#FX^mTir$Nh zR8H%+uQOyp`^BPsOLEr|yA0OpTN!tt>Wr8K+fo-)%~ z>qHILsacxq7XK3RM{fmx*27;m2clPql60Uw0cJ(9Jdx=7AFXszKBRhF7{eV%KF3xH zUQhiackT9N`>-qtLGbsVDBmBeFTV!*S3KCfYco^|MlH1@5biHIYoN06l*NG5n^$Lz zYY>p@FIie{7J32D5^L1k=K9l}$tF0(17_9jKjhF7nqU6{V);5)Alc8PSj zmDoC)PA>7Qc2wf6zmEJ@#By0W)Pqmhm7yJ5^l(+E`Hvq(1ZFluN4r5e z{b54|>0{%1BcbB?YvuHX&s zJRQ^?xVh=)azL+l+bU)Fv-zh1<-1Tl!R6~WV>k1h0ifrE0wDi_?Wx21W2u{+pk6tR z$7w4`^TS}LfH`l!t%0T#+g!Ey*~h=awPZ;G<$-l&&g{-!^Jp&uyIWZOnhI%4$rv+` zV$|O@R3~1q9f&$v7UT|Yi+rql-xczL{Vz95HR@>2<;eciVE%nV%LfTB$Z6y~HGUb4h_Ic{C~~2kg`XKKVrJ{by107js?$Ulvova`2Ab@~mCIK3QJqwD|6z z@8?vX^jr^YG`9#NIRZu8Pt+-0X^!NvTawVLYztZHcQKmRaZ(qJmVkWQeEZMo#mSmg z!mSY+lwrmpn8x^;5`F$$m`FTp)_jzOmIEPNO++fVJu@=q^Hq<2Nci{tn;zjW8LVR0 z`p;UC912pl1)H+Dl)oqC5^>5R3|F(vI-GSbJR9T|doIcYH8y?D{j{!-sw%#X5JjYw zCb2vGT877@;tUP@3qMUapvl>|DJg#;=b3I>VsQ?*?`zsTo?bRQhn%3gZ=EQoHuO<;YNYtNWLw}gEHWJ_tD z&hCw?JmIYtNl=S9CAt{51O6CdcsD|RQ}xl(0kQ3%R%XP_U1wbNgpu*&V`|5&sNl3Ll=pb@&aZylE@QI8(UvU%w-8itT zkgzMmdQADmTU@v?2*n>q4i`{#`J8?V;w4E@{`jONhd%{}^?e<4Q>AxeY;_yEw5^aX z^ctEWYnRbNFjroXk0Ko&%aZi|&S~QKju3f&Moa9EIp&`kaTFZJ!DP1uZQ71__)Pc$ zd?#}n%l1JRl?|r*MGd0lq)Xm{Kkb7Bl&XB*ngp7k<`hK^$QF^5YJV-&9h^Nnq)ih; z3THkM2EP0kq=#8}ZS08s3NN)jy)vPw(`7x{0wp(8pRw?@?@8FXMaYDbw!N2k%{$Od zET^we*4*3-F2pS#TBR*8!TREcg{+G_lN=6XM{k(B9 zba0;Ihtyrh4xiVyBFM{g(o^NtA81LZFsg{-JayvfgL7IrL4MF3O`n z#c=85y+QL_pKPeLu=Tk8LxPgd6Y=G%o@U+Zv}#CWpYyA}TxR@y{liU3K8lnin;`(A zE`T!H#{IpW75>2UrK3`}W@Dz*JW!Lq?DWpU%at>HpMVrS`;8Q%Z5}p8zin1yf4GP$ zg!%pv{utf|o%nmL;*Y#?6lYJI?hx|KhK5&yJkL#Xq6N^C z3@w`~!8WUn+6nAYE75UU`A|~bdv=WP`snjE9bT!Pd|}e=#?gl1>n0_+W)s3FbBzP* zfBWW{Km3~VR_~?oX%Sc&*{+dDbI^IS!{POT6s7eo$jOP9JYWgu7v3B=`kQ8)yW!i~ zV^Ub57Z9o`>vWU|qJKpXH5{oXi1Tm=dbqQ|UXx-@{j~ymBD-7^El%OFgfuyN zINLib_%t?BP6B-PY6)$JENxB?k~I&;d8@SM59%(hdr1i*qDBa#T;pZQM(WpZQ_%0C z%I^#^ugbY7BmA}e+|`g-ZwHr$KRB<-l8IgaR9*RKq)$04&_g;iun;rie`2Hp4bZbh z7tFotS$>yy1p50y(k4iJeR)?XaxW8Zdro)6?~aFfN-u+}GTBiudv*3hz&^bO?;fW6 zlR((SLMUB0){QF)j+%rFKC^x{;)|l5M$6*uNcWdQeZTE@%9bTBqUt6ep9gH)vQMTn zGPX^$btY`Ny~Zy3o~*@}Qn2Hwt;t3}+PWne&M@yYdbXr7lpyy4HD(nbw>~oVvn~Cy z9eH*0$gT6_n%rBlYlR3xqu)l<*glbq%AZ&)nH4&uo!_m!nU~M00?ql}Ai5C6;=N?c zHwk)=Cpcbyc;d)eC8&QN(uksUZ%&x3A%lC#C?KS_L{mh--!-GU|Jml}C~B6?iE|ED z;jbb0>VLFFSRZYhrwhy_HB&?ts0D=^z-{_`g$hADJl!4&V$z1MmD6RD8q8d2yZI`; z-=^AhOxen~xFZw{NN%nxQ=7SDwNd7kI3wz2J?7c4veZuwGTI>X++ z-+&VpWp)00?K^9}!K1TSa#-Hdg2k-;$nS4%wI%8$M*EEMGX3(k&-V+cZciZGq zNv{AJcttD0-zMk)>{(&Ox* zR;E~Gu&Wc`D#0v2lccp@K%a=}Fl?fP6Vmwgu(!mooJxW&|mPR5Q z&zz-Q23Mo1a6OY@*EI9z*V@K*(id^xDQMYrU%w!ov!Yyx9`%cHHlco zL8jt0buI(RkbpfK553k#MFPkqy_%$+W*{6y;-yaG{qVyKLC?d352STf%uoLNI|{Hn zE#Mktx#yh|xQ5>=hNmt1^`KlkDn*pdg)lUQD{R@w*rbL#9nvZV(B=zkOD`M~}H0JtL339i!L zfV);Ib@7Lq)=gc0$XE$nw&W1l-~Et|qlex5+XM~6-%k0q_G);&46d#kPSO+d8LZAz z{$VWru7twfA3qBt#R!3KZpI|z%)nb!!A78?ORxVTA%^gO5p|Y9c{E+OzHztU!4e2g zfZ%Sy-QC?SxVuYmcbDMquEE{i-67Z+p7U0HROJU1VCJ6bp6T9ut!u@{cRhAf0vqD& z21Y9o9vS|Rtq?{`MBVQ)-l%-h8kN0>$<&WB-l60?JWExDE{IG=3>N0`Emp3#SPl18 z>qxI2X=CX#PL{EE(lWt40+Xh)UXMsf<80pV5UC_toGdmrb#jAFsC2W1>R>K_!QFmJ z2>XX#4YxtQW8D71| zkpyZxCu?X{CzPP^4QJz%V@e123+_5)i?tQSJc_oyILmB0EfK{}-0tj|nT0ds(n$Xk zOMd}c8!xYoykDG8QMWy238)v27T>xcToA8ypO*JZjX6sHzffAC9n;p_oCqD2JcRZf`Pum?382I7URFc-qn(oIoXn%97^ZbX;`Uq2D!kY2u ziEElj8DP$MxScT)?tM>2EA5}UPzrEn0H^3c2pS1J&Mr^MfjDrouct=M zEO^I{Z^+m=_+Qs$Y2$YcC`c!3;TJ|U!8IkCH%CDKwDv{r|NZ>-T`c!(s6h}hBKuo? zz@IED(w81v;$UI@3i{fuF(gO_lx*O?44XD1BwN{ZUMd{`{ z8nF(_GT^9?iRpQMpXpc^eftyOlvGEOF9Q4Qy&*I34>4Cn1bv=2B0>uH^xcTpO1RE! z>Ev}^T{e5I|LkK+aB){bTS=xSCfq74aa(n0+_;{>p&IJtF+9|or9{gO#oT3VVt0NS7c06kJImT|-R0OY`Ez0GwH$s4dg z$0N|SRcYT)P*Tc)3bM1ae-LM8{n5aL!VoG`!jQF8|5M{HJ<=zAk{}z5UvO`5wn0P( zpMap%KtaTloSiHwMbubWD4fFU-2vbUW^NH|gg#n$@7AL|0z%FCGEw03=7ePQW<>+gt9S92vAx zqGHTiFUq&JTJ|*h`Fp{r*YtkniZ_eXv9r-yiE{}KAQWKmQ37Yq8D_3X-76AJIA##d zJpf4RyV_iDE^lruruR4jdon=tL6=VWuF(+j_WI0fmMFY+f#T~6c(?#$P%0t!A%612 z0m4r)R;VfiBQ#c#N^r2<)>+Wdgmgl-93)?u8t7H{oZYr(6D6WLZ35 zl$uyoz7OXsGS<>(aw2;hy$J$L?e8Avz~?$M@b<2c3yqU2yT<47Xo}6uteD@%6q1W4 z@_OV4+~6aC=Pw>$zfi@I%P4DVYJLzm+@v(VPneAjljRhZ{iY|dvkE1 z;wocK{ed;;Q33&nzv3-g$*C^ZI+Q5K$}HoH$KKMc5I!7=>DC%p*wkU1!6)k^74cNr z6dZp?kp3zIlZOf`PDt&VKi7UDnW;Z|b!T0H-;nCTNvmv^3?tM2$qa@n!P#b|kju*; z%w8x9ku)|x4qxx(VKDL&X%OUISn=5i-LKMw7Z0HliegzyWeo%CLbsTY8;`b9otedH z9!KI6mU~ROxxd+q!mCJ3OpE~o15*GXphf`k!U?xkEgfjWcdnGeLS=y>DacaEZHXSFh5?$hAvGo{bVuWrYAS>L!pHB z`N4m}yqc0|y+i2e!kceOuOe+cj(Bg5JS-vBBTm0wRVHoFxRw%-pe589*4CKDNHrTQ zf{|usgweUeFG{OpxXI@`FrO=Bro>lTs@f>XvlvqDoB#5+F8cQ|kbj>o+U5hj+Q{4? zcK=^W>gr*^!7xxzP-A6iq7bqeP#8m_3zx4VLLN`@EHLF(#REr8S02MAw!G*h!ls9F zC9&2jjhsb$DPzW1w3IkklKNg^=?4*DIF#MU?6~ z7qo#sFr)WXF|NO>TfL%<(+5?9N{bF@u)BY&Jjs=1*Kvvde=U1}#01R5Zoq1N##SoZ zNf~9D2sExg{|WK|C3CH%{VDi?Pdis@4Y<~R%EOWpAhEz-?iAhStloLRa#x!D%qI&J zQ!%>PaZkZlUt_o^CKmLJ{A^bC>`r92y4e{Z8e7sm#M+#vc6Hr?UNL%eGjmyF1p9`jg@_NZWX1ex||q5M*7bm;h36`Mdru3HiRHWg3>12iVxez4)$ zIxtRFW(Coy2-dKU#??X0dGL<*OkP5`*yG@F0QJc{SANZBT|h(+Gr7c=Fi5_8^<0E& zXg#Lv&KzJWs}>&;c;pHqqcSnT!92-eiFzlgEpPMMO`QYN;zbz-E>dt;Ug*#Se#FDxDm&~FJ) z=PUZa*#kS{?fN}rE57; zq3WGyr*wORq8L^~&X0!U!#{x*`E-oeu2{p)W>uE!@IH+fc{_e7nhli8!~?3$38J+4 zXMg!FxXYg`ZTt-#5$yM-gY{is&OlgauwOep&Jn0P}ij;Eo85 zTf=cs!OuZ{ioi=8Tdf>mP}7PyPb7CPD7=tl9*me5JQc3?WfV-Wl9Pue+u zd2=9y&kzXI!P06omR^L;n4%QXh~kF>{h4o1RegU*_h%LdvcZ%c?1S+iR_XJHdWhrFC^?E)JbVn!7htKZ{BwOnJ9L@B~*;iwxoD*)gb7%-X zn49=HEu4Wm?*V4TEu^)3m;ZIT{q!gMC9lcI4da9w`}#yK@S(52m#B5`;U~8Q;e?2jbkPCuSv^Mr{a;x$LHT} zd&28?PO}*iifk$tUT&k@T9B`dt;}&Qq!)eZo`AAk{DOa|v)KN^K_lynIR4h>L-F8l z--xus9+WaaQAtO`hfvl zvv(~@=7}oeprb<^!LIq)*{6thX?$LYyi=qW)%H_(@_u6wL+ z+t>iR5*&9svb|?or@KGNG-G6>~i2fLJ)bFYLFei}lv{fA~iola#@# zdOyn;buVprtPiV=2J=GGA3O;P40Q#N$avz5t4tUww{E)MCt(b_{OHyB!OsjzAqeggMO5AF2**Q{A|FC#5Z8%S= zRGW(m74X^{8!)45!QS>*W7ox|6#wS-cE(V^{e0x)-q=he`hax#Caf2Y<6*)3q^kVC%UjvIYRx|db5ly|QeM7X(= zz+HVer+`=IsLeaK#v@qjRdBcF((`zbbtW!8L-p()TuL4pe`jWW`&02P;U7bTja6sr zjZ>Y`Bp+VzJ#*Gii>-_{7b7*9`8^>8W2fynkp}>s_;r<;6ljLIt1;PBp8Zr5!3J8* zm+Q4pmhW;c$!NKhKp9K@TcohRFoe~zxyyO*eff_&Vkx+ynt`sD2l0-IxH)xP|yRjjfwvVD*-1q^tv9R#DiGC3h&Fd9?f{AxZVikI{R!dr_42=GO zZv%e5vnm!`UrzTx!4~Ib#5LEE16U9-DD0ABtHJhQWKL?7?!>>Taz{KuF;1FIi$gW} zRax2}BLzvUGI&MDvbQg^b-2>WctF4L#)+ItYWeBy0aYoqf+l=yPg8lG}@%d_s5lOE2Ik1BA+EY6H0R*Gz_ zW=_NC^k6hwuiezLcsYlGTDdef){g$Thm8l5OYOlaq)m~1-v$5+MoDraF^n7_VDq;G9mK*cY zf^FZ7;)!to8bvA7T4O8oZ8+gA|5c&yeC={LZ7%|Lksm`0=v?_F3V1C2(91aNKj zeU1#1h0%0+L6*XOMz$A1Vr>p%EKU{QyUCdK${IXdiKf7m%BfEvDG>EBH@qwiMn>P_ zx1vy>Jm?2e$mxm{#`6q+o@H=R+3HQKawBC{uRvMjQ{3lKugFbaORR&09mfiC+gv-@ zm_HxROZBpj_KGf+T^>XWtuipcpo9VIH)IWyij2*uqMURD{6>2Be{aSbi||vQfDv{Y z)jiB-i|x(f6wq4OQo~e)5L_-7zoI!bBLAp+%wK`H)CKi*81l&cS(kB#59pVrppFcN zuUy{BtR30=!bG)v7N}ctm_Ietd}x1R8yvZpGX9iQ1fW0i?n}ddF&#X;I^0MCAfGpU zNq0gM<*(deYMHwoqm$D#p+b{aEf_b}+_I}K+)soFG zsGluqR&eTJDjBBM$%KWEH%$0C6~p8bsb7;HL8FSSd7Xi~grI+)mGl9~=sdMFCuU z9AO53(KviKy5g?N^$mG*+YOc_UB&m~I`u%F)JU#(E<|^ISP0FA+Ky7Z?90_i7?<0! zvT?SJ&3kK5GJWnGv-VG*nQyY5x{>HbwD;@en@J7qR_U{r+R}pXL+1B`){e3)#s|-- z$Y)S3?s{rdaLNYNx5-S8G`fqiz=s3q$@444Ob@z{lL1bBCMri~T{MB&XaK1I7@Ol^ z42W((n?V|W0PPUa4&GL9AQLF6KFTqa?eEGJ1J>v&YFA@ANj7^N8)R(P+Oh^D&;}12 z7aY)cl8<5)ayUN_a0$UA()!UZ{6xQ*s;jA0a#kap)BPDKB(7G?>ZO)GZX=QX`s$g* zK9u8{C{VhEd-YYD)_K+LYXNRTL>RlG%1fjXn2ZN&gZAhmr!&eViciMIS^h@6S7bGn zV@1mhjKPvN84sFh$F{_VHZzw)w{RidvA=)W&pg`J99+c9MOFgWQ#=Yv-^z9Boimeg zQ4+ttR0`{Myz`FMFxr3IZPi~AeX(0h2-HHbegYVp@mS%!lGgn)&-9@Ci$=ZUXRG5k z9VN}N!sh2{!8*Nzee<{Un1LJ&0ssgTz^Z(UvGo&;2Vc|Mp_D!uVij3hal2oSi?VP~ zR0r7Du3Gi?IXM|_j>WjR`RUi{F{=TE{k?oFi!OaW&krcCcP z`RFh0dOEgr=hT^M+byb*(IhWNmY@bx_T}QD~(dqkLw$@S=)LrasLbR)u)`bRGT;@35R$jqoOLJf{&px--+S}bR&t&PCBkQk?M4)_U*2STaROLFfq*4GO}4nC_spYYSm`ooaA$O^0= zM?u0+Hd%kyBO=a@M$rGp=LB^OD@T*kdl=I3LIW#10C@SRwN;#$~8jec;|h2Vn%ZrDy&d+U}4+o z@(lW$rqyO@4(ForS&s#;P@F42!-dyLs54$A+33{ds^97%8e|rdeq$c-&Erba zS$~Y7I>eK`$QMSs5U4^3e{oIv9X>EGjLpHE_O9xMrwMSJ zu`^Y>?jXDrK>cDj6Q9GWIW!5wF_(jWtX^*d$Rjj6Zm^Ns3g?aY1;eM53@vV$iW(Zrq6 z{4&M;prwIUJ-M6bXs7Xb|6q)X*I6$kA3qq^qz&UJvaq}lQTRhZbrTV!!e1-a-OJgC z2DXfr>IPJ&|E>@iP$$j4XIkizSjBY_tK50hiBa*btz=Z1v$d6B9$lBnhIFR7%{*0A z6Ll+%lwYMBjD(LwU=b1l_WWDY%2%B?Pbckp-ca*zZLrz+?B}Zf=tqw({&Dhf!6-8g zjjYw!(AkwG`Bao?v<4)hhue{ID!p4)C3Sch0p9V{90V9eSUbCC0aIhJKK+U-zAIB5 z7Z^hP)s&9?^s>%|w}oa@r z^@9O`!-Jh9lwZRT857rPe=s>3U}(Tutw^hrGCKNg4JJ)D^VmD`$6hmdn*zq>&m#_F zN4NjH;Sgwep}E9Y(*5O`L=f_wE3@R{=E*=h`f7;QBeBt&wxKe$;m-TOUAz}&GI7?7 zk;@E-%R;ca`aAD25^EV9&i6zQQnaF*9LtoY;4OpCTxjy>izzs+Iss7b1ZO~Jy6DefSDtq>Pn6-Y@*=JBW&fYqNDtIFIUtU$|z$v8uc>;j@uMnEtjK0Bhl|1>cwOA z_0R$f6G3WO=U_PMLkIX*n~*NuVyiFqOGZ4GEfaP-X=Q)ojS{v9w*=+TPPH?8GcjEs zQOsqe)sZ;8WXR3j;Xc#RnL9{h?5k6-a_aVoBGq?F;rK42NFzYg;1qMHHC?}R(LPS> zO#iDW@IynVRopz6jVzOoiffL4TL_-Y31+OAg9?CDZ?Y9Y{452FYX7$m`tpWrFP-%- zPoKZDVs!}vp)FvDTAgwvfv{%T+QxMpIT}w^m0N7Dc7gzS93_7hodfDh%<4Xqbz((y z+yADt3H|zS_BX4wKVJ&?AT8y4i*+}mp%S@RlThYW5nL$$S;@0>UHEdv@H$DPt z@W>iEAks*AcK*`uQvIbGqLbUwXtPz!ql!N4(S`jxjXLX*kut0vt2veVVohNzA-@{e zix#UrY>y~29k;f45c}^V3lWyXYlenBSsyykNTw$e$Hk3fcc5D#$~&G&sVI3m&r~3W zVd`wuoT1w~YHyKuznt#X_8DbKKui!4LD|g8WJz3*Q!0c9hf&t5w`!gG2fgRC=WUJb zU&}?o1D{C49%Nqsh;HZF%as$cOviRq2ikb>Vs37A?jRU?qlcYdFWn{=cY`IQM3Ntg z(l^5UA`w&MXRIQ?$s=~;K>zdyR-PuOT+=ewTM;YU_P5jOt2F*p1^Zu@JfBm7W^)nD zT>g*2X<83(EV77B_hO@l9S-wNTK`bPt1s6^x5KF`@AorW+V42RG2G;MgD%NYL>l*9 z@fv(d(jRZD4S}7dFrFXqr8+B3CD{8PsE*3Wu&e(E46dg)BEo~P-cJ$tB&H58eIh)0|IkRQRf{$ZA-%Eb zWv40MU3o3dwc=4uB39|>ts4_YKrFIeL+|EBMEq3gP;XGOUwWkVSXM=XyW~b2z8)T9 zetmT?)#!i4ieYDx6lwSpSL7rKP{lrAWy*UK(I}G>c6c#*SXe~n!I<1z`PVz-P^Z)= zCNHCQE&=XLZpe|sKPwE*2t)@Y5xb^lt`HT9MF17n&27WqEE7Pwx|U$oY^(^!lD0fN zi-`lvdl98~lk0Jglj4V-fIpBn;e7wKBMqXGS4lrfL_+A~KSpAIt)j+a?P$-?qonZK zN)S@vIly8&iRoRHfnQWPoS1WdVlgYcjZDc`dIF<=lwTmzx^HeVEst3N$I9&&d&aEh<&}W$UMawv02Wb# z;SDbby$*KEG9D%bqxS*2KSl76uZ}VC#3+z=Q(Hak?ydi&byd84P5K;xQ{0_wS3I2L zO$(HevJNx{(qyn9F2{&EG><#rEg~^kWmoj>iROBT%N%z42gU93;^n!L6S#c7G%^5& zqli^cA&O}S)jGh81W+8)dx!hmoFA;W+Zl1a>apFAU`<0veH|^V@a31U7u$GBjsHFX z^vKoUQdzC;#i@IiQm)-;F*H!Ra(A-p?#dO;oQZMhTz93MobxKS3a5oFBz_7-&1ef? znXOkK<&qR3MnLTh_|mG>w@#v|4rd!;VR$1=Cbm$S77q`+f7=kf1$&2)94+X)uBA3l z|1CeN7kOUmlt>N|CKzH(k{S@j+*EW zveR z8G*PVa^RcUc2|I%&;(HK0P?_o*ZOCk3?flQa+TYGeOJz#2XbqeFFu+*oK0ov`!9Vu zgkwRox?Jp%XzRDP5_%g%-}B_c)!W*1g_L-mtjqN!8xffn7i}yC1ESp++N>Csmdwd9 z1U`lj?c=NP6H7TV7r%lgL4?k+)tF=s9)_e%|*Ds1EPWZU|b*A zX?m3uC25B@zur55%!C2Bo<9w@JCD@+nB@MNoa$(V>9j&HT2e{rtHy@oSpO)T_`Sl2CoPZd`u2Sz3MIll18-LG5uRoj(e5Byl!ExR@M8TU!TQ) zHb2)lK=wVC5!{tqCOiJtEMU1|jssVep0UN(#CLgV9p~cexF_ynH4eUBPOhgaGFO8} zVYMwL{tc%m0cemIR-4_!81-5tc~YCC&Rlx_3xzV3hm(+Zy&uPaK@w`~DCb;)G{_D* z3EH>2%ns5LClAs&$aDJpi{)UA2J|wNN5-zO$AolIazGGbg3aR4R#S(kre;oVuI2#! zT_->CThU0|;00wj7`j>jy_v9ckn>++#-E=sYGbBnNi#8f1kHp_nX5HEHh zM+snNnA_nKY$ZfIt04KgIM5g zl7Cf~3M+&zX3OMqGGg|M#yqQuHbtW?-WI{;77g_*S zm_twV4*U_`zGh_la*Csnou#S?NxTE#B0Jgg6>K8fkJc%zPR{R+G{yh#aOwIAY%Y9T z(}XFA?c8Ar1>7awqS-uI3xt0^l%sDC29UJ0=HynYc%3rD(aR@5&eB6kdZC@;q2=cN zVJDcX+G<8KW0{ilY|Y(Kkyk zM%Z>O8y_^Vk$X9lm!>E^R|DmUw3&gaHN6O`J?OR0d26JwUiP2Zgx0pb9L(>g0o6_+U4m%TuQdxJ!0iL!qDcIj|G@7 zg47>F1;BW!n&*nR@UTi-3>Mz%^5vwNe9BsDPiQyVLc}e#mWf38w6o0V4T)J zj&PwpNfL1rXvRmvFp&ZVNgpmHe4FPWVNd;$CX=vCEF}g2$gNJf>)fj~RiS97yY8Nv zkvl6c1$@;nZ$H^VF^dP^r@4B#R2}F)JBw5F1WOovqL}!u!l_&~Q)@y??G1RdZ}~x9 zE1NRGY=LG54aMn_bxa?FU%KRy4{f78Nv2MNR-yAy6k6}_$FJThO?k#!OL?~`cggJK zA4)}YW@6m-?;9tCmm(&UC3vEwU#&7+FA5ky%Et8vYoV=*(z6`>I{4kW5K}Kt!Ei+I z6oZl2Soivhjhgn;=Ce`c>aZW+Tyj~AN3#LFmj3u(GyY!RlpwPQ{}x|*zP$a@Hh#zD z6Af9WOpE@0qcE)U0>XJPJZXI%TxvRX!*;92^ z@2Kc>sWuoWD5S~oJOu#RC-MLtsMz@IGqo1@jf;yDUF@H&!zx>~BE$KyQ6n)o7(c&R z=|vBtLt?N)99ce58+NAS{fey;JOz-d&RPPL&h7^aMU!52;+S`&IbVx7`*LLfM9dZL zdg6AJu92Ruh2;Z?yY_)F0{z{gm^!nWpi$b^2p}&b+4W|>gg`2U?BULTC3Noez|WTh zc~&sHUtFf)ce1WQ_c(={7FyWUTCvk#ku66l@5>r$AnEvN(y8oWjW!; z4F?Pxmbz=ItHT2uy8%lUR`ZVe^TYeNGZ(LMW6tx9O52Bm=5nbcXdh_2yw-#3&Pr~x zg${}KgBkTXXN`)C>NHJro9F$MT7=qW=R6Y&xOD&Ylt#ML!)$o_za4+GHlnt$6*sqM zMyx?=0msj&_)LgcUUB^-(U?^%MFcuDmnJy|Snuv!X=O#u)?vkCncNpEQPULDuBgGt z1i64(s_kh%4M?T9ir)b;qJBwc@w^6X0T~_xOH0i^NZ_Y*;obe!^1qq#aGfX}@7d3NQu1zqqV)M||-QDwiQTgRdw)mN- zlhIns7oLB`)O&l#Y#i!MJ5gGQ1gXZ@&IjxnBO~J0ZCAqL;^MZ>&MKTyRO(7#E~VgS zssOUT&}1UhF}k*<28YQM6$oMid_Em)XLaP9;5b-V!ayDx56}I=mBmknv@se$&<7+( z=+fiNM$PDFm~*O|)8MPB^l4nMU#?wft486!L{E`1Sa@P+wU#6_J(W&a$jf0=oc|uO z3Esvdl9idleQQzby= zGZ}>B$;g@9Ni}8CXw3E8hO?|}^PA2SF%YtG0)mDBHDb*Pc$HE+!bR%^Pete3znLg> zTwJvWXJut9AnqdvNCfc!^q3~;ogZNUTvzH8{@q`MBp_m}mgu9_q7oA5b0&DameBn} zm!ncwaDY6%f12kmtq{`MHSID74wb__eF9GS_7GjHbYT8DS_;AzXk6e(FOQ8R-0n+@ zskkFM+}JnPJ*M>r^`<7RS@@jo*hxGHGV8yR-#yRA$B*wdna=6G0|ef)jC)I!OgrBR z|J_^l&5nh8jMvqGPcAHA>*#E_MW&^s41a_O0{V9#W(!mOk5)r4+8@>97Rhz*`t%7e zgxrYb6EE5B5Ow-Mb>gJ%);QlT1 zWdiZ~X{FAA!{v;Tjw?3tX8*bZW2-mNpQbJU*Pa~TO)m<)mxmR3HlQ}w+jv!5hG;E! z141c2Vh{E7^jBGW1Q~U<+Sf3vlITfy#Me?#;>US6wn2KgHG;Au0t|ZDNBEjnV)Us&xc=C7r zfv?N<%)I(S6LIs{fR$lrBAAdn+eC(KUoyqCU40I%>z{fHFg#EQcY#=LLCcm1Zf^6YviH&5~0d=z^53Q9OZwUtcJ zoS4|y2+lg0<7o)Xj0SSBDjluGRcx$B$xSheSc>-9`S)*~ zjqUSIWH%?TQu+6zehqLMJl`-30zgvmH*ZZCYB_LFnwgWn)=dU zxttw+`WJC;Y~)H;erR@)GM3s|n#|>jw-zMPC>UwgU8@1yPb3yi`G-=1qW-3RC+A$z zp0Lc|bBZ`dI`gZC;I1xURm$@O*>abSs6u6%Q<^QFiOs2-v;cyiifv~zCb@V;6XJOT zJ<4|>*{!%bPr%Y(0iMB9{7GpHn8uHNUW@kkwa(cjVTcFq)NYc_dOh-trA8DEfJ2l( z0FU&i-Knn|jOlZo^LnU;6cP#rhb2}T{!{~mJtR;{*Z1}li0(qne9A+4dfX8t1?p(R z%8_&zX9QoryQeMY+8}@(VrT`1cdo9VT5`Rd1m0fC{Nx|4&$yRMl`D1n_H4)`-?%er z+GQ$uQ9KB&KG!t=D=G>yCdJA3^Yub=^eugW=CP=MdOFwGD?o>%<`7~(lUEw!tR zUcZdr;$r7H3AXNG?ntokgESum<44TY3nk7&vuk&}Ks+fLoyac$(Eh)+Ob(H@1ld4k z$goVaoG*1pD!6uwF=v_rL`ZntuXb~dXALxCe0telBZUG~UhMOnbC33l`{iNBqZ|+S zcVhr&tfvDkSaVaVl4(x06DCO8ubZ~6C~9E4MVV0|q)2O*lgJO8dP!D)o+X1nKF&*l zAY-JIyqbaYsgSl~(H8?TwLQmZjskw_5C4~Om%cCOzth|UR8}nO600?Ly&-;}sr7e- zH*o9Af*~ulP-eX+t#C7@D9q#m|3&e^61KU{>iZ%`OiYgbm#?QGz;!7u<|;}j+L$2g zAlYRfwkq4On;6Vx1fUbMHEWL!c*LUJ zK~X1pEV3~=IZofcp89zJaHrT)L~8$@IDo(KrN;clHa9l7l5Hr_iUS%sVMcFkE)jQS z_$w*^On3#hh6J=xix6E{5-Qj7WglD-PoQ)dLY8d;0{TSUFu}>_~I`&x+LIEFHM06 zFVNxp`WZ0C3o!X+bdvp2+HeTcJQgR*!715KY?8`Xe)@H{ORxE|EB)!aZ}Z6CRcs4R zFXSqt;a7RQ$zs}OoRUIVBCI#{ypLP}?vIIWc)=Yz0G0GiZ)1Q1uMWVas*ffZj4dys zq?b|ZD4@w9o?4cXvpENnm854{5SuEgd-cuJz$VfUsQT+sBPD@dz-Qk%OQR?9&|BoO z-R}25g&C&9A9Y*1b_SaB@%fjyGk&3EK{c5u`^6<_bRPGA-kiOoRRb@l1Bxq2On z1N4|Fb6`i-=lbaF?o6nqbOWJ%N#t{vb5Al0`>gUI#(T5*DictE;^-@q>Z zXUZ&Lg8duAEnP;u0umq57y(AS(OQ_U??INo7Pdv{=`(7Znr~Gni8?{m)aD^Yl+b>@k9}-DR~XHHDU_IXQ(h!~IyO_ zp!|UaJZS@z?wD?_*CJ}LMeQ_U&xDG0nr$)QUTjQF&-NTEG3N`#-?;YDgMwlw_*GEO z-zNQ!ZK!<|Ht?<3B0V)(N20A~L;HMM|nUj1cyd!ks&s6U>IOd z);4;46Q6i02z&#Hl5Uux3iKr39d&xPdnE_ady(;xA+tH>Aek3%r45MChS4P7I@`g9 zcvlh1<`~jN`<#nGVSnly)L%4u%%M@tbFyg@t!J;YoX_;nSm6YPQl!bX1`1xm4lxgN zH=4YHdKX8Fx6Vm*qv{53-ez=gq)E0p6+!(CVc~kJ0$**gB6CB(OfODk`gi~J`UT>C zzGEVgX$hH6RDRp2y8{U_CEx%C!6`JU+gu#msI?X7gKM>smK;J? zP~}gys(YW0;mm(wsE_A8w9aQf^kaQY0#a9Ub)j5aZFJM29dY+m+$m93^CA;00^Sz5 zh%)}qN?@|$bjSFqP#9N`p4XuhwKPLd4+e&GgyKJ{f$8l0K07ohIl z`?&8DJ%4{NQc-o0hKZICMgho@Guvh_+{F(*`!O>TZ|mQQ-i|Pjw-LGj&j$u~Ihrv@ z`_Olub*u+-!ktCsPr)EnHan6?=E$I#5EoudzANs^ zPUI}Fks|gi`_XvFhPnIRXe?vyV|^0?9c!0c_!+qV@N)GkbRR;+rlPpL+*+D&i~~6k zyx13dwnl-u820xwaZ>$v_@#4fGTR88YqvH13Azd;PF3o&t zygbN!khE0rRDXl2DT8~v(VGoG&R~{vev`xmh0#B+kV;B^r@-Oj()?+PEP5jS4oUC3 zcUnNSM6_zmk>gU*1?CARD%KEkTshY*B^FsRkY#)fw;w4opB6>3eFuyuawb|=%+U=Z zpRAhrA0B_@$*Lnl1E6MmulIt`!y1>bJI`^u*6q~aC%%z@L`3$ZL>|P( z+fZ_V2Fq1v>;&5~jY0io%yZ0F*HuX+9GZ90kzQWs*G1@WeGpVK7+vmIS{7`feNU}) z{f+x;_N8H^9VRwt?AV?HVJe(&V|LQgcTW#dgJGe85r{5Fqz|2TcBtqEEcq_qNBzV{ z+s`#GTnNBf{x~~72&@(e?hx(T0O##I^HulCyjo>|!FA2BC$0zXf&dwb@hb%^KC;Uu zn5iXX(Ts!SmAs=P7}K`bTNBHvUs@2Jw4n{Jw*?a&qR-ClHwpnL*?bZv2nubwE11yp zYnbzz1GXQehqlEpV!`oA&JbN<+<7L5jOoI;>CBLmf(T$Qd&3?dpEnggJ7h==pZA>B zCr1%|QVK>=vW#6>=-VFXbgk&;H8DXBI0dmi*52MZ?k;^P#hRKqTjV!mQh;Lw!#DZG zlSCt*L=hw;IG&r^qXo+ANjr2@qw6(;{E~l+1G;!!U{OzFMxeN!kO#v+V-Jrz0Fm9! zYZ$5Ms$p$4b;6z22q=MsIJ)jH%#o0gfF!~lTeig-qwpg(;%OnKrq9d9APZ+KUA30C zax>&${}pdXOwe67x1)BUD9mC9Xgv9ce?S1_%n!l*a64f(yXIA>%VYQFZCkdE12p>| z4rNX{sg>ol$zgu*wR(bW$$tWA`C9{S*Qr_K|aZdTPnQ$Vf>+!B#oQ%)~Sad^xq&)O_+OEF?=!P30gd zwMxh5jWifE#pfo!S{5uVU1jtkA!^YySO&}V(Ec~q;$sS$?|ed1b1ratbaq2%@8A|O zI5dEwCnpaE(z~;MWwo_&&HvF5F)|_rEcI>K8@xbPJ>%OOknyWn8$uYHmS(}>Co{?B zq-l^xba(d-caSTL8N(y9A z3M$5rqaB9;9}&n*ZN3`7gF{4QA{ewV!{LGN!!!gD>e9|r%0Q9YHCVQM8x~T z3jp32j;FI*H6kD&;IdldQ!AH$p{70oa*#PP*d}bPmVmtYb{uX&gCpF}gHb1d68lmE zwlEW{QM>f&79_((mxh;Wr+&`c=j`EQ4nax5TP!VW1z5*bYGnbQ4n$7i^Fg=ZJ*<$5bU{)E#beN zgO7(l9VQ=Ij8nOCxMO%D#H75sw8Ecx#1ksF{M&31K4y?V%n~3vL&FyXqKwh+_UjI?s)G3$E!P}Tr?z0Sw@cc_udJ{fFTpm#P91%mL}VF{CH3;!yzK)58LZdGc~1k zs7@MvBinX9{2~;-zBe>U^r_C15d*`Ac8k)!+W|QnxLj)nEi)ea)ms*YCLR;(_HyhQ zhQ=r)jPA)t--xxP7=4I|&w#s}^VGL7_^e{hnl3EZck|z2%*@Plmh7?lL=5tPB?fUO z@OA~xbaj(W{SsyqhL`B$eHC(00^Hj=g}UaDeRQbHp^XRNsYAT&3al!L&lHLNSf_!N zQ&%wk-yc2l{^hMM)5dKqO=$UlzyHg$7p)xKCc0k(0jUTL?TYKa@epNAZS~yjArspD zjhHy&CzLF3d-xwTm+o(VXC0zeg*+1e_hayl_!hvA*&MOoW1wA0|95Eo$LH>bEG_Rh z9e*)FL5^VH6(P#`)B8U9e$YJeaCG&B_8t%nv9mq-`v0}?1H3_>-B{hI9d z;0XqH{wyTu;bRQkE$LKxFlgjHU{43@ShU`#2Z2anPL>oB5^M%7E4YiOZU@q3#}A4N z3318%IFxgL-z3g_9I*(OhS%G#Qgph&Cbp2!9~qK@eFh(QrGxrM%=G)xP}z71J`QWt zHSa45#{X;X?Bki<+c>V3oX*|J(UTUV@Jh9bC}&1=PGgCta`MoFRM<50P?UL`^U^46 zNey90ao3@m)es>kPtC&}Li7AME6;{B>b`!xdi8pp`@j4C>)wCgm)Wj;f7kcAuJ8By z{XXyY`_J8#x@l=?WPQCt6_a~D&*n`jyrSqZn?3RI(a{v-BBlOX=C@=^`dLM~2bm%* zEo0>ET3Y9f{!fO&T};67>R_=ky}hn?v$Bc?*rv9mwJItq@c2;n@}y}XPaCJZlZG+`oP~N*JEMgd5#VaRzzZ?bdwbxr}?nV#KeSxqxRfROswYZ z3r>Py5~Hd=9HFG4&?yK8CU{-4QPczb9NrXvjG}Z&Xg7j%^ZLLo5 zkDi?LZT2hSR=v5jhs(?2%T|NUkbZ6^B<#@F&jB%o?14>v_C`!)^N(8~9nPD~TOU4r z|KYRjXH8s70Msp)f?FWE{a{%9{0^KcJ+tn7I z-f-Gq1Q$r1%+G>G2VQ-R24hgA`DjT~yf!0OkoU)R#niImCwjp<@HLaSvCYGkWV z*^==(UGgA6peZ1O69P`vd8u(<_IbuLw2{Y+8#i1DtTyp8nrdEN9?qpCX{@7Q=EIpU zcM@HKu0fZ!$^Yptdgb*EX45s=X0t8!Ufm_hw8t;FDL$nOsgA2|Hs?7DSkPDIhb;v` zrQ_-C?J?TUZ)s^sVh_Isprup3+bTyF#(n8_mj{3ir9CQQK|$h>ccPMwF%1M`#uz4e z8$#}b2et3JiVJ36V;?86`;E*$_AaNDGnci_jdkdii%S>+=|S^8K&}g#nwo5@N17fz zf4>uJYAU})U%qnn6JB8dssqtfDYR1AG_;MK0p3&it+BtqwV%WsE)Y-A6n6ehSW~&Qf2bOF$TcB zN4OT=g?%gG614_TydVK(9mwmt+qSU)mOeQx(obQ#NB)?hH2Q7b^TIxPex55RQ$eF^ z!6cKg#lk-S?#g@|s5TM7)svHx;SlOpsC;e;TAw%5(&&Uki69Vh$i@Z$BK#aLPF&m& zH0M~y7#5=Zz)^nnCRjbtC^f`XV_(3}0#?0|Pq!J^BL=k-=ciN^m>>y+I~`Nglqij$ z=(aQAUH(sF$cmFlH_aEXE?M*$sET_k6rv_Uoyrd=s5j%|Uji8w+`448^(5-l%uN zjUcw9bkLTib3o1IM)5?cJXL-#JNwx2fe-Orn=|7LD;UTr>Bb$7r%r{4 zb2C!Yfv^FOB2VxykDWj&;OePEDaQ1>g@sA6v9UtBo}M0pweML1!qc8TupRI*$efSn za4d!dn_YX$BTAO$I#_FyvrvI+z8CFiG#Z~qzC6)g4^l@8)C(8B$2aMQhATK#5bgl8 z#onqLHyV0(;vI6GesyqgpjgQ^OH8@FF5|AUkvDWfQ&`v?nMGno#uh_E!4Q(ZVofHS4_&5G zw<~}q`{$@5SRWF)>C-xwK$#L_}c>+?OuMhF)n)QGzN} z{M2)CiW5HgID82UeO$WWAq3SGN=j;E$?1xU3bN^ye7GBdXlW@;ef{SgzKKdeA zn-jixzI1UKq{^JniL$hJGBpgrE))udd`Cb)fSbGfRIP6M&uFwK;ZA!CNi+0S_lw|n z#aAXghu-Dm4m@~XT0H(dCO_Yh#~4OmzkVILn*fxyBJ?Of5c&sAj%QiL5!~hA<3PM= zs#MlNtyVLRz0%EyN;aZQ*BLe5;Bu>c!e-Rq-WpeMMadJH^%f*6VF_CgLc)0Efs|N8 zqL&ran*Y^sFp|sN$Wqp9NP};CeK}ENdFrsUmyyL^;GL7)?aDXXS$)wEzA03GyaFiHbgB}$>i-}mSbP>d%ZltH%?cdLo9z<$G zeXt?*C9K;+2M_+x6DHS-Qr0xakch9czV2ljAP&3?XgggE=L>=3MG~r~C!!};)#sbU z-lo;Bf^1?I*w3F6!lo0_K`HEaBs{t>hBhFXq9l+nrG6_wr!jVBQ!5}PnaM{53gA>T zi!+TgF=rM5|AG?q5s7BRC??d!P6Cgg5Qm<9|2B5h?*3cysyH>$rAvY}iZRi4N4_O$ l3~J-w6ZU`P@Be+c@P(GNJ<|G + + + Graceful motion controller for Nav2 + + + diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp new file mode 100644 index 0000000000..97fb2a9956 --- /dev/null +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_ +#define NAV2_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_ + +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.h" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace nav2_graceful_motion_controller +{ + +/** + * @brief Egocentric polar coordinates defined as the difference between the + * robot pose and the target pose relative to the robot position and orientation. + */ +struct EgocentricPolarCoordinates +{ + float r; // Radial distance between the robot pose and the target pose. + // Negative value if the robot is moving backwards. + float phi; // Orientation of target with respect to the line of sight + // from the robot to the target. + float delta; // Steering angle of the robot with respect to the line of sight. + + EgocentricPolarCoordinates( + const float & r_in = 0.0, + const float & phi_in = 0.0, + const float & delta_in = 0.0) + : r(r_in), phi(phi_in), delta(delta_in) {} + + /** + * @brief Construct a new egocentric polar coordinates as the difference between the robot pose + * and the target pose relative to the robot position and orientation, both referenced to the same frame. + * + * Thus, r, phi and delta are always at the origin of the frame. + * + * @param target Target pose. + * @param current Current pose. Defaults to the origin. + * @param backward If true, the robot is moving backwards. Defaults to false. + */ + explicit EgocentricPolarCoordinates( + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current = geometry_msgs::msg::Pose(), bool backward = false) + { + // Compute the difference between the target and the current pose + float dX = target.position.x - current.position.x; + float dY = target.position.y - current.position.y; + // Compute the line of sight from the robot to the target + // Flip it if the robot is moving backwards + float line_of_sight = backward ? (std::atan2(-dY, dX) + M_PI) : std::atan2(-dY, dX); + // Compute the ego polar coordinates + r = sqrt(dX * dX + dY * dY); + phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight); + delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight); + // If the robot is moving backwards, flip the sign of the radial distance + r *= backward ? -1.0 : 1.0; + } + + /** + * @brief Construct a new egocentric polar coordinates for the target pose. + * + * @param target Target pose. + * @param backward If true, the robot is moving backwards. Defaults to false. + */ + explicit EgocentricPolarCoordinates( + const geometry_msgs::msg::Pose & target, + bool backward = false) + { + EgocentricPolarCoordinates(target, geometry_msgs::msg::Pose(), backward); + } +}; + +} // namespace nav2_graceful_motion_controller + +#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp new file mode 100644 index 0000000000..7a593c0363 --- /dev/null +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp @@ -0,0 +1,180 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_MOTION_CONTROLLER__GRACEFUL_MOTION_CONTROLLER_HPP_ +#define NAV2_GRACEFUL_MOTION_CONTROLLER__GRACEFUL_MOTION_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_core/controller.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_graceful_motion_controller/path_handler.hpp" +#include "nav2_graceful_motion_controller/parameter_handler.hpp" +#include "nav2_graceful_motion_controller/smooth_control_law.hpp" +#include "nav2_graceful_motion_controller/utils.hpp" + +namespace nav2_graceful_motion_controller +{ + +/** + * @class nav2_graceful_motion_controller::GracefulMotionController + * @brief Graceful controller plugin + */ +class GracefulMotionController : public nav2_core::Controller +{ +public: + /** + * @brief Constructor for nav2_graceful_motion_controller::GracefulMotionController + */ + GracefulMotionController() = default; + + /** + * @brief Destructor for nav2_graceful_motion_controller::GracefulMotionController + */ + ~GracefulMotionController() override = default; + + /** + * @brief Configure controller state machine + * @param parent WeakPtr to node + * @param name Name of plugin + * @param tf TF buffer + * @param costmap_ros Costmap2DROS object of environment + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup controller state machine. + */ + void cleanup() override; + + /** + * @brief Activate controller state machine. + */ + void activate() override; + + /** + * @brief Deactivate controller state machine. + */ + void deactivate() override; + + /** + * @brief Compute the best command given the current pose and velocity. + * @param pose Current robot pose + * @param velocity Current robot velocity + * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands + * @return Best command + */ + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * goal_checker) override; + + /** + * @brief nav2_core setPlan - Sets the global plan. + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path) override; + + /** + * @brief Limits the maximum linear speed of the robot. + * @param speed_limit expressed in absolute value (in m/s) + * or in percentage from maximum robot speed + * @param percentage setting speed limit in percentage if true + * or in absolute values in false case + */ + void setSpeedLimit(const double & speed_limit, const bool & percentage) override; + +protected: + /** + * @brief Get motion target point. + * @param motion_target_dist Optimal motion target distance + * @param path Current global path + * @return Motion target point + */ + geometry_msgs::msg::PoseStamped getMotionTarget( + const double & motion_target_dist, + const nav_msgs::msg::Path & path); + + /** + * @brief Simulate trajectory calculating in every step the new velocity command based on + * a new curvature value and checking for collisions. + * + * @param robot_pose Robot pose + * @param motion_target Motion target point + * @param costmap_transform Transform between global and local costmap + * @param trajectory Simulated trajectory + * @param backward Flag to indicate if the robot is moving backward + * @return true if the trajectory is collision free, false otherwise + */ + bool simulateTrajectory( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::PoseStamped & motion_target, + const geometry_msgs::msg::TransformStamped & costmap_transform, + nav_msgs::msg::Path & trajectory, + const bool & backward); + + /** + * @brief Rotate the robot to face the motion target with maximum angular velocity. + * + * @param angle_to_target Angle to the motion target + * @return geometry_msgs::msg::Twist Velocity command + */ + geometry_msgs::msg::Twist rotateToTarget( + const double & angle_to_target); + + /** + * @brief Checks if the robot is in collision + * @param x The x coordinate of the robot in global frame + * @param y The y coordinate of the robot in global frame + * @param theta The orientation of the robot in global frame + * @return Whether in collision + */ + bool inCollision(const double & x, const double & y, const double & theta); + + std::shared_ptr tf_buffer_; + std::string plugin_name_; + std::shared_ptr costmap_ros_; + std::unique_ptr> + collision_checker_; + rclcpp::Logger logger_{rclcpp::get_logger("GracefulMotionController")}; + + Parameters * params_; + double goal_dist_tolerance_; + bool goal_reached_; + + std::shared_ptr> transformed_plan_pub_; + std::shared_ptr> local_plan_pub_; + std::shared_ptr> + motion_target_pub_; + std::shared_ptr> + slowdown_pub_; + std::unique_ptr path_handler_; + std::unique_ptr param_handler_; + std::unique_ptr control_law_; +}; + +} // namespace nav2_graceful_motion_controller + +#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__GRACEFUL_MOTION_CONTROLLER_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp new file mode 100644 index 0000000000..ef2aebfb99 --- /dev/null +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_MOTION_CONTROLLER__PARAMETER_HANDLER_HPP_ +#define NAV2_GRACEFUL_MOTION_CONTROLLER__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_graceful_motion_controller +{ + +struct Parameters +{ + double transform_tolerance; + double motion_target_dist; + double max_robot_pose_search_dist; + double k_phi; + double k_delta; + double beta; + double lambda; + double v_linear_min; + double v_linear_max; + double v_linear_max_initial; + double v_angular_max; + double v_angular_max_initial; + double slowdown_radius; + bool initial_rotation; + double initial_rotation_min_angle; + bool final_rotation; + double rotation_scaling_factor; + bool allow_backward; +}; + +/** + * @class nav2_graceful_motion_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for GracefulMotionController + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_graceful_motion_controller::ParameterHandler + */ + ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x); + + /** + * @brief Destructor for nav2_graceful_motion_controller::ParameterHandler + */ + ~ParameterHandler() = default; + + std::mutex & getMutex() {return mutex_;} + + Parameters * getParams() {return ¶ms_;} + +protected: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; +}; + +} // namespace nav2_graceful_motion_controller + +#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__PARAMETER_HANDLER_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/path_handler.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/path_handler.hpp new file mode 100644 index 0000000000..4ca986c39e --- /dev/null +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/path_handler.hpp @@ -0,0 +1,83 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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_GRACEFUL_MOTION_CONTROLLER__PATH_HANDLER_HPP_ +#define NAV2_GRACEFUL_MOTION_CONTROLLER__PATH_HANDLER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_graceful_motion_controller +{ + +/** + * @class nav2_graceful_motion_controller::PathHandler + * @brief Handles input paths to transform them to local frames required + */ +class PathHandler +{ +public: + /** + * @brief Constructor for nav2_graceful_motion_controller::PathHandler + */ + PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros); + + /** + * @brief Destructor for nav2_graceful_motion_controller::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Transforms global plan into same frame as pose and clips poses ineligible for motionTarget + * Points ineligible to be selected as a motion target point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) + * @param pose pose to transform + * @param max_robot_pose_search_dist Distance to search for matching nearest path point + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist); + + /** + * @brief Sets the global plan + * + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path); + + /** + * @brief Gets the global plan + * + * @return The global plan + */ + nav_msgs::msg::Path getPlan() {return global_plan_;} + +protected: + rclcpp::Duration transform_tolerance_{0, 0}; + std::shared_ptr tf_buffer_; + std::shared_ptr costmap_ros_; + nav_msgs::msg::Path global_plan_; + rclcpp::Logger logger_ {rclcpp::get_logger("GracefulPathHandler")}; +}; + +} // namespace nav2_graceful_motion_controller + +#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp new file mode 100644 index 0000000000..276610ea6c --- /dev/null +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp @@ -0,0 +1,193 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_MOTION_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ +#define NAV2_GRACEFUL_MOTION_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" + +namespace nav2_graceful_motion_controller +{ + +/** + * @class nav2_graceful_motion_controller::SmoothControlLaw + * @brief Smooth control law for graceful motion based on "A smooth control law for graceful motion" + * (Jong Jin Park and Benjamin Kuipers). + */ +class SmoothControlLaw +{ +public: + /** + * @brief Constructor for nav2_graceful_motion_controller::SmoothControlLaw + * + * @param k_phi Ratio of the rate of change in phi to the rate of change in r. + * @param k_delta Constant factor applied to the heading error feedback. + * @param beta Constant factor applied to the path curvature: dropping velocity. + * @param lambda Constant factor applied to the path curvature for sharpness. + * @param slowdown_radius Radial threshold applied to the slowdown rule. + * @param v_linear_min Minimum linear velocity. + * @param v_linear_max Maximum linear velocity. + * @param v_angular_max Maximum angular velocity. + */ + SmoothControlLaw( + double k_phi, double k_delta, double beta, double lambda, double slowdown_radius, + double v_linear_min, double v_linear_max, double v_angular_max); + + /** + * @brief Destructor for nav2_graceful_motion_controller::SmoothControlLaw + */ + ~SmoothControlLaw() = default; + + /** + * @brief Set the values that define the curvature. + * + * @param k_phi Ratio of the rate of change in phi to the rate of change in r. + * @param k_delta Constant factor applied to the heading error feedback. + * @param beta Constant factor applied to the path curvature: dropping velocity. + * @param lambda Constant factor applied to the path curvature for sharpness. + */ + void setCurvatureConstants( + const double k_phi, const double k_delta, const double beta, const double lambda); + + /** + * @brief Set the slowdown radius + * + * @param slowdown_radius Radial threshold applied to the slowdown rule. + */ + void setSlowdownRadius(const double slowdown_radius); + + /** + * @brief Update the velocity limits. + * + * @param v_linear_min The minimum absolute velocity in the linear direction. + * @param v_linear_max The maximum absolute velocity in the linear direction. + * @param v_angular_max The maximum absolute velocity in the angular direction. + */ + void setSpeedLimit( + const double v_linear_min, const double v_linear_max, + const double v_angular_max); + + /** + * @brief Compute linear and angular velocities command using the curvature. + * + * @param target Pose of the target in the robot frame. + * @param current Current pose of the robot in the robot frame. + * @param backward If true, the robot is moving backwards. Defaults to false. + * @return Velocity command. + */ + geometry_msgs::msg::Twist calculateRegularVelocity( + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current, + const bool & backward = false); + + /** + * @brief Compute linear and angular velocities command using the curvature. + * + * @param target Pose of the target in the robot frame. + * @param backward If true, the robot is moving backwards. Defaults to false. + * @return Velocity command. + */ + geometry_msgs::msg::Twist calculateRegularVelocity( + const geometry_msgs::msg::Pose & target, + const bool & backward = false); + + /** + * @brief Calculate the next pose of the robot by generating a velocity command using the + * curvature and the current pose. + * + * @param dt Time step. + * @param target Pose of the target in the robot frame. + * @param current Current pose of the robot in the robot frame. + * @param backward If true, the robot is moving backwards. Defaults to false. + * @return geometry_msgs::msg::Pose + */ + geometry_msgs::msg::Pose calculateNextPose( + const double dt, + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current, + const bool & backward = false); + +protected: + /** + * @brief Calculate the path curvature using a Lyapunov-based feedback control law from + * "A smooth control law for graceful motion" (Jong Jin Park and Benjamin Kuipers). + * + * @param r Distance between the robot and the target. + * @param phi Orientation of target with respect to the line of sight from the robot to the target. + * @param delta Steering angle of the robot. + * @return The curvature + */ + double calculateCurvature(double r, double phi, double delta); + + /** + * @brief Ratio of the rate of change in phi to the rate of change in r. Controls the convergence + * of the slow subsystem. + * + * If this value is equal to zero, the controller will behave as a pure waypoint follower. + * A high value offers extreme scenario of pose-following where theta is reduced much faster than r. + * + * Note: This variable is called k1 in earlier versions of the paper. + */ + double k_phi_; + + /** + * @brief Constant factor applied to the heading error feedback. Controls the convergence of the + * fast subsystem. + * + * The bigger the value, the robot converge faster to the reference heading. + * + * Note: This variable is called k2 in earlier versions of the paper. + */ + double k_delta_; + + /** + * @brief Constant factor applied to the path curvature. This value must be positive. + * Determines how fast the velocity drops when the curvature increases. + */ + double beta_; + + /** + * @brief Constant factor applied to the path curvature. This value must be greater or equal to 1. + * Determines the sharpness of the curve: higher lambda implies sharper curves. + */ + double lambda_; + + /** + * @brief Radial threshold applied to the slowdown rule. + */ + double slowdown_radius_; + + /** + * @brief Minimum linear velocity. + */ + double v_linear_min_; + + /** + * @brief Maximum linear velocity. + */ + double v_linear_max_; + + /** + * @brief Maximum angular velocity. + */ + double v_angular_max_; +}; + +} // namespace nav2_graceful_motion_controller + +#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp new file mode 100644 index 0000000000..ba0868ac20 --- /dev/null +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ +#define NAV2_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ + +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "visualization_msgs/msg/marker.hpp" + +namespace nav2_graceful_motion_controller +{ +/** + * @brief Create a PointStamped message of the motion target for + * debugging / visualization porpuses. + * + * @param motion_target Motion target in PoseStamped format + * @return geometry_msgs::msg::PointStamped Motion target in PointStamped format + */ +geometry_msgs::msg::PointStamped createMotionTargetMsg( + const geometry_msgs::msg::PoseStamped & motion_target); + +/** + * @brief Create a flat circle marker of radius slowdown_radius around the motion target for + * debugging / visualization porpuses. + * + * @param motion_target Motion target + * @param slowdown_radius Radius of the slowdown circle + * @return visualization_msgs::msg::Marker Slowdown marker + */ +visualization_msgs::msg::Marker createSlowdownMarker( + const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius); + +} // namespace nav2_graceful_motion_controller + +#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ diff --git a/nav2_graceful_motion_controller/package.xml b/nav2_graceful_motion_controller/package.xml new file mode 100644 index 0000000000..17565ad784 --- /dev/null +++ b/nav2_graceful_motion_controller/package.xml @@ -0,0 +1,35 @@ + + + + nav2_graceful_motion_controller + 1.2.0 + Graceful motion controller + Alberto Tudela + Apache-2.0 + + ament_cmake + + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + rclcpp + geometry_msgs + nav2_msgs + pluginlib + tf2 + tf2_geometry_msgs + nav_2d_utils + angles + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + nav2_controller + + + ament_cmake + + + + diff --git a/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp b/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp new file mode 100644 index 0000000000..44d17d1c1d --- /dev/null +++ b/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp @@ -0,0 +1,365 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_core/controller_exceptions.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_graceful_motion_controller/graceful_motion_controller.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +namespace nav2_graceful_motion_controller +{ + +void GracefulMotionController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, const std::shared_ptr tf, + const std::shared_ptr costmap_ros) +{ + auto node = parent.lock(); + if (!node) { + throw nav2_core::ControllerException("Unable to lock node!"); + } + + costmap_ros_ = costmap_ros; + tf_buffer_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + + // Handles storage and dynamic configuration of parameters. + // Returns pointer to data current param settings. + param_handler_ = std::make_unique( + node, plugin_name_, logger_, + costmap_ros_->getCostmap()->getSizeInMetersX()); + params_ = param_handler_->getParams(); + + // Handles global path transformations + path_handler_ = std::make_unique( + tf2::durationFromSec(params_->transform_tolerance), tf_buffer_, costmap_ros_); + + // Handles the control law to generate the velocity commands + control_law_ = std::make_unique( + params_->k_phi, params_->k_delta, params_->beta, params_->lambda, params_->slowdown_radius, + params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); + + // Initialize footprint collision checker + collision_checker_ = std::make_unique>(costmap_ros_->getCostmap()); + + // Publishers + transformed_plan_pub_ = node->create_publisher("transformed_global_plan", 1); + local_plan_pub_ = node->create_publisher("local_plan", 1); + motion_target_pub_ = node->create_publisher("motion_target", 1); + slowdown_pub_ = node->create_publisher("slowdown", 1); + + RCLCPP_INFO(logger_, "Configured Graceful Motion Controller: %s", plugin_name_.c_str()); +} + +void GracefulMotionController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type graceful_motion_controller::GracefulMotionController", + plugin_name_.c_str()); + transformed_plan_pub_.reset(); + local_plan_pub_.reset(); + motion_target_pub_.reset(); + slowdown_pub_.reset(); + collision_checker_.reset(); + path_handler_.reset(); + param_handler_.reset(); + control_law_.reset(); +} + +void GracefulMotionController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type graceful_motion_controller::GracefulMotionController", + plugin_name_.c_str()); + transformed_plan_pub_->on_activate(); + local_plan_pub_->on_activate(); + motion_target_pub_->on_activate(); + slowdown_pub_->on_activate(); +} + +void GracefulMotionController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type graceful_motion_controller::GracefulMotionController", + plugin_name_.c_str()); + transformed_plan_pub_->on_deactivate(); + local_plan_pub_->on_deactivate(); + motion_target_pub_->on_deactivate(); + slowdown_pub_->on_deactivate(); +} + +geometry_msgs::msg::TwistStamped GracefulMotionController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & /*velocity*/, + nav2_core::GoalChecker * goal_checker) +{ + std::lock_guard param_lock(param_handler_->getMutex()); + + // Update for the current goal checker's state + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist velocity_tolerance; + if (!goal_checker->getTolerances(pose_tolerance, velocity_tolerance)) { + RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!"); + } else { + goal_dist_tolerance_ = pose_tolerance.position.x; + } + + // Update the smooth control law with the new params + control_law_->setCurvatureConstants( + params_->k_phi, params_->k_delta, params_->beta, params_->lambda); + control_law_->setSlowdownRadius(params_->slowdown_radius); + control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); + + // Transform path to robot base frame and publish it + auto transformed_plan = path_handler_->transformGlobalPlan( + pose, params_->max_robot_pose_search_dist); + transformed_plan_pub_->publish(transformed_plan); + + // Get the particular point on the path at the motion target distance and publish it + auto motion_target = getMotionTarget(params_->motion_target_dist, transformed_plan); + auto motion_target_point = nav2_graceful_motion_controller::createMotionTargetMsg(motion_target); + motion_target_pub_->publish(motion_target_point); + + // Publish marker for slowdown radius around motion target for debugging / visualization + auto slowdown_marker = nav2_graceful_motion_controller::createSlowdownMarker( + motion_target, + params_->slowdown_radius); + slowdown_pub_->publish(slowdown_marker); + + // Compute distance to goal as the path's integrated distance to account for path curvatures + double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_plan); + + // If the distance to the goal is less than the motion target distance, i.e. + // the 'motion target' is the goal, then we skip the motion target orientation by pointing + // it in the same orientation that the last segment of the path + double angle_to_target = atan2(motion_target.pose.position.y, motion_target.pose.position.x); + if (params_->final_rotation && dist_to_goal < params_->motion_target_dist) { + geometry_msgs::msg::PoseStamped stl_pose = + transformed_plan.poses[transformed_plan.poses.size() - 2]; + geometry_msgs::msg::PoseStamped goal_pose = transformed_plan.poses.back(); + double dx = goal_pose.pose.position.x - stl_pose.pose.position.x; + double dy = goal_pose.pose.position.y - stl_pose.pose.position.y; + double yaw = std::atan2(dy, dx); + motion_target.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + } + + // Flip the orientation of the motion target if the robot is moving backwards + bool reversing = false; + if (params_->allow_backward && motion_target.pose.position.x < 0.0) { + reversing = true; + motion_target.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(motion_target.pose.orientation) + M_PI); + } + + // Compute velocity command: + // 1. Check if we are close enough to the goal to do a final rotation in place + // 2. Check if we must do a rotation in place before moving + // 3. Calculate the new velocity command using the smooth control law + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + if (params_->final_rotation && (dist_to_goal < goal_dist_tolerance_ || goal_reached_)) { + goal_reached_ = true; + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + cmd_vel.twist = rotateToTarget(angle_to_goal); + } else if (params_->initial_rotation && // NOLINT + fabs(angle_to_target) > params_->initial_rotation_min_angle) + { + cmd_vel.twist = rotateToTarget(angle_to_target); + } else { + cmd_vel.twist = control_law_->calculateRegularVelocity(motion_target.pose, reversing); + } + + // Transform local frame to global frame to use in collision checking + geometry_msgs::msg::TransformStamped costmap_transform; + try { + costmap_transform = tf_buffer_->lookupTransform( + costmap_ros_->getGlobalFrameID(), costmap_ros_->getBaseFrameID(), + tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not transform %s to %s: %s", + costmap_ros_->getBaseFrameID().c_str(), costmap_ros_->getGlobalFrameID().c_str(), + ex.what()); + return cmd_vel; + } + + // Generate and publish local plan for debugging / visualization + nav_msgs::msg::Path local_plan; + if (!simulateTrajectory(pose, motion_target, costmap_transform, local_plan, reversing)) { + throw nav2_core::NoValidControl("Collision detected in the trajectory"); + } + local_plan.header = transformed_plan.header; + local_plan_pub_->publish(local_plan); + + return cmd_vel; +} + +void GracefulMotionController::setPlan(const nav_msgs::msg::Path & path) +{ + path_handler_->setPlan(path); + goal_reached_ = false; +} + +void GracefulMotionController::setSpeedLimit( + const double & speed_limit, const bool & percentage) +{ + std::lock_guard param_lock(param_handler_->getMutex()); + + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { + params_->v_linear_max = params_->v_linear_max_initial; + params_->v_angular_max = params_->v_angular_max_initial; + } else { + if (percentage) { + // Speed limit is expressed in % from maximum speed of robot + params_->v_linear_max = std::max( + params_->v_linear_max_initial * speed_limit / 100.0, params_->v_linear_min); + params_->v_angular_max = params_->v_angular_max_initial * speed_limit / 100.0; + } else { + // Speed limit is expressed in m/s + params_->v_linear_max = std::max(speed_limit, params_->v_linear_min); + // Limit the angular velocity to be proportional to the linear velocity + params_->v_angular_max = params_->v_angular_max_initial * + speed_limit / params_->v_linear_max_initial; + } + } +} + +geometry_msgs::msg::PoseStamped GracefulMotionController::getMotionTarget( + const double & motion_target_dist, + const nav_msgs::msg::Path & transformed_plan) +{ + // Find the first pose which is at a distance greater than the motion target distance + auto goal_pose_it = std::find_if( + transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { + return std::hypot(ps.pose.position.x, ps.pose.position.y) >= motion_target_dist; + }); + + // If the pose is not far enough, take the last pose + if (goal_pose_it == transformed_plan.poses.end()) { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } + + return *goal_pose_it; +} + +bool GracefulMotionController::simulateTrajectory( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::PoseStamped & motion_target, + const geometry_msgs::msg::TransformStamped & costmap_transform, + nav_msgs::msg::Path & trajectory, const bool & backward) +{ + // Check for collision before moving + if (inCollision( + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation))) + { + return false; + } + + // First pose + geometry_msgs::msg::PoseStamped next_pose; + next_pose.header.frame_id = costmap_ros_->getBaseFrameID(); + next_pose.pose.orientation.w = 1.0; + trajectory.poses.push_back(next_pose); + + double distance = std::numeric_limits::max(); + double resolution_ = costmap_ros_->getCostmap()->getResolution(); + double dt = (params_->v_linear_max > 0.0) ? resolution_ / params_->v_linear_max : 0.0; + + // Set max iter to avoid infinite loop + unsigned int max_iter = 2 * sqrt( + motion_target.pose.position.x * motion_target.pose.position.x + + motion_target.pose.position.y * motion_target.pose.position.y) / resolution_; + + // Generate path + do{ + // Apply velocities to calculate next pose + next_pose.pose = control_law_->calculateNextPose( + dt, motion_target.pose, next_pose.pose, backward); + + // Add the pose to the trajectory for visualization + trajectory.poses.push_back(next_pose); + + // Check for collision + geometry_msgs::msg::PoseStamped global_pose; + tf2::doTransform(next_pose, global_pose, costmap_transform); + if (inCollision( + global_pose.pose.position.x, global_pose.pose.position.y, + tf2::getYaw(global_pose.pose.orientation))) + { + return false; + } + + // Check if we reach the goal + distance = nav2_util::geometry_utils::euclidean_distance(motion_target.pose, next_pose.pose); + }while(distance > resolution_ && trajectory.poses.size() < max_iter); + + return true; +} + +geometry_msgs::msg::Twist GracefulMotionController::rotateToTarget(const double & angle_to_target) +{ + geometry_msgs::msg::Twist vel; + vel.linear.x = 0.0; + vel.angular.z = params_->rotation_scaling_factor * angle_to_target * params_->v_angular_max; + return vel; +} + +bool GracefulMotionController::inCollision(const double & x, const double & y, const double & theta) +{ + unsigned int mx, my; + if (!costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) { + RCLCPP_WARN( + logger_, "The path is not in the costmap. Cannot check for collisions. " + "Proceed at your own risk, slow the robot, or increase your costmap size."); + return false; + } + + // Calculate the cost of the footprint at the robot's current position depending + // on the shape of the footprint + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + bool consider_footprint = !costmap_ros_->getUseRadius(); + + double footprint_cost; + if (consider_footprint) { + footprint_cost = collision_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + } else { + footprint_cost = collision_checker_->pointCost(mx, my); + } + + switch (static_cast(footprint_cost)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + return true; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint ? false : true; + case (nav2_costmap_2d::NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +} // namespace nav2_graceful_motion_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_graceful_motion_controller::GracefulMotionController, + nav2_core::Controller) diff --git a/nav2_graceful_motion_controller/src/parameter_handler.cpp b/nav2_graceful_motion_controller/src/parameter_handler.cpp new file mode 100644 index 0000000000..ae3bbdc1cc --- /dev/null +++ b/nav2_graceful_motion_controller/src/parameter_handler.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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 "nav2_graceful_motion_controller/parameter_handler.hpp" + +namespace nav2_graceful_motion_controller +{ + +using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +ParameterHandler::ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x) +{ + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".motion_target_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(2.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.2)); + declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".initial_rotation_min_angle", rclcpp::ParameterValue(0.75)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".final_rotation", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false)); + + node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter(plugin_name_ + ".motion_target_dist", params_.motion_target_dist); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist); + if (params_.max_robot_pose_search_dist < 0.0) { + RCLCPP_WARN( + logger_, "Max robot search distance is negative, setting to max to search" + " every point on path for the closest value."); + params_.max_robot_pose_search_dist = std::numeric_limits::max(); + } + + node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi); + node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta); + node->get_parameter(plugin_name_ + ".beta", params_.beta); + node->get_parameter(plugin_name_ + ".lambda", params_.lambda); + node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min); + node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max); + params_.v_linear_max_initial = params_.v_linear_max; + node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max); + params_.v_angular_max_initial = params_.v_angular_max; + node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius); + node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation); + node->get_parameter( + plugin_name_ + ".initial_rotation_min_angle", params_.initial_rotation_min_angle); + node->get_parameter(plugin_name_ + ".final_rotation", params_.final_rotation); + node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor); + node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward); + + if (params_.initial_rotation && params_.allow_backward) { + RCLCPP_WARN( + logger_, "Initial rotation and allow backward parameters are both true, " + "setting allow backward to false."); + params_.allow_backward = false; + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult +ParameterHandler::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".transform_tolerance") { + params_.transform_tolerance = parameter.as_double(); + } else if (name == plugin_name_ + ".motion_target_dist") { + params_.motion_target_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".k_phi") { + params_.k_phi = parameter.as_double(); + } else if (name == plugin_name_ + ".k_delta") { + params_.k_delta = parameter.as_double(); + } else if (name == plugin_name_ + ".beta") { + params_.beta = parameter.as_double(); + } else if (name == plugin_name_ + ".lambda") { + params_.lambda = parameter.as_double(); + } else if (name == plugin_name_ + ".v_linear_min") { + params_.v_linear_min = parameter.as_double(); + } else if (name == plugin_name_ + ".v_linear_max") { + params_.v_linear_max = parameter.as_double(); + params_.v_linear_max_initial = params_.v_linear_max; + } else if (name == plugin_name_ + ".v_angular_max") { + params_.v_angular_max = parameter.as_double(); + params_.v_angular_max_initial = params_.v_angular_max; + } else if (name == plugin_name_ + ".slowdown_radius") { + params_.slowdown_radius = parameter.as_double(); + } else if (name == plugin_name_ + ".initial_rotation_min_angle") { + params_.initial_rotation_min_angle = parameter.as_double(); + } else if (name == plugin_name_ + ".rotation_scaling_factor") { + params_.rotation_scaling_factor = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".initial_rotation") { + if (parameter.as_bool() && params_.allow_backward) { + RCLCPP_WARN( + logger_, "Initial rotation and allow backward parameters are both true, " + "rejecting parameter change."); + continue; + } + params_.initial_rotation = parameter.as_bool(); + } else if (name == plugin_name_ + ".final_rotation") { + params_.final_rotation = parameter.as_bool(); + } else if (name == plugin_name_ + ".allow_backward") { + if (params_.initial_rotation && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Initial rotation and allow backward parameters are both true, " + "rejecting parameter change."); + continue; + } + params_.allow_backward = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_graceful_motion_controller diff --git a/nav2_graceful_motion_controller/src/path_handler.cpp b/nav2_graceful_motion_controller/src/path_handler.cpp new file mode 100644 index 0000000000..a2b8966224 --- /dev/null +++ b/nav2_graceful_motion_controller/src/path_handler.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2022 Samsung Research America +// +// 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 "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav_2d_utils/tf_help.hpp" +#include "nav2_graceful_motion_controller/path_handler.hpp" + +namespace nav2_graceful_motion_controller +{ + +using nav2_util::geometry_utils::euclidean_distance; + +PathHandler::PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros) +: transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros) +{ +} + +nav_msgs::msg::Path PathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist) +{ + // Check first if the plan is empty + if (global_plan_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + // Let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav_2d_utils::transformPose( + tf_buffer_, global_plan_.header.frame_id, pose, robot_pose, + transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); + } + + // Find the first pose in the global plan that's further than max_robot_pose_search_dist + // from the robot using integrated distance + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); + + // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path + auto transformation_begin = + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), closest_pose_upper_bound, + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // We'll discard points on the plan that are outside the local costmap + double dist_threshold = std::max( + costmap_ros_->getCostmap()->getSizeInMetersX(), + costmap_ros_->getCostmap()->getSizeInMetersY()) / 2.0; + auto transformation_end = std::find_if( + transformation_begin, global_plan_.poses.end(), + [&](const auto & global_plan_pose) { + return euclidean_distance(global_plan_pose, robot_pose) > dist_threshold; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + if (!nav_2d_utils::transformPose( + tf_buffer_, costmap_ros_->getBaseFrameID(), stamped_pose, + transformed_pose, transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +void PathHandler::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +} // namespace nav2_graceful_motion_controller diff --git a/nav2_graceful_motion_controller/src/smooth_control_law.cpp b/nav2_graceful_motion_controller/src/smooth_control_law.cpp new file mode 100644 index 0000000000..ee5a38ea87 --- /dev/null +++ b/nav2_graceful_motion_controller/src/smooth_control_law.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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 "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_graceful_motion_controller/ego_polar_coords.hpp" +#include "nav2_graceful_motion_controller/smooth_control_law.hpp" + +namespace nav2_graceful_motion_controller +{ + +SmoothControlLaw::SmoothControlLaw( + double k_phi, double k_delta, double beta, double lambda, double slowdown_radius, + double v_linear_min, double v_linear_max, double v_angular_max) +: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda), slowdown_radius_(slowdown_radius), + v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max) +{ +} + +void SmoothControlLaw::setCurvatureConstants( + double k_phi, double k_delta, double beta, double lambda) +{ + k_phi_ = k_phi; + k_delta_ = k_delta; + beta_ = beta; + lambda_ = lambda; +} + +void SmoothControlLaw::setSlowdownRadius(double slowdown_radius) +{ + slowdown_radius_ = slowdown_radius; +} + +void SmoothControlLaw::setSpeedLimit( + const double v_linear_min, const double v_linear_max, + const double v_angular_max) +{ + v_linear_min_ = v_linear_min; + v_linear_max_ = v_linear_max; + v_angular_max_ = v_angular_max; +} + +geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, + const bool & backward) +{ + // Convert the target to polar coordinates + auto ego_coords = EgocentricPolarCoordinates(target, current, backward); + // Calculate the curvature + double curvature = calculateCurvature(ego_coords.r, ego_coords.phi, ego_coords.delta); + + // Adjust the linear velocity as a function of the path curvature to + // slowdown the controller as it approaches its target + double v = v_linear_max_ / (1.0 + beta_ * std::pow(fabs(curvature), lambda_)); + + // Slowdown when the robot is near the target to remove singularity + v = std::min(v_linear_max_ * (ego_coords.r / slowdown_radius_), v); + + // Set some small v_min when far away from origin to promote faster + // turning motion when the curvature is very high + v = std::clamp(v, v_linear_min_, v_linear_max_); + + // Set the velocity to negative if the robot is moving backwards + v = backward ? -v : v; + + // Compute the angular velocity + double w = curvature * v; + // Bound angular velocity between [-max_angular_vel, max_angular_vel] + double w_bound = std::clamp(w, -v_angular_max_, v_angular_max_); + // And linear velocity to follow the curvature + v = (curvature != 0.0) ? (w_bound / curvature) : v; + + // Return the velocity command + geometry_msgs::msg::Twist cmd_vel; + cmd_vel.linear.x = v; + cmd_vel.angular.z = w_bound; + return cmd_vel; +} + +geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( + const geometry_msgs::msg::Pose & target, const bool & backward) +{ + return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), backward); +} + +geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose( + const double dt, + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current, + const bool & backward) +{ + geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, backward); + geometry_msgs::msg::Pose next; + double yaw = tf2::getYaw(current.orientation); + next.position.x = current.position.x + vel.linear.x * dt * cos(yaw); + next.position.y = current.position.y + vel.linear.x * dt * sin(yaw); + yaw += vel.angular.z * dt; + next.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + return next; +} + +double SmoothControlLaw::calculateCurvature(double r, double phi, double delta) +{ + // Calculate the proportional term of the control law as the product of the gain and the error: + // difference between the actual steering angle and the virtual control for the slow subsystem + double prop_term = k_delta_ * (delta - std::atan(-k_phi_ * phi)); + // Calculate the feedback control law for the steering + double feedback_term = (1.0 + (k_phi_ / (1.0 + std::pow(k_phi_ * phi, 2)))) * sin(delta); + // Calculate the path curvature + return -1.0 / r * (prop_term + feedback_term); +} + +} // namespace nav2_graceful_motion_controller diff --git a/nav2_graceful_motion_controller/src/utils.cpp b/nav2_graceful_motion_controller/src/utils.cpp new file mode 100644 index 0000000000..488b679022 --- /dev/null +++ b/nav2_graceful_motion_controller/src/utils.cpp @@ -0,0 +1,51 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_graceful_motion_controller/utils.hpp" + +namespace nav2_graceful_motion_controller +{ + +geometry_msgs::msg::PointStamped createMotionTargetMsg( + const geometry_msgs::msg::PoseStamped & motion_target) +{ + geometry_msgs::msg::PointStamped motion_target_point; + motion_target_point.header = motion_target.header; + motion_target_point.point = motion_target.pose.position; + motion_target_point.point.z = 0.01; + return motion_target_point; +} + +visualization_msgs::msg::Marker createSlowdownMarker( + const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius) +{ + visualization_msgs::msg::Marker slowdown_marker; + slowdown_marker.header = motion_target.header; + slowdown_marker.ns = "slowdown"; + slowdown_marker.id = 0; + slowdown_marker.type = visualization_msgs::msg::Marker::SPHERE; + slowdown_marker.action = visualization_msgs::msg::Marker::ADD; + slowdown_marker.pose = motion_target.pose; + slowdown_marker.pose.position.z = 0.01; + slowdown_marker.scale.x = slowdown_radius * 2.0; + slowdown_marker.scale.y = slowdown_radius * 2.0; + slowdown_marker.scale.z = 0.02; + slowdown_marker.color.a = 0.2; + slowdown_marker.color.r = 0.0; + slowdown_marker.color.g = 1.0; + slowdown_marker.color.b = 0.0; + return slowdown_marker; +} + +} // namespace nav2_graceful_motion_controller diff --git a/nav2_graceful_motion_controller/test/CMakeLists.txt b/nav2_graceful_motion_controller/test/CMakeLists.txt new file mode 100644 index 0000000000..543c9b7290 --- /dev/null +++ b/nav2_graceful_motion_controller/test/CMakeLists.txt @@ -0,0 +1,21 @@ +find_package(nav2_controller REQUIRED) + +# Tests for Graceful Motion Controller +ament_add_gtest(test_graceful_motion_controller + test_graceful_motion_controller.cpp +) +ament_target_dependencies(test_graceful_motion_controller + ${dependencies} + nav2_controller +) +target_link_libraries(test_graceful_motion_controller + ${library_name} +) + +# Egopolar test +ament_add_gtest(test_egopolar + test_egopolar.cpp +) +ament_target_dependencies(test_egopolar + geometry_msgs tf2_geometry_msgs angles +) diff --git a/nav2_graceful_motion_controller/test/test_egopolar.cpp b/nav2_graceful_motion_controller/test/test_egopolar.cpp new file mode 100644 index 0000000000..0b1bd959cf --- /dev/null +++ b/nav2_graceful_motion_controller/test/test_egopolar.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_graceful_motion_controller/ego_polar_coords.hpp" + +TEST(EgocentricPolarCoordinatesTest, constructorDefault) { + nav2_graceful_motion_controller::EgocentricPolarCoordinates coords; + + EXPECT_FLOAT_EQ(0.0, coords.r); + EXPECT_FLOAT_EQ(0.0, coords.phi); + EXPECT_FLOAT_EQ(0.0, coords.delta); +} + +TEST(EgocentricPolarCoordinatesTest, constructorWithValues) { + float r_value = 5.0; + float phi_value = 1.2; + float delta_value = -0.5; + + nav2_graceful_motion_controller::EgocentricPolarCoordinates coords(r_value, phi_value, + delta_value); + + EXPECT_FLOAT_EQ(r_value, coords.r); + EXPECT_FLOAT_EQ(phi_value, coords.phi); + EXPECT_FLOAT_EQ(delta_value, coords.delta); +} + +TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) { + geometry_msgs::msg::Pose target; + target.position.x = 3.0; + target.position.y = 4.0; + target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 0.8)); + + geometry_msgs::msg::Pose current; + current.position.x = 1.0; + current.position.y = 1.0; + current.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), -0.2)); + + nav2_graceful_motion_controller::EgocentricPolarCoordinates coords(target, current); + + EXPECT_FLOAT_EQ(3.6055512428283691, coords.r); + EXPECT_FLOAT_EQ(-0.18279374837875384, coords.phi); + EXPECT_FLOAT_EQ(-1.1827937483787536, coords.delta); +} + +TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) { + geometry_msgs::msg::Pose target; + target.position.x = -3.0; + target.position.y = -4.0; + target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 0.8)); + + geometry_msgs::msg::Pose current; + current.position.x = 1.0; + current.position.y = 1.0; + current.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), -0.2)); + + nav2_graceful_motion_controller::EgocentricPolarCoordinates coords(target, current, true); + + EXPECT_FLOAT_EQ(-6.4031243, coords.r); + EXPECT_FLOAT_EQ(-0.096055523, coords.phi); + EXPECT_FLOAT_EQ(-1.0960555, coords.delta); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + bool success = RUN_ALL_TESTS(); + return success; +} diff --git a/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp b/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp new file mode 100644 index 0000000000..da6f82fe36 --- /dev/null +++ b/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp @@ -0,0 +1,1214 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_graceful_motion_controller/ego_polar_coords.hpp" +#include "nav2_graceful_motion_controller/smooth_control_law.hpp" +#include "nav2_graceful_motion_controller/graceful_motion_controller.hpp" + +class SCLFixture : public nav2_graceful_motion_controller::SmoothControlLaw +{ +public: + SCLFixture( + double k_phi, double k_delta, double beta, double lambda, + double slowdown_radius, double v_linear_min, double v_linear_max, double v_angular_max) + : nav2_graceful_motion_controller::SmoothControlLaw(k_phi, k_delta, beta, lambda, + slowdown_radius, v_linear_min, v_linear_max, v_angular_max) {} + + double getCurvatureKPhi() {return k_phi_;} + double getCurvatureKDelta() {return k_delta_;} + double getCurvatureBeta() {return beta_;} + double getCurvatureLambda() {return lambda_;} + double getSlowdownRadius() {return slowdown_radius_;} + double getSpeedLinearMin() {return v_linear_min_;} + double getSpeedLinearMax() {return v_linear_max_;} + double getSpeedAngularMax() {return v_angular_max_;} + double calculateCurvature(geometry_msgs::msg::Pose target, geometry_msgs::msg::Pose current) + { + auto ego_coords = nav2_graceful_motion_controller::EgocentricPolarCoordinates(target, current); + return nav2_graceful_motion_controller::SmoothControlLaw::calculateCurvature( + ego_coords.r, ego_coords.phi, ego_coords.delta); + } +}; + +class GMControllerFixture : public nav2_graceful_motion_controller::GracefulMotionController +{ +public: + GMControllerFixture() + : nav2_graceful_motion_controller::GracefulMotionController() {} + + bool getInitialRotation() {return params_->initial_rotation;} + + bool getAllowBackward() {return params_->allow_backward;} + + nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} + + geometry_msgs::msg::PoseStamped getMotionTarget( + const double & motion_target_distance, const nav_msgs::msg::Path & plan) + { + return nav2_graceful_motion_controller::GracefulMotionController::getMotionTarget( + motion_target_distance, plan); + } + + geometry_msgs::msg::PointStamped createMotionTargetMsg( + const geometry_msgs::msg::PoseStamped & motion_target) + { + return nav2_graceful_motion_controller::createMotionTargetMsg(motion_target); + } + + visualization_msgs::msg::Marker createSlowdownMarker( + const geometry_msgs::msg::PoseStamped & motion_target) + { + return nav2_graceful_motion_controller::createSlowdownMarker( + motion_target, + params_->slowdown_radius); + } + + geometry_msgs::msg::Twist rotateToTarget(const double & angle_to_target) + { + return nav2_graceful_motion_controller::GracefulMotionController::rotateToTarget( + angle_to_target); + } + + bool simulateTrajectory( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::PoseStamped & motion_target, + const geometry_msgs::msg::TransformStamped & costmap_transform, + nav_msgs::msg::Path & trajectory, const bool & backward) + { + return nav2_graceful_motion_controller::GracefulMotionController::simulateTrajectory( + robot_pose, motion_target, costmap_transform, trajectory, backward); + } + + double getSpeedLinearMax() {return params_->v_linear_max;} + + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) + { + return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); + } +}; + +TEST(SmoothControlLawTest, setCurvatureConstants) { + // Initialize SmoothControlLaw + SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0); + + // Set curvature constants + scl.setCurvatureConstants(1.0, 2.0, 3.0, 4.0); + + // Set slowdown radius + scl.setSlowdownRadius(5.0); + + // Check results + EXPECT_EQ(scl.getCurvatureKPhi(), 1.0); + EXPECT_EQ(scl.getCurvatureKDelta(), 2.0); + EXPECT_EQ(scl.getCurvatureBeta(), 3.0); + EXPECT_EQ(scl.getCurvatureLambda(), 4.0); + EXPECT_EQ(scl.getSlowdownRadius(), 5.0); +} + +TEST(SmoothControlLawTest, setSpeedLimits) { + // Initialize SmoothControlLaw + SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + // Set speed limits + scl.setSpeedLimit(1.0, 2.0, 3.0); + + // Check results + EXPECT_EQ(scl.getSpeedLinearMin(), 1.0); + EXPECT_EQ(scl.getSpeedLinearMax(), 2.0); + EXPECT_EQ(scl.getSpeedAngularMax(), 3.0); +} + +TEST(SmoothControlLawTest, calculateCurvature) { + // Initialize SmoothControlLaw + SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0); + + // Initialize target + geometry_msgs::msg::Pose target; + target.position.x = 0.0; + target.position.y = 5.0; + target.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Initialize current + geometry_msgs::msg::Pose current; + current.position.x = 0.0; + current.position.y = 0.0; + current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Calculate curvature + double curvature = scl.calculateCurvature(target, current); + + // Check results: it must be positive + EXPECT_NEAR(curvature, 5.407042, 0.0001); + + // Set a new target + target.position.x = 4.5; + target.position.y = -2.17; + target.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, -0.785)); + // Set the same current + current.position.x = 0.0; + current.position.y = 0.0; + current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Calculate curvature + curvature = scl.calculateCurvature(target, current); + + // Check results: it must be neggative + EXPECT_NEAR(curvature, -0.416228, 0.0001); +} + +TEST(SmoothControlLawTest, calculateRegularVelocity) { + // Initialize SmoothControlLaw + SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0); + + // Initialize target + geometry_msgs::msg::Pose target; + target.position.x = 0.0; + target.position.y = 5.0; + target.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Initialize current + geometry_msgs::msg::Pose current; + current.position.x = 0.0; + current.position.y = 0.0; + current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Calculate velocity + auto cmd_vel = scl.calculateRegularVelocity(target, current); + + // Check results: both linear and angular velocity must be positive + EXPECT_NEAR(cmd_vel.linear.x, 0.1460446, 0.0001); + EXPECT_NEAR(cmd_vel.angular.z, 0.7896695, 0.0001); + + // Set a new target + target.position.x = 4.5; + target.position.y = -2.17; + target.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, -0.785)); + // Set the same current + current.position.x = 0.0; + current.position.y = 0.0; + current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Calculate velocity + cmd_vel = scl.calculateRegularVelocity(target, current); + + // Check results: linear velocity must be positive and angular velocity must be negative + EXPECT_NEAR(cmd_vel.linear.x, 0.96651200, 0.0001); + EXPECT_NEAR(cmd_vel.angular.z, -0.4022844, 0.0001); +} + +TEST(SmoothControlLawTest, calculateNextPose) { + // Initialize SmoothControlLaw + SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0); + + // Initialize target + geometry_msgs::msg::Pose target; + target.position.x = 0.0; + target.position.y = 5.0; + target.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Initialize current + geometry_msgs::msg::Pose current; + current.position.x = 0.0; + current.position.y = 0.0; + current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + // Calculate next pose + float dt = 0.1; + auto next_pose = scl.calculateNextPose(dt, target, current); + + // Check results + EXPECT_NEAR(next_pose.position.x, 0.1, 0.1); + EXPECT_NEAR(next_pose.position.y, 0.0, 0.1); + EXPECT_NEAR(tf2::getYaw(next_pose.orientation), 0.0, 0.1); +} + +TEST(GracefulMotionControllerTest, configure) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + auto costmap_ros = std::make_shared("global_costmap"); + + // Create controller + auto controller = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Set the plan + nav_msgs::msg::Path plan; + plan.header.frame_id = "map"; + plan.poses.resize(3); + plan.poses[0].header.frame_id = "map"; + controller->setPlan(plan); + EXPECT_EQ(controller->getPlan().poses.size(), 3u); + EXPECT_EQ(controller->getPlan().poses[0].header.frame_id, "map"); + + // Cleaning up + controller->deactivate(); + controller->cleanup(); +} + +TEST(GracefulMotionControllerTest, dynamicParameters) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + auto costmap_ros = std::make_shared("global_costmap"); + + // Set max search distant to negative so it warns + nav2_util::declare_parameter_if_not_declared( + node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(-2.0)); + + // Set initial rotation and allow backward to true so it warns and allow backward is false + nav2_util::declare_parameter_if_not_declared( + node, "test.initial_rotation", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, "test.allow_backward", rclcpp::ParameterValue(true)); + + // Create controller + auto controller = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Check first allowed backward is false + EXPECT_EQ(controller->getAllowBackward(), false); + + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + // Set parameters + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("test.transform_tolerance", 1.0), + rclcpp::Parameter("test.motion_target_dist", 2.0), + rclcpp::Parameter("test.k_phi", 4.0), + rclcpp::Parameter("test.k_delta", 5.0), + rclcpp::Parameter("test.beta", 6.0), + rclcpp::Parameter("test.lambda", 7.0), + rclcpp::Parameter("test.v_linear_min", 8.0), + rclcpp::Parameter("test.v_linear_max", 9.0), + rclcpp::Parameter("test.v_angular_max", 10.0), + rclcpp::Parameter("test.slowdown_radius", 11.0), + rclcpp::Parameter("test.initial_rotation", false), + rclcpp::Parameter("test.initial_rotation_min_angle", 12.0), + rclcpp::Parameter("test.final_rotation", false), + rclcpp::Parameter("test.rotation_scaling_factor", 13.0), + rclcpp::Parameter("test.allow_backward", true)}); + + // Spin + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Check parameters + EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.motion_target_dist").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.k_phi").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("test.k_delta").as_double(), 5.0); + EXPECT_EQ(node->get_parameter("test.beta").as_double(), 6.0); + EXPECT_EQ(node->get_parameter("test.lambda").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.v_linear_min").as_double(), 8.0); + EXPECT_EQ(node->get_parameter("test.v_linear_max").as_double(), 9.0); + EXPECT_EQ(node->get_parameter("test.v_angular_max").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("test.slowdown_radius").as_double(), 11.0); + EXPECT_EQ(node->get_parameter("test.initial_rotation").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.initial_rotation_min_angle").as_double(), 12.0); + EXPECT_EQ(node->get_parameter("test.final_rotation").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0); + EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true); + + // Set initial rotation to true + results = params->set_parameters_atomically( + {rclcpp::Parameter("test.initial_rotation", true)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + // Check parameters. Initial rotation should be false as both cannot be true at the same time + EXPECT_EQ(controller->getInitialRotation(), false); + EXPECT_EQ(controller->getAllowBackward(), true); + + // Set both initial rotation and allow backward to false + results = params->set_parameters_atomically( + {rclcpp::Parameter("test.initial_rotation", false), + rclcpp::Parameter("test.allow_backward", false)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + // Check parameters. + EXPECT_EQ(node->get_parameter("test.initial_rotation").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), false); + + // Set initial rotation to true + results = params->set_parameters_atomically( + {rclcpp::Parameter("test.initial_rotation", true)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + EXPECT_EQ(controller->getInitialRotation(), true); + + // Set allow backward to true + results = params->set_parameters_atomically( + {rclcpp::Parameter("test.allow_backward", true)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + // Check parameters. Now allow backward should be false as both cannot be true at the same time + EXPECT_EQ(controller->getInitialRotation(), true); + EXPECT_EQ(controller->getAllowBackward(), false); +} + +TEST(GracefulMotionControllerTest, getDifferentMotionTargets) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + auto costmap_ros = std::make_shared("global_costmap"); + + // Create controller + auto controller = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Set the plan + nav_msgs::msg::Path plan; + plan.header.frame_id = "map"; + plan.poses.resize(3); + plan.poses[0].header.frame_id = "map"; + plan.poses[0].pose.position.x = 1.0; + plan.poses[0].pose.position.y = 2.0; + plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[1].header.frame_id = "map"; + plan.poses[1].pose.position.x = 3.0; + plan.poses[1].pose.position.y = 4.0; + plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[2].header.frame_id = "map"; + plan.poses[2].pose.position.x = 5.0; + plan.poses[2].pose.position.y = 6.0; + plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(plan); + + // Set distance and get motion target + double motion_target_distance = 3.5; + auto motion_target = controller->getMotionTarget(motion_target_distance, plan); + + // Check results, should be the second one + EXPECT_EQ(motion_target.header.frame_id, "map"); + EXPECT_EQ(motion_target.pose.position.x, 3.0); + EXPECT_EQ(motion_target.pose.position.y, 4.0); + EXPECT_EQ(motion_target.pose.orientation, tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0))); + + // Set a new distance greater than the path length and get motion target + motion_target_distance = 10.0; + motion_target = controller->getMotionTarget(motion_target_distance, plan); + + // Check results: should be the last one + EXPECT_EQ(motion_target.header.frame_id, "map"); + EXPECT_EQ(motion_target.pose.position.x, 5.0); + EXPECT_EQ(motion_target.pose.position.y, 6.0); + EXPECT_EQ(motion_target.pose.orientation, tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0))); +} + +TEST(GracefulMotionControllerTest, createMotionTargetMsg) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + auto costmap_ros = std::make_shared("global_costmap"); + + // Create controller + auto controller = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create motion target + geometry_msgs::msg::PoseStamped motion_target; + motion_target.header.frame_id = "map"; + motion_target.pose.position.x = 1.0; + motion_target.pose.position.y = 2.0; + motion_target.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + + // Create motion target message + auto motion_target_msg = controller->createMotionTargetMsg(motion_target); + + // Check results + EXPECT_EQ(motion_target_msg.header.frame_id, "map"); + EXPECT_EQ(motion_target_msg.point.x, 1.0); + EXPECT_EQ(motion_target_msg.point.y, 2.0); + EXPECT_EQ(motion_target_msg.point.z, 0.01); +} + +TEST(GracefulMotionControllerTest, createSlowdownMsg) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + auto costmap_ros = std::make_shared("global_costmap"); + + // Create controller + auto controller = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create motion target + geometry_msgs::msg::PoseStamped motion_target; + motion_target.header.frame_id = "map"; + motion_target.pose.position.x = 1.0; + motion_target.pose.position.y = 2.0; + motion_target.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + // Set slowdown parameter + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("test.slowdown_radius", 0.2)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Create slowdown message + auto slowdown_msg = controller->createSlowdownMarker(motion_target); + + // Check results + EXPECT_EQ(slowdown_msg.header.frame_id, "map"); + EXPECT_EQ(slowdown_msg.ns, "slowdown"); + EXPECT_EQ(slowdown_msg.id, 0); + EXPECT_EQ(slowdown_msg.type, visualization_msgs::msg::Marker::SPHERE); + EXPECT_EQ(slowdown_msg.action, visualization_msgs::msg::Marker::ADD); + EXPECT_EQ(slowdown_msg.pose.position.x, 1.0); + EXPECT_EQ(slowdown_msg.pose.position.y, 2.0); + EXPECT_EQ(slowdown_msg.pose.position.z, 0.01); + EXPECT_EQ(slowdown_msg.pose.orientation.x, 0.0); + EXPECT_EQ(slowdown_msg.pose.orientation.y, 0.0); + EXPECT_EQ(slowdown_msg.pose.orientation.z, 0.0); + EXPECT_EQ(slowdown_msg.pose.orientation.w, 1.0); + EXPECT_EQ(slowdown_msg.scale.x, 0.4); + EXPECT_EQ(slowdown_msg.scale.y, 0.4); + EXPECT_EQ(slowdown_msg.scale.z, 0.02); + EXPECT_EQ(slowdown_msg.color.r, 0.0); + EXPECT_EQ(slowdown_msg.color.g, 1.0); + EXPECT_EQ(slowdown_msg.color.b, 0.0); +} + +TEST(GracefulMotionControllerTest, rotateToTarget) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + auto costmap_ros = std::make_shared("global_costmap"); + + // Create controller + auto controller = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Set max velocity + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("test.v_angular_max", 1.0), + rclcpp::Parameter("test.rotation_scaling_factor", 0.5)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Set angle to target and get velocity command + double angle_to_target = 0.5; + auto cmd_vel = controller->rotateToTarget(angle_to_target); + + // Check results: it must be a positive rotation + EXPECT_EQ(cmd_vel.linear.x, 0.0); + EXPECT_EQ(cmd_vel.angular.z, 0.25); + + // Set a new angle to target + angle_to_target = -0.5; + cmd_vel = controller->rotateToTarget(angle_to_target); + + // Check results: it must be a negative rotation + EXPECT_EQ(cmd_vel.linear.x, 0.0); + EXPECT_EQ(cmd_vel.angular.z, -0.25); +} + +TEST(GracefulMotionControllerTest, setSpeedLimit) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + auto costmap_ros = std::make_shared("global_costmap"); + + // Create controller + auto controller = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Set max velocity + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("test.v_linear_min", 2.0), + rclcpp::Parameter("test.v_linear_max", 5.0) + }); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Set relative speed limit and get speed parameter + controller->setSpeedLimit(50.0, true); + double speed_limit = controller->getSpeedLinearMax(); + + // Check results + EXPECT_EQ(speed_limit, 2.5); + + // Set absolute speed limit and get speed parameter + controller->setSpeedLimit(100.0, false); + speed_limit = controller->getSpeedLinearMax(); + + // Check results + EXPECT_EQ(speed_limit, 100.0); + + // Set a new value below the minimum and get speed parameter + controller->setSpeedLimit(1.0, false); + speed_limit = controller->getSpeedLinearMax(); + + // Check results + EXPECT_EQ(speed_limit, 2.0); +} + +TEST(GracefulMotionControllerTest, emptyPlan) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Set max search distant + nav2_util::declare_parameter_if_not_declared( + node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(5.0)); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set an empty global plan + nav_msgs::msg::Path global_plan; + global_plan.header.frame_id = "test_global_frame"; + controller->setPlan(global_plan); + + EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::InvalidPath); +} + +TEST(GracefulMotionControllerTest, poseOutsideCostmap) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Set max search distant + nav2_util::declare_parameter_if_not_declared( + node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(5.0)); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 500.0; + robot_pose.pose.position.y = 500.0; + robot_pose.pose.position.z = 0.0; + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set an empty global plan + nav_msgs::msg::Path global_plan; + global_plan.header.frame_id = "test_global_frame"; + global_plan.poses.resize(1); + global_plan.poses[0].header.frame_id = "test_global_frame"; + global_plan.poses[0].pose.position.x = 0.0; + global_plan.poses[0].pose.position.y = 0.0; + controller->setPlan(global_plan); + + EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::ControllerException); +} + +TEST(GracefulMotionControllerTest, noPruningPlan) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Set max search distant + nav2_util::declare_parameter_if_not_declared( + node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(5.0)); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set a linear global plan + nav_msgs::msg::Path global_plan; + global_plan.header.frame_id = "test_global_frame"; + global_plan.poses.resize(3); + global_plan.poses[0].header.frame_id = "test_global_frame"; + global_plan.poses[0].pose.position.x = 0.0; + global_plan.poses[0].pose.position.y = 0.0; + global_plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[1].header.frame_id = "test_global_frame"; + global_plan.poses[1].pose.position.x = 1.0; + global_plan.poses[1].pose.position.y = 1.0; + global_plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[2].header.frame_id = "test_global_frame"; + global_plan.poses[2].pose.position.x = 3.0; + global_plan.poses[2].pose.position.y = 3.0; + global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(global_plan); + + // Check results: the plan should not be pruned + auto transformed_plan = controller->transformGlobalPlan(robot_pose); + EXPECT_EQ(transformed_plan.poses.size(), global_plan.poses.size()); +} + +TEST(GracefulMotionControllerTest, pruningPlan) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 20x20 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 20), + rclcpp::Parameter("height", 20), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Set max search distant + nav2_util::declare_parameter_if_not_declared( + node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(9.0)); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set a linear global plan + nav_msgs::msg::Path global_plan; + global_plan.header.frame_id = "test_global_frame"; + global_plan.poses.resize(6); + global_plan.poses[0].header.frame_id = "test_global_frame"; + global_plan.poses[0].pose.position.x = 0.0; + global_plan.poses[0].pose.position.y = 0.0; + global_plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[1].header.frame_id = "test_global_frame"; + global_plan.poses[1].pose.position.x = 3.0; + global_plan.poses[1].pose.position.y = 3.0; + global_plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[2].header.frame_id = "test_global_frame"; + global_plan.poses[2].pose.position.x = 5.0; + global_plan.poses[2].pose.position.y = 5.0; + global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[3].header.frame_id = "test_global_frame"; + global_plan.poses[3].pose.position.x = 10.0; + global_plan.poses[3].pose.position.y = 10.0; + global_plan.poses[3].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[4].header.frame_id = "test_global_frame"; + global_plan.poses[4].pose.position.x = 20.0; + global_plan.poses[4].pose.position.y = 20.0; + global_plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[5].pose.position.x = 500.0; + global_plan.poses[5].pose.position.y = 500.0; + global_plan.poses[5].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(global_plan); + + // Check results: the plan should be pruned + auto transformed_plan = controller->transformGlobalPlan(robot_pose); + EXPECT_EQ(transformed_plan.poses.size(), 3); +} + +TEST(GracefulMotionControllerTest, pruningPlanOutsideCostmap) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Set max search distant + nav2_util::declare_parameter_if_not_declared( + node, "test.max_robot_pose_search_dist", rclcpp::ParameterValue(15.0)); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set a linear global plan + nav_msgs::msg::Path global_plan; + global_plan.header.frame_id = "test_global_frame"; + global_plan.poses.resize(3); + global_plan.poses[0].header.frame_id = "test_global_frame"; + global_plan.poses[0].pose.position.x = 0.0; + global_plan.poses[0].pose.position.y = 0.0; + global_plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[1].header.frame_id = "test_global_frame"; + global_plan.poses[1].pose.position.x = 3.0; + global_plan.poses[1].pose.position.y = 3.0; + global_plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + global_plan.poses[2].header.frame_id = "test_global_frame"; + global_plan.poses[2].pose.position.x = 200.0; + global_plan.poses[2].pose.position.y = 200.0; + global_plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(global_plan); + + // Check results: the plan should be pruned + auto transformed_plan = controller->transformGlobalPlan(robot_pose); + EXPECT_EQ(transformed_plan.poses.size(), 2); +} + +TEST(GracefulMotionControllerTest, computeVelocityCommandRotate) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + nav2_util::declare_parameter_if_not_declared( + node, "test.v_angular_max", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node, "test.rotation_scaling_factor", rclcpp::ParameterValue(0.5)); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + robot_pose.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set a plan + nav_msgs::msg::Path plan; + plan.header.frame_id = "test_global_frame"; + plan.poses.resize(3); + plan.poses[0].header.frame_id = "test_global_frame"; + plan.poses[0].pose.position.x = 0.0; + plan.poses[0].pose.position.y = 0.0; + plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[1].header.frame_id = "test_global_frame"; + plan.poses[1].pose.position.x = 1.0; + plan.poses[1].pose.position.y = 1.0; + plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[2].header.frame_id = "test_global_frame"; + plan.poses[2].pose.position.x = 2.0; + plan.poses[2].pose.position.y = 2.0; + plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(plan); + + // Set velocity + geometry_msgs::msg::Twist robot_velocity; + robot_velocity.linear.x = 0.0; + robot_velocity.linear.y = 0.0; + + // Set the goal checker + nav2_controller::SimpleGoalChecker checker; + checker.initialize(node, "checker", costmap_ros); + + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + + // Check results: the robot should rotate in place. + // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. + EXPECT_EQ(cmd_vel.twist.linear.x, 0.0); + EXPECT_GE(cmd_vel.twist.angular.x, 0.0); + EXPECT_LE(cmd_vel.twist.angular.x, 0.5); +} + +TEST(GracefulMotionControllerTest, computeVelocityCommandRegular) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + robot_pose.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set a plan in a straight line from the robot + nav_msgs::msg::Path plan; + plan.header.frame_id = "test_global_frame"; + plan.poses.resize(3); + plan.poses[0].header.frame_id = "test_global_frame"; + plan.poses[0].pose.position.x = 0.0; + plan.poses[0].pose.position.y = 0.0; + plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[1].header.frame_id = "test_global_frame"; + plan.poses[1].pose.position.x = 1.0; + plan.poses[1].pose.position.y = 0.0; + plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[2].header.frame_id = "test_global_frame"; + plan.poses[2].pose.position.x = 2.0; + plan.poses[2].pose.position.y = 0.0; + plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(plan); + + // Set velocity + geometry_msgs::msg::Twist robot_velocity; + robot_velocity.linear.x = 0.0; + robot_velocity.linear.y = 0.0; + + // Set the goal checker + nav2_controller::SimpleGoalChecker checker; + checker.initialize(node, "checker", costmap_ros); + + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + + // Check results: the robot should go straight to the target. + // So, linear velocity should be some positive value and angular velocity should be zero. + EXPECT_GT(cmd_vel.twist.linear.x, 0.0); + EXPECT_EQ(cmd_vel.twist.angular.z, 0.0); +} + +TEST(GracefulMotionControllerTest, computeVelocityCommandRegularBackwards) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Set initial rotation false and allow backward to true + nav2_util::declare_parameter_if_not_declared( + node, "test.initial_rotation", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, "test.allow_backward", rclcpp::ParameterValue(true)); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + robot_pose.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set a plan in a straight line from the robot + nav_msgs::msg::Path plan; + plan.header.frame_id = "test_global_frame"; + plan.poses.resize(3); + plan.poses[0].header.frame_id = "test_global_frame"; + plan.poses[0].pose.position.x = 0.0; + plan.poses[0].pose.position.y = 0.0; + plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[1].header.frame_id = "test_global_frame"; + plan.poses[1].pose.position.x = -1.0; + plan.poses[1].pose.position.y = 0.0; + plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[2].header.frame_id = "test_global_frame"; + plan.poses[2].pose.position.x = -2.0; + plan.poses[2].pose.position.y = 0.0; + plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(plan); + + // Set velocity + geometry_msgs::msg::Twist robot_velocity; + robot_velocity.linear.x = 0.0; + robot_velocity.linear.y = 0.0; + + // Set the goal checker + nav2_controller::SimpleGoalChecker checker; + checker.initialize(node, "checker", costmap_ros); + + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + + // Check results: the robot should go straight to the target. + // So, both linear velocity should be some negative values. + EXPECT_LT(cmd_vel.twist.linear.x, 0.0); + EXPECT_LT(cmd_vel.twist.angular.z, 0.0); +} + +TEST(GracefulMotionControllerTest, computeVelocityCommandFinal) { + auto node = std::make_shared("testGraceful"); + auto tf = std::make_shared(node->get_clock()); + + // Create a costmap of 10x10 meters + auto costmap_ros = std::make_shared("test_costmap"); + auto results = costmap_ros->set_parameters( + {rclcpp::Parameter("global_frame", "test_global_frame"), + rclcpp::Parameter("robot_base_frame", "test_robot_frame"), + rclcpp::Parameter("width", 10), + rclcpp::Parameter("height", 10), + rclcpp::Parameter("resolution", 0.1)}); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + // Create controller + auto controller = std::make_shared(); + controller->configure(node, "test", tf, costmap_ros); + controller->activate(); + + // Create the robot pose + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "test_robot_frame"; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + robot_pose.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + + // Set transform between global and robot frame + geometry_msgs::msg::TransformStamped global_to_robot; + global_to_robot.header.frame_id = "test_global_frame"; + global_to_robot.header.stamp = node->get_clock()->now(); + global_to_robot.child_frame_id = "test_robot_frame"; + global_to_robot.transform.translation.x = robot_pose.pose.position.x; + global_to_robot.transform.translation.y = robot_pose.pose.position.y; + global_to_robot.transform.translation.z = robot_pose.pose.position.z; + tf->setTransform(global_to_robot, "test", false); + + // Set a plan in a straight line from the robot + nav_msgs::msg::Path plan; + plan.header.frame_id = "test_global_frame"; + plan.poses.resize(5); + plan.poses[0].header.frame_id = "test_global_frame"; + plan.poses[0].pose.position.x = 0.0; + plan.poses[0].pose.position.y = 0.0; + plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[1].header.frame_id = "test_global_frame"; + plan.poses[1].pose.position.x = 0.1; + plan.poses[1].pose.position.y = 0.0; + plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[2].header.frame_id = "test_global_frame"; + plan.poses[2].pose.position.x = 0.15; + plan.poses[2].pose.position.y = 0.0; + plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[3].header.frame_id = "test_global_frame"; + plan.poses[3].pose.position.x = 0.18; + plan.poses[3].pose.position.y = 0.0; + plan.poses[3].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + plan.poses[4].header.frame_id = "test_global_frame"; + plan.poses[4].pose.position.x = 0.2; + plan.poses[4].pose.position.y = 0.0; + plan.poses[4].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); + controller->setPlan(plan); + + // Set velocity + geometry_msgs::msg::Twist robot_velocity; + robot_velocity.linear.x = 0.0; + robot_velocity.linear.y = 0.0; + + // Set the goal checker + nav2_controller::SimpleGoalChecker checker; + checker.initialize(node, "checker", costmap_ros); + + auto cmd_vel = controller->computeVelocityCommands(robot_pose, robot_velocity, &checker); + + // Check results: the robot should do a final rotation near the target. + // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. + EXPECT_EQ(cmd_vel.twist.linear.x, 0.0); + EXPECT_GE(cmd_vel.twist.angular.x, 0.0); + EXPECT_LE(cmd_vel.twist.angular.x, 0.5); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool success = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return success; +}