From f2caf27c94761558889749832fd885afc24c55b0 Mon Sep 17 00:00:00 2001 From: Heiko Schoon <135117019+scarecrow3773@users.noreply.github.com> Date: Mon, 24 Jul 2023 21:35:30 +0200 Subject: [PATCH] Add files via upload --- m-explore-ros2/LICENSE | 31 ++ m-explore-ros2/explore/CHANGELOG.rst | 8 + m-explore-ros2/explore/CMakeLists.txt | 121 +++++ m-explore-ros2/explore/config/params.yaml | 14 + .../explore/config/params_costmap.yaml | 13 + m-explore-ros2/explore/doc/architecture.dia | Bin 0 -> 1236 bytes m-explore-ros2/explore/doc/screenshot.png | Bin 0 -> 24644 bytes m-explore-ros2/explore/doc/wiki_doc.txt | 148 ++++++ .../explore/include/explore/costmap_client.h | 137 ++++++ .../explore/include/explore/costmap_tools.h | 135 ++++++ .../explore/include/explore/explore.h | 146 ++++++ .../explore/include/explore/frontier_search.h | 88 ++++ .../explore/launch/explore.launch.py | 46 ++ m-explore-ros2/explore/package.xml | 33 ++ m-explore-ros2/explore/src/costmap_client.cpp | 279 +++++++++++ m-explore-ros2/explore/src/explore.cpp | 432 +++++++++++++++++ .../explore/src/frontier_search.cpp | 201 ++++++++ m-explore-ros2/explore/test/test_explore.cpp | 77 +++ m-explore-ros2/map_merge/CHANGELOG.rst | 58 +++ m-explore-ros2/map_merge/CMakeLists.txt | 164 +++++++ m-explore-ros2/map_merge/config/params.yaml | 24 + m-explore-ros2/map_merge/doc/architecture.dia | Bin 0 -> 1825 bytes m-explore-ros2/map_merge/doc/screenshot.jpg | Bin 0 -> 29775 bytes m-explore-ros2/map_merge/doc/wiki_doc.txt | 168 +++++++ .../include/combine_grids/grid_compositor.h | 60 +++ .../include/combine_grids/grid_warper.h | 61 +++ .../include/combine_grids/merging_pipeline.h | 146 ++++++ .../map_merge/include/map_merge/map_merge.h | 128 +++++ .../include/map_merge/ros1_names.hpp | 141 ++++++ .../launch/from_map_server.launch.py | 141 ++++++ .../map_merge/launch/map_merge.launch.py | 57 +++ .../map_merge/launch/map_merge.rviz | 154 ++++++ .../launch/tb3_simulation/bringup_launch.py | 202 ++++++++ .../config/nav2_multirobot_params_1.yaml | 287 +++++++++++ .../config/nav2_multirobot_params_2.yaml | 287 +++++++++++ .../multi_tb3_simulation_launch.py | 360 ++++++++++++++ .../launch/tb3_simulation/slam_toolbox.py | 49 ++ .../tb3_simulation/tb3_simulation_launch.py | 245 ++++++++++ .../tb3_simulation/worlds/world_only.model | 56 +++ m-explore-ros2/map_merge/package.xml | 31 ++ .../src/combine_grids/estimation_internal.h | 152 ++++++ .../src/combine_grids/grid_compositor.cpp | 88 ++++ .../src/combine_grids/grid_warper.cpp | 90 ++++ .../src/combine_grids/merging_pipeline.cpp | 279 +++++++++++ m-explore-ros2/map_merge/src/map_merge.cpp | 454 ++++++++++++++++++ m-explore-ros2/map_merge/test/download.sh | 21 + .../map_merge/test/download_data.sh | 5 + .../map_merge/test/test_merging_pipeline.cpp | 323 +++++++++++++ .../map_merge/test/testing_helpers.h | 180 +++++++ 49 files changed, 6320 insertions(+) create mode 100644 m-explore-ros2/LICENSE create mode 100644 m-explore-ros2/explore/CHANGELOG.rst create mode 100644 m-explore-ros2/explore/CMakeLists.txt create mode 100644 m-explore-ros2/explore/config/params.yaml create mode 100644 m-explore-ros2/explore/config/params_costmap.yaml create mode 100644 m-explore-ros2/explore/doc/architecture.dia create mode 100644 m-explore-ros2/explore/doc/screenshot.png create mode 100644 m-explore-ros2/explore/doc/wiki_doc.txt create mode 100644 m-explore-ros2/explore/include/explore/costmap_client.h create mode 100644 m-explore-ros2/explore/include/explore/costmap_tools.h create mode 100644 m-explore-ros2/explore/include/explore/explore.h create mode 100644 m-explore-ros2/explore/include/explore/frontier_search.h create mode 100644 m-explore-ros2/explore/launch/explore.launch.py create mode 100644 m-explore-ros2/explore/package.xml create mode 100644 m-explore-ros2/explore/src/costmap_client.cpp create mode 100644 m-explore-ros2/explore/src/explore.cpp create mode 100644 m-explore-ros2/explore/src/frontier_search.cpp create mode 100644 m-explore-ros2/explore/test/test_explore.cpp create mode 100644 m-explore-ros2/map_merge/CHANGELOG.rst create mode 100644 m-explore-ros2/map_merge/CMakeLists.txt create mode 100644 m-explore-ros2/map_merge/config/params.yaml create mode 100644 m-explore-ros2/map_merge/doc/architecture.dia create mode 100644 m-explore-ros2/map_merge/doc/screenshot.jpg create mode 100644 m-explore-ros2/map_merge/doc/wiki_doc.txt create mode 100644 m-explore-ros2/map_merge/include/combine_grids/grid_compositor.h create mode 100644 m-explore-ros2/map_merge/include/combine_grids/grid_warper.h create mode 100644 m-explore-ros2/map_merge/include/combine_grids/merging_pipeline.h create mode 100644 m-explore-ros2/map_merge/include/map_merge/map_merge.h create mode 100644 m-explore-ros2/map_merge/include/map_merge/ros1_names.hpp create mode 100644 m-explore-ros2/map_merge/launch/from_map_server.launch.py create mode 100644 m-explore-ros2/map_merge/launch/map_merge.launch.py create mode 100644 m-explore-ros2/map_merge/launch/map_merge.rviz create mode 100644 m-explore-ros2/map_merge/launch/tb3_simulation/bringup_launch.py create mode 100644 m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml create mode 100644 m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml create mode 100644 m-explore-ros2/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py create mode 100644 m-explore-ros2/map_merge/launch/tb3_simulation/slam_toolbox.py create mode 100644 m-explore-ros2/map_merge/launch/tb3_simulation/tb3_simulation_launch.py create mode 100644 m-explore-ros2/map_merge/launch/tb3_simulation/worlds/world_only.model create mode 100644 m-explore-ros2/map_merge/package.xml create mode 100644 m-explore-ros2/map_merge/src/combine_grids/estimation_internal.h create mode 100644 m-explore-ros2/map_merge/src/combine_grids/grid_compositor.cpp create mode 100644 m-explore-ros2/map_merge/src/combine_grids/grid_warper.cpp create mode 100644 m-explore-ros2/map_merge/src/combine_grids/merging_pipeline.cpp create mode 100644 m-explore-ros2/map_merge/src/map_merge.cpp create mode 100644 m-explore-ros2/map_merge/test/download.sh create mode 100644 m-explore-ros2/map_merge/test/download_data.sh create mode 100644 m-explore-ros2/map_merge/test/test_merging_pipeline.cpp create mode 100644 m-explore-ros2/map_merge/test/testing_helpers.h diff --git a/m-explore-ros2/LICENSE b/m-explore-ros2/LICENSE new file mode 100644 index 0000000..35366cf --- /dev/null +++ b/m-explore-ros2/LICENSE @@ -0,0 +1,31 @@ +Software License Agreement (BSD License) + +Copyright (c) 2015-2016, Carlos Alvarez. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +* Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. +* Neither the name of the Carlos Alvarez nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/m-explore-ros2/explore/CHANGELOG.rst b/m-explore-ros2/explore/CHANGELOG.rst new file mode 100644 index 0000000..3ca938b --- /dev/null +++ b/m-explore-ros2/explore/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package explore_lite +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.0 (2021-08-01) +------------------ +* First working port in ros2 foxy +* Contributors: Carlos Alvarez, Juan Gavlis \ No newline at end of file diff --git a/m-explore-ros2/explore/CMakeLists.txt b/m-explore-ros2/explore/CMakeLists.txt new file mode 100644 index 0000000..1ebcfc6 --- /dev/null +++ b/m-explore-ros2/explore/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.5) +project(explore_lite) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Set flag depending on distro +if(NOT DEFINED ENV{ROS_DISTRO}) + message(FATAL_ERROR "ROS_DISTRO is not defined." ) +endif() +if("$ENV{ROS_DISTRO}" STREQUAL "eloquent") + message(STATUS "Build for ROS2 eloquent") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT") +elseif("$ENV{ROS_DISTRO}" STREQUAL "dashing") + message(STATUS "Build for ROS2 dashing") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING") +else() + message(STATUS "BuilD for ROS2: " "$ENV{ROS_DISTRO}") +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(map_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) + + +set(DEPENDENCIES + rclcpp + std_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + nav2_msgs + nav_msgs + map_msgs + nav2_costmap_2d + visualization_msgs +) + +include_directories( + include +) + +install( + DIRECTORY include/explore/ + DESTINATION include/explore/ +) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + + +add_executable(explore + src/costmap_client.cpp + src/explore.cpp + src/frontier_search.cpp +) + +target_include_directories(explore PUBLIC + $ + $) + + +target_link_libraries(explore ${rclcpp_LIBRARIES}) + +ament_target_dependencies(explore ${DEPENDENCIES}) + +install(TARGETS explore + DESTINATION lib/${PROJECT_NAME}) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") + +############# +## Testing ## +############# +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) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_explore test/test_explore.cpp) + target_link_libraries(test_explore ${catkin_LIBRARIES}) + ament_target_dependencies(test_explore ${DEPENDENCIES}) + + +endif() + + +ament_export_include_directories(include) +ament_package() \ No newline at end of file diff --git a/m-explore-ros2/explore/config/params.yaml b/m-explore-ros2/explore/config/params.yaml new file mode 100644 index 0000000..d42b402 --- /dev/null +++ b/m-explore-ros2/explore/config/params.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + robot_base_frame: base_link + return_to_init: false + costmap_topic: map + costmap_updates_topic: map_updates + visualize: true + planner_frequency: 0.05 #0.15 + progress_timeout: 30.0 #30.0 + potential_scale: 0.001 #3.0 + orientation_scale: 0.0 + gain_scale: 4.0 #1.0 + transform_tolerance: 0.25 #0.3 + min_frontier_size: 0.05 #0.75 diff --git a/m-explore-ros2/explore/config/params_costmap.yaml b/m-explore-ros2/explore/config/params_costmap.yaml new file mode 100644 index 0000000..7eb950e --- /dev/null +++ b/m-explore-ros2/explore/config/params_costmap.yaml @@ -0,0 +1,13 @@ +explore_node: + ros__parameters: + robot_base_frame: base_link + costmap_topic: /global_costmap/costmap + costmap_updates_topic: /global_costmap/costmap_updates + visualize: true + planner_frequency: 0.2 #0.15 + progress_timeout: 300.0 + potential_scale: 0.001 + orientation_scale: 0.0 + gain_scale: 1.0 #1.0 + transform_tolerance: 0.25 #0.3 + min_frontier_size: 0.3 diff --git a/m-explore-ros2/explore/doc/architecture.dia b/m-explore-ros2/explore/doc/architecture.dia new file mode 100644 index 0000000000000000000000000000000000000000..cb0a472d0c404a6661084ea299d76e2928fc7c15 GIT binary patch literal 1236 zcmV;_1S|U=iwFP!000021MOPNa-%pD-RCPPvQ5i8#H-UzSI=T*7MmEZoUKreHMRViXuhyV43_hRhOJ6pC^;&=Vw4S8RBsYC|Q9FPyR)eqKPn? zs1G-avhN^39Lct_6>*-D`HEwOp$JdaIr4vnX}n^AT3D5KeoW(3StB}CZx{KaPD(eE zik}0%6BOckiqWrb*P3`duaD~lr**ec5@$r1@J-S%>F^`}@7a`AnJ{AE!|gYCTMVge z*!gN3XeS7dP#O}}#1XNfMIa2|8ipZ)VR^2nIndF!F*zM>epxtrS-AAFa3q^0amrIf zcoT9S#}p%0pz?Hu`{QIjq9Vl(O>4j`7KHQojqxuK&5oHs(fqpFPF=ZSN`k`+w_?%Z z<(UM0IeQr?T!r_|NZ!{ZBXf$|Mo$<&oBPdq+}Gbu&fdH31GAy<)+rBxi!E%OE)U^~ z1UP#m&8jvXo|dKAG8YrNqNLsCNx>yZ9&8agalLdr&f(seRjf^putA4lK3^F3)c3!llgwo025uWryZtsjICG(0kdoJHODcJ~&=YaC$_ zFYzKH#Dc7KGj|f2$b&i>W~-=}$bc!lwYVH$JC@7}=jaTVIVTp5SmjwG_SNXOvrLSK0HLHEiCKc8OR5TYf)t9&Qp}b`Sm@hXtB4gRWv^_hA z(Hz^n*9E9%xOX}P@S`(UOZ=KfV!|f$a*BqIZC?qiov;{M&!Q~MCO>_Dl_2JC{t#Q? z56fD*20Y1H5E$dK7OXOla|&y9Gu~j<`Vj|hlw}O1EJ+_y`Rx%YORm^_d!bqP#ik@@ zE9*US6{a{6t2SlLa?kmqmV@Ny!e->QXF>~j7W8(HZ4JO$ViGQSH<%7wZSOHP@7;3V zf>3${M77L#8bqy**P3B9>g_7Tj0)Es^}z#&>aG~YjJJWxc4dJg zLN{V;h-Iph@lDBoMb|$uU1Lssw9k8|kD!roElHhN;Hq-oAom@XqsvfweBqwK0mqpOl>=6gwa)uiz?2@cjIVCNJ& z=7C9INC0O9LZzj3ybN?=8Vqh2FIa{q_CZ<5hsh79a@pygqY17M)ds zx^Eh+^V~Fyt3vXo!8C;Yh>UbS{azR6Bg?f5Ir9Do7Y&v09lU7Jtm{2P*TpDGTJ9O1 yk^prSv_8abgLvfz^kj-ak@}arZZIx--B5hae;V>|Q~B-4!_9vR42)i1DgXf5dshPh literal 0 HcmV?d00001 diff --git a/m-explore-ros2/explore/doc/screenshot.png b/m-explore-ros2/explore/doc/screenshot.png new file mode 100644 index 0000000000000000000000000000000000000000..216ade5b74a4b4bbe525c49d24b6e5b46cf278f1 GIT binary patch literal 24644 zcmXtg1yqz>+x38=NDtCT4GKtybi>d{cY}bG#83`x00W3frywAmA`OCyNOyNgH$(S- zc;4@KDK3dS&beY=d+%eIhMEFC4iydr0>M{Oglj<{cbFg$40~)$@R^lAMMvPTJMJ<{ zI@s9QQw!?z;GYj%6%E}X5Q5FyKNxO3Z{i^kdWaHSTIY4z)=Z#>$ug4beD|`yoyyYE zdQcHVnSPj8Vr6#NV>o2`53JJgO=`Y8MVfkvOIz@Aw?U)yTi zri}P_{>jPjFS{r%x(*~Rkh0$5hY(aglomPvYlDlI2LqD*@5Kp0bJ5$HmxI(TEb!?2 zM6ygDbp&&xj$Nw7kRE>Be&wr&>o^jLsQi)l)c$wi!-_t$8&W%C-O7#Q#XKj;VN@t6T^4Z9l@8tn^y0^fa1mQW9gwf?!R z`?}@+i~WC)V`haDW(5oqXv(kpo>+yVMm89X_r(&-sCAg}y{T#Fk0$T7(IN~IB3k7w z@Vl4(-p^8hT6Pa8RF`>jjha5|$-5RZMv6awBsND{HuOo4n8|1+S5F9z5Ah_OBzljL zFjyi*9SYOZX4Z$`^!@crSXg1YY9*s=-EgAZdmMrI-3#Vi_D}?P!rOPksEn)OY_u*Y zDJ|4J(fHh2?*++!2F9N2tij!l?St zhH@kAy0{5B74$WIscq8-mP#-qQ-fuggDEeLuA6g4Sr?6A(#Cexv|js;5LA;J+_r>3 zD-rE2%^f?t_HV=?(<$F-1C}-N7B>Qp?*&#k{XF^Tr#By+@2cZ=^nKbm^2iN+Mi>r> zULESK+`XP6c}{+Y-&{xbY5K?_Do!>7u$lvfKf%+z4pBpvMINMU#bv75j&Ygeo1@8~ znz5EkFr2&Mwi9w>4R)mYBk~757pXsUkGH=Gwabu0v1l2|H8o6LcMP!Jm&L=QIKgya zVw9b_nz4#O<)?$Se=C>LgCEaO@M1#u`@MVBoj3h!!#0Nf6=@=R7?gN_OK8LF;ujxA zz>AVngQ{-!eo2*x>4h9|z>K!RD&VjbP$XZ}9MehUCsuXiwQVNUIOs2GA-kM0C71^u zYVLjOl&ciklJqN&vrnnBR2g$WKh$w?nP`-QSF&>7?hqrP*eUk>Q+s?oCvlH8?v2_) zgX~{KT1^N7V-T4Cy*P=<$SQ?8vdhwVvomGX-%xieRg59Cll#G2r3jQB`7yYR+Kg9w zSZma#sIjP%<11l&6}OKeBdSfceBZVsI(EA@(}(hoZgl;g!?HpwmS7HQckUR^sOFTd z_l{N|HCvt4rbVJ^%>`xQ!v-b>j~$Gplkj=IxrmmNG&bt(BJ3{o(5csR522cj;F0+` zM&Rb!JJh|y$<7sHM`5DJ%*|#CAhECr8OfpXtGnGN6%&piE8aYm2!KH~TR=oqRT^Br z=KLXTCMxz#sLDm1!)tPY&6+i!!zVvg{uwMw_x<((TzYBjt;Gb*o5N#dC^MPmLBCbx zUpF2XsOACK;Vj4lnJ!7FCdcg)2n!mUZwjdENyrx|n`k}GV*1-IM1xZnnCf&yWcb$^XVSb)(hs_fcLDlZh zRUtov6Q4QpVnH?iU0dj~eolb0BmwnseeGnGQL}^S;K?dttIgpp|04(Ic&E#mWua-Q zQWZNVddYd>l9-aPwjtt_T=R%(ByV8*`a25ME)J4G?~QCIfheKRp3W#vZ?4F@77!1b zMPU1ISY)|Vl77yA*-yhEBf>9Akp2lC-2n@{^RFjtK{21|>j~6VopcL|7M(fF(Fsve zY{bwi?Atg=X&U-YTXOGyqEVIDa*0$V^>QH$T15gX;!n+BmV*e^m-53_N^Y*Ni&OSU zer$B>xRgbm$ihifZ;Q{Ot>a67r2*;BxJJZ{(=WLaYC)+kPmIX_W!snLb~>oa?+K#V zlf`EwPhmDKZ5Uei{SOM2Ld{4%urV~^NrAog40&|AFuykl4c#LBQK;JV80J6)0?sJj z8aaS8NTmLD>UJa&*$b1wN|hw@3852sg&+ z5HBs-Ebq$Ye3^VGf7L}4Zb`!4mo{0Gw>8rJuv$K(dT!raU!hFIy=|^TX7x+wZANLT zEc}KII6Qf78rg(Pn5F-@ZljPqOaH1YvIz>yx@Qp|ksp0%i%dM?c(*Y|)>Sc$oNN@b zGhC#iq5JoD1P;Gle^lpSn0Z%3xD8jLwX{1|BHvkJrBc;zlOu%&%>f4>_$>Q^Othe6N-c!BR^kE|Mf2jLv>n#o^pg2T*~U3 zH~c$`p=KR7;=Cm!0lE1-%m>fv3Cu_iL~lxDUX@tOZ!0K+dWMbg+QRW4e6Q6h{`YRF za{mkY=Q(eQ^`V(Cnc$q8V0SxrcSB0|N%!;0h11?Y99{=H3T-(_!I5>9n`x|GTZ^juQ^m-GX)QMQ462EHyO5T=>yZS6c%zB}Gl|bn z2ZJzppw^{ zL#*Rh)H9cCemmV)^~ugGn&F!8$ndkf8;p&}pgo7*#f2^{TVTu8g^Iymx|z)_J%EVk zsTa+9xpxnenoj?`dZ_Jt-?#9J2ZhpO0AU^y1}ifoA;Oe_JPsInyHtXWhGez3etIRF zOB2Cko!yR!olXC07rl7ULDx?GIhHJlCTt3_r7ls=$dg6joG#{58R#AvKn6-yXf$|1 ztDNdQ0c#xW#6`8*AXkBzF*5HV^<`;_R%{&2rKN09ZaRhfhXssR#@)_G(G zWv$cSuAke?t#R{!3!JEsJEydWLFjehEPaw%D3sG)pF-9>p_|C%eIl&)eXUOSUPaj5 z|0Ku$F6**k{l&)MeZfkGhYFJnE_KN`44PX8H%#_toeU`o23m|zXfi0#BMCO~ijqRS zl~<3;ZP1E1pCN;&J2a3WBBB@ts-msjL}GpHU>3xycE}pMb7u)Bq`(VSg*s-R+^YV= z1cQMBUeebZii(X61<26a&R-AG z8ddnP6GdJAMDg*=xpjeB$xtx#p2pQU%JnHii7U~jtC87ZrO?EoWl4QmyReZRs`(ux zN#L4v-j*sxDBGI$7o&tGwL}G|icMi>um)ZxOM%LYw85n4IOCL>`u5a|nwv`QzS<~Q zm#!QPcK?Hk4u`u;Arl{u3x?2Ovkkbc&1cTWN>cS@5T&em6Wam#?de0}Erir&Yh}9` zNxaQGA7tX|KB4mKco3S)6j2Te;qneAEh2f8q~C{eTO469ba-?G>#y?V!~%a`iz;YU zot;jDX^@A5ENz!o!aL00a*`rfVW<>RMsnoe#^A|BzwQUQi;biabbUF3?VF&=UG!`{ zu|?t=!$PYue`CG>&5}DnSwkly4IfScD^qn!9nY%5?3{m>@mEnNW=x8prqn6kXhwu# zhO>J|LI>l}S}&uPl#t1!Pz(y?@xf$C`o*TPL~hQc+!OP)2Y(amOdyY4Iy_GCCaYCx z3j);M$H=V0CYM71f`(fMM@L*{(z<@XNZ>o5=9nX0%CGb;Z`(P3)MR6C^L9V46N>}n zwkgxhh1Jl~H{s$QSb{8x(<%&B0e*Z0ai)>#zQRo?PN-Oqexj&5Nkht!P-rqD*h@}? z!_{bWoN)>BLEER=WYrGZ3a@!}eilZS;7-Io9nAswVs7 zzlR~g)_RuGVgxC)TiWx;)JGRv-9T z|D=LAyktmRI#YQ1oAipU6^txA@|+F=g+5EO1J^xOwGUrbM8krR9^Kip#2@~UC*u#0 zNiJN3cJhB;m!N+p<}n1rsrZm2*R36?oa=`Hk#TrUaboZoh5#6Pq|p9pyXC6^VIw7s zj!4g>Z(?Sl3QXDUSvr=T46rVDNI3S0hf7BFX&5NBVl7Du(4e?lL{9MU3LWj{`VsKS z3+klFnl?l_HdOQBi=`~1r^c_s8UC#p3Ws7fRn)PTJkJP+jfJR+lD%jzEWY!zjm)!b z>7I^TzCk#z=eUaI^5e6^w_emXfzlhUzHoRCsG^0shL1(=KRrO0VQrh(lQW7xvGBBz z_~_Bf2Yu8o(O=H>LHLrG@>r|bJ%z_FZ7YrAsIYVAaj0V4&}tZ}m>Uso9KU`1;v<|! zF(dT5v*hp$lVtKB2^(L04J2)C_q%8bCaYq+>06I>;~er#s{+QtH;-+72f;_Ax3A_{+?e9K3ekKFf#pz-E;`iEG7idI2_ZNq**cKu(OEl#52ZL z2E(Y>h@$0kr+Pva^#xK`d(De)51-Nnk~O!_jYsau<$owsCES>Sf&Hg)I9dr^59>cUv00F z#VSD6w>|su_wNBwIJ%h|V5zVEO_(EG0pM#^$Z{9-pGTve^5G6#tzY76S%2m}1BqOW z(ELoIA}cWx?ap0egGP-J>wv~VHjqAcUZdmB5E>Zh3P_bdJ^Exy7{9G{{ zt-0hzP=CRMb~`cT2gtB=HU$X7Z=s5LMBVe)Hg;wq#r6ERM?&K7m4r*)E~|(>_2^xA zbo5X0uhAbW&$n^=IM`ipbGI_`n{w1*zga%7OHzcx6Tr3fx*M)YRO0i*vM5R35D3o- zB7c_?_T_fGKzmj3uGx#LA^ND;+Y{3}?QLwJUG_JNBt(O__Boiwim%8^US+aK24EL8 zYp0`7jaExp=h0be0yFpDt2Cd;KoBU-EPv3_vfHVuQ_>Y0HIz-yeN2om;eS@N!?6pN zRUi>|E^S#{CtRPc(MGR$0B$I&iYUmt+dU;$gr?p!hQm2f(Gd>qh`x1@{7y#4B6y_G za9Cnn3j3taR4n}AdNlb&4zHG?ZHCBdJecMLW|+GYA=X$!&SzShQIsi(^fsgKW|eXi ztE|H`{41ARZ5r%C)m<KT*f;lH$4U-YZ5Hg35y8L1N)PQxBDW?^-VVq|I1my&r_Q6GyULs4oPUKXYCU>7w%ShQsl}{Q7IrD>U4MlO5%nR?EKh$;Ur_Sn*6fm|P7 zU$u2xD$ALAWi14N3?G_} z!LmPM)R)qwOPWm+b_Wl_%BTARAoJ;sG8s60_%}t}+xD{jrNk!1Zj(h;w#Z(iX*?)& zz=0%A;Uo+N#$pQ4K{9ce;N{-wkPjcDzPY8NjEKPJWGM~ARuy|XxyzbSpJuS9qcgWX zX*cBI!>*v(Qf_@_P6T+>lEj)|!&LSi14)a)esQ5+#KBrvK0k=WCtu-iH-B`pUEX6Z zb&#JB1a5JKhDuP%(Y7nbt5}f*r3y5g|4w+cFbT$fDzG{4FJ=M|sMG89tp~Vo-@GZw zePNt&jt(!A6h*HxW?bMRuUdB1(uB@qeWod)n)EyfEVX_nIMI4>OhQRtBXKx8~I%olxuW9Hw(Om>Y+I2$w04b0Tptt(vmasUi#g%NdT;Y)e1%3-~q z`qaaKLmy>FyMW1QACZwiMJV&Xv4LrD&FPTiE=tyj*TBG_Qgp%@|5@q|ZW|5+)~*m8 z!7vrl-PadcR_D~x%GFFIC-6ZAW8u3Hp%lge5{}L}tJJh!CVb;mA5{QIf?N>BPlRG= z(uAFkw51FJO`t`7x&t;w2$XWUx=I4wYQYco*jWl`>6Dt=F*^B2h2UG0)2i54?TtP? zWRU1OVMr=d6h=FM-PLlHFQIonNv8%pO}e&>{1}CCltct3okP$VWx$Ni{!X6)>&i_I z66qsR>SToIRr+lMad2V|xGN0dv3^;$6$ulp*-2~Vcbqk=Is9rLux$isI~VLoG-itW z+Mg`{2-1%M!N$n+d&&YvZO;;OuCj*FvYXWAn@)58m7Vh|Zot3X4s4c*H6E13?k4sTg@*f;?T2DB^FbQSUcQ34E-g@iFc3m$n zNGvM(=vSxD2$sI{#oqJ4bdj@^Wzv9jpNpB#yPp+I3QdIJIIqpw1JZ;$ywT6BD1x!k znS;1j?*krVsKPQoE{&DOk8%!#4U@;t-S79*6n8!aWczS)BZ%6PK1P^>{0EaBS#=dt zN0IW<7^7g?VbB($`yhYoOtiTqFVJ0mlN{`3BHy`xAQpv3)Lz>xXeC#}}%}X0e2m5S~q}m;0nRhMBH_ih)FyB}6l=37z$+x+8|9=29zk7HP}%AHw~` z+rZt-niXRj<_IQACTe1+DsR6ue{b{Va>mVhXM|?BNvz^p!RF{M!a(;$a}8E!z=9Fr zMFbt~x8EV;_56m zVb)@x2s1DW>*xF)$GeuAIB^baBpddL6u|w(h-krHYIe*ddGF^gLFy8E7FKhypuA!| zUc)1ZikA#S*?9u4p-^v6`-|!LhOj>sM!T`kVEtZ9Hh$wnSk@#Uq+LK835hUD{LNWv zgw6G;Fe!>*=*^)>clZ3;I!{Lqbu|mE8!fsXpgOS?6fqbK!Lky7!ZNG6JvGF4S$>y` z4{72SV1FY~cspq|b*S6-Wn?XPY`T*DBs}q_)T=5DS>kB;+Imj*A2E$~0%}*b%bNr+ zYSQDS(h$FNrF}cl$}c45b>@3ec0KJQ!g&C`Rb0N5NoSLp4k%1NZJ&XJQbXle{U7(< zRjSNDGWO$gB`4{+#!-U4jX8Sl?~UoPmiCVyK&_iE$apnMsJ3;$r7jZTQ$pkq#jR{!|Zj3rno1iOpI9yBI9pT<$qjOJ6}U1 zR!L|4)9v>hx~21yLu1ntVwcJMlPja)8_O{eYZmWh-MPKquYd}Vh>j?JesnzP-{pMR z_(ZSH;n(FKSWxy=^_z9`<16tXLc&1X3X(>&;}~X=(S&pvT4Q2sIX(Sb{b)%>>i*_$ zIJ^jtQD5V(>tQ2JXrqmpfi+j+u2Z@}C6hWk(|`*FDZ^H}!Ly~WxUw(jlUg_fU1+7o zkj)k6D`TRqvIcUy)@_3?h3p#w!D1i1okcFmg5+nI`6J;{jdIz9EPcoa)JMO}Ns3_!-Z zoQAWDpRf}_3+TB|PDaKNwTYe>TP7F&u;}7pMxjthJp|FB=9Dc^!i9XK-5hVU2T&E= zc^VojdB@~ggn3gq?xBjQ>jrtiB?}y`1LQB{d}CjfDyLmAYa#<_U5AowLX>MgvXU>h zNvEpsj!gnqutSF6BbTiZNiYQfT1d>@E<-Zn;E*eOKenowrtdK=2RnPI;gFfM&fBLP zSPpmPt(~3g`mr7l5Ehj`Nvc7iHbSC{Chl%@w5j?q`f+&lba(6NX$>lg;pnIp(j0wR zY~H%?1$-`w3d|T{?rY096HcXgsiRNIMoTswqVt!e=JxJ)`Lc5ksF<*9e`5hkA$(X0 zG;w9~$M@#nveo#X=1MB8F9^LG({{&g$7s*uh*HGJbe*DvtmBc9k@@<}>dqNE+d8z+ z#T^^mEk~l)08tVWESK!>P*VyHxS)7abVc z#FY(nY@5E0xCF+hLgUiv;Ojs$SX$~y=8LboLkx}n7#-2*2HN)Kveg|uB3H|3RQ@** z8q}e|a$RPnlXK9z-@$UQtiPZ>IePgTrQSBuaX_J$A(n}xLr0~i|GyU?*15b^8OV?S z-Qjso?!gahb;pC>ivS2iEJ+NOaWGpB&jKDVV-=ae;gPNFBd^~~l73?sRA*-`MvRzS zJFDGj<@VjbE4m++9}nVOkicD|XZxTSXwkQ@9{snplT_aF-sWL&D`ucK)&t+${f!8G zN?cV6)JAH5Tu_bQQeS_P+$jGA1ITxym6aUndh;jFlq*rJR4A%e zR?{1x7YCrIGKPwmfa)crnvplpC-(F8HCKRU&&cU1!~ZmTj<0k|@%5a2jEMc5{hB$> zi_%hlnsQ}*QanLRWyY+8)^<5s9dN^uc9V^xTS17iy$^P0{#AFLTGPER@j*{CKX@*fDnusZ+M0&--2@_v)E#7#^xRskYs z;Hf&hay%PZU(f>6>%##MLOOyUOr%#OxX<{@5QU8Q3@ZCX`#!5@4-MsgdY4*FAm)Fn zk-hsCg}VF$LajHmQ`Q1!@Dp~g^b;6tkO#qU=87PGBxvwe_c_@^{3)Z`oO6Z0W^{XP z|Ip$))ql?<*VIwGRm%*#3&GJvO63;va5#xI$R6;EzpY(PK3%|IS`1m4D=0hSi&zYe zY{G)UK$@5@6WJefXdpN0T3e5g!SCC6`vz3a{}kbr=l@0eU)i)!%7%lw1wyDM&F!?S zeR2HkoXkj(M!wIzg$|Z={*OhcFu|$ZE{w0}E#sRG%Cxby26|wFC`RzBe*fsnvL~oJ zQhmK)5n0*|nzm;4o{Iva6$$w6L@*%!3&`_=W!Zw%%}TIct2&{%Lk5iojI#~tuKj2E zyk>8is(<#cG%>ZuQV4WX(z=UQ@htyC^C{B(lfQwtqsct>^C@YPq>rkZiCe1%`%M#P zaox0s8Vj*#o5UTCt0tUr7jEFP071gyN^tXc1acd#YV9x0?-R-Je6YHEW@rs*j64IK zY3aR{ukW#b^Uis@-WfVx|1Vb^I?XWcgq5?ujIk(O{5e(L{}=-2`8?{6&-QFlEZ9Jz za`?@_Eer;V%Vj=7KS2T9pP7Y;O6EB9A7n_*D0tnzLOfoGR4wodGc!Uuy})i*h=;5d?)T=9ju5r0(~$6gZPk4g{0BI`#au)QCEr z;3S}q>eK;`j-#7l7q^bU-HUU!HmF;Zpu~U@PT$y`d4_a4hefDehLEInpUDq%E^m$d@CO0(sk#QCmldh! z@vjX0zHAKoxguDa55g9@WJ#O`HP<5r+5ywiC7s8W1Y3nx7B0mEct|#0!Bu`%n$0<^|0!gw`}+*iRV7ze7yF-P zrwd3M`8gTw0?ms?hUom)apn!9uGZFe4ald4h?th69%R>LCc?=4vDf07I;&$wGlQFG zkbw1=#loXTM(I_>8?pv!?($2GgTYM0TV63oD&Gw92|qC?u3?HfE8*6&!XB?%7mmGN zR9|6URq@j5m+N&ncO`nMvzDD#DSnfW?|bdupuI=)J9mVg_8bSjj+8;bCm0Y40o7i7 zWi#X-!fcf{;D1v<_iuP}$3=>a3+qP!jdlG zA{KqULGH!8rR0bC6E^uBWz!GzLn%PZ{bnE;F$p2~+Sg(@E+%b%t#Krxaqu(cwFKp@ z5Iw+aA1zVtC>TUd8#sJL&9^7e6S3uVHS#NukPZ>pv{WJngYeo-@gm_=sht}N#FO3F z>uce!4J{9;s)rjE@88TFh0Wa~l9mW5W)&p`f?v;cR*hWi6a9@Yz zY&=$(0@^dyZJI63&qwEui9l9W$~>4GpjDvyIrMMt+5JBpu?mn&bk=OOAmU)HT=f8C zLzgaRlp}J+B@N4!?5}<4$YTx|Ob|dissp_l6W{Hs^F*#jjkEWtXN$akzmRp$l}$EX zkHxyvT18IA#`qE%6Zl1CTf6GU$yye}t~nsphc_i>k4KoR)+1j4d0ts1A1RsJR4{Pn z_{r>@$9nlxqYbB(u3j0V7xAp3KC_-^I4? z>rLFh2lwLS*u&G#p2>z4(RCQ#lZ=B)2Hj;k3Z=}6njN|_=HzO6S9OD)xbbQ0r@M7X zu!TSXR#v!stbPpeCx^K!HlQ>h-C2$pj@LYAaRIi_Bmv#RPB1z9^iW4YW`79-$yJUz z2=n*(!w7<9&MrAwPYtcOOUaj6#L<+DV;f{?p>#RSA!0nM9A)7uxDlF=TZJUd9Axg3 zF+;)e8QxO{NT9$a1|+stL0#DFARz~=w1njK&E80u@X#fme1>{lv!lZ*8?o_5f}d4}s?fx1G4-mWpN@iP+p9B=f1dX#@gRQLjCmOC<{-!4#I3Ad1w&c&PH=-+2Ln4jy^h{?wa_09A&+b zL4dJtZ?inYZ#*5xJtn90FAy33zEo}e-u^v|5`?Y(58QO_50 z!cd&pl6N(>Qo#GJa9(p6wR&AfM{^Y>;!OEnhk&}-Ul20isUS8hzaIJex+I5p@1*IR zG-`|Of9Q$cM0{(9G5jk~Y@r#tPF4pB zQKoLK{S%3EwU<{9T54pXb7sr>NmBz)XC6R|In4vMczkU|JkeE6ipAe6hq9XJu`Ldk z2G)BcFsz9h&b>Kf1J1-?(4~w=H={GMV;Dk&FUbW=sXU+n89ml!PhU`6d zm7Ova*n#dZ3_r*zd7tj*R+fHQvEIaj25y#9;BAzcy$6Ldl2ht(>Ai_1EsBv;Uiz)kTv40EVd%Sa-@N1PeUarNOde}KyQvJn#q&Qj z=(sFPL%#P`*m4hy8;GYsA#JI}Kbd;;6j)^es)R8DCZ&y+kAojkFpmX!y;0zjtX{43 zjdH??C%kd%{hfKSw5yT&qAI4hftsb=fB)|5$LTIYx|9^{MNcWbS3)Q#`z(ct6@y&}Uu zR%KW0jD53OHDKd$VL}8O8ND1$`+u214G$M zllug8ZgywbsRo;&uLRzCKvwnK**rJR2SW=Bv7>UrON?2Y`>OQ@hx7;z}4N1|vg6|}&S z*vqb(EgIR2X9aY0l0Av4a*9vvgDGf~twuLYB(w^0&ZO=E%i&s`h_8JtR(DAjoOv;+v}!Y4Xs*d2w}@_7#!*zK1e(^Il0g2lFcseqBLGe z|7JbPHKiccd!qeGqSYSEf&YVvr>>i6Codo17^UjipL7PSfofskoRLTl2GF0j=a(xT z+0GJ)8%lGFlNMAn)J%a?a%55z!P!@-N4w6+Vz4fh&udH($l>#JNPpNc;d2Zlqb6%C zzIOrzr5z~E_(pH1A0oF34TjX+Gyi|;3S2QXwNuNTJ{y?j_GklwR~Z?X!t=&Uz}~uP z)NE#zU^CA92u^Cl{hwv-NUwU{xsMl<-uxnQ8@dN)t9_LJStBwES1HqxUr+)2{~DTK%CwQ3dwWLE8J zQM}o`NAlxM21G#h+T%FOy*Kcm>aSc1y_~$J%lHOb9}UT@m;}>0aD<~58wa|C0gsOF z$S-8k-6xX@*=H;E{BK8na4i%I5#++4uKU$JEYvc-e7{5u!Geq6rS7o7z6yxo0Z_o;M5XG0t-eyEbj0>ShD@^{ z`2oLsRgf!mVQ}+P<{z7&;7oqKh^CbHPZ^60Gi4dU#UG1f7CZ`tIC=3b{)TB5 z0UHP@=4p-05Es^{;@9nx>eu(AZl^7)Uv6zJz=FB0Bpaut!}J5HBD>V$6Qpg7M>%Vw zFqgqYCP~}dz2a6dt&TxKdv0CovGVWVeF(&N`sS;y7E(pGt~+@vdFfw>H$Jm7=3X#v znIgt#MPU>3fITpD`38y^7A4<3J|+o_UdzHoZf9%#fD3K9-YY-hEi-ewr{Q%%PR;GF09F?~Lqh(9-d zjt!abXNZVQ&^sPxOx@AWDf92hJL21Ion)!dy2IB!Pt-F;rUW+ww%vI*^-a(llEiT6 zUrjy?dCNs6$$55tD=hNx5}=2$q<(PZO0?E)g3Qkjv_ zKX>cvzcy7_AqNm1g)H>!1u7v55Ib3UI$8|v`#+Q|?t5($%uk9#NA833AS0j5b_dKI zBmD8{yHFQ|bbkBhk#(J=b!OL+srQ2JbhJUH(`xzO2`HI%wn_R6sz$3c#SxQhlWTSX z<}N$@Mnc0;c1jYhX}*g}QO^FIz-kAgEAr!=;}@Ew?w6PZnB&nDVK8dhdq&!c?>STe zIr7Al#|c`+E6-WMM9r@E6ETrbrC=uh7x}PfsXw#jW}j|`hDINcV08W8t?gp_W;KT~ zM4qkXW4PD02{=OX85q^0@0XkV2JJWf;lCA_9d)NGHWd$SuyLU!T7YCC4K;ioFo8JB zH43~tG7%Vfbr$7kao*1$<(QoDd1m7U_7t*jhV>+=0euuKss4aH6!n#p|J}7gV20@+ zuV3fvFWPd`nEYT+AYOWj9zyHBw%L(tgCiTejW6W{KpmdizcAmAYpMLIyV9W`{%M`< zgO{)Aomz%(7*g25l1f{5$+k+Jl79jz7PM7c|A^ats}k{dG`sef;?L1b1DertuvsAQ zaF((;t%OJWDTl1!c&~3lN|Gw+AVReHae z+;I@83k|m5I{b@-2r@fw|MbKA4*7WK!c%4>;7~~c#d^d@K7kPCnnEy2;^c<&4gLO|)hxz1PvL}Hn{&YxAhtNx=bytK?oA%m+6|Z@M=IDF(?h(+4EbS#_ z9(Yc-aW!FXS)ssvVrjQ;ElImyGvAoNZ5@YS$G@ z*v+Q>tu0p^?gXTOm%8Igbbe6z7@=n={%|{3^|4QY%*wRTi+QUQC9yf`IdL_(UZC=0 zz-G4MQsW1Q>Ub-A=suB%ea3xPPFwX!?~sY@vljfo|6E}l4Ga4UN1sa^joub+EsjX? zw0+Wf$$D6O#>^SaDZ9a2GdT4;kOJ7S?T#xNp0u*N-#ww&bFiYpxx!YB-ZQ^XA6DvroxNntS^;v6$~3#zS0K*J z|BSk6?&50Zji3m+oD(Y>U#2ZFrGefdVr-#l3Aln8e?}FQ6U(8)>=nsoodM=aqrdb+B+@7{D zYRB3fO(jgH4*N{^5%R71YB&S)v@7uR?_h_%b8Jw~=Cmti(D$1?oxWX@ljVJK$TY8q zI{|ckIlPt6LdHRNN1t#z|5N3^6CNQ0zl|;iS?$DY>5v*Bb0UGN($b^N&-Y~wV%859 zd;bVP9C^Hp1Bcd5lMdXYD^{asY{K%_K>#wb-Xgvr-2jBGu)Y4*=a}+Rb5#%Th&YHd{k#e*6DdPEHwc6((k(F?Lzl+nhPPp`9R=*jo zxcKfi=LUBBcM=TlFgi$oFgbc*XpOcO=HvG$-vGmXfB!b9j(jNo4chz;3f>_QpZD)| ze&tMjb(FB0x;^ewWz)_3%=4AXG^I4g&o@iikfsE`(S=Z6YxYp8@mnq-bdyNa-D0Wm z{sUu-G>NiZ`|Ux1PE$(?O@^l(&dc*G<61g}v~ovtI`vm<;07f!R4curF1s&w3IYKE zfb^GLJjOr2!R8?5IBt4|-S?gW4ns}yy58C2WHjIuDDy~UNsI=HM58syRVEk;4~_s8&a zb3?ww)x78N6n^FsPm}1dwmA9i$ibgdljhl@H!swGus)vweJaH1{^0Gh=#j>3F9L(4 zLErk2dF6z~z_N!P%!m{KAj2W@`tc8QuB@+|>il8@OqRdCdGjXTQpHfsGyyky1+6>r zxxA-4m@_o}95}kAC|dJk{|34NW_tJtK#W@mZ<=Q|7H9yeh&m!Crh9IF+HkNYRUmV$ zigxR|;CW#L+Wj_xXEcf>dmSQ&+ z;1E&bnvP~6toMY)TMyVShQBU63dN*&NO5~KbbvU~)WqS??-8)|VZA>w-P4c~3a!SQ zgs^9cM;P2|Fv#Ro%?v8BSgFxu|ZvS6^%Xn~_A0AcF_oLL!o6R^j)NF<8A zHeXDcYjl=sN=r(rWk07y&b&D+(|+kIG8ZdN0HOrq7vsiHMKNxa+C`vKA7khJt3-6S zuA$;bO~yx zWc&%mZJYx}ZB zZ@O^V^ha!su!|vC5guDu!nY3Blu{?{#E8guovMM{f4J1&F-#XT*LUbXYEoW`=r<)4 z?_cw^LtGyt&R+9}nXJ^AXf0Pv?VhqA62Im!MH6h6NY-YxX z-e!%vL#tOm<{xf0`Av0;2blKk%zpjz&deAV$TB?Ygd|puzMGxcC16>asHGj-?FVqEtXr$1@W@;6`mE(@? zCgz^}&dWr(xmY=4;C2t6pUe^!d_{9Q>CIeTTAiWQF#SS+kIy)RkoQBXcVA|w&53Ik z`)XZs468nO8<)Fb*KqcOu#hFRp@_P62*Cy(Tfa%TCXR6rAo=diSJ z^@1+tNOHQh^0^T8W%mUKWzklPKfx0=S-87lF|Rr0PE2f9j*|L*QAIr!o$up9Vn+Rl znBnCW)z$NaEb@AY>av=bf?yX(mW)l!xU%!4$ll?CEZ<%oyS1X?RJFg1S$AJErc{)+ z?ZH8rhlk0)2!Dw=M-tTC!a}MVGMJ33X;WlqQ}gthHPe;f>N>G=y6dxy^@@qE9SItV z)}D^uw7FIydF!y~BF|)P{hge|EPc})I=61)zcOR<^Yb;$S0Bz1BHop3=TpLPRx<;< zt+{6|F)J>*H+ryL#|^KAU>*JsK|i zC$T7mmzV$gSz;b$(uEdTB7NAj#gaonh_2ks@~;0}5m;A9;Avh>XY{lR*N;OjHM53d zm-Y03^L2H92LpfM)Qhp#hu;0ahw7%Sb471l_Z;UwC;ci$E|;eKv9FEm6fxo$Z0`|% z{r0{*)$U7d1>{MUtt|F4AejB29$x_CrD zK?DT_5e22!fS@8>s`MT@LFr8bQ9wF2q$r3GLI_={p)1ni0qFrk?+{ujp@$MfeaHX% zDQo4!%-l6+?z-pfy?-&4gyU5TJ$Ut6jiGza`IdBJl<`i=IQa+i*^!uREx*~>7qJ4& z+-XhQJ^AOWUs1rj94m7353S3K!>lc$mY+Cn5#Wj)rBfL7EUbJ4G;H(iy88GE!zUEP zkFBaclYjsw#EoMiTRu%imWRe8^~hIEkpLaSDh*BOF!zln$ z7WZuA*B`luNiq*-9Gr-gFK)lXUDN3O0x6lK4@KzY4?im3MkA(^gnZfQZ3l-TPj7`E zTbUo2&@#m5=i~+#VAd?V^`fvl1}46_$8UlvMPAc=8rVzHv+6_KTH#KjEcm(gkRH#0 zy{l2%PX07?TpguE1K3Ih4-mKqZlkiKQZoz@1tTg|oVp=>MJ8h>C)vRhtf20$Z-Yx%T_1QSH z$Wd*2h@VYZU_6z*xA$6xQBIriD~w(KFOSMk*wN?D)du8+TVy;f5Bs*uCiIQc)@&3c ze+V|fx`O*j^kre-!Tt>bf8vwZMdm|Tf}5ne6W4q{c$q}6ru?U~l0Ls@xH9)5fs`!c^Eh{7jO-g zB}c+dN>j}_{F4u>Ovi>!jRvK=Sec6mLM2V_d;=7(f8&~s zMUhy64x(5}#spF9o+NY^a9)KirWEI+wP1}W>f<9LWm$BMb`pQ9^4fdl=qbk>|LrVg zUL5mxeYv9GkD+NbCeq)PeckCL|6zuBf8%G2DuXauW+SCxBO8s5II572gIoSsa5r-* zJ_SSi)tVd#t6`-u0r$&*wxdHdKKuQum{!Eyc1xO#o2y1&Ds;K{sK-!DZ{A8#5W^_NJm7c*v2*4c9zxi7f~fRDT*Bb{PVyZ zV$xV4{}O=ajO?$KJ$Lti#QL9L8Bj2*?|j$o(qunw~rD%J0Y#f#I3*9jaJv;}Og z8uL9Aaw>Q?+VE#PW!&W$+%V}lrntQvuYnbrYRkQQSO>1POc|c~otlt|;I%`C=D$Y;cLEGO~X;`8e}wb!$Ms1OdgHP6 zdvObfyKt}?>SrR9H-RT&q@lt|;?!j6i1}L?+_LvRXMt-@2J8v=V-B}UL=UfN@LM5M z_P+Atl8M+>&FlB0Oc*~3sxD2&S6>ChhcNBXKZHvcs%3mUs`oD;RhQrrsnv9Vx8qK> zfdCoeO>+sblrO&rbhG1Qphz-OC%@PDzUhWMhgB&0v2Ee z`4H2Yqm&@nT_VoJ;Mo8q6gF9@2_I)%X~{94BRZX+nXmI@CKUMH*lTXk86-X8t$gls zR`P0Ta8vU$XguSN@87M#ms6<@2PHZ8X#TseGm|`iQ{}=e57NurKIp$)rWB^5SnWf}l<1 z2g4k!HA;Z$ic(H`shecf1-4r2*j$KLm8gACekEM=+|oH0@aD)gO8MwqxIe)-0tNvM zW@UI;>Q5!g2Z9Wc?3J1S-OAVnjT7kD=tB;HJ{Voj`fEW7|AXp_V3EGqnJE^r(YRD4 zT-^ddTF!!WL&w<1`2?g3u=AV$znU^&HCnbTEV$yR!|(gjCKF9UA8(`%pMBmAt%3!K z*2dUqckWcmgwgOKxvwH*YNiG`i$jW^GqVX zcmGV;Mu@G%eA!JS- zWS?wkFHZdEO%S91>MZe#jWEbEkkr-y@}i8N3iADZmM$AO*4LYKU0c5&jcQMbyKVoK z;FCs>5r%&e;6hYwl=uYlZeFkWZ1cdeOppY}gV zILahOEkQ{q_~Do8I}|IDC06Q1f&;Mhfpa+VX247Hx z9%LfYCPk;A@%P?fMV^)~H8_-QLR}E}T*l^F&zABjnG|bb6-$kh(=Ma6wcnrhz)V)0 zH3~lOO^4|(V@`gFlq-C&FW=`Fg!k&Fq^4pF$^43}{}UZ?d}5pqi7ZQ89|&|HKx^j# zUaCn4Fja=3&#jy<8yrc{8-NriG^hgY|l2CzeGF7RZ<~P41nAhg8d>$ir|rB<~c52 zNSv>>{l#qTt|D}_p*vk6<#R>*AP>z_m1N()3T&!0j%7DKOENO`c2${r2FlCi>W}io z-RL&k=E;5atAQtL1*4}dw2f* zu=MF5V)CCyVl%!(-+%!CxuY5lXMErX5xF_Q02{*`Nj1Q&y#Gad8sfq zl`KmdV2V^eV3mu<{k2{{YTnBuOu!mi3C^mrCbPnLZNkCq2?mZB_)p*=$CMmbT;TEc zfxmHS>3aS6JT>`?O|(_9R8gaHjA6w4WhpUS91lcIlkZlS0VxOi5?Btu9aTRLS(VHr zY|Ll;qO*eZ8tCEgOp3;Rzwwd}uJx|`c-=1a0-+ArsI6Jw?sqz|1@5np`@D)JSgTD) z%cgp=*W4-Lks$$AM(GTXvwIf4;fMxCj13SH!I+upEXkI6_zODrNb_+TkF~bG8l3v^ z>3-aL6s=+&80-Afz}(xxKARL(3iudzEyCKkP6yU!ZFhq((huI2M1WyQauD zVE+04=)H{-==;*MZ6>O}`*upJM5r5Lu7x|xLUi&6xJu+R3kw#<(%aWp*kd%qs-Uh) z9Asm^p6T0ub5!kEYKpr_;s8 zrNe0$H*5(#T+C=Zo*l#w@mTe0vCe5|ue%br;dP6&6+eH>S3`4jA8qyxyD``QH5`O;m6n-nSqWB1Vh}R1#+~ za^7^$r|`!7zFM=!aof!f426H+Dz}~ZB(;SfnbtI(*ZZDVqkA5rk1Wks_|g^iw_R>m zZ>bn;AB?q9_r*1C1;=*lf^v<|cl96pPRKeO*V{UorjtxcGj4@xaGPIl^U~Gv(R?J8 z^=m7)w7RQm1ByAzGp0Ij22{Q(?6pM_wQpsGTRX+1jO1TEIX`ux>?iH^x6;>6tFTQw zdn_e2FUI@?s_H4MQ<@Jsgu2rd!7FPo*7;qmg8>VYr$sk{wd6;ebxL7Efmv8({)T}W zD+>x7dFBJ4z)<|p^B)s_mbwgL!f%wjB>zld;s@^@X zdu4GtIr$AZ8h^O%p3XXRd+VY{-ix2D&JIT}d;G0)L}q}P)AIZ8It8v$;pd)#n*>fX z)AapuN0cW6jc^^uP-#)qfaHGlo$H>B>_xh6UZbqN=I`|mg@aU_fWcK$V0uYZpn86{ zeZssYH4;`Z_Z~nUw%8X_2fr{X7A|?x2*Uwfld&`)8548MjJ&+UrbOxDxZ1HefcL3x z*|sxRw)Q}&YkV?zC^`k%Uvs(>7^;3=J=XJ@-`73Fh0><{hm;z4TFw!MAL^jE+EZ=E zSt;eYp-^hXC66=?FlKI1bZmQLbU5{#TUrgQ>i1SC;K_{=QG@@w8C)7G&|-4?J^LxNt~(CFG4OOPy5_~Uld z7QF#%AE@qZ^MJEcOP?jlGF>G`1Sb-A+SOdqYrQPq%T_MeaX(rnIDgMz{;a`I+Bvhy zJWkoGP!B9(tgK@Mje!7}HD)%}u9iJm1A*U{J+N?pQGze)^;U-HO?4)%0p6^eVCD>e&8-dO1luUkY|I5S$i zKk5&uD7OI*mpVM@@Y=+cRnwV-!hZ(3YS|CPg(lK?|5=Q?4aShZMk{Zzo%O^mqf8PB zi$TqF);l~GYq<2A>qmfiRbkYoE3cGl60OO1U&y$zNd{> z^&_2`IuFo8*{NVHlI#lKM;N^Auu7@+I}LPQb9BdZg8p%}=tr1ceY!1LZFU!dnm)3?IBJqY%i55T}2D>pE#L!vwpf!Hx(qr&3thG5f*h zt3M*{`bQPEVw34-9eet!D_CcKQ#R#WHFLMVZS-|??>j)lMe1Bv&!|r`B|1_7O^xA2 zr=(Q@%#zc;I`m4R(Gr{sz2W!e2W#s`b>Te$_ykbF^)y2Z-%I(xrw`h5HV(QBz)H_h z&GoVervoJ!IV}~QWc1jGqxl`_EO$sh$IoeA8H};Bo&{@uPn)nbFn2|$?YV!yTk!Zx z5;G3a`gqX|IrIs4YW&8wm_p0+^q&fy=D*rVZoGAGAV3omq5B;T;Ne9ik( zVhc_9Cv#&5&NsX%0wkbzDuDze3@U9nA;8GU=r#5D^^l%)2q|HfbIxD+z;~_KR!4k4 zpPVqg5&U9zGj&#jkOv%$!!ho!cPkj0HAB5RcsG)=f)TQm$uH)oCG)BDw^te@d3jHF zrB5>n9@s0|prg45qJE>Pba*h=QNJ`@MR&|t}Z84g9CwU z|I%QC`@Wy9Kq)H#W+Mp*qKCG`n**^!zypzy<9xon-NZ0gs`$!)hHHQ0=ORuq=)X^L zznxPj`SwX@Cv#LBfLA=A ze=)4ehB2t?A8n%eC1VZW*J<+d_p8Ua&wFOu8j7i~F)p?!)0tc9#yrai*rM!CucRK! zudEzb%98thmIdFF|B#Or@FrHX(fwfq)=XojD}6BB13RXHIv$hl0`~-Z4PmJouI;+L zhr3fL@q)~83nRIma?6z6akZ}%9HfBAD7@IFo}Cg+XLRStodgH{j3RNbS#EA|*rX^f zV}(4qEXGaRE5%23HlhMTawi-q%b%v)d&!roLXQ@ZT%|n#C+cvXFAv-sX~&*EGQ%Lt zE8|CZCErSjO)0YJA0FVP|B6SlqX`?Q@@{)`BX7ROXcfTlFY# zoaX#dk(WQg@1|aDE8-Sf(n*}{XjVEcI`J1W{(Xpldb;9i=%jRh8G ztuOublyhhkq zR%1JwB&PRdD$N2KDm*a$&>}BkGwx2k9|U$Ysy<`$5(hK+PuBA!#=8TTb|Qt5VURv+ z?O2=)`RZ6Kdi~1vnv4hKpwXzbjEN>JmUz#uMyGNG;OsnhI$b<|MJT@9F}^=xZkO*A|%zkNECOlx^zHaERR$}(bMn7$h&dW->29r3PqxNky@El>W zfTXxA9{ljHa2AcR(W`af9$eF`T3HvqMvbC#mEmTiDl{cZ!j13-+pia3?U$M!&l;^* z+mF8eGqkKj_WC|K8gEd=CgyXW+MXqR8hBRLf`V!$Pa*fqT32WC?wa%ZS`rP9bke?9 z+?0`fvEMkP)gV+cCFj^^ngFGl9!chbRhD5iov932x!KZDtG0b(Pam#ndGEam2~iNr zOy=2aRu4TJR#7?ka#!}^)T`zZU*mqMBbdZmy1uA+Qkd8p_Cc0g{X#wsw|QMM`RzK# zS+5lmh2mbo>C1?46NQ>!v39FU9rBjOCdkA=$Bo0>p~pzsysrTYC{7)>kg#5RGry4% ziyYO#-{ree9Uk)GG*3CM)lBM(TTj-JKmLpnI7q^Fem7sx5w(r&y;P_x))Uj&`<<&D zoYfCU{*6^GPkfva43)^afoZv!BxrfTRoE=+$ehq%v&?CFnj3((|FKVaqvMdPZl_hC zbA_|Mw!uw2UZj=LU>iSlcJX5~ER*`SZenf0sIDOO!dNjkP`Dho`TB?C<36#|#~N+cL|Ok`ECNDx6_oi< ztz>lKa7k9D*v@U|!nMECJqT1y zqBtYEHWt?#?HEHrNaDba7X=(cbCK zsI6^SJN9=XePIpxwLbn(;(puJPF;r73s0dykz(d%{MHC1S!RBxr>pS?HIXCVDizqu z1}{ZNJTvP~ru%;=(rtkw?QXOQAUW2S_!m)*bnN&!^sEZz zPZ`jCbO9vK4bOu2*v~JG|E-BZs`2LF1l`n24P0=M8%L+Jb~xzv*Ds4f#*Uw^gFyG; z5F0631T;6Lh%6->vbekAqOMlkpc<9ODnCX80>xi>4{<5%y8q_D(yCscNCh&z5RVZ{ z`GwJ<5%x0fCqubLrm=*lz19C`LYH011xeR)_=8{!B`w4e--^y`~Vux>;sr!wUlC@W3yC zK-ABc0M`HI*iK*(*Y3n$0RsF_xRG4C@fSg$E8ONWU$0yM310c%|JCOgo+sJ}k?-vw Qfop-Z)b-U$RGz>4KQ+=K^Z)<= literal 0 HcmV?d00001 diff --git a/m-explore-ros2/explore/doc/wiki_doc.txt b/m-explore-ros2/explore/doc/wiki_doc.txt new file mode 100644 index 0000000..5ddb4fe --- /dev/null +++ b/m-explore-ros2/explore/doc/wiki_doc.txt @@ -0,0 +1,148 @@ +<> + +<> + +<> + +== Overview == +This package provides greedy frontier-based exploration. When node is running, robot will greedily explore its environment until no frontiers could be found. Movement commands will be send to [[move_base]]. + +{{attachment:screenshot.png||width="755px"}} + +Unlike similar packages, {{{explore_lite}}} does not create its own costmap, which makes it easier to configure and more efficient (lighter on resources). Node simply subscribes to <> messages. Commands for robot movement are send to [[move_base]] node. + +Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot. + +<> + +== Architecture == +{{{explore_lite}}} uses [[move_base]] for navigation. You need to run properly configured [[move_base]] node. + +{{attachment:architecture.svg||width="755px"}} + +{{{explore_lite}}} subscribes to a <> and <> messages to construct a map where it looks for frontiers. You can either use costmap published by [[move_base]] (ie. `/global_costmap/costmap`) or you can use map constructed by mapping algorithm (SLAM). + +Depending on your environment you may achieve better results with either SLAM map or costmap published by `move_base`. Advantage of `move_base` costmap is the inflation which helps to deal with some very small unexplorable frontiers. When you are using a raw map produced by SLAM you should set the `min_frontier_size` parameter to some reasonable number to deal with the small frontiers. For details on both setups check the `explore.launch` and `explore_costmap.launch` launch files. + +== Setup == + +Before starting experimenting with {{{explore_lite}}} you need to have working [[move_base]] for navigation. You should be able to navigate with [[move_base]] manually through [[rviz]]. Please refer to [[navigation#Tutorials]] for setting up [[move_base]] and the rest of the navigation stack with your robot. + +You should be also able to to navigate with [[move_base]] though unknown space in the map. If you set the goal to unknown place in the map, planning and navigating should work. With most planners this should work by default, refer to [[navfn#Parameters]] if you need to setup this for [[navfn]] planner (but should be enabled by default). Navigation through unknown space is required for {{{explore_lite}}}. + +If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`. + +If you have [[move_base]] configured correctly, you can start experimenting with {{{explore_lite}}}. Provided `explore.launch` should work out-of-the box in most cases, but as always you might need to adjust topic names and frame names according to your setup. + +== ROS API == +{{{ +#!clearsilver CS/NodeAPI + +name = explore +desc = Provides exploration services offered by this package. Exploration will start immediately after node initialization. + +pub { + 0.name = ~frontiers + 0.type = visualization_msgs/MarkerArray + 0.desc = Visualization of frontiers considered by exploring algorithm. Each frontier is visualized by frontier points in blue and with a small sphere, which visualize the cost of the frontiers (costlier frontiers will have smaller spheres). +} +sub { + 0.name = costmap + 0.type = nav_msgs/OccupancyGrid + 0.desc = Map which will be used for exploration planning. Can be either costmap from [[move_base]] or map created by SLAM (see above). Occupancy grid must have got properly marked unknown space, mapping algorithms usually track unknown space by default. If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`. + + 1.name = costmap_updates + 1.type = map_msgs/OccupancyGridUpdate + 1.desc = Incremental updates on costmap. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. +} + +param { + 0.name = ~robot_base_frame + 0.default = `base_link` + 0.type = string + 0.desc = The name of the base frame of the robot. This is used for determining robot position on map. Mandatory. + + 1.name = ~costmap_topic + 1.default = `costmap` + 1.type = string + 1.desc = Specifies topic of source <>. Mandatory. + + 3.name = ~costmap_updates_topic + 3.default = `costmap_updates` + 3.type = string + 3.desc = Specifies topic of source <>. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. + + 4.name = ~visualize + 4.default = `false` + 4.type = bool + 4.desc = Specifies whether or not publish visualized frontiers. + + 6.name = ~planner_frequency + 6.default = `1.0` + 6.type = double + 6.desc = Rate in Hz at which new frontiers will computed and goal reconsidered. + + 7.name = ~progress_timeout + 7.default = `30.0` + 7.type = double + 7.desc = Time in seconds. When robot do not make any progress for `progress_timeout`, current goal will be abandoned. + + 8.name = ~potential_scale + 8.default = `1e-3` + 8.type = double + 8.desc = Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier). + + 9.name = ~orientation_scale + 9.default = `0` + 9.type = double + 9.desc = Used for weighting frontiers. This multiplicative parameter affects frontier orientation component of the frontier weight. This parameter does currently nothing and is provided solely for forward compatibility. + + 10.name = ~gain_scale + 10.default = `1.0` + 10.type = double + 10.desc = Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size). + + 11.name = ~transform_tolerance + 11.default = `0.3` + 11.type = double + 11.desc = Transform tolerance to use when transforming robot pose. + + 12.name = ~min_frontier_size + 12.default = `0.5` + 12.type = double + 12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters. +} + +req_tf { + 0.from = global_frame + 0.to = robot_base_frame + 0.desc = This transformation is usually provided by mapping algorithm. Those frames are usually called `map` and `base_link`. For adjusting `robot_base_frame` name see respective parameter. You don't need to set `global_frame`. The name for `global_frame` will be sourced from `costmap_topic` automatically. +} + +act_called { + 0.name = move_base + 0.type = move_base_msgs/MoveBaseAction + 0.desc = [[move_base]] actionlib API for posting goals. See [[move_base#Action API]] for details. This expects [[move_base]] node in the same namespace as `explore_lite`, you may want to remap this node if this is not true. +} +}}} + +== Acknowledgements == + +This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague. + +{{{ +@masterthesis{Hörner2016, + author = {Jiří Hörner}, + title = {Map-merging for multi-robot system}, + address = {Prague}, + year = {2016}, + school = {Charles University in Prague, Faculty of Mathematics and Physics}, + type = {Bachelor's thesis}, + URL = {https://is.cuni.cz/webapps/zzp/detail/174125/}, +} +}}} + +This project was initially based on [[explore]] package by Charles !DuHadway. Most of the node has been rewritten since then. The current frontier search algorithm is based on [[frontier_exploration]] by Paul Bovbel. + +## AUTOGENERATED DON'T DELETE +## CategoryPackage diff --git a/m-explore-ros2/explore/include/explore/costmap_client.h b/m-explore-ros2/explore/include/explore/costmap_client.h new file mode 100644 index 0000000..0adee31 --- /dev/null +++ b/m-explore-ros2/explore/include/explore/costmap_client.h @@ -0,0 +1,137 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef COSTMAP_CLIENT_ +#define COSTMAP_CLIENT_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace explore +{ +class Costmap2DClient +{ +public: + /** + * @brief Contructs client and start listening + * @details Constructor will block until first map update is received and + * map is ready to use, also will block before trasformation + * robot_base_frame <-> global_frame is available. + * + * @param node node handle to retrieve parameters from + * @param tf_listener Will be used for transformation of robot pose. + */ + Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf_listener); + /** + * @brief Get the pose of the robot in the global frame of the costmap + * @return pose of the robot in the global frame of the costmap + */ + geometry_msgs::msg::Pose getRobotPose() const; + + /** + * @brief Return a pointer to the "master" costmap which receives updates from + * all the layers. + * + * This pointer will stay the same for the lifetime of Costmap2DClient object. + */ + nav2_costmap_2d::Costmap2D* getCostmap() + { + return &costmap_; + } + + /** + * @brief Return a pointer to the "master" costmap which receives updates from + * all the layers. + * + * This pointer will stay the same for the lifetime of Costmap2DClient object. + */ + const nav2_costmap_2d::Costmap2D* getCostmap() const + { + return &costmap_; + } + + /** + * @brief Returns the global frame of the costmap + * @return The global frame of the costmap + */ + const std::string& getGlobalFrameID() const + { + return global_frame_; + } + + /** + * @brief Returns the local frame of the costmap + * @return The local frame of the costmap + */ + const std::string& getBaseFrameID() const + { + return robot_base_frame_; + } + +protected: + void updateFullMap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void updatePartialMap(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg); + + nav2_costmap_2d::Costmap2D costmap_; + bool costmap_received_ = false; ///< @brief Flag indicating whether costmap + ///< callback has been called + + const tf2_ros::Buffer* const tf_; ///< @brief Used for transforming + /// point clouds + rclcpp::Node& node_; + std::string global_frame_; ///< @brief The global frame for the costmap + std::string robot_base_frame_; ///< @brief The frame_id of the robot base + double transform_tolerance_; ///< timeout before transform errors + +private: + // will be unsubscribed at destruction + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr + costmap_updates_sub_; +}; + +} // namespace explore + +#endif diff --git a/m-explore-ros2/explore/include/explore/costmap_tools.h b/m-explore-ros2/explore/include/explore/costmap_tools.h new file mode 100644 index 0000000..d5d2fb4 --- /dev/null +++ b/m-explore-ros2/explore/include/explore/costmap_tools.h @@ -0,0 +1,135 @@ +#ifndef COSTMAP_TOOLS_H_ +#define COSTMAP_TOOLS_H_ + +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace frontier_exploration +{ +/** + * @brief Determine 4-connected neighbourhood of an input cell, checking for map + * edges + * @param idx input cell index + * @param costmap Reference to map data + * @return neighbour cell indexes + */ +std::vector nhood4(unsigned int idx, + const nav2_costmap_2d::Costmap2D& costmap) +{ + // get 4-connected neighbourhood indexes, check for edge of map + std::vector out; + + unsigned int size_x_ = costmap.getSizeInCellsX(), + size_y_ = costmap.getSizeInCellsY(); + + if (idx > size_x_ * size_y_ - 1) { + RCLCPP_WARN(rclcpp::get_logger("FrontierExploration"), "Evaluating nhood " + "for offmap point"); + return out; + } + + if (idx % size_x_ > 0) { + out.push_back(idx - 1); + } + if (idx % size_x_ < size_x_ - 1) { + out.push_back(idx + 1); + } + if (idx >= size_x_) { + out.push_back(idx - size_x_); + } + if (idx < size_x_ * (size_y_ - 1)) { + out.push_back(idx + size_x_); + } + return out; +} + +/** + * @brief Determine 8-connected neighbourhood of an input cell, checking for map + * edges + * @param idx input cell index + * @param costmap Reference to map data + * @return neighbour cell indexes + */ +std::vector nhood8(unsigned int idx, + const nav2_costmap_2d::Costmap2D& costmap) +{ + // get 8-connected neighbourhood indexes, check for edge of map + std::vector out = nhood4(idx, costmap); + + unsigned int size_x_ = costmap.getSizeInCellsX(), + size_y_ = costmap.getSizeInCellsY(); + + if (idx > size_x_ * size_y_ - 1) { + return out; + } + + if (idx % size_x_ > 0 && idx >= size_x_) { + out.push_back(idx - 1 - size_x_); + } + if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) { + out.push_back(idx - 1 + size_x_); + } + if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) { + out.push_back(idx + 1 - size_x_); + } + if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) { + out.push_back(idx + 1 + size_x_); + } + + return out; +} + +/** + * @brief Find nearest cell of a specified value + * @param result Index of located cell + * @param start Index initial cell to search from + * @param val Specified value to search for + * @param costmap Reference to map data + * @return True if a cell with the requested value was found + */ +bool nearestCell(unsigned int& result, unsigned int start, unsigned char val, + const nav2_costmap_2d::Costmap2D& costmap) +{ + const unsigned char* map = costmap.getCharMap(); + const unsigned int size_x = costmap.getSizeInCellsX(), + size_y = costmap.getSizeInCellsY(); + + if (start >= size_x * size_y) { + return false; + } + + // initialize breadth first search + std::queue bfs; + std::vector visited_flag(size_x * size_y, false); + + // push initial cell + bfs.push(start); + visited_flag[start] = true; + + // search for neighbouring cell matching value + while (!bfs.empty()) { + unsigned int idx = bfs.front(); + bfs.pop(); + + // return if cell of correct value is found + if (map[idx] == val) { + result = idx; + return true; + } + + // iterate over all adjacent unvisited cells + for (unsigned nbr : nhood8(idx, costmap)) { + if (!visited_flag[nbr]) { + bfs.push(nbr); + visited_flag[nbr] = true; + } + } + } + + return false; +} +} // namespace frontier_exploration +#endif diff --git a/m-explore-ros2/explore/include/explore/explore.h b/m-explore-ros2/explore/include/explore/explore.h new file mode 100644 index 0000000..6c6adbc --- /dev/null +++ b/m-explore-ros2/explore/include/explore/explore.h @@ -0,0 +1,146 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Robert Bosch LLC. + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef NAV_EXPLORE_H_ +#define NAV_EXPLORE_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +using namespace std::placeholders; +#ifdef ELOQUENT +#define ACTION_NAME "NavigateToPose" +#elif DASHING +#define ACTION_NAME "NavigateToPose" +#else +#define ACTION_NAME "navigate_to_pose" +#endif +namespace explore +{ +/** + * @class Explore + * @brief A class adhering to the robot_actions::Action interface that moves the + * robot base to explore its environment. + */ +class Explore : public rclcpp::Node +{ +public: + Explore(); + ~Explore(); + + void start(); + void stop(bool finished_exploring = false); + void resume(); + + using NavigationGoalHandle = + rclcpp_action::ClientGoalHandle; + +private: + /** + * @brief Make a global plan + */ + void makePlan(); + + // /** + // * @brief Publish a frontiers as markers + // */ + void visualizeFrontiers( + const std::vector& frontiers); + + bool goalOnBlacklist(const geometry_msgs::msg::Point& goal); + + NavigationGoalHandle::SharedPtr navigation_goal_handle_; + // void + // goal_response_callback(std::shared_future + // future); + void reachedGoal(const NavigationGoalHandle::WrappedResult& result, + const geometry_msgs::msg::Point& frontier_goal); + + rclcpp::Publisher::SharedPtr + marker_array_publisher_; + rclcpp::Logger logger_ = rclcpp::get_logger("ExploreNode"); + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + Costmap2DClient costmap_client_; + rclcpp_action::Client::SharedPtr + move_base_client_; + frontier_exploration::FrontierSearch search_; + rclcpp::TimerBase::SharedPtr exploring_timer_; + // rclcpp::TimerBase::SharedPtr oneshot_; + + rclcpp::Subscription::SharedPtr resume_subscription_; + void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg); + + std::vector frontier_blacklist_; + geometry_msgs::msg::Point prev_goal_; + double prev_distance_; + rclcpp::Time last_progress_; + size_t last_markers_count_; + + geometry_msgs::msg::Pose initial_pose_; + void returnToInitialPose(void); + + // parameters + double planner_frequency_; + double potential_scale_, orientation_scale_, gain_scale_; + double progress_timeout_; + bool visualize_; + bool return_to_init_; + std::string robot_base_frame_; + bool resuming_ = false; +}; +} // namespace explore + +#endif diff --git a/m-explore-ros2/explore/include/explore/frontier_search.h b/m-explore-ros2/explore/include/explore/frontier_search.h new file mode 100644 index 0000000..a20ab2f --- /dev/null +++ b/m-explore-ros2/explore/include/explore/frontier_search.h @@ -0,0 +1,88 @@ +#ifndef FRONTIER_SEARCH_H_ +#define FRONTIER_SEARCH_H_ + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace frontier_exploration +{ +/** + * @brief Represents a frontier + * + */ +struct Frontier { + std::uint32_t size; + double min_distance; + double cost; + geometry_msgs::msg::Point initial; + geometry_msgs::msg::Point centroid; + geometry_msgs::msg::Point middle; + std::vector points; +}; + +/** + * @brief Thread-safe implementation of a frontier-search task for an input + * costmap. + */ +class FrontierSearch +{ +public: + FrontierSearch() + { + } + + /** + * @brief Constructor for search task + * @param costmap Reference to costmap data to search. + */ + FrontierSearch(nav2_costmap_2d::Costmap2D* costmap, double potential_scale, + double gain_scale, double min_frontier_size); + + /** + * @brief Runs search implementation, outward from the start position + * @param position Initial position to search from + * @return List of frontiers, if any + */ + std::vector searchFrom(geometry_msgs::msg::Point position); + +protected: + /** + * @brief Starting from an initial cell, build a frontier from valid adjacent + * cells + * @param initial_cell Index of cell to start frontier building + * @param reference Reference index to calculate position from + * @param frontier_flag Flag vector indicating which cells are already marked + * as frontiers + * @return new frontier + */ + Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference, + std::vector& frontier_flag); + + /** + * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate + * for a new frontier. + * @param idx Index of candidate cell + * @param frontier_flag Flag vector indicating which cells are already marked + * as frontiers + * @return true if the cell is frontier cell + */ + bool isNewFrontierCell(unsigned int idx, + const std::vector& frontier_flag); + + /** + * @brief computes frontier cost + * @details cost function is defined by potential_scale and gain_scale + * + * @param frontier frontier for which compute the cost + * @return cost of the frontier + */ + double frontierCost(const Frontier& frontier); + +private: + nav2_costmap_2d::Costmap2D* costmap_; + unsigned char* map_; + unsigned int size_x_, size_y_; + double potential_scale_, gain_scale_; + double min_frontier_size_; +}; +} // namespace frontier_exploration +#endif diff --git a/m-explore-ros2/explore/launch/explore.launch.py b/m-explore-ros2/explore/launch/explore.launch.py new file mode 100644 index 0000000..5ae49ef --- /dev/null +++ b/m-explore-ros2/explore/launch/explore.launch.py @@ -0,0 +1,46 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory("explore_lite"), "config", "params.yaml" + ) + use_sim_time = LaunchConfiguration("use_sim_time") + namespace = LaunchConfiguration("namespace") + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + declare_namespace_argument = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for the explore node", + ) + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + node = Node( + package="explore_lite", + name="explore_node", + namespace=namespace, + executable="explore", + parameters=[config, {"use_sim_time": use_sim_time}], + output="screen", + remappings=remappings, + ) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_namespace_argument) + ld.add_action(node) + return ld diff --git a/m-explore-ros2/explore/package.xml b/m-explore-ros2/explore/package.xml new file mode 100644 index 0000000..ff821c0 --- /dev/null +++ b/m-explore-ros2/explore/package.xml @@ -0,0 +1,33 @@ + + + + explore_lite + 1.0.0 + + Lightweight frontier-based exploration ROS2 port. + + Carlos Alvarez + Carlos Alvarez + BSD + + ament_cmake + + ament_lint_auto + ament_lint_common + ament_cmake + map_msgs + nav2_costmap_2d + nav2_msgs + nav_msgs + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + + + ament_cmake + + diff --git a/m-explore-ros2/explore/src/costmap_client.cpp b/m-explore-ros2/explore/src/costmap_client.cpp new file mode 100644 index 0000000..ae2da83 --- /dev/null +++ b/m-explore-ros2/explore/src/costmap_client.cpp @@ -0,0 +1,279 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include +#include + +#include +#include +#include + +namespace explore +{ +// static translation table to speed things up +std::array init_translation_table(); +static const std::array cost_translation_table__ = + init_translation_table(); + +Costmap2DClient::Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf) + : tf_(tf), node_(node) +{ + std::string costmap_topic; + std::string costmap_updates_topic; + + node_.declare_parameter("costmap_topic", std::string("costmap")); + node_.declare_parameter("costmap_updates_topic", + std::string("costmap_updates")); + node_.declare_parameter("robot_base_frame", std::string("base_" + "link")); + // transform tolerance is used for all tf transforms here + node_.declare_parameter("transform_tolerance", 0.3); + + node_.get_parameter("costmap_topic", costmap_topic); + node_.get_parameter("costmap_updates_topic", costmap_updates_topic); + node_.get_parameter("robot_base_frame", robot_base_frame_); + node_.get_parameter("transform_tolerance", transform_tolerance_); + + /* initialize costmap */ + costmap_sub_ = node_.create_subscription( + costmap_topic, 1000, + [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + costmap_received_ = true; + updateFullMap(msg); + }); + + // ROS1 CODE + // auto costmap_msg = + // ros::topic::waitForMessage( + // costmap_topic, subscription_nh); + + // Spin some until the callback gets called to replicate + // ros::topic::waitForMessage + RCLCPP_INFO(node_.get_logger(), + "Waiting for costmap to become available, topic: %s", + costmap_topic.c_str()); + while (!costmap_received_) { + rclcpp::spin_some(node_.get_node_base_interface()); + // Wait for a second + usleep(1000000); + } + // updateFullMap(costmap_msg); // this is already called in the callback of + // the costmap_sub_ + + /* subscribe to map updates */ + costmap_updates_sub_ = + node_.create_subscription( + costmap_updates_topic, 1000, + [this](const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) { + updatePartialMap(msg); + }); + + // ROS1 CODE. + // TODO: Do we need this? + /* resolve tf prefix for robot_base_frame */ + // std::string tf_prefix = tf::getPrefixParam(node_); + // robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_); + + // we need to make sure that the transform between the robot base frame and + // the global frame is available + + // the global frame is set in the costmap callback. This is why we need to + // ensure that a costmap is received + + /* tf transform is necessary for getRobotPose */ + auto last_error = node_.now(); + std::string tf_error; + while (rclcpp::ok() && + !tf_->canTransform(global_frame_, robot_base_frame_, + tf2::TimePointZero, tf2::durationFromSec(0.1), + &tf_error)) { + rclcpp::spin_some(node_.get_node_base_interface()); + if (last_error + tf2::durationFromSec(5.0) < node_.now()) { + RCLCPP_WARN(node_.get_logger(), + "Timed out waiting for transform from %s to %s to become " + "available " + "before subscribing to costmap, tf error: %s", + robot_base_frame_.c_str(), global_frame_.c_str(), + tf_error.c_str()); + last_error = node_.now(); + ; + } + // The error string will accumulate and errors will typically be the same, + // so the last + // will do for the warning above. Reset the string here to avoid + // accumulation. + tf_error.clear(); + } +} + +void Costmap2DClient::updateFullMap( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + global_frame_ = msg->header.frame_id; + + unsigned int size_in_cells_x = msg->info.width; + unsigned int size_in_cells_y = msg->info.height; + double resolution = msg->info.resolution; + double origin_x = msg->info.origin.position.x; + double origin_y = msg->info.origin.position.y; + + RCLCPP_DEBUG(node_.get_logger(), "received full new map, resizing to: %d, %d", + size_in_cells_x, size_in_cells_y); + costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x, + origin_y); + + // lock as we are accessing raw underlying map + auto* mutex = costmap_.getMutex(); + std::lock_guard lock(*mutex); + + // fill map with data + unsigned char* costmap_data = costmap_.getCharMap(); + size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY(); + RCLCPP_DEBUG(node_.get_logger(), "full map update, %lu values", costmap_size); + for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) { + unsigned char cell_cost = static_cast(msg->data[i]); + costmap_data[i] = cost_translation_table__[cell_cost]; + } + RCLCPP_DEBUG(node_.get_logger(), "map updated, written %lu values", + costmap_size); +} + +void Costmap2DClient::updatePartialMap( + const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) +{ + RCLCPP_DEBUG(node_.get_logger(), "received partial map update"); + global_frame_ = msg->header.frame_id; + + if (msg->x < 0 || msg->y < 0) { + RCLCPP_DEBUG(node_.get_logger(), + "negative coordinates, invalid update. x: %d, y: %d", msg->x, + msg->y); + return; + } + + size_t x0 = static_cast(msg->x); + size_t y0 = static_cast(msg->y); + size_t xn = msg->width + x0; + size_t yn = msg->height + y0; + + // lock as we are accessing raw underlying map + auto* mutex = costmap_.getMutex(); + std::lock_guard lock(*mutex); + + size_t costmap_xn = costmap_.getSizeInCellsX(); + size_t costmap_yn = costmap_.getSizeInCellsY(); + + if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn || + y0 > costmap_yn) { + RCLCPP_WARN(node_.get_logger(), + "received update doesn't fully fit into existing map, " + "only part will be copied. received: [%lu, %lu], [%lu, %lu] " + "map is: [0, %lu], [0, %lu]", + x0, xn, y0, yn, costmap_xn, costmap_yn); + } + + // update map with data + unsigned char* costmap_data = costmap_.getCharMap(); + size_t i = 0; + for (size_t y = y0; y < yn && y < costmap_yn; ++y) { + for (size_t x = x0; x < xn && x < costmap_xn; ++x) { + size_t idx = costmap_.getIndex(x, y); + unsigned char cell_cost = static_cast(msg->data[i]); + costmap_data[idx] = cost_translation_table__[cell_cost]; + ++i; + } + } +} + +geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const +{ + geometry_msgs::msg::PoseStamped robot_pose; + geometry_msgs::msg::Pose empty_pose; + robot_pose.header.frame_id = robot_base_frame_; + robot_pose.header.stamp = node_.now(); + + auto& clk = *node_.get_clock(); + + // get the global pose of the robot + try { + robot_pose = tf_->transform(robot_pose, global_frame_, + tf2::durationFromSec(transform_tolerance_)); + } catch (tf2::LookupException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, + "No Transform available Error looking up robot pose: " + "%s\n", + ex.what()); + return empty_pose; + } catch (tf2::ConnectivityException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, + "Connectivity Error looking up robot pose: %s\n", + ex.what()); + return empty_pose; + } catch (tf2::ExtrapolationException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, + "Extrapolation Error looking up robot pose: %s\n", + ex.what()); + return empty_pose; + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, "Other error: %s\n", + ex.what()); + return empty_pose; + } + + return robot_pose.pose; +} + +std::array init_translation_table() +{ + std::array cost_translation_table; + + // lineary mapped from [0..100] to [0..255] + for (size_t i = 0; i < 256; ++i) { + cost_translation_table[i] = + static_cast(1 + (251 * (i - 1)) / 97); + } + + // special values: + cost_translation_table[0] = 0; // NO obstacle + cost_translation_table[99] = 253; // INSCRIBED obstacle + cost_translation_table[100] = 254; // LETHAL obstacle + cost_translation_table[static_cast(-1)] = 255; // UNKNOWN + + return cost_translation_table; +} + +} // namespace explore diff --git a/m-explore-ros2/explore/src/explore.cpp b/m-explore-ros2/explore/src/explore.cpp new file mode 100644 index 0000000..acdbc91 --- /dev/null +++ b/m-explore-ros2/explore/src/explore.cpp @@ -0,0 +1,432 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Robert Bosch LLC. + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include + +#include + +inline static bool same_point(const geometry_msgs::msg::Point& one, + const geometry_msgs::msg::Point& two) +{ + double dx = one.x - two.x; + double dy = one.y - two.y; + double dist = sqrt(dx * dx + dy * dy); + return dist < 0.01; +} + +namespace explore +{ +Explore::Explore() + : Node("explore_node") + , tf_buffer_(this->get_clock()) + , tf_listener_(tf_buffer_) + , costmap_client_(*this, &tf_buffer_) + , prev_distance_(0) + , last_markers_count_(0) +{ + double timeout; + double min_frontier_size; + this->declare_parameter("planner_frequency", 1.0); + this->declare_parameter("progress_timeout", 30.0); + this->declare_parameter("visualize", false); + this->declare_parameter("potential_scale", 1e-3); + this->declare_parameter("orientation_scale", 0.0); + this->declare_parameter("gain_scale", 1.0); + this->declare_parameter("min_frontier_size", 0.5); + this->declare_parameter("return_to_init", false); + + this->get_parameter("planner_frequency", planner_frequency_); + this->get_parameter("progress_timeout", timeout); + this->get_parameter("visualize", visualize_); + this->get_parameter("potential_scale", potential_scale_); + this->get_parameter("orientation_scale", orientation_scale_); + this->get_parameter("gain_scale", gain_scale_); + this->get_parameter("min_frontier_size", min_frontier_size); + this->get_parameter("return_to_init", return_to_init_); + this->get_parameter("robot_base_frame", robot_base_frame_); + + progress_timeout_ = timeout; + move_base_client_ = + rclcpp_action::create_client( + this, ACTION_NAME); + + search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(), + potential_scale_, gain_scale_, + min_frontier_size); + + if (visualize_) { + marker_array_publisher_ = + this->create_publisher("explore/" + "frontier" + "s", + 10); + } + + // Subscription to resume or stop exploration + resume_subscription_ = this->create_subscription( + "explore/resume", 10, + std::bind(&Explore::resumeCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server"); + move_base_client_->wait_for_action_server(); + RCLCPP_INFO(logger_, "Connected to move_base nav2 server"); + + if (return_to_init_) { + RCLCPP_INFO(logger_, "Getting initial pose of the robot"); + geometry_msgs::msg::TransformStamped transformStamped; + std::string map_frame = costmap_client_.getGlobalFrameID(); + try { + transformStamped = tf_buffer_.lookupTransform( + map_frame, robot_base_frame_, tf2::TimePointZero); + initial_pose_.position.x = transformStamped.transform.translation.x; + initial_pose_.position.y = transformStamped.transform.translation.y; + initial_pose_.orientation = transformStamped.transform.rotation; + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(logger_, "Couldn't find transform from %s to %s: %s", + map_frame.c_str(), robot_base_frame_.c_str(), ex.what()); + return_to_init_ = false; + } + } + + exploring_timer_ = this->create_wall_timer( + std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)), + [this]() { makePlan(); }); + // Start exploration right away + exploring_timer_->execute_callback(); +} + +Explore::~Explore() +{ + stop(); +} + +void Explore::resumeCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + if (msg->data) { + resume(); + } else { + stop(); + } +} + +void Explore::visualizeFrontiers( + const std::vector& frontiers) +{ + std_msgs::msg::ColorRGBA blue; + blue.r = 0; + blue.g = 0; + blue.b = 1.0; + blue.a = 1.0; + std_msgs::msg::ColorRGBA red; + red.r = 1.0; + red.g = 0; + red.b = 0; + red.a = 1.0; + std_msgs::msg::ColorRGBA green; + green.r = 0; + green.g = 1.0; + green.b = 0; + green.a = 1.0; + + RCLCPP_DEBUG(logger_, "visualising %lu frontiers", frontiers.size()); + visualization_msgs::msg::MarkerArray markers_msg; + std::vector& markers = markers_msg.markers; + visualization_msgs::msg::Marker m; + + m.header.frame_id = costmap_client_.getGlobalFrameID(); + m.header.stamp = this->now(); + m.ns = "frontiers"; + m.scale.x = 1.0; + m.scale.y = 1.0; + m.scale.z = 1.0; + m.color.r = 0; + m.color.g = 0; + m.color.b = 255; + m.color.a = 255; + // lives forever +#ifdef ELOQUENT + m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning +#elif DASHING + m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning +#else + m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards +#endif + // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in + // galactic + m.frame_locked = true; + + // weighted frontiers are always sorted + double min_cost = frontiers.empty() ? 0. : frontiers.front().cost; + + m.action = visualization_msgs::msg::Marker::ADD; + size_t id = 0; + for (auto& frontier : frontiers) { + m.type = visualization_msgs::msg::Marker::POINTS; + m.id = int(id); + // m.pose.position = {}; // compile warning + m.scale.x = 0.1; + m.scale.y = 0.1; + m.scale.z = 0.1; + m.points = frontier.points; + if (goalOnBlacklist(frontier.centroid)) { + m.color = red; + } else { + m.color = blue; + } + markers.push_back(m); + ++id; + m.type = visualization_msgs::msg::Marker::SPHERE; + m.id = int(id); + m.pose.position = frontier.initial; + // scale frontier according to its cost (costier frontiers will be smaller) + double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5); + m.scale.x = scale; + m.scale.y = scale; + m.scale.z = scale; + m.points = {}; + m.color = green; + markers.push_back(m); + ++id; + } + size_t current_markers_count = markers.size(); + + // delete previous markers, which are now unused + m.action = visualization_msgs::msg::Marker::DELETE; + for (; id < last_markers_count_; ++id) { + m.id = int(id); + markers.push_back(m); + } + + last_markers_count_ = current_markers_count; + marker_array_publisher_->publish(markers_msg); +} + +void Explore::makePlan() +{ + // find frontiers + auto pose = costmap_client_.getRobotPose(); + // get frontiers sorted according to cost + auto frontiers = search_.searchFrom(pose.position); + RCLCPP_DEBUG(logger_, "found %lu frontiers", frontiers.size()); + for (size_t i = 0; i < frontiers.size(); ++i) { + RCLCPP_DEBUG(logger_, "frontier %zd cost: %f", i, frontiers[i].cost); + } + + if (frontiers.empty()) { + RCLCPP_WARN(logger_, "No frontiers found, stopping."); + stop(true); + return; + } + + // publish frontiers as visualization markers + if (visualize_) { + visualizeFrontiers(frontiers); + } + + // find non blacklisted frontier + auto frontier = + std::find_if_not(frontiers.begin(), frontiers.end(), + [this](const frontier_exploration::Frontier& f) { + return goalOnBlacklist(f.centroid); + }); + if (frontier == frontiers.end()) { + RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping."); + stop(true); + return; + } + geometry_msgs::msg::Point target_position = frontier->centroid; + + // time out if we are not making any progress + bool same_goal = same_point(prev_goal_, target_position); + + prev_goal_ = target_position; + if (!same_goal || prev_distance_ > frontier->min_distance) { + // we have different goal or we made some progress + last_progress_ = this->now(); + prev_distance_ = frontier->min_distance; + } + // black list if we've made no progress for a long time + if ((this->now() - last_progress_ > + tf2::durationFromSec(progress_timeout_)) && !resuming_) { + frontier_blacklist_.push_back(target_position); + RCLCPP_DEBUG(logger_, "Adding current goal to black list"); + makePlan(); + return; + } + + // ensure only first call of makePlan was set resuming to true + if (resuming_) { + resuming_ = false; + } + + // we don't need to do anything if we still pursuing the same goal + if (same_goal) { + return; + } + + RCLCPP_DEBUG(logger_, "Sending goal to move base nav2"); + + // send goal to move_base if we have something new to pursue + auto goal = nav2_msgs::action::NavigateToPose::Goal(); + goal.pose.pose.position = target_position; + goal.pose.pose.orientation.w = 1.; + goal.pose.header.frame_id = costmap_client_.getGlobalFrameID(); + goal.pose.header.stamp = this->now(); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + // send_goal_options.goal_response_callback = + // std::bind(&Explore::goal_response_callback, this, _1); + // send_goal_options.feedback_callback = + // std::bind(&Explore::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + [this, + target_position](const NavigationGoalHandle::WrappedResult& result) { + reachedGoal(result, target_position); + }; + move_base_client_->async_send_goal(goal, send_goal_options); +} + +void Explore::returnToInitialPose() +{ + RCLCPP_INFO(logger_, "Returning to initial pose."); + auto goal = nav2_msgs::action::NavigateToPose::Goal(); + goal.pose.pose.position = initial_pose_.position; + goal.pose.pose.orientation = initial_pose_.orientation; + goal.pose.header.frame_id = costmap_client_.getGlobalFrameID(); + goal.pose.header.stamp = this->now(); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + move_base_client_->async_send_goal(goal, send_goal_options); +} + +bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal) +{ + constexpr static size_t tolerace = 5; + nav2_costmap_2d::Costmap2D* costmap2d = costmap_client_.getCostmap(); + + // check if a goal is on the blacklist for goals that we're pursuing + for (auto& frontier_goal : frontier_blacklist_) { + double x_diff = fabs(goal.x - frontier_goal.x); + double y_diff = fabs(goal.y - frontier_goal.y); + + if (x_diff < tolerace * costmap2d->getResolution() && + y_diff < tolerace * costmap2d->getResolution()) + return true; + } + return false; +} + +void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result, + const geometry_msgs::msg::Point& frontier_goal) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_DEBUG(logger_, "Goal was successful"); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_DEBUG(logger_, "Goal was aborted"); + frontier_blacklist_.push_back(frontier_goal); + RCLCPP_DEBUG(logger_, "Adding current goal to black list"); + // If it was aborted probably because we've found another frontier goal, + // so just return and don't make plan again + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_DEBUG(logger_, "Goal was canceled"); + // If goal canceled might be because exploration stopped from topic. Don't make new plan. + return; + default: + RCLCPP_WARN(logger_, "Unknown result code from move base nav2"); + break; + } + // find new goal immediately regardless of planning frequency. + // execute via timer to prevent dead lock in move_base_client (this is + // callback for sendGoal, which is called in makePlan). the timer must live + // until callback is executed. + // oneshot_ = relative_nh_.createTimer( + // ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); }, + // true); + + // Because of the 1-thread-executor nature of ros2 I think timer is not + // needed. + makePlan(); +} + +void Explore::start() +{ + RCLCPP_INFO(logger_, "Exploration started."); +} + +void Explore::stop(bool finished_exploring) +{ + RCLCPP_INFO(logger_, "Exploration stopped."); + move_base_client_->async_cancel_all_goals(); + exploring_timer_->cancel(); + + if (return_to_init_ && finished_exploring) { + returnToInitialPose(); + } +} + +void Explore::resume() +{ + resuming_ = true; + RCLCPP_INFO(logger_, "Exploration resuming."); + // Reactivate the timer + exploring_timer_->reset(); + // Resume immediately + exploring_timer_->execute_callback(); +} + +} // namespace explore + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + // ROS1 code + /* + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } */ + rclcpp::spin( + std::make_shared()); // std::move(std::make_unique)? + rclcpp::shutdown(); + return 0; +} diff --git a/m-explore-ros2/explore/src/frontier_search.cpp b/m-explore-ros2/explore/src/frontier_search.cpp new file mode 100644 index 0000000..42d3f73 --- /dev/null +++ b/m-explore-ros2/explore/src/frontier_search.cpp @@ -0,0 +1,201 @@ +#include +#include + +#include +#include + +#include "nav2_costmap_2d/cost_values.hpp" + +namespace frontier_exploration +{ +using nav2_costmap_2d::FREE_SPACE; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +FrontierSearch::FrontierSearch(nav2_costmap_2d::Costmap2D* costmap, + double potential_scale, double gain_scale, + double min_frontier_size) + : costmap_(costmap) + , potential_scale_(potential_scale) + , gain_scale_(gain_scale) + , min_frontier_size_(min_frontier_size) +{ +} + +std::vector +FrontierSearch::searchFrom(geometry_msgs::msg::Point position) +{ + std::vector frontier_list; + + // Sanity check that robot is inside costmap bounds before searching + unsigned int mx, my; + if (!costmap_->worldToMap(position.x, position.y, mx, my)) { + RCLCPP_ERROR(rclcpp::get_logger("FrontierSearch"), "Robot out of costmap " + "bounds, cannot search " + "for frontiers"); + return frontier_list; + } + + // make sure map is consistent and locked for duration of search + std::lock_guard lock( + *(costmap_->getMutex())); + + map_ = costmap_->getCharMap(); + size_x_ = costmap_->getSizeInCellsX(); + size_y_ = costmap_->getSizeInCellsY(); + + // initialize flag arrays to keep track of visited and frontier cells + std::vector frontier_flag(size_x_ * size_y_, false); + std::vector visited_flag(size_x_ * size_y_, false); + + // initialize breadth first search + std::queue bfs; + + // find closest clear cell to start search + unsigned int clear, pos = costmap_->getIndex(mx, my); + if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) { + bfs.push(clear); + } else { + bfs.push(pos); + RCLCPP_WARN(rclcpp::get_logger("FrontierSearch"), "Could not find nearby " + "clear cell to start " + "search"); + } + visited_flag[bfs.front()] = true; + + while (!bfs.empty()) { + unsigned int idx = bfs.front(); + bfs.pop(); + + // iterate over 4-connected neighbourhood + for (unsigned nbr : nhood4(idx, *costmap_)) { + // add to queue all free, unvisited cells, use descending search in case + // initialized on non-free cell + if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) { + visited_flag[nbr] = true; + bfs.push(nbr); + // check if cell is new frontier cell (unvisited, NO_INFORMATION, free + // neighbour) + } else if (isNewFrontierCell(nbr, frontier_flag)) { + frontier_flag[nbr] = true; + Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag); + if (new_frontier.size * costmap_->getResolution() >= + min_frontier_size_) { + frontier_list.push_back(new_frontier); + } + } + } + } + + // set costs of frontiers + for (auto& frontier : frontier_list) { + frontier.cost = frontierCost(frontier); + } + std::sort( + frontier_list.begin(), frontier_list.end(), + [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; }); + + return frontier_list; +} + +Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, + unsigned int reference, + std::vector& frontier_flag) +{ + // initialize frontier structure + Frontier output; + output.centroid.x = 0; + output.centroid.y = 0; + output.size = 1; + output.min_distance = std::numeric_limits::infinity(); + + // record initial contact point for frontier + unsigned int ix, iy; + costmap_->indexToCells(initial_cell, ix, iy); + costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); + + // push initial gridcell onto queue + std::queue bfs; + bfs.push(initial_cell); + + // cache reference position in world coords + unsigned int rx, ry; + double reference_x, reference_y; + costmap_->indexToCells(reference, rx, ry); + costmap_->mapToWorld(rx, ry, reference_x, reference_y); + + while (!bfs.empty()) { + unsigned int idx = bfs.front(); + bfs.pop(); + + // try adding cells in 8-connected neighborhood to frontier + for (unsigned int nbr : nhood8(idx, *costmap_)) { + // check if neighbour is a potential frontier cell + if (isNewFrontierCell(nbr, frontier_flag)) { + // mark cell as frontier + frontier_flag[nbr] = true; + unsigned int mx, my; + double wx, wy; + costmap_->indexToCells(nbr, mx, my); + costmap_->mapToWorld(mx, my, wx, wy); + + geometry_msgs::msg::Point point; + point.x = wx; + point.y = wy; + output.points.push_back(point); + + // update frontier size + output.size++; + + // update centroid of frontier + output.centroid.x += wx; + output.centroid.y += wy; + + // determine frontier's distance from robot, going by closest gridcell + // to robot + double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) + + pow((double(reference_y) - double(wy)), 2.0)); + if (distance < output.min_distance) { + output.min_distance = distance; + output.middle.x = wx; + output.middle.y = wy; + } + + // add to queue for breadth first search + bfs.push(nbr); + } + } + } + + // average out frontier centroid + output.centroid.x /= output.size; + output.centroid.y /= output.size; + return output; +} + +bool FrontierSearch::isNewFrontierCell(unsigned int idx, + const std::vector& frontier_flag) +{ + // check that cell is unknown and not already marked as frontier + if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) { + return false; + } + + // frontier cells should have at least one cell in 4-connected neighbourhood + // that is free + for (unsigned int nbr : nhood4(idx, *costmap_)) { + if (map_[nbr] == FREE_SPACE) { + return true; + } + } + + return false; +} + +double FrontierSearch::frontierCost(const Frontier& frontier) +{ + return (potential_scale_ * frontier.min_distance * + costmap_->getResolution()) - + (gain_scale_ * frontier.size * costmap_->getResolution()); +} +} // namespace frontier_exploration diff --git a/m-explore-ros2/explore/test/test_explore.cpp b/m-explore-ros2/explore/test/test_explore.cpp new file mode 100644 index 0000000..2fe0913 --- /dev/null +++ b/m-explore-ros2/explore/test/test_explore.cpp @@ -0,0 +1,77 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Carlos Alvarez nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include +#include +#include + +#define private public + +inline static bool same_point(const geometry_msgs::msg::Point& one, + const geometry_msgs::msg::Point& two) +{ + double dx = one.x - two.x; + double dy = one.y - two.y; + double dist = sqrt(dx * dx + dy * dy); + return dist < 0.01; +} + +TEST(Explore, testSameGoal) +{ + geometry_msgs::msg::Point goal1; + geometry_msgs::msg::Point goal2; + // Populate the goal with known values + goal1.x = 1.0; + goal1.y = 2.0; + goal1.z = 3.0; + + goal2.x = 0.0; + goal2.y = 0.0; + goal2.z = 0.0; + auto same_goal = same_point(goal1, goal2); + EXPECT_FALSE(same_goal); + goal2.x = goal1.x; + goal2.y = goal1.y; + goal2.z = goal1.z; + same_goal = same_point(goal1, goal2); + EXPECT_TRUE(same_goal); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/m-explore-ros2/map_merge/CHANGELOG.rst b/m-explore-ros2/map_merge/CHANGELOG.rst new file mode 100644 index 0000000..9246ee4 --- /dev/null +++ b/m-explore-ros2/map_merge/CHANGELOG.rst @@ -0,0 +1,58 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package multirobot_map_merge +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.1.4 (2021-01-07) +------------------ +* use C++14 +* support both OpenCV 3 and OpenCV 4 (support Debian Buster) +* Contributors: Jiri Horner + +2.1.3 (2021-01-03) +------------------ +* add missing dependencies to catkin_package calls +* update map_merge for OpenCV 4 +* Contributors: Jiri Horner + +2.1.2 (2021-01-02) +------------------ +* support for ROS Melodic +* bugfix: zero resolution of the merged grid for known initial posiiton +* bugfix: estimation_confidence parameter had no effect +* map_merge: set origin of merged grid in its centre +* map_merge: add new launch file + * map_merge with 2 maps served by map_server +* bugfix: ensure that we never output map with 0 resolution +* bugfix: nullptr derefence while setting resolution of output grid +* Contributors: Jiri Horner + +2.1.1 (2017-12-16) +------------------ +* fix bugs in CMakeLists.txt: install nodes in packages, so they get shipped in debian packages. fixes `#11 `_ +* map_merge: add bibtex to wiki page +* Contributors: Jiri Horner + +2.1.0 (2017-10-30) +------------------ +* no major changes. Released together with explore_lite. + +2.0.0 (2017-03-26) +------------------ +* map_merge: upgrade to package format 2 +* node completely rewritten based on my work included in opencv +* uses more reliable features by default -> more robust merging +* known_init_poses is now by default false to make it easy to start for new users +* Contributors: Jiri Horner + +1.0.1 (2017-03-25) +------------------ +* map_merge: use inverted tranform + * transform needs to be inverted before using +* map_merge: change package description + * we support merging with unknown initial positions +* Contributors: Jiri Horner + +1.0.0 (2016-05-11) +------------------ +* initial release +* Contributors: Jiri Horner diff --git a/m-explore-ros2/map_merge/CMakeLists.txt b/m-explore-ros2/map_merge/CMakeLists.txt new file mode 100644 index 0000000..702b4e1 --- /dev/null +++ b/m-explore-ros2/map_merge/CMakeLists.txt @@ -0,0 +1,164 @@ +cmake_minimum_required(VERSION 3.5) +project(multirobot_map_merge) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) +find_package(map_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +find_package(Boost REQUIRED COMPONENTS thread) + +# OpenCV is required for merging without initial positions +find_package(OpenCV REQUIRED) +if("${OpenCV_VERSION}" VERSION_LESS "3.0") +message(FATAL_ERROR "This package needs OpenCV >= 3.0") +endif() +if("${OpenCV_VERSION}" VERSION_LESS "4.0") +message(WARNING "This package supports OpenCV 3, but some features may not be " +"available. Upgrade to OpenCV 4 to take advantage of all features.") +endif() + +set(DEPENDENCIES + rclcpp + geometry_msgs + image_geometry + map_msgs + nav_msgs + tf2_geometry_msgs + tf2 + tf2_ros + OpenCV +) + +## Specify additional locations of header files +include_directories( + # ${Boost_INCLUDE_DIRS} + # ${OpenCV_INCLUDE_DIRS} + include +) + +install( + DIRECTORY include/map_merge/ + DESTINATION include/map_merge/ +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +# we want static linking for now +add_library(combine_grids STATIC +src/combine_grids/grid_compositor.cpp +src/combine_grids/grid_warper.cpp +src/combine_grids/merging_pipeline.cpp +) + +add_executable(map_merge + src/map_merge.cpp +) + +############# +## Install ## +############# +target_include_directories(map_merge PUBLIC +$ +$) +target_include_directories(combine_grids PUBLIC + "$" + "$" + ${rclcpp_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries(combine_grids ${rclcpp_LIBRARIES} ${OpenCV_LIBS}) +# target_link_libraries(combine_grids ${OpenCV_LIBRARIES}) + +target_link_libraries(map_merge combine_grids) +# target_link_libraries(map_merge) + +ament_target_dependencies(map_merge ${DEPENDENCIES}) +ament_target_dependencies(combine_grids ${DEPENDENCIES}) + +install( + TARGETS combine_grids + EXPORT export_combine_grids + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install(TARGETS map_merge + DESTINATION lib/${PROJECT_NAME}) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") + + +############# +## Testing ## +############# +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) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + # download test data: TODO: ROS2 alternative? For now you'll need to download them manually + set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge) + # ament_download(${base_url}/hector_maps/map00.pgm + # MD5 915609a85793ec1375f310d44f2daf87 + # FILENAME ${PROJECT_NAME}_map00.pgm + # ) + execute_process( + COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map00.pgm ${base_url}/hector_maps/map00.pgm 915609a85793ec1375f310d44f2daf87 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test + ) + execute_process( + COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map05.pgm ${base_url}/hector_maps/map05.pgm cb9154c9fa3d97e5e992592daca9853a + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test + ) + execute_process( + COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm 3c2c38e7dec2b7a67f41069ab58badaa + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test + ) + execute_process( + COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm 681e704044889c95e47b0c3aadd81f1e + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test + ) + + ament_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp) + # ensure that test data are downloaded before we run tests + # add_dependencies(test_merging_pipeline ${PROJECT_NAME}_map00.pgm ${PROJECT_NAME}_map05.pgm ${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${PROJECT_NAME}_2012-01-28-11-12-01.pgm) + target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES}) + +endif() + + +ament_export_include_directories(include) +ament_export_libraries(combine_grids) +ament_package() \ No newline at end of file diff --git a/m-explore-ros2/map_merge/config/params.yaml b/m-explore-ros2/map_merge/config/params.yaml new file mode 100644 index 0000000..53e2b24 --- /dev/null +++ b/m-explore-ros2/map_merge/config/params.yaml @@ -0,0 +1,24 @@ +map_merge: + ros__parameters: + merging_rate: 4.0 + discovery_rate: 0.05 + estimation_rate: 0.5 + estimation_confidence: 0.6 + robot_map_topic: map + robot_map_updates_topic: map_updates + robot_namespace: "" + merged_map_topic: map + world_frame: world + known_init_poses: true + # known_init_poses: false + + # Define here robots' positions in the map if known_init_poses is true + /robot1/map_merge/init_pose_x: 0.0 + /robot1/map_merge/init_pose_y: 0.0 + /robot1/map_merge/init_pose_z: 0.0 + /robot1/map_merge/init_pose_yaw: 0.0 + + /robot2/map_merge/init_pose_x: -3.0 + /robot2/map_merge/init_pose_y: 1.0 + /robot2/map_merge/init_pose_z: 0.0 + /robot2/map_merge/init_pose_yaw: 0.0 diff --git a/m-explore-ros2/map_merge/doc/architecture.dia b/m-explore-ros2/map_merge/doc/architecture.dia new file mode 100644 index 0000000000000000000000000000000000000000..5afa6667f62d9a901065675f83f0dfd69a16f505 GIT binary patch literal 1825 zcmV++2j2J}iwFP!000021MOT}bE7sCe$TI9oSA(}FkmncZ?e5Jcgcn+}i2x*K+cso;NWhTMUB_0a> z+u4^tf4;V6AMP&RdkEa{AD@Ci;!lKe`tEkNVJy0t&!3*2WW0?5BUHvHkz+Xj2Ve~5 zywQAicOgly1$cmg^j>inFhh&;C}TfNkVUyw<_AX1QRMf0(?8We8{fZ zybv=l$vGf90zO<*2)?$9R`_MxLqtcAR>cMpi4kwYwo$D~i=6cJRhyz!%o~ON-Q`dE zW!|OY#$HxaM_WNy0H}|`Iu6$j&K04_mZoW38HR1yim99Gi%$B(tHh=3ahCz)w|Cyw*K>V}yu*%L zYSAL)33+VeJWr&qT>5q*>5nKzYYdxuk3x1b@!Qjgug*>uz4wogw1!+-YkKf3Xh5TX zdhiqE!T5-omD{vP+7!*^$FyE;*Htn3txB@aATZcMT8RI+Pt#H{!9~Q8fXmS1%ez-P zA?v@O%ZkB%`TU!7Eq%fuj%WJP9E$Kl8ZaI)CWCs(Sse#eF13aLbsW-opu*lJ)7%pU z(e$IqP{0FsPsiPWTFl%<>4aoMI1=*} zSuMw8MI8ZFuAy2M*~-9~c|aH+M=0+Q^h03qnN2%}knlw#b{UR1?wn^su7K*~jYA_+y14#R)R2}62dS(PqaeJ~r};9cYT zZpqQT;>z0L9cHV1C6uyu*odQUGI7cC3=}H_ksGuG%OA~?!^vJU+Upb?2=~l z7!23Auk{8!Y%4rWc$o082|es@;bFqVgojO;!@!g9G2vrBl8^N|6l~qrZgA0E<4Ilg zWR}!;)HR*q9s>%&qZ7n_d~=ZppCbW#*f z9N4!6((U&gdRjSyl>g>_7uVS@qv8~ULBvZ0IMtETi~P7ugvkjWhZ!Hg6Av~JKGd;< zkDP$9l(h-z9x29EO1@ZWCP(9VmaGhGJ6Hb=MD#uG5j+p9a zZ{s3!&C>dwvXRG0=cLat zj)C8lmbB4nNt1O`)iciCU{Xc58y-Z(& zv{-Md@g$%sR+^c0-7-cpK&HzFXGl4fSYM9cEi1=f@*8N8jV|3=^+$XvHE~6~;#Sd> zsG3?Dcb9bKUQ_nVC(;IlUXe@~$wK?AzNYjh4 z!7B5e)XB`ngn^2qn9IzSt(C6IOEsfSTT>RgqX+i?;!SIUH!XP6f;TOA(}FiGc+-M6 zEqK#{H!XP6f;TOA(}FiGc+-LpBRFS*H!VC&c$nZ#3l9?>COk~=riG6Q-n8IN3*NNg zO$*+%;7y;HH~j~<77tKQx|Z$}mL3!}a6k01+z(xs7lxuWIU*e&-E`NVL77k8Kh!WJ5FgwbV65Rr#wi;VJF+ z&I%s`5pT$o)9|I*19=6fVAUF;>Lx2qK?iw6|ySrNm?hxE%aCi6MPOzYXU?D&txN8C=xC9LZatHG5 zZ|`&dv+p_QKL73Rr=)AuyQEf0ch}7Ch2I+hSYBFA8UTSnfGqR`elG(O00JC50z4c7 z0z3jDA_5W$7zG6x83h~tDJmEb8y_DJ8yA<5gocceh>92&mzh`*NrR9I+DFf14VK<$u!*8cyJf~EZ*)#AW6M?#(MA1MIj6!*INMQ+bOZtzzx z6pKD)T#(B{77$QF5L6HorRkDY5C9<5O%%Y6`kNLoq_Sq5w_^K8UG%C;`iaL3_({-! zfH5>l9E?$aObP+YNdN$z(&?d4sT)hJidN77gHLcRUulv({S~P1PkJSJ&x~Uz)W50B2=dRwB_34+j6(LK35~^n=#CE@~ty)k3~{ z)b;@caMx*&hCHT14IW}R(;@?8V*sR04FC*S&^uVTmiktDF^+HF&MP&IVb)Ghy#^2t zolB>&?H+Jp7IdF=*Bn8JxXL+f)=i`amk!I8zf|QsU?FPxVp~IF06>E8d5BPwOlhDJ z>n8ps@rC_C6WDogZ>dLinO<++&RrCsc1UB*y|;VY#}oVp-Kk~WUvC9q*MGID9IvcX z`Znite@21m1Jr+(8<_Ftg#g?*jiLZd5+AfNElykB&WC4pLmZdsHRCQ80pP;x%FmWy zvySJXsWw0J)N)U!LqOk|ThEs==08f``rc`r?R@?IXzhQ(2OIP}@Pi(6+W%-fw5G66 z%8~C{Y+euNIa3iFK>){jRB0SpnYt^eOTDu*vF~2um;l)GeSKK;TJfa21a-Y?9w7e3 zB2sODru?K8Fk>hRZQR#u8u5@nlEe7_v~M|@46PbFNbfYGP|=y!<~JS=ff>uz2{ICP z=Vv*s!SQP8`{H{YvlsKTBq;W5aWa6~Nzv#%p?A#x$;4HqfJ_4+B+(PXB%LgZm?B4< zsr^ayFC#USlUcT&3Mb?np1nP_Z2z{iWXewfkSgY%N8fpENMByhp}Z3~yI_e|Ta16l zb;16XP8;Lcdy+Wmt5^L5)WHDYvG*dTAfV=peH3^Nh_mzne&;t}L1K*#Krk_q_Q9GU z*<1i{XyLHzBKTDP;;leK8g94a%+~tz!#pmX2H9bV`R90vkR;_CKRbB+i=PJ|%lQP^4 zShIvDBNIa@A4UyC^_4jychNe6=_)P#UBB5pQz5VCSgqpCd5)Lv0O~$*+u3#Mew%c? z)|>V0X@MhUIyrQIwYsWhoZQS*hv!UglMg)q15;qPz=0Yf#V7IjB_}Mp3GPgHq4>2E1LH>Ti)YaYDr~TfKMWa}#-W+!J zD{MSJCAioeI-vd4h0qXEdYj|Xoy{MH6*x1SL8gcafC^Led07gy>4;7F9z6xIbJN}- z`ltc`)>{l$Y@R8{S`M+94^~`!sT-VII!4yNJ853}`N;{m@I>qX{6_Q#gov@?UqBF0Apj!Vr4NVs8?1-Xkn}KY>JFn}0Te+${zJAX`5rASc#efxT zHuDWRb!62s(|uK@%gi*0SptiLid2hM@|+$(wLT+$>tJ8Tjo3rk3MW@ zT;sKv0$?pu&*Rj}OH7_ODLTzo7e3FVS`wo2!I*4eIr_CwuJq5T5QW_vPGge1gh=@W zm@?iclrx4-jnsRoF(!+xyr!%79I1(-2yv(5gmn5m%I4QmN&auA<#av;8F@DdV05f@ z+g{n*#u_nk*M< z41ap8w_-=(^<^2d%p8VBA2v1`5U4GFgHDmC5Nb>s_SEf{8l>OyP0i}JbrjG%AJ_nJ z;tMEA4v$G_;1nP9k`}v*&tbAThFkfz7=Ld0gSqiC_oCtE;6k|lx_yZbQu^L}Ts>Nj z&e(Hyp@kBKc@!S66}v?=oKF_(Em*5k4@u5AJ~EFkAhqD0K{Yr!G|n#D$lkN!p=F-`^K&9uecw&FO55EV7|&u%F?oG zaF{=0wrAHMo3dEVv~g~~*0I~?!}po`I8Huy&i=NrsNKG^DSmev=@jRru1T+oPTwtC zW6wBOG9@hdUpopKO)mD-7Qmfmy7b5s7C~NZ=6OoA89mex3)3^ZyDw^feTbUn)q+?~ ze&al!2VdEq73CpK*m80sB|D))j328BNEXe+9AY7A_2;6@e@ghaH1eT~wtsiP z_L~1yG-nUM)H`teJ3{C>=ef;ag&TPr!1IsAslNZ77+sXZn}_tNOz{wcA_M?tj8|-g zve?kdCI5Yu|8)uUF@OsUZ6+941Oylu5d5Dfg~!JLEDTIeFg7_GyC@C?2N$;(0ws^J z6BRX$xP&V7`2Y#}*Z_iu`3>NhY~LmP2FMNquYUtsj%}>}>yzNj(kG62)#=6m@)Xri zJw6ZTvgH>V6ELV3-xlHcAgN!F{XtTLZ|i(-|H`3>}%Nf#HdOuy|!5&I2%f}74^_|@_LgESSRo_7{5eE&~$ z{7Xf)^6-rf7pzK_InmVuqNa^SeQ_f0tU)IRguL*@WDoXYwaFu!rPVO1?dMgdzSwPH zVK|{*zvBcdG=}s>g%W(LydM{pTd^M(Y|a|yQJiOKV)VX1wVrYF&Gu+Dkzr=cU3&jP z%09PTI)sqpy87ltD&}uM0rDK=O`LC8&l3syjcr@+vs zE-gc%k(FCv9!oL~!^GYjRwOe~o;zS5V;J>MFOX8hSS}p6eCK}0>T7K|>PaDsyBRB+ z$iefY5LcUTB6sH*7XsAuY%bqj=X=sDkP_c&=ImKT!z7y?kV#ST1MDy&uKrkA_NTI4g+gup z6uw>bS5}?#_MyGht(dE~h-qL09-9?OX_L2@+I59-gz>R1qmDP%fd}h#CDM^Y%1i~H z%k4a{15*!OXtx6uaznHG?^J?hLNy79m7j5Q6XeKw(`G!{0ClRh%TL7m;*(Ur1+q?p(DqISznV;5o!za6UlnN_emR&M8Hs{2j(`4qCG z=~wsx6v?l7c;QJuaz(-`Qsfp&X(+{GK6s%<_H8DrW9N?{k6MxRF?#b^5U(~rwcpM* zJywEoF2A1KufTYv`_R5Za4xk!g;m5R6n_1Zq2?* zF7gu|;+RL5WN=Wl3Xb~m4%4h#<(>|^pzOfCgr89@$ETu-P%{8skl2gw#F?NtLQQ9~0hMZJ4f`(CD-IgU(+Dd)WB22K2p_M60xGJIj!AaB!F1^xCMFc|8 z`VqWNltnIA_B>kQ61FA-Q?$C}qfTyZ_LHO7$EK|uh~IDXktNPJU+a80)UZ~+9CjIX zn?+Nq6RQ$aaHjhzPkmh&Hck__t9#5yw3u|p;1idH_Te2b?tq7SZ~ovNiiv`ABl7`y z)6?#!x0;_26RM%z1v_%B2Y0+_@tn@r(OtkGSmuZI`{Al}8p&(_B&v3X8#jd4O}$W$ zMcm-oEC};^Nusb>SvAO_XO3mJd0`WE&eNd#r*ZB>z z`2^0ILTjYDs83D6Tv=p{d9@4b!Wvea>2+Z&3>^AOz6>ISR~<`Vr>30?92JONNEOhW z#Fyid6{RDi;oO2_hJQ_#yMiGF`65{+Cz1mrwzx=d**7Uv_Kz_A?9% za+;Tkzp6r%csZ&zt$3=G8MVx@t9sU|C46ckR%!%^q$Ll-TKGBNXGVFIvNRWwd#kUl zcLZfs3|Zi#f{;o>fz<4mSg5qEo zbDR`gHJX~i1HS9FHs+aGIeHhROYsVnK z5dR{l$0|rX(A%nh#7cYUwX{rwQAh=El(K`gJ2p)A$m%*i(TekXX?PxvAo+N4f;7k^ zMu<8x%AHt2PMg)ze9hg_phQQk_4p(Pi9?HL=Ein2x+&XwETe1Nt2f4#tQAI1eKWQw z)R`YaksmW2Cff0~s`&Ab7uR^O*v6L*3@1#wlHltz@)KbI6;gYF_bT|@6#6&7qi9XPzG0M^F+v|%q2iY|I<8*+T zHvNA+P3Yx)@y#=A4Y1&4Q%p`xPSP#Drc;}AzN2R|Ebl#FOhLzDE0=X2vQ`%<5gY&d zA{#gDR7U|@&^$w=L_@u?>e8*lOmpjGG&)MMVk13`=|*6(gt(Mjg#E1mreksDq$>Av zMQB-3>K8QvTV0Dvv-A?ADqRybd`)Q_j4;W+6a2Gc12k1ZOem2WTJZ2iMI~Dm-_#iy z2^gPmeB>MY4N(6EpzBm#=zDDt3>+LBbg2q`IsI@z*QpqoSQPAFN)9!0HZfIfQ4=^$ zb?3qpDskhW#0H7p$u*xMCf}s&tUZQE_+0x?o>45jVGIQ3zC@NOs!_U%_Irb7JvOr{fj6 zyI_V`$ks61#K^cP@w#d3ZbbRb_#X+A6`t(3CwAWFlF(#ecQj}(?;6`|$Vn+}tNdxG`klOhlVp@Fqr+Zo#PS8l%ZZT; z=c)H+SfO|tB-r!xyL@_$=@~C6$UM^roLxRxi{sDH&{~KLh-b%if1RjQLFu5VnNKn> zAMx!*N@5xDf6~v#aP(a11x*8k(gnu89|g~P9+_v=A=bg2Y;4{7bg#kdaw5$iOsoc} zHwAFA8R`t@&oKRtf4scZ@{0}mhDE0JZs)ekzeaUUaZ-8p7fy?8*{RZdA{$3-&*%m; z+G|RQRgMgmn0u82%^lM(8gk2@6FO)7PKC!OU3~Pe8IPhFrvmsVeUmz3TKfEcVNe4b!S#o+%f!dr2ZQG}M|9 zOs(KQJ>f zTp{yqdj4~!4;kMQ;AAU{+Us^^%DF4!Nk$hivQruv-PRizOeP&o9OXECUY=2PLG`Tp zc5H5%nI1;t&E%BbF&*dFLQ^{8P8>*4ErM6f-LVmcNL7I|h zd^VxENus8we|5HbRBh0xxk>1GL7p~cn#NtCIK;y0(A(U~_^I-WEf7_Hms%kZub5`H z7n8|eeN9?ZrY>CKh|$@Itl3>eSa@eOZHR@7Y1+lGxafGw{CyuI$GOaOm{-d=$GT}b zivZYfD1d%%*Z|pgk5cMI%5NsX2f@-q{k&=g30KX z`r_!9(xPGNTke2x0{uD6#s}J(I~1m@pWFpBG5XF%T$%=nenr%pyYxQSi5(e3A6IGS zqMxL_r|lj};C@M2_T%m*^+r;*Ku%)zIw`>72m2Kwvq09+XK8!MW7-idOkLgcjcxo3 zf|>g=H`E=1dQNh!?6ey7+fP4nZWa|vBwOejBxb$RH>U>qt~e4J z4O22N(!Ytxk&IGL)mp-KwvSX#@i`Fk(pKt>_px_v%C+^xw&bO0m#H4Ap8E|1gfZz# zw$L;vRNn@EkFW90GU=^Y33X0Z7ObgM5~HZa&-+zz8-8Z5nOj*6F8O*YTaCz7qY*3e*;F(#S~l_ zNh0dL$~p8cG@DQ9$R#B=#M9aAsz;-dR~*HA7X1yuByDrEsV$c`b(0K(hZK9iR{ii8?GRWduD-l<)7VZC zr>P%t?Qg(~dO6-;I_hDNoE`tQ7Oy+po|kb<#WZ|v-Jn6H?k?iR4T?=a!RgaU46ixL z(WWoYjdgImUuOE*$qfz@q#TLsZhCGMNU*bCeYs4Ke)i6knuGlV6S9f+iSDxV@G}i! z-}oB5${Z|iA1Z+z{@;Lv(EGwu%p>_S7QJ|tb{i21)J}YAi=j{cqp2F&)62{Ev}Jjp z>b~WDDrv=D@vwAzj}adjbHT218c>p3+Q~o{%}b!I%p}24Hcwp2TJ(W;c%xvo`{{I; ze-e5F<`)7JPl=j|{Z2 zPELeL>G}t*y$YUUefjDqs1gx%a*D&1cdg7fo0Wpa%F~d6zJ^;@W5LRyn-*K@d)g2z z6pL|SoM1ZCVEQ2jms$6p(feLH=&bYXan^x{g@uLt$9Gf^1`H;coPu2pmO~B8*o0Cw zh)vx21P(q?qW5vOf%!ArG=fFPd#^=cBndIX^A|H&S+5^XUX3sjD#L@6;O|zAKBe!zitcvaS^~DXw^9|Zx)m3KIPm@mER!8JpKVS3dk=c8`OS#cuMxUicrEH2(mMa>Titf1DN^Nfnx`h)Qo1aSPD1(CH}hA$${(-q zWTy65i@vH2xXNY^m^XXX5dc+YXUb_&y{r0ApdX7vLRZ5iy`T#^V| z50ucQ_=kdN$=}xSy;%BGMS9n!sJBY zeqo)c^oqc`Y?Yf;uI7|?KgHIlvqjf~!)-(p%yziyN{tf3h&Do5hZjh5)c@`(At9lN zF>^FFy;4z)?RKXAB|(u~bC~eU$nEknikkLUDXrEx9B&Nx($XRs6?1oiNX;EXqfg~7 zr9>pcU%Rrj@Iv#12!^&TlT5AD5j)}GjV<(F#v+O51T zS&|Y-qNEh-LQc(=WnP+9{!)tMgJA-;#)`;vxuzKE+w?&~Z`$N7Udk2Ugu|U5+PTz` zEZ&+YFZ4Gq^!A!&O)K9qy|N0fd#bTf9nPv;4%NU#v2wVjRDb^_c}&lanb{(&dMH^s zHf*CZ&A&Uzp^~|CnE6O`T*+IAd+YpOc@g1MNEjqLv4= zUOVQB^F^y++aLOoC1D%Wz3%ky+A}z{?UL2#lfT?`T_*n`3w{~m*#b6vhZ!Z9r;MQ! zqBOpVPAb#bdr5?I7zR#=vK=o0+Q-Rp7b4fpTBBjA5^`Ak-#L97}`c3!EfL2hVZ zCr8oWu&;`AUV7;_eBq#S9`3WH%8`lbwhyg!`Z&lr@Otm*U+i|07Cs``6Wdw*=-QpH z5~R{0>#b8l97QR{$6tlnTKi;PY#EtxOD7i^M3Uk^xZi%n!I|xal(&3yZSsRyPqoUF-65|LP|Yq>D#iEkEo$6lY{Z+Z zE1GT`g>w?2E;V8%17KS!Yj|H&jqkt3BmTuc`%R$yD&X_e5Tl5YxsSelRh#+DHm7Ee zVbAL@7>~`|;7`+cEHk>-q$nv%%|m zd%0GRda4}V<>Ee>pct=aGqj&J)9LN&W}kVz%pMiQx!1A0y!R@IN7+m~Fdl? zp-Be`ldeR;Ms3yI!A0fs*K71Mg93e77*}(|+6P}gs6FROJ^3cr;lJ=~JZ7%7q}mJV z&TR3~RT3FynY3=YcC-oZ)x)Rk_>i$mO|>gWiNdL6w9H&98Wy_=eKdJ3Gtk|gECe*m zXxsX2>%#^eX9pc;h+S$MHlMiNA20Sr8j9lmG?-Z~YJjoSt`e|hLQ>|qzcgNIEt8)d z?ZHhh)78{K4T?0l-BaO;+HCZ6HZ+EDU)5PF^>0;72|*nqMmRUKzFK+cl#&(dF=&u} zGAf)rYl)LA!Qa5gI4!Qs=^d)}SL;MO$3>fxT04VUM?~hV3fw~^GOPW0hJ?J)!smXj zcY7&y+Pk)7mOC*|N4P>M#e~fGdRL9w0*XUWs7SBZQBmVmLLB|ptGwN0At<8ms`hI` z_cwJdZFxF~==BxT(bvzSkPqLWUA!={ft?F-7jZ+zkBW<#=w@GKUG+2Tc+qkF(XG?{ zG=A4(k4lg-g`~Bm?PXC)Y-hV}O@qxh=9|}@tH&yiHrU_4!hdX+ZC8|yX`!c*DBro$ zN=IW%U+F7(t&NRwAbJv^6%@YUmy5I}y~b|DT#zfw~d(Z(LD#%M#~W zXq~1l5q9s^n?lfU_F6wiIy4b5K8qJFgv__cvA)7ivhIn&)u@=-rEPEK*UQXT;^z&T zX2K2MI#=mZRAcbZZV6GMD?O6WaglLP(Qm~jp)@EC?SG7kpvK}r!$=1`%bWaNJ zEDL0Q1Fe4)8=oP5;@)}fS{)*NDzlalAK0btKC;Rpaj{W2juKt@MRDrGU`n479=U!4D30RcPDA;Q~@j-De*22by*-taE*Mtc!JBy1S zJ-g#)zb|83tA9Z$7Q?xX;?q1&q54k&K^|#%AIVgEBG8UOr*xV&J*8gdKB;u|qqv(J zh;d=RfrfX?3{iZ|n0$;9gLG1?SbG~3ZUycSW2jxCPkpdo)8;?s8eX$CPn>dZ*~qYM6<-TRRR| z^yoNID*N>maMb3;*3zWW%u^+@TTvD`B zTq5314VX=f+H%$tZ`*dB_UnEd2ldMrg4#Yu60=W6f-Rf6io+L@m{elA(y9oIGd(gA zYphDD<}2LaQinuTP!&ovZGW2VeJ}=EO44mlo?Ag$tITD^U5d9-8H3v-wIc|QPKzVKRzul8roC$qOp;`{C!9(OS za?p=;P-X>1EPANbi|Mb6R`9Z?r$W^wceppd0Sn^iwRi2QF&>yWL94}`^LVHE=gh+1 z$J%&JC4yz_Ewh-NY7LZa{*2oyeP6XEU-^{OcO6xd(;me%KnC?i7olz?QMY-~rl6(M zHcZM{zGEnNQ5h*QQnuiLSgAX~x~cs{!w!ae>!{AGgO!E*g>TYyS7!&h*~+OOj3efl zM{}5sysZ*|>S4I`8~91G)-G}{<5{Qd`(Fpuhom?Fq40}igsx}@u6PW{psI{C?|QUT5SF&S7YCUieEMt3J?Mwz)PpC;dW zf+}oiYrYtYk7SaYN*nYw?2iFxV|l3!gA!@ZoVW~ z9X)GSbpij8(S39}&aC^R^U}M)B#c2C3GxY3rEW2(MEu2z@2yh?67Uc#UOr)bmAmLp z;7Vv1dX@Ma7@+;FrB*h7o264KVq#}fM0~a+pJgih6e_zZwJkaYuWh{+q&CZ~`Y9~GuDntLyhS8ydr58Z!r zIDmne^h(~#EToFRy0rX;{A2OdC?0OOd4G-cMTZLN#Ez2lQjp`Wu=jW5_g(Y?*Qp0- z_2TIF}Ryrlk5bt+5Zxi_!;60B%C2!g8YovdRUFt zdOYj?`=DsLgCpOgH|e7CH-Ir6|8%e2*B7*w_iIoqv?k=HX#Fdo{D~a>{KYR(14~KI z!e0Z(1dDKF| z%X606DNhmPm0xtOC#k47)$EU{;Lc;myzxFW))NOxNeA*2SwvaK9O__H>NgnmZ5y5N zDef0!!hSz*FOHm|cOqWiKeZllag5k@Ju2Qvo~rv$Y%HhP2Sl615x!|(a2;i_VeFkp zf&&bvkU++GRhKAic*V|J7uRkQzB7{JUq#5Bk+;C@JOHBZ5BHyNT4B8?P7(8RNJw{6 z$+TZFzTlY9Cu*x#x`QVU4hfW%CVtiV&-jtPT8oU%WefTYsRx&)W%uCIAi{Fhzj@PU z89;X1*cxaM@P%dsn^NY809F*$_)V02<5>hiX)#}R%kX5}5anucJ|mF{Xm9x^BA6O> z?L~xOC-hT+_ll-yupvV|OpO-2kbghtNe?Qk-8qshD(z`WkZynzO36OKOpq?V7$9a@ zOA^d@wB+$%S%0K$jC4v!B&)G4Kj79;SW=5jP2D83e*_}}Ti{NHm~&;PF|2wTR98>j zn1&MWrM#H3KvO*@0=VI#e8&>#zvU)ZXO-igwI>I`QuM;SqY@Vi0lhPr{Uc;gk^HE9 zMfV1Ia9(>K^Dbq@3zxq1TE5!MT%Ozq3yi8|iRN^Xw>R+Xxv4`gdf*2P0LNj*;3(#PqQ zFsTJeZ_HNMy#x6_GX}|sGjy3?YSEcBTRi8LU;!h-rJ4Q)gmhmgQeK3YqNPsI6D=BA z4wNK>#m4LRA`hUY6QP6$rF7|ZjdvsjBS7xocopoui(=h-`w9&ucK|u>*qXcLmTsj zj!2*DkDVP2T6qs`qJmEntH%#L_$(mH;vCG0-#tXwXOX-|VJZ%J(t22d1Ri86{0@hr z2eW{`(89~&4`Ui&y|d9*UL4Zzf(iEl`zD6A(T+~lesOR$X3yizf@+gmoF+=~&Ii?I zkl0*lQ1|nF`PYmT^`PD3up)tO2;!xe7#^%B3Q`#lO?!%U`Z-Jn8#fz0FQN;ZX}f&+ zvnd0dZKljG_(soCrHF>gnjU?R)B#ot{JoyBe2Oqy6myj4j62AN)c8#!1_HzmISf=X zFj61-DLB;3X9)CCoPR&01OsJ$?fa@sIg+*W&=jAz8ZtyO6@G}bmo8WWjJUvgd*K>T zu{6ofm;QQhUK&^n zlbx9#L_{SZt&!r!hCPH%>=ulljj}tU6@!nQz4`?f9y|K!bsR1tI6oa;8;LQP#`U00 z9jaWlEHHYf&##%*-!YzNUK0}G&2-5b&TPHTe=}=@xCz0pdvg$alZ3b`Gp?x%dt^^ z!x_SC!;Mhcykdso4g%-A1ZtY+$f)2~k(jlHO8IpsxuS7A_yfM16C)^}KAj$)9Tm^K zvivNd?OOlsT*o$8-;Pc^jowDWzmsAvWd~kY( zOGz=0jXi`>BI4S#t)Yt^Y-Z48CUAV9UVNGe;W(pO!Gq}0fY@2$4b@}hJ^%x7gal#v z4Bt(GWH>%%CZ&~89=XvE;7mK05PAs&@-tRdQ@G$EB=VZ)mY{M?{UR3a>3Vjt*nwsS z$v%t+t2!-jszlb=nax2a+839C6COUY}zHl`wOD3F>l3Nl`y zeID%`;qE{R(`)4$l^HpU8^_c!QPIzyfM7h4U(jSkS7laF zV26PWd$Y-dZN+~MBI|N12R879CyMg(+4>*(&-loj^H`%6EXEvcg~{L{P2yw9K{BlA ziI%-KOiC7qm&tT^TN>bhX$3k~eFW~k2&gKi z3dSR0F(B9j0JW@D{2#F*rMb;ZlK6`eGHG)Y|n^eXJ259Fy$rHqU@^4W$}X2Q-9>>St-BI+7Q zFfkr8!G;xcz{Xa3M0M&jC2qpnI`AwgVY4mQKLeu=KMJ)FTu!GehVs#CKbB9OQyt+$ z=>fve_!R~QHm<9mS86BTUG*`8)-su5`m)4Mqo1a%_pvx==HQ~EVXcEnT&{II8JA!a zO8lut#8DcJgM>uMoKh3!rV4eH#25*qic%dZX(uMYuOS^ z9c0Yryvzuv@xau?6#P~lunYzVHJUFn$!$}qCIX)2g4>#4xY3SzZ%EgnYfNNm@j`N3 z&R1_QbJgE=E&8Gmb1jHp&ad@WtF$RZY8d6T`l4W)(mHU+BFozWatfH_-ry2|fcxo! znks5In&fM0RPsE}0g!11!VWbKI?`wHDy-te^Y0}*T+Yx9VHgbc{)dzoHeUWuI@26# z;DaJ8)Nez3hLI>^IPtJ_zf6I}^veh4Ck7e&v(!&fHfX6n2AAHCdKTXW^BJIlF>rYZ zlqook-hW|+4qqdI4X@0QJBe|r3-GCx{N4`EhC0qHD!vaQ%>gw_3geE;_~vSng+f1(b;C%3GxP>pM$4#!YM<$ey! zFaV0PIpp^J4OsR9zX28X!fp6{={!{}9@Q7o1(Fg`z_Ok`Z%XTa5PF^J102UJf6zj{j2+QB47y4O$k6QPMe7{CXg z>o~727&iyL!Fp)qJ4^;A7E(p1h8<2)i}l(dPuOTKIb9N~OOHXHNszcGei(0$XqF2o zlLjvfeisu$Z%xq=<7O2eSc1i9SyQ`poP^8vIVsT zN`zWkr^2i22`VgB$LQ>-6-&nv9CSgt$J>;yc$TsVrZgdVFOAVCD2PU^P@roolQllp z_$PVcn##B7W-P&?n#C#=80jFAVum3f0#p32mdYVN04AjV1V7sJv=Kz6WU!5{z1<<~ zLSZLGE}!Dnr$m_7%$;SYugV3KWO^NYx)i2S@|xAak&Jo?KhuPTjKZVI>7Uj4!fD94 z^nl}1gN=oWag4PDR}iX@BXUcKjg>}=;z|lxaltQKY?&iW`i-k(g8})*iY#=5wDi%h z=9DOWVvV}su*CZ$5P4y9{c-pPbj@W~7~AHiX-ZAccAF3vI*5ggf`AcYh;-{z_73{2 zjkE#N|6IbTQDPGW11!Feq@Oj~mxxrieyK&Nju@;*1{s$YSEW z)HZ`e#b_@NHtZ3^CMXXh3*!^<0rwog14>FZwav)kB%7#E{rwrf~72r*=oNk?gr zikQ|q%tN(}{RnyO?MIb&VyNKz)5xP=-^mV%oWks;s+ORBUx=+fIhw0RYU95N3qrBp zorHOfudMlg5&PF~Ac|hjrC!rQJ{rON=-RXwFiAo0c(Er?qQ3YOpSc#5dvW$PK<)Y) zh*a%}0;sF@UXp+lPK}@U@JrdK5(k6pHQ#_>5hU1#FzK`)RP^+)i6-;6DoXQrYv z$;I6igFW*SQtdfKj-Qafle%~HI*j-Xhg$mbuB)H``ZD0``+)m~_$|1=6#}Pa(ymzD zv>hnR1(zocYj70m)Tb-XOGFND31U$D(lv!@j)K!c>k$}i3f;ATP9psN{0(&X)#Kk* z01rDdz{zsd|Nmcb{}UF8|07BMM^OLD&Y@-b&*k{-uY^O3`M(l8q^18Z`u;b)=^uHZ zMiK>rAJcyqIePp}|HpbzA^RC51-YS+La zpiXxCE-m+*VeWzKp9%s16!4E!LUNQ)gT(unB%{?o>iaKHP{IC4)i#ibMxA2xU0sga zI{cqd|4G5;LtFqTr_RkhnG*RwK#4+upjLfUu+s<`1_*|f*F;gF4E`$=+=JO=$zdLC zmC0VFDn>ct>T44G@h>Q3s9+B;5a3ZlXbZ-#Mw`7(v@R!_1gfKXB($Z_pnosN*FSe884t0ehFaS1@R`7f+Z;rkZA2U<)LJM zI|BqN{DWZBPD2?3D|fQ8l<_n@5T6dC2giP3mn=sPmW6{V2#Auy$iySd)~t>sk!_j2 zd-&PF!Ba6-=(fBEDyWJ8)E-bg0Dwz6EXlXJr}rAcv{QwE;3CzSw#c>~!2mD_4$AQ1 zkOa>n>Wy@uK^dj{q8rAlc*i^J$4l@2QH_2bK#WC zY`{MvNoF7oiP^2~KEX+nRzB*jocBBR6sgG8JDp$4+$OZ6^G!x{E`^RP$@JfbaWo9( zo#o}Y@tKFwaR_L7W`e%N3JZASHtWU&&IXPx@7i!Jjh;T6Y(04)ot9iC;b>3ueLz+L z`-pcPil`W;V{Kb(DsQe>#oLyCbmK}?6E9*QHSimN{|&H(i`ckYzI8t0F0gAWl8@r~ zg0qxu6%kk@t7G=2VBCa}`4$h#M#=#TTw(`nvwt=!-6|L#sz$sj%bY8M!dO{-E?-(8 z9F~4Dcf@KVS!ldme*fhDMd1={>UR1&DK)EV92xG8NbYssILBo>Lb;gCG#92 z{DA&GUEYaO#%ZXK&0Z^hlcQ$=nf4`i;`FXuZ=2G5E%d^0pxav%(b>)^KfSx1|Mf2N zjd^?Jw;^U4`m%x3S9m*bTZW;y%j{C43uJi=?+nuU6&XJ)E z7jFi~&+Y%GldlYmqYKiVnG6mI3~pf<1`BROkdR>*T!MRWcY*{D?hxGFo!|s_Cjo*7 z4^9XU34vt!zJ2!Dy?cM%+kd*d`kX#bS9hO!>#b8YdQCW|gJOD8+xc$1lQoJxQ*?uA z9-dt%bw0?x%3G3gF?soegQKve+O7j9DH(?R)L~mNperd<8}In8{A<%BVfl_^sF=Svam^KV3+ZU)U%(PbMj+&YrFNc~iM zGT2Ph5xe6Rf2Llp(>9rY1!F^`ffLVD$F!N$hgUTboY6y9VBU=tb znf{S!rheDX@0+{I(Js@eoa88x&R@hSX&2ki?d0ESA{cvBN~SI~*1(8Lf#;va8#3zU zGk8?EV(|WyUh)PnuPZ)i{?l}Rl-Q26))OG~doswhg3L_>ouHh=g&Ie^LA@zm>)v4k zm(@7_nv5LXVHfS@`I>9utLB^ZjJC3pN-pY|(^SoL6P?>KVn$0FdullmuI`Eb@iv}b zRF;v<=f(yC=XrP@Sufq+x0?y>s77A-d`rvDcVp;*ePe@;h^mB9zu@IM_D^1iyguJP z{J2-doy_Ab?hLOZi;Kq+6TU6tWfCYFcQj~0!QwK-4f$X%b{?D@ag3RI!>rC^_`o{j zEt7Mw3+=rYePYFJFHGoRv{~)U7h3ptR|&=^>*ujq^M=z=+~2}(mgH}5iBKM{wblBv zwYl^Z@VkToUG@(N%}b4#CRi&=`9pGqRF zU--3qTXcvapg^UaZX;$U;R>!M>p+X1Vv!;iCSbZ`dX}tIP>eF8=kJP3*l4BS4H=1d zklqL@K0Np?Y8Nz4nZiMO-n@849LAs@sc|-V2r2raNu15%m1=xU&?k|zF*6L2ddUU` zUtHJz!rbQ0Sud>rJE~;@)G>|#A%QsOYq*xBx}?9e7nu}b(l5sn(+!eM<&8&n>RPs7rt;zSX zdX$Lo5^@5m!y~qYpQ%qC?8omnqh8dmOmh(ynrlRpPid<<3m}JwY&P1BvaBV%1Z)J( z@vOS9y3)&K6ix*scqy41KMK+kvED*rV3yZfLPXNNgDuMJv;U<_yao$!pu6>UO%@x zBZ##?yagA*jo?NQmUzd9nBx0)?8{U)yWb4%R&3WZW4te%YyBJqPA2=LxF&}>f3>RW z3tOSM30xpWs-Dar1Lel2p^F*2tiknDAFp{@UdAz!P-VuA%srNq;&;|(j7upy*6GD{(9)kN1=!W~h zq0K*eLjORU$mD10<*LPU|7%C_zXku<>iwPkldG3!>@R1pSD!s{1?Io=s9*gH;7a!~ zFy#H&(PhMV@XVGts!z2lIm3j&MC~>HL(9d>!M3pA=LV4Wg!VXvXA>oPSnh<_a`tIx z3igv1vbFhPw>e+E3UHIXn4@d_ieqRCe^?@%)9{G3Bpw&@PTAR3(kiD2_~nS^m2~%9 z_Z07J=+f|zIAr2R8ns5|bY=d?XrquXK4WB{Iu9g0zQCHlE7V%)X;L^#RtIypveeau zLzmwL9J^=-W7eg8l?_|{60#j)t_8EOB*=^KjApQFe8ao^*b2J|9HeR&3f{ zyJZHnYCars^S*K3Aj~`)K$m{(;)JtxYr2fHM!GF_uK?_Wa@4ofbj2#Ahau>QAQ~}qet9Xhks#dAK*;YH+a6n6?+b>?!2HBNy9!PXb{A#^vd5hyvGi@czQZ4flUgYbU70C?36w9MslXU*sR4&bHN_j);nX1iya&tE_`mCvq3`%nQO>JFR4wNVPMb7D;x!v>rCCFrG9?;~PHEB&*lWxBR1XucZSr9rG99cAkkIq2<4* zb?W5NZ3dI!q)l1&sd$=KO}EtAbXZn?cjZpsGG}+0?`ly_A zMt4@8fg*~-tT&=<2GZ>jdwliE~DZXmVzRrT{J&=i?p2ZOt+*5bl(4#oJ7Ua z^chiI{RNhRrS37iTj`IQP+{AL>F zK0-csvA!vph2=oqfM@V|*Mpvpk z)E`x(^9GVx!==|6QpEaSj^eY0#`Ms_?dul8e=M6GTWFrVGpG;#Qmpm$i~qbxAmg~J zJHhw`X(?y3v;Y^@vW+QDjdAAV<}Vx^CSBO4rtt;}7@_=Zzq z^kumEKmp?qc_ZjDB_$;wZ~b?mEkN1^W0;U5WhLu<=S+L9cd;N;py)#&7$J6{R&~_Q zM!EQV(m8mJsy0_=M=+M z7meF1w*exOvQnkY@rMlf?p&6+a_s5n0SC z3SDsaGZ|%66q$(YO6q>@VMhhUFfr^=aEjikQU9D$YzBc#BqmemFlhcAa?Y zNzeb>JObZ;Y|U$y)Et+;#PbTAVw3fB2(UwiEj-OkS?B1|^J)ISYA`&cYbE}>GVYH1%Q>?Y#f zfhg1GduD1rw^EP4fDHr5wT`GxPjatw&a3U$MZ(T}X4DQ9_J*UR6s z=EUHE#j%BJ-<#20;Aox6Q^B31o0$#xGTEzF{dQdfE57ImAFgM!{((kwELOzR;g7h) zwOeoXTDF^_;1m0$p-*i7!4&mKm2^UWdK0=+pP|pcvM4~v*({ul3-%h=}UCiyI7<AU+Slp)liBFUWkarIiJ zz+qwYCVnOmQWjnrZM7BxY&=mKcod$;3~6pXwUQC`@{Gy7c+P$P5T;DWJkkT9$fS6& z=QL)`0E`eRMrah{0VE=o`knKYd?S3~I+wre*wDRr6$z8(omNT3Etjk|M(mTbmvNEz zTJtKzF~FHtS(Hb1+$&EP9*Xjfpe>kDj4sMu+N~^V$pK+#B((6H0f0oZQ~RUxz;!zJ zqW|j0AXo}JbA7D)<>#q)1lo%rq5R;|%!+&J05fvQy4(u=1>C9`(Br=Vv7V5*w)L}D zSo(Qp?$z#5Pf32D|BBmE-f3zd9`{B*y z1@(&#aNmJGE}=HNjBb3!vT50~iE;;kd9YrL1UCkE)QaTRk&Hlb#9u&Q|LDmdRd_;* zNO;^au7pKsYrsR6Ds~DXS}1q32VjBHbra*cgjh8-W02s;LOi_e&p%xjLr;^6Ki9pN z-L*se%U@IB1E5I#!i9*4#Gja%*pji~-yR)##^hvyRiUZORQ|fDQPWUxv`nB`ijGT} zHM>q-D3XJc@be2;0K3o&RR{l6f;3%oIYJ^UVr~i?t#QpqItkhl6R0tq%$bcTD1aN$ zWtAUnigW#7l}U_+c^+{ZxK<$v@dpwg7^|?v^}A+%CnG{6JqB}>^4~n3O2eM0_w&<46^o zfr&G)MV|bt9M6ejA5sP_*_)QIBg7Kf_Tk~%$;qOys^k%p-1?dETjEsRn+j+D(c{Zf zXGl0{y&A9;cHj?<&5i5D$^42@YixmUNuM7p*@^RkdS22VCU)=lF<=17J)wOUadtPt z&+gxs2C|r<)XH24{d##6{W+pC$7_bxgg#u*+_5o$)C?03Bg%W%DUx4fTG7J~s!7jqf3C;=uElO%n;dE9@lFl-ew#G6$m*kDe^6y1X zFwT63^jN8L$G4NTc7aUXnX*O*j5sOuG@>nZa%+vZZyp%GkCN*6f$}T5K`D0mTC8u{ zet$oS^Qqe1WrEpX;-$yuE82-fkI1=5Ei{!qq)KL4o_q7&F2Ibmf2&;l(hzsb+ptbA z#A-97)`_f#<;i3zm@I5_Ce0!H20jwfCI9o)E8z(@@~y zY$fmI1Vslg$8pz#f&Il|*g>7gj%j7bv-u@w)baXQy4I@`_{2c;&s>pEV>ho_VFa!1 zedMO_0?vG^QX{WUjyf;tn{lQD#qdcC91hog79QY*$n0u;f}q+qUG6KMhA%yz-?>!+{YHNG~6 z1yX;65ml*4A=Co{6gu5&Z<783JYU@;`F>(oqzIH)S0N(*!^DD#=1?XWrR1T!bhWOI zU`a=eLIxFr1N6qKRX z8LvRxc+o+xGhVSSo#rIrU=H=+zvHjikj2zR#-HRSs`Bt}fCB+ZaI{nvi9G*Wel3lI z>7{RZBc#USi`@*=!d>WC<~&^&lZyYV7iFYkY#6T90u9u?Y-?7(U1cz@Sr-w=eCjdl zNqZ0vZ`hW)veE!WllQ5KKFSpM=MNGw%0|!rf)`mkDLkSltk+5JW>d}9Mgx8Y6BXL6 z&PvlDF(Cs310Tmx-+r*-Q6y`ZFyZs1RrMppo{^!q$nowXAT$DcEeJR?ki@Uq0+5~?x z()!L%WiP4Chqg3#sH0$m0s6u)mr#`PkoCE=JKKISwY7rNZ2>PH=o z)z@K8MyF-wh7<$^bC{6>47iB;o-Oh+67*KiltBy&RT5U^VprwxP0MAQ{iM2A@+-r4 zLxMX6`qGV*dg~0oDF?bNdO6j95fzNYE0R{gRFxmJ9+YMB_vcKi!D;-g-%wl{4`nIu zn_9Mi)uQHr6yzlWO-Sw+J_X_T1F`sUg1^90Pvz$vwm3HkUXVW;XNABXCiXUN7m_iY zF~y`rA+=5>(pEjE2^h=bNagHE~R68;Uvv zi^g8E>}vG1+|a&MQ9Ujf@>{wk9VgZR%Q#znv7&t_?X~orlSqi!kN+q*0*`UBCYQ#R z6OBXQ8OpHpDN$jC5X95)$;QcqG}`1n5xqPA{j4gvMrUE>ifbhr@V$D<=W<_IR9}*? zp3T|Vf+J&2Hz$q2`cTM0q}u zl#^PyOH{6jZS*O6?T=P%%Puj7DG&Fm|J?On{A3{#5)nsC`QFlSK4+VUhnN8BwZTJd zZJ1?H9}+^gb-~AYN@La|R&YNPTtjHE)bM30&EG}_7}eVCqXLGahjux?;W?20xsBGC zpfW@D?0F2I<6s`Ms~T=fBR;?>g0~CHI)Ft}le}mQXr?*WBvaoLsy<1Q3~gvaNVIR> z-I{RYQmt-E3I%qD@^a(GKGQk7;!pdbG#~|lE1#@{goP&2)P#m82Lsoz$FIzP3h}M4 zwv(9h*+|l-Io~lgBvRhxo6HM40$|6)t(wD%)_`UZZ3yaThaAndg@BDU%4kaO;-<#3 z8YIuLQgaC*oE}8_b6?kv_`O2WDv;QbG!*ZL#H>wSJ@t6$dubVtIt20_nhiqH7DRPt#1aVX86K&g2?}ez@srC4WZ%BHP`i&A4`yJ`=48aq%=U~) z!n9z)-1?N%;8vlIdtDe+5X8xhMMXML*Fxp?`0d7Yh57vV87Kgsl&{CXh#yWC?<3fO zVeB2wKVgHv1cDeP^bQ`$Nxv&Je##!u)c~E^nn9ZdxFMKy9EXxVXUIP*URE-|v$k>1SFd;2iy9Qy$SlF^7?9_R)@^N|L1CY>tHL8RAes=J9 z-)~rYllrZnR#@uO^icIUE;fR0`MlRb#juSx47!QDExZ?h0saq= z_DWp0QHm6xn4Ur7uK-3-<RM_Yk*#asq24~)S({_9$giFrGgi@O&RljwHE_U|8Wt)_f z%o!B&fu{;=0k{{BF4Kv`^=+KXM+)qie_@`3S6E_^dTvisK=ASCcW!^$Zwp-P#&5qF z?sr!JB~}~Tr)rHmNl`yT1E>MqJ#8adY`DA^mpnQ`UKT!{GXXF2*g6o#N^-VRyYWxm zYI*82So)U>Cr#cK^mD{J=F;czY1?u-%)uRuk2R@78w0q!jSFP)mj;n8fWG#(nR(S9WR)NIUbnk z&0ZkKVnQQ^8Yu8E6Y#x$5_A?A5G^ObK_3VLZAe(n=(QpHAMY^JNpRfd$lH`zWlWgY zOxIgYcK^8;bvA`hNJP1&chXbop89?j4R=*Ra(>zx?|ug4iPw&X zut4e}wAiF-F1MmslH&nsiSWq@_4%U%VWi0?>_pwV#5TQ&EmOxRlUiFME_a8(yID4S zd<2h2w`gN92XR2cZxxQGice+=`2Fo04;Vi9m8t%)^Q*jjbR~|T%O4HXkb(=8>FGylH2`8ZUG6g+CpqK6sDE0ktr%+_I!_Z z0xvK|>v`cJ#I5Ec>sCzY457w2n)fH`@lh~vb0S65JL{0RD4dAZdzD8Ggm1<e&|D3w1yn~YwdDYzQbnJ~!lM(ZRr z_9fRpa`RB%vpX*4#Lh8O`32RcnxjcB%{D1Dsvn6pttNXMM7oPm=j6F9C;euT)a%5g z%HOe0_I1jQ()FA2HW@V*)vQih`lfCubrS;hV=aK1G;0~iy_^})>N+@vZN>U7!_^{M zHw9e(*y_B$`4X=31^p79m@L=#X>L#p>qMK!NA=N&xKK80!Ur^^DUV~8tyEE}N-uG< zY?duaw8Z=ai;R{@Rl}U{Er(K>mB9dw#ILY#IUgL73NxA*t1%!Ri)1ikgfEP7&g0S? zbP1Np3}N9%Bf(9w;H*kzv&&Oz^<{kMuq$ZaTb_(vXUUJdyOhw}nT2aXW*C_afn8#f4Wgf3^G4_SECiol7$Dbi4b?z2> z0A+25Cz{^sdvZu4#IvVRDE1ia1+&7JwU2Fq)FwJ@5%Y;^)mG7ouGKvz1(l=@jo{iu z&0MFlDH-PAmIh^HQnXC?8y&W|dt5vM)c$6<5+-TadQLna=dNJ6Igs0YYqmb{_oo>A zLTYvP{&?B45dN|RO{mN6EM`@myK4ar)+-g5j`>Uqzmg1Gi7@pwhf!gh=yOLa5<}I+ zZz!wLHloTuszg(I05F`O#=m;WdP+20VHL-r)5ff^BA;Bx?~09NDyD*k0yKKvgas3g z)Y|pd_{_r2c$k|Z<&OmOSD~734^WJ^6e(|P6cUU>CrlRz$vBjO*b)%`iq%W;7Wxdr z5mI7YL}@rappB>^Vou|89vZCCAvm6MsF2QUHXI}Sx?Uo~{w}rG;&yyLorX*NX$%c5 z#uz5e;zYgNt!#HA1B3TwAx}VqD&J%GVwyL$L#<+sea#D#67M=0m_j=asgt5O_Ue>> z%ng}b|FmY^SGK7g^OJ+BLm`z&49v&kp%2JZ0+&2O5eFfUv8tMZIdS552@kbTh-a){ zDyLyr{|hL7j^I5S3bYufaw&R7=DsO8G5o|#qeAiU2S$0GJ|0SLvM>B~N6Dt#RDKRE z6)aDDmNH^bk;=GN)8cWydk<&wjblF3aa(An<^U;PxmX2XRUc@3BHR(YcISdVHLt{yB(*&p0vIy6% zl!5~oP-&pxcv<@!f+nN4I5JT)3Czh<#8?29lnu!1O`TRg3gxlfEmE{lew*j7Id`wf zv#2SCt=U~%KM4eBCp93%>8%O)LC$47>68<1+SnZr(!REuIbsRo&K zfY*Akq||3)tn0EArYD#f5uw=8d`8^r#-vv+W5JKWBHxV3_I#$;d0?2+PW4Wn(`i#U zW(;7fd?pIG^(c2JCLwAXj8i=lzk`YCwwl}wMJEJzi>FienOsS~u(v8o`V<_^U^aTdxq^ZWP%|mLtJt!h4v2Hh z45c+P&x*&DZI5$eN#@1QA7y13#lJi5k5CDeZtRFQTRfP+5B&Vow*QvZD3QamB9Zhf zgI--Ro_1Q^EAa|{5(|2);Ym={u!!n{DUR?|d9ZxAWQEp8XU}k}^&&tfvYFZz+@dgB zkOz`dIBO+aDo;p03K-+29z#GlQ1=rfgyx7_)Yne5SKnJK?+f} z$K3$S_`;KjH!Wct^(djL+G{_5KPxYgA4$Hu>q!2gjn0Nlp_ATmxIm+ zJZ|BgPFADb%FyA|ss&e;)j{{xP7UtplABtnN?#$8QAQmJO$Kn3cudDcs??3B28_h8 z%%mrv;;y})|h>oD9XX`zi2K38G^g;b{XP@S_Y0DLU(y*}8 zaZSs|_3jjV?%A9_o-OfFBHgB~91+oqbFYOS8;CDI^z_7fxb)EBe~L5O;sT`RZwWiD zi32wB87pVJ3NPwS=+jVXoDCfJEVSPNSUOWd)cZq(%nzZ$p*E{wZYuR+a-$+0RpVy^ z8u6_fsJU7w4tnX!z7xL;5n0^0QgGfA%;WaF2zyf{1e+%ox3=Om(O4qr9S9{Y1%h`2s0ZF{b*Cb?$GkUw2_`Y$__|+4P)aRo`H9X z)tYg$BTX3fx|RS3a{@Wobo?JRI1d)?OBO?!w1tZ>xkpwc{sJB|{4TyAq+Vr@W+all z^4@lc2B-T2kYvS6tzlO5>zVaxY8sJI2|XNf^$A0|rptp&4w_{4u7Gir{x95=HuzT! zx5Byz{{cDKx4usxXsUqV?3S*up%?%kSWocsRR(=lRG|2;PnJ!T>7d&7#XnCrMLo{G z4ZnZNHeG_xU>zzfE1NMEVUhVz%paTg_UR@%s@P&nm2;>sX%WSX4o(@3Bz`DkEF%vZ n)6f4> + +<> + +<> + +== Overview == +This package provides global map for multiple robots. It can merge maps from arbitrary number of robots. It expects maps from individual robots as ROS topics. If your run multiple robots under the same ROS master then {{{multirobot_map_merge}}} will probably work for you out-of-the-box. It is also very easy to setup an simulation experiment. + +{{attachment:screenshot.jpg||width="755px"}} + +If your run your robots under multiple ROS masters you need to run your own way of communication between robots and provide maps from robots on local topics (under the same master). Also if you want to distribute merged map back to robots your communication must take care of it. + +<> + +{{{multirobot_map_merge}}} does not depend on any particular communication between robots. + +== Architecture == + +{{{multirobot_map_merge}}} finds robot maps dynamically and new robots can be added to system at any time. + +{{attachment:architecture.svg||width="755px"}} + +To make this dynamic behaviour possible there are some constrains placed on robots. First all robots must publish map under `/map`, where topic name (`map`) is configurable, but must be same for all robots. For each robot `` will be of cause different. + +This node support merging maps with known initial positions of the robots or without. See below for details. + +== Merging modes == + +Two merging modes are currently supported as orthogonal options. If you know initial positions of robots you may preferably use the first mode and get exact results (rigid transformation will be computed according to initial positions). If you don't know robot's starting points you are still able to use the second mode where transformation between grids will be determined using heuristic algorithm. You can choose between these two modes using the `known_init_poses` parameter. + +=== merging with known initial positions === + +This is preferred mode whenever you are able to determine exact starting point for each robot. You need to provide initial position for each robot. You need to provide set of `/map_merge/init_pose` parameters. These positions should be in `world_frame`. See [[#ROS API]]. + +In this merging these parameters are mandatory. If any of the required parameters is missing robot won't be considered for merging (you will get warning with name of affected robot). + +=== merging without known initial positions === + +If you can't provide initial poses for robots this mode has minimal configuration requirements. You need to provide only map topic for each robot. Transformation between grids is estimated by feature-matching algorithm and therefore requires grids to have sufficient amount of overlapping space to make a high-probability match. If grids don't have enough overlapping space to make a solid match, merged map can differ greatly from physical situation. + +Estimating transforms between grids is cpu-intesive so you might want to tune `estimation_rate` parameter to run re-estimation less often if it causes any troubles. + +== ROS API == +{{{ +#!clearsilver CS/NodeAPI + +name = map_merge +desc = Provides map merging services offered by this package. Dynamically looks for new robots in the system and merges their maps. + +pub { + 0.name = map + 0.type = nav_msgs/OccupancyGrid + 0.desc = Merged map from all robots in the system. +} +sub { + 0.name = /map + 0.type = nav_msgs/OccupancyGrid + 0.desc = Local map for specific robot. + + 1.name = /map_updates + 1.type = map_msgs/OccupancyGridUpdate + 1.desc = Local map updates for specific robot. Most of the <> sources (mapping algorithms) provides incremental map updates via this topic so they don't need to send always full map. This topic is optional. If your mapping algorithm does not provide this topic it is safe to ignore this topic. However if your mapping algorithm does provide this topic, it is preferable to subscribe to this topic. Otherwise map updates will be slow as all partial updates will be missed and map will be able to update only on full map updates. +} + +param { + group.0 { + name = ROBOT PARAMETERS + desc = Parameters that should be defined in the namespace of each robot if you want to use merging with known initial poses of robots (`known_init_poses` is `true`). Without these parameters robots won't be considered for merging. If you can't provide these parameters use merging without known initial poses. See [[#Merging modes]] + + 0.name = /map_merge/init_pose_x + 0.default = `` + 0.type = double + 0.desc = `x` coordinate of robot initial position in `world_frame`. Should be in meters. It does not matter which frame you will consider global (preferably it should be different from all robots frames), but relative positions of robots in this frame must be correct. + + 1.name = /map_merge/init_pose_y + 1.default = `` + 1.type = double + 1.desc = `y` coordinate of robot initial position in `world_frame`. + + 2.name = /map_merge/init_pose_z + 2.default = `` + 2.type = double + 2.desc = `z` coordinate of robot initial position in `world_frame`. + + 3.name = /map_merge/init_pose_yaw + 3.default = `` + 3.type = double + 3.desc = `yaw` component of robot initial position in `world_frame`. Represents robot rotation in radians. + } + + group.1 { + name = NODE PARAMETERS + desc = Parameters that should be defined in the namespace of this node. + + 0.name = ~robot_map_topic + 0.default = `map` + 0.type = string + 0.desc = Name of robot map topic without namespaces (last component of topic name). Only topics with this name will be considered when looking for new maps to merge. This topics may be subject to further filtering (see below). + + 1.name = ~robot_map_updates_topic + 1.default = `map_updates` + 1.type = string + 1.desc = Name of robot map updates topic of <> without namespaces (last component of topic name). This topic will be always subscribed in the same namespace as `robot_map_topic`. You'll likely need to change this only when you changed `robot_map_topic`. These topics are never considered when searching for new robots. + + 2.name = ~robot_namespace + 2.default = `` + 2.type = string + 2.desc = Fixed part of robot map topic. You can employ this parameter to further limit which topics will be considered during dynamic lookup for robots. Only topics which contain (anywhere) this string will be considered for lookup. Unlike `robot_map_topic` you are not limited by namespace logic. Topics will be filtered using text-based search. Therefore `robot_namespace` does not need to be ROS namespace, but can contain slashes etc. This must be common part of all robots map topics name (all robots for which you want to merge map). + + 3.name = ~known_init_poses + 3.default = `true` + 3.type = bool + 3.desc = Selects between merging modes. `true` if merging with known initial positions. See [[#Merging modes]] + + 4.name = ~merged_map_topic + 4.default = `map` + 4.type = string + 4.desc = Topic name where merged map will be published. + + 5.name = ~world_frame + 5.default = `world` + 5.type = string + 5.desc = Frame id (in [[tf]] tree) which will be assigned to published merged map. This should be frame where you specified robot initial positions. + + 6.name = ~merging_rate + 6.default = `4.0` + 6.type = double + 6.desc = Rate in Hz. Basic frequency on which this node discovers merges robots maps and publish merged map. Increase this value if you want faster updates. + + 7.name = ~discovery_rate + 7.default = `0.05` + 7.type = double + 7.desc = Rate in Hz. Frequency on which this node discovers new robots. Increase this value if you need more agile behaviour when adding new robots. Robots will be discovered sooner. + + 8.name = ~estimation_rate + 8.default = `0.5` + 8.type = double + 8.desc = Rate in Hz. This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Frequency on which this node re-estimates transformation between grids. Estimation is cpu-intensive, so you may wish to lower this value. + + 9.name = ~estimation_confidence + 9.default = `1.0` + 9.type = double + 9.desc = This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Confidence according to probabilistic model for initial positions estimation. Default value 1.0 is suitable for most applications, increase this value for more confident estimations. Number of maps included in the merge may decrease with increasing confidence. Generally larger overlaps between maps will be required for map to be included in merge. Good range for tuning is [1.0, 2.0]. + } +} +}}} + +== Acknowledgements == + +This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague. + +{{{ +@masterthesis{Hörner2016, + author = {Jiří Hörner}, + title = {Map-merging for multi-robot system}, + address = {Prague}, + year = {2016}, + school = {Charles University in Prague, Faculty of Mathematics and Physics}, + type = {Bachelor's thesis}, + URL = {https://is.cuni.cz/webapps/zzp/detail/174125/}, +} +}}} + +Idea for dynamic robot discovery is from [[map_merging]] package from Zhi Yan. Merging algorithm and configuration are different. + +## AUTOGENERATED DON'T DELETE +## CategoryPackage diff --git a/m-explore-ros2/map_merge/include/combine_grids/grid_compositor.h b/m-explore-ros2/map_merge/include/combine_grids/grid_compositor.h new file mode 100644 index 0000000..2fc17fe --- /dev/null +++ b/m-explore-ros2/map_merge/include/combine_grids/grid_compositor.h @@ -0,0 +1,60 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef GRID_COMPOSITOR_H_ +#define GRID_COMPOSITOR_H_ + +#include + +#include + +namespace combine_grids +{ +namespace internal +{ +class GridCompositor +{ +public: + nav_msgs::msg::OccupancyGrid::SharedPtr compose(const std::vector& grids, + const std::vector& rois); +}; + +} // namespace internal + +} // namespace combine_grids + +#endif // GRID_COMPOSITOR_H_ diff --git a/m-explore-ros2/map_merge/include/combine_grids/grid_warper.h b/m-explore-ros2/map_merge/include/combine_grids/grid_warper.h new file mode 100644 index 0000000..c8cac08 --- /dev/null +++ b/m-explore-ros2/map_merge/include/combine_grids/grid_warper.h @@ -0,0 +1,61 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef GRID_WARPER_H_ +#define GRID_WARPER_H_ + +#include + +namespace combine_grids +{ +namespace internal +{ +class GridWarper +{ +public: + cv::Rect warp(const cv::Mat& grid, const cv::Mat& transform, + cv::Mat& warped_grid); + +private: + cv::Rect warpRoi(const cv::Mat& grid, const cv::Mat& transform); +}; + +} // namespace internal + +} // namespace combine_grids + +#endif // GRID_WARPER_H_ diff --git a/m-explore-ros2/map_merge/include/combine_grids/merging_pipeline.h b/m-explore-ros2/map_merge/include/combine_grids/merging_pipeline.h new file mode 100644 index 0000000..7fe1435 --- /dev/null +++ b/m-explore-ros2/map_merge/include/combine_grids/merging_pipeline.h @@ -0,0 +1,146 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef MERGING_PIPELINE_H_ +#define MERGING_PIPELINE_H_ + +#include + +#include +#include +#include + +#include + +namespace combine_grids +{ +enum class FeatureType { AKAZE, ORB, SURF }; + +/** + * @brief Pipeline for merging overlapping occupancy grids + * @details Pipeline works on internally stored grids. + */ +class MergingPipeline +{ +public: + template + void feed(InputIt grids_begin, InputIt grids_end); + bool estimateTransforms(FeatureType feature = FeatureType::AKAZE, + double confidence = 1.0); + nav_msgs::msg::OccupancyGrid::SharedPtr composeGrids(); + + std::vector getTransforms() const; + template + bool setTransforms(InputIt transforms_begin, InputIt transforms_end); + +private: + std::vector grids_; + std::vector images_; + std::vector transforms_; + double max_confidence_achieved_ = 0.0; +}; + +template +void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end) +{ + static_assert(std::is_assignable::value, + "grids_begin must point to nav_msgs::msg::OccupancyGrid::ConstSharedPtr " + "data"); + + // we can't reserve anything, because we want to support just InputIt and + // their guarantee validity for only single-pass algos + images_.clear(); + grids_.clear(); + for (InputIt it = grids_begin; it != grids_end; ++it) { + if (*it && !(*it)->data.empty()) { + grids_.push_back(*it); + /* convert to opencv images. it creates only a view for opencv and does + * not copy or own actual data. */ + images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1, + const_cast((*it)->data.data())); + } else { + grids_.emplace_back(); + images_.emplace_back(); + } + } +} + +template +bool MergingPipeline::setTransforms(InputIt transforms_begin, + InputIt transforms_end) +{ + static_assert(std::is_assignable::value, + "transforms_begin must point to geometry_msgs::msg::Transform " + "data"); + + decltype(transforms_) transforms_buf; + for (InputIt it = transforms_begin; it != transforms_end; ++it) { + const geometry_msgs::msg::Quaternion& q = it->rotation; + if ((q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) < + std::numeric_limits::epsilon()) { + // represents invalid transform + transforms_buf.emplace_back(); + continue; + } + double s = 2.0 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + double a = 1 - q.y * q.y * s - q.z * q.z * s; + double b = q.x * q.y * s + q.z * q.w * s; + double tx = it->translation.x; + double ty = it->translation.y; + cv::Mat transform = cv::Mat::eye(3, 3, CV_64F); + transform.at(0, 0) = transform.at(1, 1) = a; + transform.at(1, 0) = b; + transform.at(0, 1) = -b; + transform.at(0, 2) = tx; + transform.at(1, 2) = ty; + + transforms_buf.emplace_back(std::move(transform)); + } + + if (transforms_buf.size() != images_.size()) { + return false; + } + std::swap(transforms_, transforms_buf); + + return true; +} + +} // namespace combine_grids + +#endif // MERGING_PIPELINE_H_ diff --git a/m-explore-ros2/map_merge/include/map_merge/map_merge.h b/m-explore-ros2/map_merge/include/map_merge/map_merge.h new file mode 100644 index 0000000..481c8ec --- /dev/null +++ b/m-explore-ros2/map_merge/include/map_merge/map_merge.h @@ -0,0 +1,128 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, Zhi Yan. + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef MAP_MERGE_H_ +#define MAP_MERGE_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace map_merge +{ +struct MapSubscription { + // protects consistency of writable_map and readonly_map + // also protects reads and writes of shared_ptrs + std::mutex mutex; + + geometry_msgs::msg::Transform initial_pose; + nav_msgs::msg::OccupancyGrid::SharedPtr writable_map; + nav_msgs::msg::OccupancyGrid::ConstSharedPtr readonly_map; + + // ros::Subscriber map_sub; + // ros::Subscriber map_updates_sub; + rclcpp::Subscription::SharedPtr map_sub; + rclcpp::Subscription::SharedPtr map_updates_sub; +}; + +class MapMerge : public rclcpp::Node +{ +private: + /* parameters */ + double merging_rate_; + double discovery_rate_; + double estimation_rate_; + double confidence_threshold_; + std::string robot_map_topic_; + std::string robot_map_updates_topic_; + std::string robot_namespace_; + std::string world_frame_; + bool have_initial_poses_; + + // publishing + rclcpp::Publisher::SharedPtr merged_map_publisher_; + // maps robots namespaces to maps. does not own + std::unordered_map robots_; + // owns maps -- iterator safe + std::forward_list subscriptions_; + size_t subscriptions_size_; + boost::shared_mutex subscriptions_mutex_; + combine_grids::MergingPipeline pipeline_; + std::mutex pipeline_mutex_; + + rclcpp::Logger logger_ = rclcpp::get_logger("MapMergeNode"); + + // timers + rclcpp::TimerBase::SharedPtr map_merging_timer_; + rclcpp::TimerBase::SharedPtr topic_subscribing_timer_; + rclcpp::TimerBase::SharedPtr pose_estimation_timer_; + + std::string robotNameFromTopic(const std::string& topic); + // bool isRobotMapTopic(const ros::master::TopicInfo& topic); + bool isRobotMapTopic(const std::string topic, std::string type); + bool getInitPose(const std::string& name, geometry_msgs::msg::Transform& pose); + + void fullMapUpdate(const nav_msgs::msg::OccupancyGrid::SharedPtr msg, + MapSubscription& map); + void partialMapUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg, + MapSubscription& map); + +public: + MapMerge(); + + void topicSubscribing(); + void mapMerging(); + /** + * @brief Estimates initial positions of grids + * @details Relevant only if initial poses are not known + */ + void poseEstimation(); +}; + +} // namespace map_merge + +#endif /* MAP_MERGE_H_ */ diff --git a/m-explore-ros2/map_merge/include/map_merge/ros1_names.hpp b/m-explore-ros2/map_merge/include/map_merge/ros1_names.hpp new file mode 100644 index 0000000..d66bc29 --- /dev/null +++ b/m-explore-ros2/map_merge/include/map_merge/ros1_names.hpp @@ -0,0 +1,141 @@ +/* +* Copyright (C) 2009, Willow Garage, Inc. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright notice, +* this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*/ +#include +#include +#include + +// Almost all code was taken from http://docs.ros.org/en/indigo/api/roscpp/html/names_8cpp_source.html + +class InvalidNameException : public std::runtime_error +{ +public: + InvalidNameException(const std::string& msg) + : std::runtime_error(msg) + {} +}; + +namespace ros1_names +{ + +bool isValidCharInName(char c) +{ + if (isalnum(c) || c == '/' || c == '_') + { + return true; + } + + return false; +} + +bool validate(const std::string& name, std::string& error) +{ + if (name.empty()) + { + return true; + } + + // First element is special, can be only ~ / or alpha + char c = name[0]; + if (!isalpha(c) && c != '/' && c != '~') + { + std::stringstream ss; + ss << "Character [" << c << "] is not valid as the first character in Graph Resource Name [" << name << "]. Valid characters are a-z, A-Z, / and in some cases ~."; + error = ss.str(); + return false; + } + + for (size_t i = 1; i < name.size(); ++i) + { + c = name[i]; + if (!isValidCharInName(c)) + { + std::stringstream ss; + ss << "Character [" << c << "] at element [" << i << "] is not valid in Graph Resource Name [" << name <<"]. Valid characters are a-z, A-Z, 0-9, / and _."; + error = ss.str(); + + return false; + } + } + + return true; +} + +std::string parentNamespace(const std::string& name) +{ + std::string error; + if (!validate(name, error)) + { + throw InvalidNameException(error); + } + + if (!name.compare("")) return ""; + if (!name.compare("/")) return "/"; + + std::string stripped_name; + + // rstrip trailing slash + if (name.find_last_of('/') == name.size()-1) + stripped_name = name.substr(0, name.size() -2); + else + stripped_name = name; + + //pull everything up to the last / + size_t last_pos = stripped_name.find_last_of('/'); + if (last_pos == std::string::npos) + { + return ""; + } + else if (last_pos == 0) + return "/"; + return stripped_name.substr(0, last_pos); +} + +std::string clean(const std::string& name) +{ + std::string clean = name; + + size_t pos = clean.find("//"); + while (pos != std::string::npos) + { + clean.erase(pos, 1); + pos = clean.find("//", pos); + } + + if (*clean.rbegin() == '/') + { + clean.erase(clean.size() - 1, 1); + } + + return clean; +} + +std::string append(const std::string& left, const std::string& right) +{ + return clean(left + "/" + right); +} + +} // namespace ros1_names \ No newline at end of file diff --git a/m-explore-ros2/map_merge/launch/from_map_server.launch.py b/m-explore-ros2/map_merge/launch/from_map_server.launch.py new file mode 100644 index 0000000..0bad14c --- /dev/null +++ b/m-explore-ros2/map_merge/launch/from_map_server.launch.py @@ -0,0 +1,141 @@ +# showcases map_merge with static maps served by map_server + +# you can run this with test maps provided in m-explore-extra repo +# https://github.com/hrnr/m-explore-extra + +# roslaunch multirobot_map_merge from_map_server.launch map1:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2011-08-09-12-22-52.yaml map2:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2012-01-28-11-12-01.yaml rviz:=True + +import os + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, PushRosNamespace + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + GroupAction, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory("multirobot_map_merge"), "config", "params.yaml" + ) + use_sim_time = LaunchConfiguration("use_sim_time") + namespace = LaunchConfiguration("namespace") + known_init_poses = LaunchConfiguration("known_init_poses") + rviz = LaunchConfiguration("rviz") + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + declare_namespace_argument = DeclareLaunchArgument( + "namespace", + default_value="/", + description="Namespace for the explore node", + ) + declare_known_init_poses_argument = DeclareLaunchArgument( + "known_init_poses", + default_value="False", + description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file", + ) + declare_rviz_argument = DeclareLaunchArgument( + "rviz", + default_value="False", + description="Run rviz2", + ) + + num_maps = 2 + group_actions = [] + + for i in range(num_maps): + map_argument_name = f"map{i+1}" + map_yaml_file = LaunchConfiguration(map_argument_name) + declare_map_argument = DeclareLaunchArgument( + map_argument_name, + default_value=f"{map_argument_name}.yaml", + description="Full path to map yaml file to load", + ) + map_server = Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + namespace=f"robot{i+1}", + output="screen", + parameters=[ + {"yaml_filename": map_yaml_file}, + {"use_sim_time": use_sim_time}, + ], + ) + map_server_manager = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_localization", + output="screen", + namespace=f"robot{i+1}", + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": True}, + {"node_names": ["map_server"]}, + ], + ) + group_action = GroupAction( + [ + PushRosNamespace(namespace=namespace), + map_server_manager, + map_server, + declare_map_argument, + ] + ) + + group_actions.append(group_action) + + node = Node( + package="multirobot_map_merge", + name="map_merge", + namespace=namespace, + executable="map_merge", + parameters=[ + config, + {"use_sim_time": use_sim_time}, + {"known_init_poses": known_init_poses}, + ], + output="screen", + ) + + rviz_config_file = os.path.join( + get_package_share_directory("multirobot_map_merge"), "launch", "map_merge.rviz" + ) + start_rviz_cmd = Node( + condition=IfCondition(rviz), + package="rviz2", + executable="rviz2", + arguments=["-d", rviz_config_file], + output="screen", + ) + exit_event_handler = RegisterEventHandler( + condition=IfCondition(rviz), + event_handler=OnProcessExit( + target_action=start_rviz_cmd, + on_exit=EmitEvent(event=Shutdown(reason="rviz exited")), + ), + ) + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_known_init_poses_argument) + ld.add_action(declare_namespace_argument) + ld.add_action(declare_rviz_argument) + for group_action in group_actions: + ld.add_action(group_action) + ld.add_action(node) + ld.add_action(start_rviz_cmd) + ld.add_action(exit_event_handler) + + return ld diff --git a/m-explore-ros2/map_merge/launch/map_merge.launch.py b/m-explore-ros2/map_merge/launch/map_merge.launch.py new file mode 100644 index 0000000..ff037b9 --- /dev/null +++ b/m-explore-ros2/map_merge/launch/map_merge.launch.py @@ -0,0 +1,57 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory("multirobot_map_merge"), "config", "params.yaml" + ) + use_sim_time = LaunchConfiguration("use_sim_time") + namespace = LaunchConfiguration("namespace") + known_init_poses = LaunchConfiguration("known_init_poses") + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + declare_namespace_argument = DeclareLaunchArgument( + "namespace", + default_value="/", + description="Namespace for the explore node", + ) + declare_known_init_poses_argument = DeclareLaunchArgument( + "known_init_poses", + default_value="True", + description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file", + ) + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + node = Node( + package="multirobot_map_merge", + name="map_merge", + namespace=namespace, + executable="map_merge", + parameters=[ + config, + {"use_sim_time": use_sim_time}, + {"known_init_poses": known_init_poses}, + ], + output="screen", + remappings=remappings, + ) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_known_init_poses_argument) + ld.add_action(declare_namespace_argument) + ld.add_action(node) + return ld diff --git a/m-explore-ros2/map_merge/launch/map_merge.rviz b/m-explore-ros2/map_merge/launch/map_merge.rviz new file mode 100644 index 0000000..9190edb --- /dev/null +++ b/m-explore-ros2/map_merge/launch/map_merge.rviz @@ -0,0 +1,154 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 1123 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 33.400630950927734 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.5996218919754028 + Y: 0.5541106462478638 + Z: -0.0447520911693573 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1304056644439697 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1600 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000551fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000005510000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000003aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000069000003ae0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ae00000051fc0100000002fb0000000800540069006d00650100000000000004ae0000045300fffffffb0000000800540069006d00650100000000000004500000000000000000000002c60000055100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1198 + X: 1322 + Y: 115 diff --git a/m-explore-ros2/map_merge/launch/tb3_simulation/bringup_launch.py b/m-explore-ros2/map_merge/launch/tb3_simulation/bringup_launch.py new file mode 100644 index 0000000..584305f --- /dev/null +++ b/m-explore-ros2/map_merge/launch/tb3_simulation/bringup_launch.py @@ -0,0 +1,202 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import PushRosNamespace + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + launch_dir = os.path.join(bringup_dir, "launch") + + # Get the launch directory of gmapping + slam_gmapping_dir = get_package_share_directory("slam_gmapping") + slam_gmapping_launch_dir = os.path.join(slam_gmapping_dir, "launch") + + # Get the launch directory of map_merge + map_merge_dir = get_package_share_directory("multirobot_map_merge") + map_merge_launch_dir = os.path.join(map_merge_dir, "launch", "tb3_simulation") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + slam = LaunchConfiguration("slam") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + params_file = LaunchConfiguration("params_file") + autostart = LaunchConfiguration("autostart") + slam_toolbox = LaunchConfiguration("slam_toolbox") + slam_gmapping = LaunchConfiguration("slam_gmapping") + + stdout_linebuf_envvar = SetEnvironmentVariable( + "RCUTILS_LOGGING_BUFFERED_STREAM", "1" + ) + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="False", description="Whether run a SLAM" + ) + declare_slam_toolbox_cmd = DeclareLaunchArgument( + "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox" + ) + declare_slam_gmapping_cmd = DeclareLaunchArgument( + "slam_gmapping", + default_value="False", + description="Whether run a SLAM gmapping", + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", description="Full path to map yaml file to load" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation (Gazebo) clock if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ) + + # Specify the actions + bringup_cmd_group = GroupAction( + [ + PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # os.path.join(launch_dir, "slam_launch.py") + # ), + # condition=IfCondition( + # PythonExpression( + # [slam, " and ", slam_toolbox, " and not ", slam_gmapping] + # ) + # ), + # launch_arguments={ + # "namespace": namespace, + # "use_sim_time": use_sim_time, + # "autostart": autostart, + # "params_file": params_file, + # }.items(), + # ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(map_merge_launch_dir, "slam_toolbox.py") + ), + condition=IfCondition( + PythonExpression( + [slam, " and ", slam_toolbox, " and not ", slam_gmapping] + ) + ), + launch_arguments={ + "use_sim_time": use_sim_time, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "localization_launch.py") + ), + condition=IfCondition(PythonExpression(["not ", slam])), + launch_arguments={ + "namespace": namespace, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "use_lifecycle_mgr": "false", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "navigation_launch.py") + ), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "use_lifecycle_mgr": "false", + "map_subscribe_transient_local": "true", + }.items(), + ), + ] + ) + # Not in GroupAction because namespace were prepended twice because + # slam_gmapping.launch.py already accepts a namespace argument + slam_gmapping_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(slam_gmapping_launch_dir, "slam_gmapping.launch.py") + ), + condition=IfCondition( + PythonExpression([slam, " and ", slam_gmapping, " and not ", slam_toolbox]) + ), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + }.items(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_slam_toolbox_cmd) + ld.add_action(declare_slam_gmapping_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + ld.add_action(slam_gmapping_cmd) + + return ld diff --git a/m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml b/m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml new file mode 100644 index 0000000..8620f2e --- /dev/null +++ b/m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml @@ -0,0 +1,287 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # if enable_groot_monitoring is set to True, ports need to be different for each robot !! + enable_groot_monitoring: False + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: True + global_frame: odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["obstacle_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + scan: + topic: /robot1/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: True + robot_radius: 0.22 + obstacle_layer: + enabled: True + scan: + topic: /robot1/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file diff --git a/m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml b/m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml new file mode 100644 index 0000000..498fb36 --- /dev/null +++ b/m-explore-ros2/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml @@ -0,0 +1,287 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # if enable_groot_monitoring is set to True, ports need to be different for each robot !! + enable_groot_monitoring: False + groot_zmq_publisher_port: 1789 + groot_zmq_server_port: 1887 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: True + global_frame: odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["obstacle_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + scan: + topic: /robot2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: True + robot_radius: 0.22 + obstacle_layer: + enabled: True + scan: + topic: /robot2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file diff --git a/m-explore-ros2/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py b/m-explore-ros2/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py new file mode 100644 index 0000000..c217ebd --- /dev/null +++ b/m-explore-ros2/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py @@ -0,0 +1,360 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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. + +""" +Example for spawing multiple robots in Gazebo. + +This is an example on how to create a launch file for spawning multiple robots into Gazebo +and launch multiple instances of the navigation stack, each controlling one robot. +The robots co-exist on a shared environment and are controlled by independent nav stacks +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription, condition +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + LogInfo, +) +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + launch_dir = os.path.join(bringup_dir, "launch") + + # Get the launch directory for multirobot_map_merge where we have a modified launch files + map_merge_dir = get_package_share_directory("multirobot_map_merge") + launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation") + + # Names and poses of the robots for known poses demo + robots_known_poses = [ + {"name": "robot1", "x_pose": 0.0, "y_pose": 0.5, "z_pose": 0.01}, + {"name": "robot2", "x_pose": -3.0, "y_pose": 1.5, "z_pose": 0.01}, + ] + # Names and poses of the robots for unknown poses demo, the must be very close at beginning + robots_unknown_poses = [ + {"name": "robot1", "x_pose": -2.0, "y_pose": 0.5, "z_pose": 0.01}, + {"name": "robot2", "x_pose": -3.0, "y_pose": 0.5, "z_pose": 0.01}, + ] + + # Simulation settings + world = LaunchConfiguration("world") + simulator = LaunchConfiguration("simulator") + + # On this example all robots are launched with the same settings + map_yaml_file = LaunchConfiguration("map") + + autostart = LaunchConfiguration("autostart") + rviz_config_file = LaunchConfiguration("rviz_config") + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + log_settings = LaunchConfiguration("log_settings", default="true") + + known_init_poses = LaunchConfiguration("known_init_poses") + declare_known_init_poses_cmd = DeclareLaunchArgument( + "known_init_poses", + default_value="True", + description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file", + ) + + # Declare the launch arguments + declare_world_cmd = DeclareLaunchArgument( + "world", + default_value=os.path.join(launch_dir_map_merge, "worlds", "world_only.model"), + description="Full path to world file to load", + ) + + declare_simulator_cmd = DeclareLaunchArgument( + "simulator", + default_value="gazebo", + description="The simulator to use (gazebo or gzserver)", + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + declare_robot1_params_file_cmd = DeclareLaunchArgument( + "robot1_params_file", + default_value=os.path.join( + launch_dir_map_merge, "config", "nav2_multirobot_params_1.yaml" + ), + description="Full path to the ROS2 parameters file to use for robot1 launched nodes", + ) + + declare_robot2_params_file_cmd = DeclareLaunchArgument( + "robot2_params_file", + default_value=os.path.join( + launch_dir_map_merge, "config", "nav2_multirobot_params_2.yaml" + ), + description="Full path to the ROS2 parameters file to use for robot2 launched nodes", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the stacks", + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config", + default_value=os.path.join(bringup_dir, "rviz", "nav2_namespaced_view.rviz"), + description="Full path to the RVIZ config file to use.", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + slam_toolbox = LaunchConfiguration("slam_toolbox") + slam_gmapping = LaunchConfiguration("slam_gmapping") + declare_slam_toolbox_cmd = DeclareLaunchArgument( + "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox" + ) + declare_slam_gmapping_cmd = DeclareLaunchArgument( + "slam_gmapping", + default_value="False", + description="Whether run a SLAM gmapping", + ) + + # Start Gazebo with plugin providing the robot spawing service + start_gazebo_cmd = ExecuteProcess( + cmd=[ + simulator, + "--verbose", + "-s", + "libgazebo_ros_init.so", + "-s", + "libgazebo_ros_factory.so", + world, + ], + output="screen", + ) + + robot_sdf = LaunchConfiguration("robot_sdf") + declare_robot_sdf_cmd = DeclareLaunchArgument( + "robot_sdf", + default_value=os.path.join(bringup_dir, "worlds", "waffle.model"), + description="Full path to robot sdf file to spawn the robot in gazebo", + ) + + # Define commands for spawing the robots into Gazebo + spawn_robots_cmds = [] + for robot_known, robot_unknown in zip(robots_known_poses, robots_unknown_poses): + # after humble release, use spawn_entity.py + if os.getenv("ROS_DISTRO") == "humble": + spawn_robots_cmds.append( + Node( + package="gazebo_ros", + executable="spawn_entity.py", + output="screen", + arguments=[ + "-entity", + robot_known["name"], + "-file", + robot_sdf, + "-robot_namespace", + TextSubstitution(text=str(robot_known["name"])), + "-x", + TextSubstitution(text=str(robot_known["x_pose"])), + "-y", + TextSubstitution(text=str(robot_known["y_pose"])), + "-z", + TextSubstitution(text=str(robot_known["z_pose"])), + "-R", + "0.0", + "-P", + "0.0", + "-Y", + "0.0", + ], + condition=IfCondition(known_init_poses), + ) + ) + spawn_robots_cmds.append( + Node( + package="gazebo_ros", + executable="spawn_entity.py", + output="screen", + arguments=[ + "-entity", + robot_unknown["name"], + "-file", + robot_sdf, + "-robot_namespace", + TextSubstitution(text=str(robot_unknown["name"])), + "-x", + TextSubstitution(text=str(robot_unknown["x_pose"])), + "-y", + TextSubstitution(text=str(robot_unknown["y_pose"])), + "-z", + TextSubstitution(text=str(robot_unknown["z_pose"])), + "-R", + "0.0", + "-P", + "0.0", + "-Y", + "0.0", + ], + condition=UnlessCondition(known_init_poses), + ) + ) + else: + spawn_robots_cmds.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py") + ), + launch_arguments={ + "x_pose": TextSubstitution(text=str(robot_known["x_pose"])), + "y_pose": TextSubstitution(text=str(robot_known["y_pose"])), + "z_pose": TextSubstitution(text=str(robot_known["z_pose"])), + "robot_name": robot_known["name"], + "turtlebot_type": TextSubstitution(text="waffle"), + }.items(), + condition=IfCondition(known_init_poses), + ) + ) + spawn_robots_cmds.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py") + ), + launch_arguments={ + "x_pose": TextSubstitution(text=str(robot_unknown["x_pose"])), + "y_pose": TextSubstitution(text=str(robot_unknown["y_pose"])), + "z_pose": TextSubstitution(text=str(robot_unknown["z_pose"])), + "robot_name": robot_unknown["name"], + "turtlebot_type": TextSubstitution(text="waffle"), + }.items(), + condition=UnlessCondition(known_init_poses), + ) + ) + + # Define commands for launching the navigation instances + nav_instances_cmds = [] + for robot in robots_known_poses: + params_file = LaunchConfiguration(f"{robot['name']}_params_file") + + group = GroupAction( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "rviz_launch.py") + ), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": TextSubstitution(text=robot["name"]), + "use_namespace": "True", + "rviz_config": rviz_config_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir_map_merge, "tb3_simulation_launch.py") + ), + launch_arguments={ + "namespace": robot["name"], + "use_namespace": "True", + "map": map_yaml_file, + "use_sim_time": "True", + "params_file": params_file, + "autostart": autostart, + "use_rviz": "False", + "use_simulator": "False", + "headless": "False", + "slam": "True", + "slam_toolbox": slam_toolbox, + "slam_gmapping": slam_gmapping, + "use_robot_state_pub": use_robot_state_pub, + }.items(), + ), + LogInfo( + condition=IfCondition(log_settings), + msg=["Launching ", robot["name"]], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot["name"], " map yaml: ", map_yaml_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot["name"], " params yaml: ", params_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot["name"], " rviz config file: ", rviz_config_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[ + robot["name"], + " using robot state pub: ", + use_robot_state_pub, + ], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot["name"], " autostart: ", autostart], + ), + ] + ) + + nav_instances_cmds.append(group) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_robot1_params_file_cmd) + ld.add_action(declare_robot2_params_file_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_slam_toolbox_cmd) + ld.add_action(declare_slam_gmapping_cmd) + ld.add_action(declare_known_init_poses_cmd) + ld.add_action(declare_robot_sdf_cmd) + + # Add the actions to start gazebo, robots and simulations + ld.add_action(start_gazebo_cmd) + + for spawn_robot_cmd in spawn_robots_cmds: + ld.add_action(spawn_robot_cmd) + + for simulation_instance_cmd in nav_instances_cmds: + ld.add_action(simulation_instance_cmd) + + return ld diff --git a/m-explore-ros2/map_merge/launch/tb3_simulation/slam_toolbox.py b/m-explore-ros2/map_merge/launch/tb3_simulation/slam_toolbox.py new file mode 100644 index 0000000..3212f9d --- /dev/null +++ b/m-explore-ros2/map_merge/launch/tb3_simulation/slam_toolbox.py @@ -0,0 +1,49 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time") + slam_params_file = LaunchConfiguration("slam_params_file") + + remappings = [ + ("/map", "map"), + ("/scan", "scan"), + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ] + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + declare_slam_params_file_cmd = DeclareLaunchArgument( + "slam_params_file", + default_value=os.path.join( + get_package_share_directory("slam_toolbox"), + "config", + "mapper_params_online_sync.yaml", + ), + description="Full path to the ROS2 parameters file to use for the slam_toolbox node", + ) + + start_sync_slam_toolbox_node = Node( + parameters=[slam_params_file, {"use_sim_time": use_sim_time}], + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + output="screen", + remappings=remappings, + ) + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(start_sync_slam_toolbox_node) + + return ld diff --git a/m-explore-ros2/map_merge/launch/tb3_simulation/tb3_simulation_launch.py b/m-explore-ros2/map_merge/launch/tb3_simulation/tb3_simulation_launch.py new file mode 100644 index 0000000..7e89dbe --- /dev/null +++ b/m-explore-ros2/map_merge/launch/tb3_simulation/tb3_simulation_launch.py @@ -0,0 +1,245 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + launch_dir = os.path.join(bringup_dir, "launch") + + # Get the launch directory for multirobot_map_merge where we have a modified bringup launch file + map_merge_dir = get_package_share_directory("multirobot_map_merge") + launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation") + + # Create the launch configuration variables + slam = LaunchConfiguration("slam") + slam_toolbox = LaunchConfiguration("slam_toolbox") + slam_gmapping = LaunchConfiguration("slam_gmapping") + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + params_file = LaunchConfiguration("params_file") + autostart = LaunchConfiguration("autostart") + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration("rviz_config_file") + use_simulator = LaunchConfiguration("use_simulator") + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + headless = LaunchConfiguration("headless") + world = LaunchConfiguration("world") + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="False", description="Whether run a SLAM" + ) + declare_slam_toolbox_cmd = DeclareLaunchArgument( + "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox" + ) + declare_slam_gmapping_cmd = DeclareLaunchArgument( + "slam_gmapping", + default_value="False", + description="Whether run a SLAM gmapping", + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config_file", + default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + declare_use_simulator_cmd = DeclareLaunchArgument( + "use_simulator", + default_value="True", + description="Whether to start the simulator", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + declare_simulator_cmd = DeclareLaunchArgument( + "headless", default_value="False", description="Whether to execute gzclient)" + ) + + declare_world_cmd = DeclareLaunchArgument( + "world", + # TODO(orduno) Switch back once ROS argument passing has been fixed upstream + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 + # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), + # worlds/turtlebot3_worlds/waffle.model') + default_value=os.path.join(bringup_dir, "worlds", "waffle.model"), + description="Full path to world model file to load", + ) + + # Specify the actions + start_gazebo_server_cmd = ExecuteProcess( + condition=IfCondition(use_simulator), + cmd=[ + "gzserver", + "-s", + "libgazebo_ros_init.so", + "-s", + "libgazebo_ros_factory.so", + world, + ], + cwd=[launch_dir], + output="screen", + ) + + start_gazebo_client_cmd = ExecuteProcess( + condition=IfCondition(PythonExpression([use_simulator, " and not ", headless])), + cmd=["gzclient"], + cwd=[launch_dir], + output="screen", + ) + + urdf = os.path.join(bringup_dir, "urdf", "turtlebot3_waffle.urdf") + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + arguments=[urdf], + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "rviz_launch.py")), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": "", + "use_namespace": "False", + "rviz_config": rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir_map_merge, "bringup_launch.py") + ), + launch_arguments={ + "namespace": namespace, + "use_namespace": use_namespace, + "slam": slam, + "slam_toolbox": slam_toolbox, + "slam_gmapping": slam_gmapping, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "params_file": params_file, + "autostart": autostart, + }.items(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_slam_toolbox_cmd) + ld.add_action(declare_slam_gmapping_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any conditioned actions + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/m-explore-ros2/map_merge/launch/tb3_simulation/worlds/world_only.model b/m-explore-ros2/map_merge/launch/tb3_simulation/worlds/world_only.model new file mode 100644 index 0000000..a1a4d7c --- /dev/null +++ b/m-explore-ros2/map_merge/launch/tb3_simulation/worlds/world_only.model @@ -0,0 +1,56 @@ + + + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + 1 + + + + model://turtlebot3_house + + + + + diff --git a/m-explore-ros2/map_merge/package.xml b/m-explore-ros2/map_merge/package.xml new file mode 100644 index 0000000..a60e0a7 --- /dev/null +++ b/m-explore-ros2/map_merge/package.xml @@ -0,0 +1,31 @@ + + + + multirobot_map_merge + 1.0.0 + + Merging multiple maps without knowledge of initial + positions of robots ROS2 Port. + + Carlos Alvarez + Carlos Alvarez + BSD + + ament_cmake + + ament_lint_auto + ament_lint_common + ament_cmake + geometry_msgs + nav_msgs + map_msgs + tf2_geometry_msgs + tf2 + tf2_ros + + image_geometry + + + ament_cmake + + diff --git a/m-explore-ros2/map_merge/src/combine_grids/estimation_internal.h b/m-explore-ros2/map_merge/src/combine_grids/estimation_internal.h new file mode 100644 index 0000000..5bdb288 --- /dev/null +++ b/m-explore-ros2/map_merge/src/combine_grids/estimation_internal.h @@ -0,0 +1,152 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef ESTIMATION_INTERNAL_H_ +#define ESTIMATION_INTERNAL_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#ifdef HAVE_OPENCV_XFEATURES2D +#include +#endif + +namespace combine_grids +{ +namespace internal +{ +#if CV_VERSION_MAJOR >= 4 + +static inline cv::Ptr chooseFeatureFinder(FeatureType type) +{ + switch (type) { + case FeatureType::AKAZE: + return cv::AKAZE::create(); + case FeatureType::ORB: + return cv::ORB::create(); + case FeatureType::SURF: +#ifdef HAVE_OPENCV_XFEATURES2D + return cv::xfeatures2d::SURF::create(); +#else + return cv::AKAZE::create(); +#endif + } + + assert(false); + return {}; +} + +#else // (CV_VERSION_MAJOR < 4) + +static inline cv::Ptr +chooseFeatureFinder(FeatureType type) +{ + switch (type) { + case FeatureType::AKAZE: + return cv::makePtr(); + case FeatureType::ORB: + return cv::makePtr(); + case FeatureType::SURF: + return cv::makePtr(); + } + + assert(false); + return {}; +} + +#endif // CV_VERSION_MAJOR >= 4 + +static inline void writeDebugMatchingInfo( + const std::vector& images, + const std::vector& image_features, + const std::vector& pairwise_matches) +{ + for (auto& match_info : pairwise_matches) { + if (match_info.H.empty() || + match_info.src_img_idx >= match_info.dst_img_idx) { + continue; + } + std::cout << match_info.src_img_idx << " " << match_info.dst_img_idx + << std::endl + << "features: " + << image_features[size_t(match_info.src_img_idx)].keypoints.size() + << " " + << image_features[size_t(match_info.dst_img_idx)].keypoints.size() + << std::endl + << "matches: " << match_info.matches.size() << std::endl + << "inliers: " << match_info.num_inliers << std::endl + << "inliers/matches ratio: " + << match_info.num_inliers / double(match_info.matches.size()) + << std::endl + << "confidence: " << match_info.confidence << std::endl + << match_info.H << std::endl; + cv::Mat img; + // draw all matches + cv::drawMatches(images[size_t(match_info.src_img_idx)], + image_features[size_t(match_info.src_img_idx)].keypoints, + images[size_t(match_info.dst_img_idx)], + image_features[size_t(match_info.dst_img_idx)].keypoints, + match_info.matches, img); + cv::imwrite(std::to_string(match_info.src_img_idx) + "_" + + std::to_string(match_info.dst_img_idx) + "_matches.png", + img); + // draw inliers only + cv::drawMatches( + images[size_t(match_info.src_img_idx)], + image_features[size_t(match_info.src_img_idx)].keypoints, + images[size_t(match_info.dst_img_idx)], + image_features[size_t(match_info.dst_img_idx)].keypoints, + match_info.matches, img, cv::Scalar::all(-1), cv::Scalar::all(-1), + *reinterpret_cast*>(&match_info.inliers_mask)); + cv::imwrite(std::to_string(match_info.src_img_idx) + "_" + + std::to_string(match_info.dst_img_idx) + + "_matches_inliers.png", + img); + } +} + +} // namespace internal +} // namespace combine_grids + +#endif // ESTIMATION_INTERNAL_H_ diff --git a/m-explore-ros2/map_merge/src/combine_grids/grid_compositor.cpp b/m-explore-ros2/map_merge/src/combine_grids/grid_compositor.cpp new file mode 100644 index 0000000..768b5ad --- /dev/null +++ b/m-explore-ros2/map_merge/src/combine_grids/grid_compositor.cpp @@ -0,0 +1,88 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include + +#include + +#include + + +namespace combine_grids +{ +namespace internal +{ +nav_msgs::msg::OccupancyGrid::SharedPtr GridCompositor::compose( + const std::vector& grids, const std::vector& rois) +{ + rcpputils::require_true(grids.size() == rois.size()); + + nav_msgs::msg::OccupancyGrid::SharedPtr result_grid(new nav_msgs::msg::OccupancyGrid()); + + std::vector corners; + corners.reserve(grids.size()); + std::vector sizes; + sizes.reserve(grids.size()); + for (auto& roi : rois) { + corners.push_back(roi.tl()); + sizes.push_back(roi.size()); + } + cv::Rect dst_roi = cv::detail::resultRoi(corners, sizes); + + result_grid->info.width = static_cast(dst_roi.width); + result_grid->info.height = static_cast(dst_roi.height); + result_grid->data.resize(static_cast(dst_roi.area()), -1); + // create view for opencv pointing to newly allocated grid + cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data()); + + for (size_t i = 0; i < grids.size(); ++i) { + // we need to compensate global offset + cv::Rect roi = cv::Rect(corners[i] - dst_roi.tl(), sizes[i]); + cv::Mat result_roi(result, roi); + // reinterpret warped matrix as signed + // we will not change this matrix, but opencv does not support const matrices + cv::Mat warped_signed (grids[i].size(), CV_8S, const_cast(grids[i].ptr())); + // compose img into result matrix + cv::max(result_roi, warped_signed, result_roi); + } + + return result_grid; +} + +} // namespace internal + +} // namespace combine_grids diff --git a/m-explore-ros2/map_merge/src/combine_grids/grid_warper.cpp b/m-explore-ros2/map_merge/src/combine_grids/grid_warper.cpp new file mode 100644 index 0000000..dc13d31 --- /dev/null +++ b/m-explore-ros2/map_merge/src/combine_grids/grid_warper.cpp @@ -0,0 +1,90 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include + +#include + +#include + +namespace combine_grids +{ +namespace internal +{ +cv::Rect GridWarper::warp(const cv::Mat& grid, const cv::Mat& transform, + cv::Mat& warped_grid) +{ + // for validating function inputs. Throws an std::invalid_argument exception if the condition fails. + rcpputils::require_true(transform.type() == CV_64F); + cv::Mat H; + invertAffineTransform(transform.rowRange(0, 2), H); + cv::Rect roi = warpRoi(grid, H); + // shift top left corner for warp affine (otherwise the image is cropped) + H.at(0, 2) -= roi.tl().x; + H.at(1, 2) -= roi.tl().y; + warpAffine(grid, warped_grid, H, roi.size(), cv::INTER_NEAREST, + cv::BORDER_CONSTANT, + cv::Scalar::all(255) /* this is -1 for signed char */); + + // For verifying results. Throws a rcpputils::AssertionException if the condition fails. + // This function becomes a no-op in release builds. + rcpputils::assert_true(roi.size() == warped_grid.size()); + + return roi; +} + +cv::Rect GridWarper::warpRoi(const cv::Mat& grid, const cv::Mat& transform) +{ + cv::Ptr warper = + cv::makePtr(); + cv::Mat H; + transform.convertTo(H, CV_32F); + + // separate rotation and translation for plane warper + // 3D translation + cv::Mat T = cv::Mat::zeros(3, 1, CV_32F); + H.colRange(2, 3).rowRange(0, 2).copyTo(T.rowRange(0, 2)); + // 3D rotation + cv::Mat R = cv::Mat::eye(3, 3, CV_32F); + H.colRange(0, 2).copyTo(R.rowRange(0, 2).colRange(0, 2)); + + return warper->warpRoi(grid.size(), cv::Mat::eye(3, 3, CV_32F), R, T); +} + +} // namespace internal + +} // namespace combine_grids diff --git a/m-explore-ros2/map_merge/src/combine_grids/merging_pipeline.cpp b/m-explore-ros2/map_merge/src/combine_grids/merging_pipeline.cpp new file mode 100644 index 0000000..bfbcef0 --- /dev/null +++ b/m-explore-ros2/map_merge/src/combine_grids/merging_pipeline.cpp @@ -0,0 +1,279 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include + +#include "estimation_internal.h" + +namespace combine_grids +{ +bool MergingPipeline::estimateTransforms(FeatureType feature_type, + double confidence) +{ + std::vector image_features; + std::vector pairwise_matches; + std::vector transforms; + std::vector good_indices; + // TODO investigate value translation effect on features + auto finder = internal::chooseFeatureFinder(feature_type); + cv::Ptr matcher = + cv::makePtr(); + cv::Ptr estimator = + cv::makePtr(); + cv::Ptr adjuster = + cv::makePtr(); + + if (images_.empty()) { + return true; + } + + /* find features in images */ + static rclcpp::Logger logger = rclcpp::get_logger("estimateTransforms"); + RCLCPP_DEBUG(logger, "computing features"); + image_features.reserve(images_.size()); + for (const cv::Mat& image : images_) { + image_features.emplace_back(); + if (!image.empty()) { +#if CV_VERSION_MAJOR >= 4 + cv::detail::computeImageFeatures(finder, image, image_features.back()); +#else + (*finder)(image, image_features.back()); +#endif + } + } + finder = {}; + + /* find corespondent features */ + RCLCPP_DEBUG(logger, "pairwise matching features"); + (*matcher)(image_features, pairwise_matches); + matcher = {}; + +#ifndef NDEBUG + internal::writeDebugMatchingInfo(images_, image_features, pairwise_matches); +#endif + + /* use only matches that has enough confidence. leave out matches that are not + * connected (small components) */ + good_indices = cv::detail::leaveBiggestComponent( + image_features, pairwise_matches, static_cast(confidence)); + + // no match found. try set first non-empty grid as reference frame. we try to + // avoid setting empty grid as reference frame, in case some maps never + // arrive. If all is empty just set null transforms. + if (good_indices.size() == 1) { + transforms_.clear(); + transforms_.resize(images_.size()); + + // Making some tests to see if it is better to just return false if no match is found + // and not clear the last good transforms found + // if (images_.size() != transforms_.size()) { + // transforms_.clear(); + // transforms_.resize(images_.size()); + // } + // return false; + + for (size_t i = 0; i < images_.size(); ++i) { + if (!images_[i].empty()) { + // set identity + transforms_[i] = cv::Mat::eye(3, 3, CV_64F); + break; + } + } + // RCLCPP_INFO(logger, "No match found between maps, setting first non-empty grid as reference frame"); + return true; + } + + // // Experimental: should we keep only the best confidence match overall? + // bool max_confidence_achieved_surpassed = false; + // for (auto &match_info : pairwise_matches) { + // RCLCPP_INFO(logger, "match info: %f", match_info.confidence); + // if (match_info.confidence > max_confidence_achieved_){ + // max_confidence_achieved_surpassed = true; + // max_confidence_achieved_ = match_info.confidence; + // } + // } + // if (!max_confidence_achieved_surpassed) { + // RCLCPP_INFO(logger, "Max confidence achieved not surpassed, not using matching"); + // return false; + // } + // else + // RCLCPP_INFO(logger, "Max confidence achieved surpassed, optimizing"); + + + /* estimate transform */ + RCLCPP_DEBUG(logger, "calculating transforms in global reference frame"); + // note: currently used estimator never fails + if (!(*estimator)(image_features, pairwise_matches, transforms)) { + return false; + } + + /* levmarq optimization */ + // openCV just accepts float transforms + for (auto& transform : transforms) { + transform.R.convertTo(transform.R, CV_32F); + } + RCLCPP_DEBUG(logger, "optimizing global transforms"); + adjuster->setConfThresh(confidence); + if (!(*adjuster)(image_features, pairwise_matches, transforms)) { + RCLCPP_WARN(logger, "Bundle adjusting failed. Could not estimate transforms."); + return false; + } + + transforms_.clear(); + transforms_.resize(images_.size()); + size_t i = 0; + for (auto& j : good_indices) { + // we want to work with transforms as doubles + transforms[i].R.convertTo(transforms_[static_cast(j)], CV_64F); + ++i; + } + + return true; +} + +// checks whether given matrix is an identity, i.e. exactly appropriate Mat::eye +static inline bool isIdentity(const cv::Mat& matrix) +{ + if (matrix.empty()) { + return false; + } + cv::MatExpr diff = matrix != cv::Mat::eye(matrix.size(), matrix.type()); + return cv::countNonZero(diff) == 0; +} + +nav_msgs::msg::OccupancyGrid::SharedPtr MergingPipeline::composeGrids() +{ + // for checking states. Throws a rcpputils::IllegalStateException if the condition fails. + rcpputils::check_true(images_.size() == transforms_.size()); + rcpputils::check_true(images_.size() == grids_.size()); + static rclcpp::Logger logger = rclcpp::get_logger("composeGrids"); + + if (images_.empty()) { + return nullptr; + } + + RCLCPP_DEBUG(logger, "warping grids"); + internal::GridWarper warper; + std::vector imgs_warped; + imgs_warped.reserve(images_.size()); + std::vector rois; + rois.reserve(images_.size()); + + for (size_t i = 0; i < images_.size(); ++i) { + if (!transforms_[i].empty() && !images_[i].empty()) { + imgs_warped.emplace_back(); + rois.emplace_back( + warper.warp(images_[i], transforms_[i], imgs_warped.back())); + } + } + + if (imgs_warped.empty()) { + return nullptr; + } + + RCLCPP_DEBUG(logger, "compositing result grid"); + nav_msgs::msg::OccupancyGrid::SharedPtr result; + internal::GridCompositor compositor; + result = compositor.compose(imgs_warped, rois); + + // set correct resolution to output grid. use resolution of identity (works + // for estimated transforms), or any resolution (works for know_init_positions) + // - in that case all resolutions should be the same. + float any_resolution = 0.0; + for (size_t i = 0; i < transforms_.size(); ++i) { + // check if this transform is the reference frame + if (isIdentity(transforms_[i])) { + result->info.resolution = grids_[i]->info.resolution; + break; + } + if (grids_[i]) { + any_resolution = grids_[i]->info.resolution; + } + } + if (result->info.resolution <= 0.f) { + result->info.resolution = any_resolution; + } + + // set grid origin to its centre + result->info.origin.position.x = + -(result->info.width / 2.0) * double(result->info.resolution); + result->info.origin.position.y = + -(result->info.height / 2.0) * double(result->info.resolution); + result->info.origin.orientation.w = 1.0; + + return result; +} + +std::vector MergingPipeline::getTransforms() const +{ + std::vector result; + result.reserve(transforms_.size()); + + for (auto& transform : transforms_) { + if (transform.empty()) { + result.emplace_back(); + continue; + } + + rcpputils::require_true(transform.type() == CV_64F); + geometry_msgs::msg::Transform ros_transform; + ros_transform.translation.x = transform.at(0, 2); + ros_transform.translation.y = transform.at(1, 2); + ros_transform.translation.z = 0.; + + // our rotation is in fact only 2D, thus quaternion can be simplified + double a = transform.at(0, 0); + double b = transform.at(1, 0); + ros_transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5; + ros_transform.rotation.x = 0.; + ros_transform.rotation.y = 0.; + ros_transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b); + + result.push_back(ros_transform); + } + + return result; +} + +} // namespace combine_grids diff --git a/m-explore-ros2/map_merge/src/map_merge.cpp b/m-explore-ros2/map_merge/src/map_merge.cpp new file mode 100644 index 0000000..71cd88f --- /dev/null +++ b/m-explore-ros2/map_merge/src/map_merge.cpp @@ -0,0 +1,454 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, Zhi Yan. + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include + +#include +#include +#include +#include + + +namespace map_merge +{ +MapMerge::MapMerge() : Node("map_merge", rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)), +subscriptions_size_(0) +{ + std::string frame_id; + std::string merged_map_topic; + + if (!this->has_parameter("merging_rate")) this->declare_parameter("merging_rate", 4.0); + if (!this->has_parameter("discovery_rate")) this->declare_parameter("discovery_rate", 0.05); + if (!this->has_parameter("estimation_rate")) this->declare_parameter("estimation_rate", 0.5); + if (!this->has_parameter("known_init_poses")) this->declare_parameter("known_init_poses", true); + if (!this->has_parameter("estimation_confidence")) this->declare_parameter("estimation_confidence", 1.0); + if (!this->has_parameter("robot_map_topic")) this->declare_parameter("robot_map_topic", "map"); + if (!this->has_parameter("robot_map_updates_topic")) this->declare_parameter("robot_map_updates_topic", "map_updates"); + if (!this->has_parameter("robot_namespace")) this->declare_parameter("robot_namespace", ""); + if (!this->has_parameter("merged_map_topic")) this->declare_parameter("merged_map_topic", "map"); + if (!this->has_parameter("world_frame")) this->declare_parameter("world_frame", "world"); + + this->get_parameter("merging_rate", merging_rate_); + this->get_parameter("discovery_rate", discovery_rate_); + this->get_parameter("estimation_rate", estimation_rate_); + this->get_parameter("known_init_poses", have_initial_poses_); + this->get_parameter("estimation_confidence", confidence_threshold_); + this->get_parameter("robot_map_topic", robot_map_topic_); + this->get_parameter("robot_map_updates_topic", robot_map_updates_topic_); + this->get_parameter("robot_namespace", robot_namespace_); + this->get_parameter("merged_map_topic", merged_map_topic); + this->get_parameter("world_frame", world_frame_); + + + /* publishing */ + // Create a publisher using the QoS settings to emulate a ROS1 latched topic + merged_map_publisher_ = + this->create_publisher(merged_map_topic, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + // Timers + map_merging_timer_ = this->create_wall_timer( + std::chrono::milliseconds((uint16_t)(1000.0 / merging_rate_)), + [this]() { mapMerging(); }); + // execute right away to simulate the ros1 first while loop on a thread + map_merging_timer_->execute_callback(); + + topic_subscribing_timer_ = this->create_wall_timer( + std::chrono::milliseconds((uint16_t)(1000.0 / discovery_rate_)), + [this]() { topicSubscribing(); }); + + // For topicSubscribing() we need to spin briefly for the discovery to happen + rclcpp::Rate r(100); + int i = 0; + while (rclcpp::ok() && i < 100) { + rclcpp::spin_some(this->get_node_base_interface()); + r.sleep(); + i++; + } + topic_subscribing_timer_->execute_callback(); + + if (!have_initial_poses_){ + pose_estimation_timer_ = this->create_wall_timer( + std::chrono::milliseconds((uint16_t)(1000.0 / estimation_rate_)), + [this]() { poseEstimation(); }); + // execute right away to simulate the ros1 first while loop on a thread + pose_estimation_timer_->execute_callback(); + } +} + +/* + * Subcribe to pose and map topics + */ +void MapMerge::topicSubscribing() +{ + RCLCPP_DEBUG(logger_, "Robot discovery started."); + RCLCPP_INFO_ONCE(logger_, "Robot discovery started."); + + // ros::master::V_TopicInfo topic_infos; + geometry_msgs::msg::Transform init_pose; + std::string robot_name; + std::string map_topic; + std::string map_updates_topic; + + // ros::master::getTopics(topic_infos); + std::map> topic_infos = this->get_topic_names_and_types(); + + for (const auto& topic_it : topic_infos) { + std::string topic_name = topic_it.first; + std::vector topic_types = topic_it.second; + // iterate over all topic types + for (const auto& topic_type : topic_types) { + // RCLCPP_INFO(logger_, "Topic: %s, type: %s", topic_name.c_str(), topic_type.c_str()); + + // we check only map topic + if (!isRobotMapTopic(topic_name, topic_type)) { + continue; + } + + robot_name = robotNameFromTopic(topic_name); + if (robots_.count(robot_name)) { + // we already know this robot + continue; + } + + if (have_initial_poses_ && !getInitPose(robot_name, init_pose)) { + RCLCPP_WARN(logger_, "Couldn't get initial position for robot [%s]\n" + "did you defined parameters map_merge/init_pose_[xyz]? in robot " + "namespace? If you want to run merging without known initial " + "positions of robots please set `known_init_poses` parameter " + "to false. See relevant documentation for details.", + robot_name.c_str()); + continue; + } + + RCLCPP_INFO(logger_, "adding robot [%s] to system", robot_name.c_str()); + { + // We don't lock since because of ROS2 default executor only a callback can run at a time + // std::lock_guard lock(subscriptions_mutex_); + subscriptions_.emplace_front(); + ++subscriptions_size_; + } + + // no locking here. robots_ are used only in this procedure + MapSubscription& subscription = subscriptions_.front(); + robots_.insert({robot_name, &subscription}); + subscription.initial_pose = init_pose; + + /* subscribe callbacks */ + map_topic = ros1_names::append(robot_name, robot_map_topic_); + map_updates_topic = + ros1_names::append(robot_name, robot_map_updates_topic_); + RCLCPP_INFO(logger_, "Subscribing to MAP topic: %s.", map_topic.c_str()); + auto map_qos = rclcpp::QoS(rclcpp::KeepLast(50)).transient_local().reliable(); + subscription.map_sub = this->create_subscription( + map_topic, map_qos, + [this, &subscription](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + fullMapUpdate(msg, subscription); + }); + RCLCPP_INFO(logger_, "Subscribing to MAP updates topic: %s.", + map_updates_topic.c_str()); + subscription.map_updates_sub = + this->create_subscription( + map_updates_topic, map_qos, + [this, &subscription]( + const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) { + partialMapUpdate(msg, subscription); + }); + } + } +} + +/* + * mapMerging() + */ +void MapMerge::mapMerging() +{ + RCLCPP_DEBUG(logger_, "Map merging started."); + RCLCPP_INFO_ONCE(logger_, "Map merging started."); + + if (have_initial_poses_) { + // TODO: attempt fix for SLAM toolbox: add method for padding grids to same size + + std::vector grids; + std::vector transforms; + grids.reserve(subscriptions_size_); + { + // We don't lock since because of ROS2 default executor only a callback can run + // boost::shared_lock lock(subscriptions_mutex_); + for (auto& subscription : subscriptions_) { + // std::lock_guard s_lock(subscription.mutex); + + grids.push_back(subscription.readonly_map); + transforms.push_back(subscription.initial_pose); + } + } + + + // we don't need to lock here, because when have_initial_poses_ is true we + // will not run concurrently on the pipeline + pipeline_.feed(grids.begin(), grids.end()); + pipeline_.setTransforms(transforms.begin(), transforms.end()); + } + + // nav_msgs::OccupancyGridPtr merged_map; + nav_msgs::msg::OccupancyGrid::SharedPtr merged_map; + { + std::lock_guard lock(pipeline_mutex_); + merged_map = pipeline_.composeGrids(); + } + if (!merged_map) { + // RCLCPP_INFO(logger_, "No map merged"); + return; + } + + RCLCPP_DEBUG(logger_, "all maps merged, publishing"); + // RCLCPP_INFO(logger_, "all maps merged, publishing"); + auto now = this->now(); + merged_map->info.map_load_time = now; + merged_map->header.stamp = now; + merged_map->header.frame_id = world_frame_; + + rcpputils::assert_true(merged_map->info.resolution > 0.f); + merged_map_publisher_->publish(*merged_map); +} + +void MapMerge::poseEstimation() +{ + RCLCPP_DEBUG(logger_, "Grid pose estimation started."); + RCLCPP_INFO_ONCE(logger_, "Grid pose estimation started."); + std::vector grids; + grids.reserve(subscriptions_size_); + + { + // We don't lock since because of ROS2 default executor only a callback can run + // boost::shared_lock lock(subscriptions_mutex_); + for (auto& subscription : subscriptions_) { + // std::lock_guard s_lock(subscription.mutex); + grids.push_back(subscription.readonly_map); + } + } + + // Print grids size + // RCLCPP_INFO(logger_, "Grids size: %d", grids.size()); + + std::lock_guard lock(pipeline_mutex_); + pipeline_.feed(grids.begin(), grids.end()); + // TODO allow user to change feature type + bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::AKAZE, + confidence_threshold_); + // bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::SURF, + // confidence_threshold_); + // bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::ORB, + // confidence_threshold_); + if (!success) { + RCLCPP_INFO(logger_, "No grid poses estimated"); + } +} + +// void MapMerge::fullMapUpdate(const nav_msgs::OccupancyGrid::ConstPtr& msg, +// MapSubscription& subscription) +void MapMerge::fullMapUpdate(const nav_msgs::msg::OccupancyGrid::SharedPtr msg, + MapSubscription& subscription) +{ + RCLCPP_DEBUG(logger_, "received full map update"); + // RCLCPP_INFO(logger_, "received full map update"); + std::lock_guard lock(subscription.mutex); + if (subscription.readonly_map){ + // ros2 header .stamp don't support > operator, we need to create them explicitly + auto t1 = rclcpp::Time(subscription.readonly_map->header.stamp); + auto t2 = rclcpp::Time(msg->header.stamp); + if (t1 > t2) { + // we have been overrunned by faster update. our work was useless. + return; + } + } + + subscription.readonly_map = msg; + subscription.writable_map = nullptr; +} + +// void MapMerge::partialMapUpdate( +// const map_msgs::OccupancyGridUpdate::ConstPtr& msg, +// MapSubscription& subscription) +void MapMerge::partialMapUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg, + MapSubscription& subscription) +{ + RCLCPP_DEBUG(logger_, "received partial map update"); + + if (msg->x < 0 || msg->y < 0) { + RCLCPP_ERROR(logger_, "negative coordinates, invalid update. x: %d, y: %d", msg->x, + msg->y); + return; + } + + size_t x0 = static_cast(msg->x); + size_t y0 = static_cast(msg->y); + size_t xn = msg->width + x0; + size_t yn = msg->height + y0; + + nav_msgs::msg::OccupancyGrid::SharedPtr map; + nav_msgs::msg::OccupancyGrid::ConstSharedPtr readonly_map; // local copy + { + // load maps + std::lock_guard lock(subscription.mutex); + map = subscription.writable_map; + readonly_map = subscription.readonly_map; + } + + if (!readonly_map) { + RCLCPP_WARN(logger_, "received partial map update, but don't have any full map to " + "update. skipping."); + return; + } + + // // we don't have partial map to take update, we must copy readonly map and + // // update new writable map + if (!map) { + map.reset(new nav_msgs::msg::OccupancyGrid(*readonly_map)); + } + + size_t grid_xn = map->info.width; + size_t grid_yn = map->info.height; + + if (xn > grid_xn || x0 > grid_xn || yn > grid_yn || y0 > grid_yn) { + RCLCPP_WARN(logger_, "received update doesn't fully fit into existing map, " + "only part will be copied. received: [%lu, %lu], [%lu, %lu] " + "map is: [0, %lu], [0, %lu]", + x0, xn, y0, yn, grid_xn, grid_yn); + } + + // update map with data + size_t i = 0; + for (size_t y = y0; y < yn && y < grid_yn; ++y) { + for (size_t x = x0; x < xn && x < grid_xn; ++x) { + size_t idx = y * grid_xn + x; // index to grid for this specified cell + map->data[idx] = msg->data[i]; + ++i; + } + } + // update time stamp + map->header.stamp = msg->header.stamp; + + { + // store back updated map + std::lock_guard lock(subscription.mutex); + if (subscription.readonly_map){ + // ros2 header .stamp don't support > operator, we need to create them explicitly + auto t1 = rclcpp::Time(subscription.readonly_map->header.stamp); + auto t2 = rclcpp::Time(map->header.stamp); + if (t1 > t2) { + // we have been overrunned by faster update. our work was useless. + return; + } + } + subscription.writable_map = map; + subscription.readonly_map = map; + } +} + +std::string MapMerge::robotNameFromTopic(const std::string& topic) +{ + return ros1_names::parentNamespace(topic); +} + +/* identifies topic via suffix */ +bool MapMerge::isRobotMapTopic(const std::string topic, std::string type) +{ + /* test whether topic is robot_map_topic_ */ + std::string topic_namespace = ros1_names::parentNamespace(topic); + bool is_map_topic = ros1_names::append(topic_namespace, robot_map_topic_) == topic; + + // /* test whether topic contains *anywhere* robot namespace */ + auto pos = topic.find(robot_namespace_); + bool contains_robot_namespace = pos != std::string::npos; + + // /* we support only occupancy grids as maps */ + bool is_occupancy_grid = type == "nav_msgs/msg/OccupancyGrid"; + + // /* we don't want to subcribe on published merged map */ + bool is_our_topic = merged_map_publisher_->get_topic_name() == topic; + + return is_occupancy_grid && !is_our_topic && contains_robot_namespace && + is_map_topic; +} + +/* + * Get robot's initial position + */ +bool MapMerge::getInitPose(const std::string& name, + geometry_msgs::msg::Transform& pose) +{ + std::string merging_namespace = ros1_names::append(name, "map_merge"); + double yaw = 0.0; + + bool success = + this->get_parameter(ros1_names::append(merging_namespace, "init_pose_x"), + pose.translation.x) && + this->get_parameter(ros1_names::append(merging_namespace, "init_pose_y"), + pose.translation.y) && + this->get_parameter(ros1_names::append(merging_namespace, "init_pose_z"), + pose.translation.z) && + this->get_parameter(ros1_names::append(merging_namespace, "init_pose_yaw"), + yaw); + + tf2::Quaternion q; + q.setEuler(0., 0., yaw); + pose.rotation = toMsg(q); + + return success; +} +} // namespace map_merge + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + // ROS1 code + // // this package is still in development -- start wil debugging enabled + // if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + // ros::console::levels::Debug)) { + // ros::console::notifyLoggerLevelsChanged(); + // } + + // ROS2 code + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/m-explore-ros2/map_merge/test/download.sh b/m-explore-ros2/map_merge/test/download.sh new file mode 100644 index 0000000..07496d5 --- /dev/null +++ b/m-explore-ros2/map_merge/test/download.sh @@ -0,0 +1,21 @@ +file_name=$1 +url=$2 +md5=$3 + +# Download the file if it doesn't exist +if [ ! -f $file_name ]; then + wget $url -O $file_name +fi + +# Check the MD5 sum of the file +echo "Checking MD5 sum of $file_name" +md5sum -c <<<"$md5 $file_name" +if [ $? -ne 0 ]; then + echo "MD5 sum of $file_name does not match. Downloading it again" + wget $url -O $file_name + md5sum -c <<<"$md5 $file_name" + if [ $? -ne 0 ]; then + echo "MD5 sum of $file_name still does not match. Aborting." + exit 1 + fi +fi \ No newline at end of file diff --git a/m-explore-ros2/map_merge/test/download_data.sh b/m-explore-ros2/map_merge/test/download_data.sh new file mode 100644 index 0000000..574f53a --- /dev/null +++ b/m-explore-ros2/map_merge/test/download_data.sh @@ -0,0 +1,5 @@ +base_url=https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge +wget ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm -P build/multirobot_map_merge +wget ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm -P build/multirobot_map_merge +wget ${base_url}/hector_maps/map05.pgm -P build/multirobot_map_merge +wget ${base_url}/hector_maps/map00.pgm -P build/multirobot_map_merge \ No newline at end of file diff --git a/m-explore-ros2/map_merge/test/test_merging_pipeline.cpp b/m-explore-ros2/map_merge/test/test_merging_pipeline.cpp new file mode 100644 index 0000000..d3066e5 --- /dev/null +++ b/m-explore-ros2/map_merge/test/test_merging_pipeline.cpp @@ -0,0 +1,323 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2022, Carlos Alvarez. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Jiri Horner nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include +#include +// #include +#include +#include "testing_helpers.h" + +#define private public +#include + +const std::array hector_maps = { + "map00.pgm", + "map05.pgm", +}; + +const std::array gmapping_maps = { + "2011-08-09-12-22-52.pgm", + "2012-01-28-11-12-01.pgm", +}; + +constexpr bool verbose_tests = false; + +#define EXPECT_VALID_GRID(grid) \ + ASSERT_TRUE(static_cast(grid)); \ + EXPECT_TRUE(consistentData(*grid)); \ + EXPECT_GT(grid->info.resolution, 0); \ + EXPECT_TRUE(isIdentity(grid->info.origin.orientation)) + +TEST(MergingPipeline, canStich0Grid) +{ + std::vector maps; + combine_grids::MergingPipeline merger; + merger.feed(maps.begin(), maps.end()); + EXPECT_TRUE(merger.estimateTransforms()); + EXPECT_EQ(merger.composeGrids(), nullptr); + EXPECT_EQ(merger.getTransforms().size(), (long unsigned int) 0); +} + +TEST(MergingPipeline, canStich1Grid) +{ + auto map = loadMap(hector_maps[1]); + combine_grids::MergingPipeline merger; + merger.feed(&map, &map + 1); + merger.estimateTransforms(); + auto merged_grid = merger.composeGrids(); + + EXPECT_VALID_GRID(merged_grid); + // don't use EXPECT_EQ, since it prints too much info + EXPECT_TRUE(maps_equal(*merged_grid, *map)); + + // check estimated transforms + auto transforms = merger.getTransforms(); + EXPECT_EQ(transforms.size(), (long unsigned int) 1); + EXPECT_TRUE(isIdentity(transforms[0])); +} + +TEST(MergingPipeline, canStich2Grids) +{ + auto maps = loadMaps(hector_maps.begin(), hector_maps.end()); + combine_grids::MergingPipeline merger; + merger.feed(maps.begin(), maps.end()); + merger.estimateTransforms(); + auto merged_grid = merger.composeGrids(); + + EXPECT_VALID_GRID(merged_grid); + // grid size should indicate sucessful merge + EXPECT_NEAR(2091, merged_grid->info.width, 30); + EXPECT_NEAR(2091, merged_grid->info.height, 30); + + if (verbose_tests) { + saveMap("test_canStich2Grids.pgm", merged_grid); + } +} + +TEST(MergingPipeline, canStichGridsGmapping) +{ + auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end()); + combine_grids::MergingPipeline merger; + merger.feed(maps.begin(), maps.end()); + merger.estimateTransforms(); + auto merged_grid = merger.composeGrids(); + + EXPECT_VALID_GRID(merged_grid); + // grid size should indicate sucessful merge + EXPECT_NEAR(5427, merged_grid->info.width, 30); + EXPECT_NEAR(5427, merged_grid->info.height, 30); + + if (verbose_tests) { + saveMap("canStichGridsGmapping.pgm", merged_grid); + } +} + +TEST(MergingPipeline, estimationAccuracy) +{ + // for this test we measure estimation on the same grid artificially + // transformed + double angle = 0.523599 /* 30 deg in rads*/; + // TODO test also translations + double tx = 0; + double ty = 0; + cv::Matx23d transform{std::cos(angle), -std::sin(angle), tx, + std::sin(angle), std::cos(angle), ty}; + + auto map = loadMap(hector_maps[1]); + combine_grids::MergingPipeline merger; + merger.feed(&map, &map + 1); + + // warp the map with Affine Transform + combine_grids::internal::GridWarper warper; + cv::Mat warped; + auto roi = warper.warp(merger.images_[0], cv::Mat(transform), warped); + + // add warped map + // this relies on internal implementation of merging pipeline + merger.grids_.emplace_back(); + merger.images_.push_back(warped); + + merger.estimateTransforms(); + auto merged_grid = merger.composeGrids(); + + EXPECT_VALID_GRID(merged_grid); + // transforms + auto transforms = merger.getTransforms(); + EXPECT_EQ(transforms.size(), (long unsigned int) 2); + EXPECT_TRUE(isIdentity(transforms[0])); + tf2::Transform t; + tf2::fromMsg(transforms[1], t); + + EXPECT_NEAR(angle, t.getRotation().getAngle(), 1e-2); + EXPECT_NEAR(tx - roi.tl().x, t.getOrigin().x(), 2); + EXPECT_NEAR(ty - roi.tl().y, t.getOrigin().y(), 2); +} + +TEST(MergingPipeline, transformsRoundTrip) +{ + auto map = loadMap(hector_maps[0]); + combine_grids::MergingPipeline merger; + merger.feed(&map, &map + 1); + for (size_t i = 0; i < 1000; ++i) { + auto in_transform = randomTransform(); + merger.setTransforms(&in_transform, &in_transform + 1); + + auto out_transforms = merger.getTransforms(); + ASSERT_EQ(out_transforms.size(), (long unsigned int) 1); + auto out_transform = out_transforms[0]; + EXPECT_FLOAT_EQ(in_transform.translation.x, out_transform.translation.x); + EXPECT_FLOAT_EQ(in_transform.translation.y, out_transform.translation.y); + EXPECT_FLOAT_EQ(in_transform.translation.z, out_transform.translation.z); + EXPECT_FLOAT_EQ(in_transform.rotation.x, out_transform.rotation.x); + EXPECT_FLOAT_EQ(in_transform.rotation.y, out_transform.rotation.y); + EXPECT_FLOAT_EQ(in_transform.rotation.z, out_transform.rotation.z); + EXPECT_FLOAT_EQ(in_transform.rotation.w, out_transform.rotation.w); + } +} + +TEST(MergingPipeline, setTransformsInternal) +{ + auto map = loadMap(hector_maps[0]); + combine_grids::MergingPipeline merger; + merger.feed(&map, &map + 1); + + for (size_t i = 0; i < 1000; ++i) { + auto transform = randomTransform(); + merger.setTransforms(&transform, &transform + 1); + + ASSERT_EQ(merger.transforms_.size(), (long unsigned int) 1); + auto& transform_internal = merger.transforms_[0]; + // verify that transforms are the same in 2D + tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}}; + cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}}; + for (auto j : {0, 1}) { + tf2::Transform t; + fromMsg(transform, t); + auto p1 = t * a[j]; + cv::Mat p2 = transform_internal * cv::Mat(b[j]); + // some precision is naturally lost during conversion, float precision is + // still good for us + EXPECT_FLOAT_EQ(p1.x(), p2.at(0)); + EXPECT_FLOAT_EQ(p1.y(), p2.at(1)); + } + } +} + +TEST(MergingPipeline, getTransformsInternal) +{ + auto map = loadMap(hector_maps[0]); + combine_grids::MergingPipeline merger; + merger.feed(&map, &map + 1); + + // set internal transform + merger.transforms_.resize(1); + for (size_t i = 0; i < 1000; ++i) { + cv::Mat transform_internal = randomTransformMatrix(); + merger.transforms_[0] = transform_internal; + auto transforms = merger.getTransforms(); + ASSERT_EQ(transforms.size(), (long unsigned int) 1); + // output quaternion should be normalized + auto& q = transforms[0].rotation; + EXPECT_DOUBLE_EQ(1., q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + + // verify that transforms are the same in 2D + tf2::Transform transform; + fromMsg(transforms[0], transform); + tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}}; + cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}}; + for (auto j : {0, 1}) { + auto p1 = transform * a[j]; + cv::Mat p2 = transform_internal * cv::Mat(b[j]); + EXPECT_FLOAT_EQ(p1.x(), p2.at(0)); + EXPECT_FLOAT_EQ(p1.y(), p2.at(1)); + } + } +} + +TEST(MergingPipeline, setEmptyTransforms) +{ + constexpr size_t size = 2; + std::vector maps(size); + std::vector transforms(size); + combine_grids::MergingPipeline merger; + merger.feed(maps.begin(), maps.end()); + merger.setTransforms(transforms.begin(), transforms.end()); + EXPECT_EQ(merger.composeGrids(), nullptr); + EXPECT_EQ(merger.getTransforms().size(), size); +} + +/* empty image may end with identity transform. */ +TEST(MergingPipeline, emptyImageWithTransform) +{ + constexpr size_t size = 1; + std::vector maps(size); + std::vector transforms(size); + transforms[0].rotation.z = 1; // set transform to identity + combine_grids::MergingPipeline merger; + merger.feed(maps.begin(), maps.end()); + merger.setTransforms(transforms.begin(), transforms.end()); + EXPECT_EQ(merger.composeGrids(), nullptr); + EXPECT_EQ(merger.getTransforms().size(), size); +} + +/* one image may be empty */ +TEST(MergingPipeline, oneEmptyImage) +{ + std::vector maps{nullptr, + loadMap(gmapping_maps[0])}; + combine_grids::MergingPipeline merger; + merger.feed(maps.begin(), maps.end()); + merger.estimateTransforms(); + auto merged_grid = merger.composeGrids(); + auto transforms = merger.getTransforms(); + + EXPECT_VALID_GRID(merged_grid); + // don't use EXPECT_EQ, since it prints too much info + EXPECT_TRUE(maps_equal(*merged_grid, *maps[1])); + // transforms + EXPECT_EQ(transforms.size(), (long unsigned int) 2); + EXPECT_TRUE(isIdentity(transforms[1])); +} + +// non-identity known positions etc. +TEST(MergingPipeline, knownInitPositions) +{ + auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end()); + combine_grids::MergingPipeline merger; + merger.feed(maps.begin(), maps.end()); + + for (size_t i = 0; i < 5; ++i) { + std::vector transforms{randomTransform(), + randomTransform()}; + merger.setTransforms(transforms.begin(), transforms.end()); + auto merged_grid = merger.composeGrids(); + + EXPECT_VALID_GRID(merged_grid); + } +} + +int main(int argc, char** argv) +{ + // ros::Time::init(); + // if (verbose_tests && + // ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + // ros::console::levels::Debug)) { + // ros::console::notifyLoggerLevelsChanged(); + // } + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/m-explore-ros2/map_merge/test/testing_helpers.h b/m-explore-ros2/map_merge/test/testing_helpers.h new file mode 100644 index 0000000..bae04d3 --- /dev/null +++ b/m-explore-ros2/map_merge/test/testing_helpers.h @@ -0,0 +1,180 @@ +#ifndef TESTING_HELPERS_H_ +#define TESTING_HELPERS_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +const float resolution = 0.05f; + +nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename); +void saveMap(const std::string& filename, + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map); +std::tuple randomAngleTxTy(); +geometry_msgs::msg::Transform randomTransform(); +cv::Mat randomTransformMatrix(); + +/* map_server is really bad. until there is no replacement I will implement it + * by myself */ +template +std::vector loadMaps(InputIt filenames_begin, + InputIt filenames_end) +{ + std::vector result; + + for (InputIt it = filenames_begin; it != filenames_end; ++it) { + result.emplace_back(loadMap(*it)); + } + return result; +} + +nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename) +{ + cv::Mat lookUpTable(1, 256, CV_8S); + signed char* p = lookUpTable.ptr(); + p[254] = 0; + p[205] = -1; + p[0] = 100; + + cv::Mat img = cv::imread(filename, cv::IMREAD_GRAYSCALE); + if (img.empty()) { + throw std::runtime_error("could not load map"); + } + nav_msgs::msg::OccupancyGrid::SharedPtr grid{new nav_msgs::msg::OccupancyGrid()}; + grid->info.width = static_cast(img.size().width); + grid->info.height = static_cast(img.size().height); + grid->info.resolution = resolution; + grid->data.resize(static_cast(img.size().area())); + cv::Mat grid_view(img.size(), CV_8S, + const_cast(grid->data.data())); + cv::LUT(img, lookUpTable, grid_view); + + return grid; +} + +void saveMap(const std::string& filename, + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map) +{ + cv::Mat lookUpTable(1, 256, CV_8U); + uchar* p = lookUpTable.ptr(); + for (int i = 0; i < 255; ++i) { + if (i >= 0 && i < 10) + p[i] = 254; + else + p[i] = 0; + } + p[255] = 205; + + cv::Mat img(map->info.height, map->info.width, CV_8S, + const_cast(map->data.data())); + cv::Mat out_img; + cv::LUT(img, lookUpTable, out_img); + cv::imwrite(filename, out_img); +} + +std::tuple randomAngleTxTy() +{ + static std::mt19937_64 g(156468754 /*magic*/); + std::uniform_real_distribution rotation_dis(0., 2 * std::acos(-1)); + std::uniform_real_distribution translation_dis(-1000, 1000); + + return std::tuple(rotation_dis(g), translation_dis(g), + translation_dis(g)); +} + +geometry_msgs::msg::Transform randomTransform() +{ + double angle, tx, ty; + std::tie(angle, tx, ty) = randomAngleTxTy(); + tf2::Transform transform; + tf2::Quaternion rotation; + rotation.setEuler(0., 0., angle); + rotation.normalize(); + transform.setRotation(rotation); + transform.setOrigin(tf2::Vector3(tx, ty, 0.)); + + auto msg = toMsg(transform); + // normalize quaternion such that w > 0 (q and -q represents the same + // transformation) + if (msg.rotation.w < 0.) { + msg.rotation.x *= -1.; + msg.rotation.y *= -1.; + msg.rotation.z *= -1.; + msg.rotation.w *= -1.; + } + + return msg; +} + +cv::Mat randomTransformMatrix() +{ + double angle, tx, ty; + std::tie(angle, tx, ty) = randomAngleTxTy(); + cv::Mat transform = + (cv::Mat_(3, 3) << std::cos(angle), -std::sin(angle), tx, + std::sin(angle), std::cos(angle), ty, 0., 0., 1.); + + return transform; +} + +static inline bool isIdentity(const geometry_msgs::msg::Transform& transform) +{ + tf2::Transform t; + tf2::fromMsg(transform, t); + return tf2::Transform::getIdentity() == t; +} + +static inline bool isIdentity(const geometry_msgs::msg::Quaternion& rotation) +{ + tf2::Quaternion q; + tf2::fromMsg(rotation, q); + return tf2::Quaternion::getIdentity() == q; +} + +// data size is consistent with height and width +static inline bool consistentData(const nav_msgs::msg::OccupancyGrid& grid) +{ + return grid.info.width * grid.info.height == grid.data.size(); +} + +// ignores header, map_load_time and origin +// static inline bool operator==(const nav_msgs::msg::OccupancyGrid::SharedPtr grid1, +// const nav_msgs::msg::OccupancyGrid::SharedPtr grid2) +// { +// bool equal = true; +// equal &= grid1->info.width == grid2->info.width; +// equal &= grid1->info.height == grid2->info.height; +// equal &= std::abs(grid1->info.resolution - grid2->info.resolution) < +// std::numeric_limits::epsilon(); +// equal &= grid1->data.size() == grid2->data.size(); +// for (size_t i = 0; i < grid1->data.size(); ++i) { +// equal &= grid1->data[i] == grid2->data[i]; +// } +// return equal; +// } + +// ignores header, map_load_time and origin +static inline bool maps_equal(const nav_msgs::msg::OccupancyGrid& grid1, + const nav_msgs::msg::OccupancyGrid& grid2) +{ + // std::cout << "asdasdadsdth: " << std::endl; + bool equal = true; + equal &= grid1.info.width == grid2.info.width; + equal &= grid1.info.height == grid2.info.height; + equal &= std::abs(grid1.info.resolution - grid2.info.resolution) < + std::numeric_limits::epsilon(); + equal &= grid1.data.size() == grid2.data.size(); + for (size_t i = 0; i < grid1.data.size(); ++i) { + equal &= grid1.data[i] == grid2.data[i]; + } + return equal; +} + +#endif // TESTING_HELPERS_H_