├── .github └── workflows │ └── industrial_ci_action.yml ├── .gitignore ├── README.md ├── octomap_mapping ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml └── octomap_server ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg └── OctomapServer.cfg ├── color_nodelet_plugins.xml ├── include └── octomap_server │ ├── OctomapServer.h │ ├── OctomapServerMultilayer.h │ └── TrackingOctomapServer.h ├── launch ├── octomap_mapping.launch ├── octomap_mapping_nodelet.launch ├── octomap_tracking_client.launch └── octomap_tracking_server.launch ├── mainpage.dox ├── nodelet_plugins.xml ├── package.xml ├── params └── default.yaml ├── scripts └── octomap_eraser_cli.py └── src ├── OctomapServer.cpp ├── OctomapServerMultilayer.cpp ├── TrackingOctomapServer.cpp ├── octomap_saver.cpp ├── octomap_server_multilayer.cpp ├── octomap_server_node.cpp ├── octomap_server_nodelet.cpp ├── octomap_server_static.cpp └── octomap_tracking_server_node.cpp /.github/workflows/industrial_ci_action.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | on: [push, pull_request] 3 | 4 | jobs: 5 | CI: 6 | strategy: 7 | matrix: 8 | env: 9 | #- {ROS_DISTRO: melodic} 10 | #- {ROS_DISTRO: melodic, PRERELEASE: true} 11 | - {ROS_DISTRO: noetic} 12 | - {ROS_DISTRO: noetic, PRERELEASE: true} 13 | #- {ROS_DISTRO: noetic, OS_NAME: debian, OS_CODE_NAME: buster} 14 | env: 15 | PARALLEL_BUILDS: 4 16 | runs-on: ubuntu-latest 17 | steps: 18 | - uses: actions/checkout@v3 19 | - uses: 'ros-industrial/industrial_ci@master' 20 | env: ${{ matrix.env }} 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | octomap_server/docs 16 | octomap_server/cfg 17 | octomap_server/src/octomap_server 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | octomap_mapping ![CI](https://github.com/OctoMap/octomap_mapping/workflows/CI/badge.svg) 2 | =============== 3 | 4 | ROS stack for mapping with OctoMap, contains the `octomap_server` package. 5 | 6 | The main branch for ROS1 Kinetic, Melodic, and Noetic is `kinetic-devel`. 7 | 8 | The main branch for ROS2 Foxy and newer is `ros2`. 9 | -------------------------------------------------------------------------------- /octomap_mapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package octomap_mapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.8 (2024-03-24) 6 | ------------------ 7 | 8 | 0.6.7 (2021-12-24) 9 | ------------------ 10 | * Address warnings on Noetic (`#81 `_) 11 | * Contributors: Wolfgang Merkt 12 | 13 | 0.6.6 (2020-12-08) 14 | ------------------ 15 | 16 | 0.6.5 (2020-04-23) 17 | ------------------ 18 | * Update maintainer email 19 | * Contributors: Wolfgang Merkt 20 | 21 | 0.6.4 (2020-01-08) 22 | ------------------ 23 | 24 | 0.6.3 (2019-01-28) 25 | ------------------ 26 | 27 | 0.6.2 (2019-01-27) 28 | ------------------ 29 | * Update maintainer email (Wolfgang Merkt) 30 | * Contributors: Wolfgang Merkt 31 | 32 | 0.6.1 (2016-10-19) 33 | ------------------ 34 | * Adjust maintainer email 35 | * Contributors: Armin Hornung 36 | 37 | 0.6.0 (2016-03-25) 38 | ------------------ 39 | 40 | 0.5.3 (2014-01-09) 41 | ------------------ 42 | 43 | 0.5.2 (2014-01-09) 44 | ------------------ 45 | 46 | 0.5.1 (2013-11-25) 47 | ------------------ 48 | 49 | 0.5.0 (2013-10-24) 50 | ------------------ 51 | * URLs in package.xml 52 | * Catkinization, remove support for arm_navigation 53 | 54 | 0.4.9 (2013-06-27) 55 | ------------------ 56 | 57 | 0.4.8 (2013-01-08) 58 | ------------------ 59 | 60 | 0.4.6 (2013-01-28) 61 | ------------------ 62 | 63 | 0.4.5 (2012-06-18) 64 | ------------------ 65 | 66 | 0.4.4 (2012-04-20) 67 | ------------------ 68 | 69 | 0.4.3 (2012-04-17) 70 | ------------------ 71 | 72 | 0.4.2 (2012-03-16) 73 | ------------------ 74 | 75 | 0.4.1 (2012-02-21 16:50) 76 | ------------------------ 77 | 78 | 0.4.0 (2012-02-21 15:37) 79 | ------------------------ 80 | 81 | 0.3.8 (2012-04-26) 82 | ------------------ 83 | 84 | 0.3.7 (2012-02-22) 85 | ------------------ 86 | 87 | 0.3.6 (2012-01-09) 88 | ------------------ 89 | 90 | 0.3.5 (2011-10-30) 91 | ------------------ 92 | 93 | 0.3.4 (2011-10-12) 94 | ------------------ 95 | 96 | 0.3.3 (2011-08-17 07:41) 97 | ------------------------ 98 | 99 | 0.3.2 (2011-08-09) 100 | ------------------ 101 | 102 | 0.3.1 (2011-07-15) 103 | ------------------ 104 | 105 | 0.3.0 (2011-06-28) 106 | ------------------ 107 | 108 | 0.2.0 (2011-03-16) 109 | ------------------ 110 | 111 | 0.1.2 (2010-11-23) 112 | ------------------ 113 | 114 | 0.1.1 (2010-11-17) 115 | ------------------ 116 | 117 | 0.1.0 (2010-11-16) 118 | ------------------ 119 | -------------------------------------------------------------------------------- /octomap_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(octomap_mapping) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /octomap_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap_mapping 3 | 0.6.8 4 | 5 | Mapping tools to be used with the OctoMap library, implementing a 3D occupancy grid mapping. 6 | 7 | Armin Hornung 8 | Wolfgang Merkt 9 | BSD 10 | 11 | http://ros.org/wiki/octomap_mapping 12 | https://github.com/OctoMap/octomap_mapping/issues 13 | https://github.com/OctoMap/octomap_mapping 14 | 15 | catkin 16 | octomap_server 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /octomap_server/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package octomap_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.8 (2024-03-24) 6 | ------------------ 7 | * Change frame id "/map" to "map" to prevent RViz visualization warnings in ROS Noetic caused by tf (`#124 `_ 8 | * Initialize quaternion to avoid RViz warning (`#112 `_) 9 | * Update for PCL deprecated and removed API (`#118 `_) 10 | * Update pluginlib headers (`#114 `_) 11 | * Update boost::placeholders (`#105 `_) 12 | * Fix publishing of projected map on reset (`#92 `_) 13 | * Add a min range parameter (`#100 `_) 14 | * Contributors: Lovro Markovic, Lucas Walter, Wolfgang Merkt, Zhefan-Xu, yuri-r 15 | 16 | 0.6.7 (2021-12-24) 17 | ------------------ 18 | * Address warnings on Noetic (`#81 `_) 19 | * Contributors: Wolfgang Merkt 20 | 21 | 0.6.6 (2020-12-08) 22 | ------------------ 23 | * Update CI, package format, dependencies to address dependency issue on Debian Buster (`#79 `_) 24 | * Contributors: Wolfgang Merkt 25 | 26 | 0.6.5 (2020-04-23) 27 | ------------------ 28 | * Add color server nodelet (`#68 `_, `#67 `_) 29 | * Updated maintainer email 30 | * Contributors: clunietp, Wolfgang Merkt 31 | 32 | 0.6.4 (2020-01-08) 33 | ------------------ 34 | * Add private node handle to fix nodelet support (`#61 `_), fixes `#39 `_ 35 | * Add octomap_server_color library by default (`#60 `_) - by Matthew Powelson 36 | * Check if part of a voxel is in occupancy range (`#59 `_) - by Jasper v. B. 37 | * Contributors: Matthew Powelson, Wolfgang Merkt, Jasper v. B. 38 | 39 | 0.6.3 (2019-01-28) 40 | ------------------ 41 | * Fix compilation on Debian Stretch 42 | * Get rgb from point cloud iterator without byte shift 43 | * Contributors: Kentaro Wada, Wolfgang Merkt 44 | 45 | 0.6.2 (2019-01-27) 46 | ------------------ 47 | * Update maintainer email (Wolfgang Merkt) 48 | * Change catkin_package `DEPENDS` to `OCTOMAP` to avoid CMake warning 49 | * Update maintainer email (Arming Hornung) 50 | * Update to use non deprecated pluginlib macro 51 | * Fixed memory leak of colors pointer if COLOR_OCTOMAP_SERVER defined 52 | * Contributors: Armin Hornung, Mikael Arguedas, Ronky, Wolfgang Merkt 53 | 54 | 0.6.1 (2016-10-19) 55 | ------------------ 56 | * Fix for Colored Octomap: Use PCLPoint everywhere 57 | Fixes compiler error when enabling the define 58 | for color. 59 | * Fixed maxRange bug in OctomapServer.cpp for clearing 60 | * Adjust maintainer email 61 | * Contributors: Armin Hornung, Brandon Kinman, Felix Endres 62 | 63 | 0.6.0 (2016-03-25) 64 | ------------------ 65 | * Add sensor model parameters to dynamic_reconfigure 66 | * Load map file from rosparam 67 | * Add x and y filter for pointcloud 68 | * Preparations for ColorOctomapServer (compile option, from source) 69 | * Fix iterator in OctomapServer 70 | * TrackingOctomapServer: Publish node center rather than index, prevent from publishing empty cloud 71 | * Contributors: Armin Hornung, Javier V. Gomez, JJeremie Deray, MasakiMurooka, Shohei Fujii, Wolfgang Merkt 72 | 73 | 0.5.3 (2014-01-09) 74 | ------------------ 75 | * Fixing PCL linking errors on build farm 76 | 77 | 0.5.2 (2014-01-09) 78 | ------------------ 79 | * Fixing PCL linking errors on build farm 80 | 81 | 0.5.1 (2013-11-25) 82 | ------------------ 83 | * Fix missing nodelet plugin from install 84 | 85 | 0.5.0 (2013-10-24) 86 | ------------------ 87 | * Small fix in octomap_server_static usage 88 | * Catkinization, remove support for arm_navigation 89 | 90 | 0.4.9 (2013-06-27) 91 | ------------------ 92 | * cleanup of unused functions 93 | * Parameters, reading .bt files in octomap_server_static 94 | * added simple octomap_server_static node to serve OctoMaps from .ot files (no scan integration) 95 | * Fix for incremental 2D projection map updates (thx to B.Coenen for the report) 96 | * Publish free space as MarkerArray and CollisionMap (set parameter ~publish_free_space=True to enable). Thx to I. Wieser! 97 | * renamed OctomapServer's NodeHandle constructor parameter to be more clear, added the same to OctomapServerMultilayer 98 | 99 | 0.4.8 (2013-01-08) 100 | ------------------ 101 | * Applied patch from issue `#7 `_: Nodelet version of octomap_server 102 | Modified to not change the global namespace 103 | * fixes for cmake / catkin 104 | * fixed octomap_server for OctoMap 1.5 (deprecations), adjusted to new msg format 105 | * changed message format to contain only data, meta information stored in new message fields (untested for Groovy) 106 | 107 | 0.4.6 (2013-01-28) 108 | ------------------ 109 | * Added NodeHandle parameter to OctomapServerMultilayer constructor 110 | * Commited patch `#7 `_, contributed by M. Liebhardt: Nodelet version of the octomap server 111 | * octomap_server and octomap_saver now fully support both binary and full occupancy maps 112 | * octomap_server can now open .ot files properly, updated octomap_ros to new-style stack.xml 113 | * deprecated OctomapROS in octomap_ros => directly use octomap lib and conversions.h 114 | * removed OctomapROS wrapper from octomap_server classes 115 | * octomap_server manifest exports dynamic_reconfigure path in cppflags 116 | * parameter in launch file adjusted 117 | 118 | 0.4.5 (2012-06-18) 119 | ------------------ 120 | * new parameter to enable incremental 2D mapping (experimental, default: false) 121 | * bug fix for OctomapServer map projection 122 | * Fixed OctomapServer not clearing obstacles in projected 2D map properly 123 | * fixed map reset and incremental 2D updates 124 | * added arm layer height lookup 125 | * Fixed resolution change (dynamic_reconfigure) and dynamic map size w. incremental updates 126 | * incremental update of projected 2D maps only in updated 3D region, map dynamically grows 127 | * increased Electric compatibility of octomap_server 128 | * OctomapServer keeps track of update region for downprojected 2D map 129 | 130 | 0.4.4 (2012-04-20) 131 | ------------------ 132 | * Turned octomap_msgs and octomap_ros into unary stacks, code in octomap_mapping adjusted 133 | 134 | 0.4.3 (2012-04-17) 135 | ------------------ 136 | * Merged rev 2477:2613 from trunk: 137 | - fixed ground filter 138 | - added missing license headers, improved code layout to ROS standard 139 | - adjusted to OctoMap 1.4 changes 140 | - collision map publisher & eraser script ported from branch 141 | - disabled lazy update temporarily (needs param) 142 | - dynamic reconfigure interface to limit query depth (and voxel resolution) on the fly 143 | 144 | 0.4.2 (2012-03-16) 145 | ------------------ 146 | * fixed ground filter (from trunk, electric)\nVersion increased to 0.4.2 147 | 148 | 0.4.1 (2012-02-21 16:50) 149 | ------------------------ 150 | * switched octomap_ros and octomap_server to pure CMake-style linking, version 0.4.1 151 | * removed uneccesary FindEigen.cmake files 152 | 153 | 0.4.0 (2012-02-21 15:37) 154 | ------------------------ 155 | * removed eigen package from depends 156 | * Transitioned octomap package to deprecated, now forwards flags with pkg-config to system dependency 157 | 158 | 0.3.8 (2012-04-26) 159 | ------------------ 160 | * increased octomap version to 1.4.2, stack version 0.3.8 161 | 162 | 0.3.7 (2012-02-22) 163 | ------------------ 164 | * removed temp. workaround for unstable (Eigen for PCL included), increased stack version to 0.3.7 165 | * server/client architecture for octomap_server 166 | * octomap_server: ground plane filter defaults to false, base_footprint frame now only required when filtering 167 | 168 | 0.3.6 (2012-01-09) 169 | ------------------ 170 | * changed to Eigen rosdep for electric and fuerte 171 | 172 | 0.3.5 (2011-10-30) 173 | ------------------ 174 | * added OctomapServerMultilayer as stub 175 | * More refactoring of octomap_server, added hooks for node traversal 176 | * OctomapServerCombined is now OctomapServer 177 | * cleanup of octomap_server 178 | * - adjusted octomap_mapping trunk to compile against ROS electric (only affects octomap_server). 179 | => use branch for diamondback! 180 | 181 | 0.3.4 (2011-10-12) 182 | ------------------ 183 | * publish empty map (+vis) after reset 184 | * OctomapServerCombined: Drop old octree completely when resetting 185 | * OctomapServerCombined: Parameter for latching topics, reset service 186 | * added srv and service implementation to clear a bbx region in OctomapServerCombined 187 | * OctomapServer: 188 | private -> protected 189 | added default constructor 190 | * octomap_server: 191 | - fixed 2D map for larger volumes 192 | - now handles an initial file always as static, topics are published latched then 193 | * removed debug PCD writing 194 | * - ground filter now more reliable, filtering in base frame of robot instead of global frame. 195 | - more parameters for ground filter 196 | 197 | 0.3.3 (2011-08-17 07:41) 198 | ------------------------ 199 | * octomap package udpate to use new OctoMap 1.2 library only (no visualization). Removed dependency on Qt / QGLViewer. 200 | * fixed ground plane appearing as occupied 201 | 202 | 0.3.2 (2011-08-09) 203 | ------------------ 204 | * merged in changes of octomap_mapping trunk (up to rev 1781): 205 | - octomap updated to 1.1.1 (testing), tarball URL on ros.org 206 | - ground plane extraction OctomapServerCombined, configurable using PCL 207 | - fixes and cleanup in OctomapServerCombined 208 | * parameters for ground plane filtering 209 | * Ground plane extraction improved 210 | * Ground plane extraction (pcl) for testing 211 | * - octomap: use OctoMap 1.1.1 (testing) 212 | - octomap_server: handle larger pruned nodes in 2D map projection 213 | * refactoring & cleanup of OctomapServerCombined, ready for ground plane extraction 214 | * merged back octomap_server from experimental branch: 215 | - proper class with more capabilities 216 | - now sends out map in various representations / visualizations 217 | - subscribes to PointCloud2 with tf::MessageFilter 218 | - uses octomap_ros wrapper / conversions 219 | - OctomapServerCombined (experimental): also builds downprojected 2D map 220 | * added MoveMap.msg from octomap2, extended conversions.h 221 | * templated octomapMsg conversion functions 222 | * octomap_saver adjusted to moved locations 223 | * Moved messages and conversions to octomap_ros from octomap_server 224 | * Removed unnecessary exports in manifests 225 | * - fixes in mainfest / stack.xml for ROS 1.3 226 | - doxygen properly configured with rosdoc 227 | - stack release 0.1.2 prep 228 | * Preparations for .deb releases 229 | * License in cpp files, restored compatibility with boxturtle 230 | * Adjusted license to BSD, more parameters in octomap_server 231 | * OctoMap server (copied from octomap repo, trunk) 232 | * Initial checkin of octomap stack (nearly empty at the moment) 233 | 234 | 0.3.1 (2011-07-15) 235 | ------------------ 236 | * Patched for arm_navigation changes in "unstable" 237 | 238 | 0.3.0 (2011-06-28) 239 | ------------------ 240 | * merged back octomap_server from experimental branch: 241 | - proper class with more capabilities 242 | - now sends out map in various representations / visualizations 243 | - subscribes to PointCloud2 with tf::MessageFilter 244 | - uses octomap_ros wrapper / conversions 245 | - OctomapServerCombined (experimental): also builds downprojected 2D map 246 | * added MoveMap.msg from octomap2, extended conversions.h 247 | * templated octomapMsg conversion functions 248 | 249 | 0.2.0 (2011-03-16) 250 | ------------------ 251 | * updated stack.xml for cturtle only 252 | * octomap_saver adjusted to moved locations 253 | * Moved messages and conversions to octomap_ros from octomap_server 254 | * Removed unnecessary exports in manifests 255 | 256 | 0.1.2 (2010-11-23) 257 | ------------------ 258 | * - fixes in mainfest / stack.xml for ROS 1.3 259 | - doxygen properly configured with rosdoc 260 | - stack release 0.1.2 prep 261 | 262 | 0.1.1 (2010-11-17) 263 | ------------------ 264 | 265 | 0.1.0 (2010-11-16) 266 | ------------------ 267 | * Preparations for .deb releases 268 | * License in cpp files, restored compatibility with boxturtle 269 | * Adjusted license to BSD, more parameters in octomap_server 270 | * OctoMap server (copied from octomap repo, trunk) 271 | * Initial checkin of octomap stack (nearly empty at the moment) 272 | -------------------------------------------------------------------------------- /octomap_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(octomap_server) 3 | 4 | set(PACKAGE_DEPENDENCIES 5 | roscpp 6 | visualization_msgs 7 | sensor_msgs 8 | pcl_ros 9 | pcl_conversions 10 | nav_msgs 11 | std_msgs 12 | std_srvs 13 | octomap_ros 14 | octomap_msgs 15 | dynamic_reconfigure 16 | nodelet 17 | ) 18 | 19 | find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) 20 | 21 | find_package(octomap REQUIRED) 22 | add_definitions(-DOCTOMAP_NODEBUGOUT) 23 | 24 | include_directories( 25 | include 26 | ${catkin_INCLUDE_DIRS} 27 | ${OCTOMAP_INCLUDE_DIRS} 28 | ) 29 | 30 | generate_dynamic_reconfigure_options(cfg/OctomapServer.cfg) 31 | 32 | catkin_package( 33 | INCLUDE_DIRS include 34 | LIBRARIES ${PROJECT_NAME} 35 | CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} 36 | DEPENDS OCTOMAP 37 | ) 38 | 39 | set(LINK_LIBS 40 | ${OCTOMAP_LIBRARIES} 41 | ${catkin_LIBRARIES} 42 | ) 43 | 44 | add_library(${PROJECT_NAME} src/OctomapServer.cpp src/OctomapServerMultilayer.cpp src/TrackingOctomapServer.cpp) 45 | target_link_libraries(${PROJECT_NAME} ${LINK_LIBS}) 46 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 47 | 48 | add_library(${PROJECT_NAME}_color src/OctomapServer.cpp src/OctomapServerMultilayer.cpp src/TrackingOctomapServer.cpp) 49 | target_link_libraries(${PROJECT_NAME}_color ${LINK_LIBS}) 50 | add_dependencies(${PROJECT_NAME}_color ${PROJECT_NAME}_gencfg) 51 | target_compile_definitions(${PROJECT_NAME}_color PUBLIC COLOR_OCTOMAP_SERVER) 52 | 53 | add_executable(octomap_server_node src/octomap_server_node.cpp) 54 | target_link_libraries(octomap_server_node ${PROJECT_NAME} ${LINK_LIBS}) 55 | 56 | add_executable(octomap_color_server_node src/octomap_server_node.cpp) 57 | target_link_libraries(octomap_color_server_node ${PROJECT_NAME}_color ${LINK_LIBS}) 58 | 59 | add_executable(octomap_server_static src/octomap_server_static.cpp) 60 | target_link_libraries(octomap_server_static ${PROJECT_NAME} ${LINK_LIBS}) 61 | 62 | add_executable(octomap_server_multilayer src/octomap_server_multilayer.cpp) 63 | target_link_libraries(octomap_server_multilayer ${PROJECT_NAME} ${LINK_LIBS}) 64 | 65 | add_executable(octomap_saver src/octomap_saver.cpp) 66 | target_link_libraries(octomap_saver ${PROJECT_NAME} ${LINK_LIBS}) 67 | 68 | add_executable(octomap_tracking_server_node src/octomap_tracking_server_node.cpp) 69 | target_link_libraries(octomap_tracking_server_node ${PROJECT_NAME} ${LINK_LIBS}) 70 | 71 | # Nodelet 72 | add_library(octomap_server_nodelet src/octomap_server_nodelet.cpp) 73 | target_link_libraries(octomap_server_nodelet ${PROJECT_NAME} ${LINK_LIBS}) 74 | 75 | add_library(octomap_color_server_nodelet src/octomap_server_nodelet.cpp) 76 | target_link_libraries(octomap_color_server_nodelet ${PROJECT_NAME}_color ${LINK_LIBS}) 77 | 78 | # install targets: 79 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_color 80 | octomap_server_node 81 | octomap_color_server_node 82 | octomap_server_static 83 | octomap_server_multilayer 84 | octomap_saver 85 | octomap_tracking_server_node 86 | octomap_server_nodelet 87 | octomap_color_server_nodelet 88 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 89 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 90 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 91 | ) 92 | 93 | install(DIRECTORY include/${PROJECT_NAME}/ 94 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 95 | ) 96 | 97 | install(DIRECTORY launch/ 98 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 99 | ) 100 | 101 | install(FILES 102 | nodelet_plugins.xml 103 | color_nodelet_plugins.xml 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 105 | ) 106 | -------------------------------------------------------------------------------- /octomap_server/cfg/OctomapServer.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "octomap_server" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("compress_map", bool_t, 0, "Compresses the map losslessly", True) 9 | gen.add("incremental_2D_projection", bool_t, 0, "Incremental 2D projection", False) 10 | gen.add("filter_speckles", bool_t, 0, "Filter speckle nodes (with no neighbors)", False) 11 | gen.add("max_depth", int_t, 0, "Maximum depth when traversing the octree to send out markers. 16: full depth / max. resolution", 16, 1, 16) 12 | gen.add("pointcloud_min_z", double_t, 0, "Minimum height of points to consider for insertion", -100, -100, 100) 13 | gen.add("pointcloud_max_z", double_t, 0, "Maximum height of points to consider for insertion", 100, -100, 100) 14 | gen.add("occupancy_min_z", double_t, 0, "Minimum height of occupied cells to consider in the final map", -100, -100, 100) 15 | gen.add("occupancy_max_z", double_t, 0, "Maximum height of occupied cells to consider in the final map", 100, -100, 100) 16 | gen.add("sensor_model_max_range", double_t, 0, "Sensor maximum range", -1.0, -1.0, 100) 17 | gen.add("sensor_model_min_range", double_t, 0, "Sensor minimum range", -1.0, -1.0, 100) 18 | gen.add("sensor_model_hit", double_t, 0, "Probabilities for hits in the sensor model when dynamically building a map", 0.7, 0.5, 1.0) 19 | gen.add("sensor_model_miss", double_t, 0, "Probabilities for misses in the sensor model when dynamically building a map", 0.4, 0.0, 0.5) 20 | gen.add("sensor_model_min", double_t, 0, "Minimum probability for clamping when dynamically building a map", 0.12, 0.0, 1.0) 21 | gen.add("sensor_model_max", double_t, 0, "Maximum probability for clamping when dynamically building a map", 0.97, 0.0, 1.0) 22 | gen.add("filter_ground", bool_t, 0, "Filter ground plane", False) 23 | gen.add("ground_filter_distance", double_t, 0, "Distance threshold to consider a point as ground", 0.04, 0.001, 1) 24 | gen.add("ground_filter_angle", double_t, 0, "Angular threshold of the detected plane from the horizontal plane to be detected as ground ", 0.15, 0.001, 15) 25 | gen.add("ground_filter_plane_distance", double_t, 0, "Distance threshold from z=0 for a plane to be detected as ground", 0.07, 0.001, 1) 26 | 27 | exit(gen.generate(PACKAGE, "octomap_server_node", "OctomapServer")) 28 | -------------------------------------------------------------------------------- /octomap_server/color_nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Nodelet for running the color Octomap server 5 | 6 | 7 | -------------------------------------------------------------------------------- /octomap_server/include/octomap_server/OctomapServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010-2013, A. Hornung, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef OCTOMAP_SERVER_OCTOMAPSERVER_H 31 | #define OCTOMAP_SERVER_OCTOMAPSERVER_H 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | // #include 39 | // #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #pragma GCC diagnostic push 51 | #pragma GCC diagnostic ignored "-Wdeprecated-declarations" // pcl::SAC_SAMPLE_SIZE is protected since PCL 1.8.0 52 | #include 53 | #pragma GCC diagnostic pop 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | 61 | 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include 71 | #include 72 | #include 73 | 74 | //#define COLOR_OCTOMAP_SERVER // switch color here - easier maintenance, only maintain OctomapServer. Two targets are defined in the cmake, octomap_server_color and octomap_server. One has this defined, and the other doesn't 75 | 76 | #ifdef COLOR_OCTOMAP_SERVER 77 | #include 78 | #endif 79 | 80 | namespace octomap_server { 81 | class OctomapServer { 82 | 83 | public: 84 | #ifdef COLOR_OCTOMAP_SERVER 85 | typedef pcl::PointXYZRGB PCLPoint; 86 | typedef pcl::PointCloud PCLPointCloud; 87 | typedef octomap::ColorOcTree OcTreeT; 88 | #else 89 | typedef pcl::PointXYZ PCLPoint; 90 | typedef pcl::PointCloud PCLPointCloud; 91 | typedef octomap::OcTree OcTreeT; 92 | #endif 93 | typedef octomap_msgs::GetOctomap OctomapSrv; 94 | typedef octomap_msgs::BoundingBoxQuery BBXSrv; 95 | 96 | OctomapServer(const ros::NodeHandle private_nh_ = ros::NodeHandle("~"), const ros::NodeHandle &nh_ = ros::NodeHandle()); 97 | virtual ~OctomapServer(); 98 | virtual bool octomapBinarySrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res); 99 | virtual bool octomapFullSrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res); 100 | bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp); 101 | bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp); 102 | 103 | virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud); 104 | virtual bool openFile(const std::string& filename); 105 | 106 | protected: 107 | inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min) { 108 | for (unsigned i = 0; i < 3; ++i) 109 | min[i] = std::min(in[i], min[i]); 110 | }; 111 | 112 | inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max) { 113 | for (unsigned i = 0; i < 3; ++i) 114 | max[i] = std::max(in[i], max[i]); 115 | }; 116 | 117 | /// Test if key is within update area of map (2D, ignores height) 118 | inline bool isInUpdateBBX(const OcTreeT::iterator& it) const { 119 | // 2^(tree_depth-depth) voxels wide: 120 | unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth())); 121 | octomap::OcTreeKey key = it.getIndexKey(); // lower corner of voxel 122 | return (key[0] + voxelWidth >= m_updateBBXMin[0] 123 | && key[1] + voxelWidth >= m_updateBBXMin[1] 124 | && key[0] <= m_updateBBXMax[0] 125 | && key[1] <= m_updateBBXMax[1]); 126 | } 127 | 128 | void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level); 129 | void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const; 130 | void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const; 131 | void publishProjected2DMap(const ros::Time& rostime = ros::Time::now()); 132 | virtual void publishAll(const ros::Time& rostime = ros::Time::now()); 133 | 134 | /** 135 | * @brief update occupancy map with a scan labeled as ground and nonground. 136 | * The scans should be in the global map frame. 137 | * 138 | * @param sensorOrigin origin of the measurements for raycasting 139 | * @param ground scan endpoints on the ground plane (only clear space) 140 | * @param nonground all other endpoints (clear up to occupied endpoint) 141 | */ 142 | virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground); 143 | 144 | /// label the input cloud "pc" into ground and nonground. Should be in the robot's fixed frame (not world!) 145 | void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const; 146 | 147 | /** 148 | * @brief Find speckle nodes (single occupied voxels with no neighbors). Only works on lowest resolution! 149 | * @param key 150 | * @return 151 | */ 152 | bool isSpeckleNode(const octomap::OcTreeKey& key) const; 153 | 154 | /// hook that is called before traversing all nodes 155 | virtual void handlePreNodeTraversal(const ros::Time& rostime); 156 | 157 | /// hook that is called when traversing all nodes of the updated Octree (does nothing here) 158 | virtual void handleNode(const OcTreeT::iterator& it) {}; 159 | 160 | /// hook that is called when traversing all nodes of the updated Octree in the updated area (does nothing here) 161 | virtual void handleNodeInBBX(const OcTreeT::iterator& it) {}; 162 | 163 | /// hook that is called when traversing occupied nodes of the updated Octree 164 | virtual void handleOccupiedNode(const OcTreeT::iterator& it); 165 | 166 | /// hook that is called when traversing occupied nodes in the updated area (updates 2D map projection here) 167 | virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it); 168 | 169 | /// hook that is called when traversing free nodes of the updated Octree 170 | virtual void handleFreeNode(const OcTreeT::iterator& it); 171 | 172 | /// hook that is called when traversing free nodes in the updated area (updates 2D map projection here) 173 | virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it); 174 | 175 | /// hook that is called after traversing all nodes 176 | virtual void handlePostNodeTraversal(const ros::Time& rostime); 177 | 178 | /// updates the downprojected 2D map as either occupied or free 179 | virtual void update2DMap(const OcTreeT::iterator& it, bool occupied); 180 | 181 | inline unsigned mapIdx(int i, int j) const { 182 | return m_gridmap.info.width * j + i; 183 | } 184 | 185 | inline unsigned mapIdx(const octomap::OcTreeKey& key) const { 186 | return mapIdx((key[0] - m_paddedMinKey[0]) / m_multires2DScale, 187 | (key[1] - m_paddedMinKey[1]) / m_multires2DScale); 188 | 189 | } 190 | 191 | /** 192 | * Adjust data of map due to a change in its info properties (origin or size, 193 | * resolution needs to stay fixed). map already contains the new map info, 194 | * but the data is stored according to oldMapInfo. 195 | */ 196 | 197 | void adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const; 198 | 199 | inline bool mapChanged(const nav_msgs::MapMetaData& oldMapInfo, const nav_msgs::MapMetaData& newMapInfo) { 200 | return ( oldMapInfo.height != newMapInfo.height 201 | || oldMapInfo.width != newMapInfo.width 202 | || oldMapInfo.origin.position.x != newMapInfo.origin.position.x 203 | || oldMapInfo.origin.position.y != newMapInfo.origin.position.y); 204 | } 205 | 206 | static std_msgs::ColorRGBA heightMapColor(double h); 207 | ros::NodeHandle m_nh; 208 | ros::NodeHandle m_nh_private; 209 | ros::Publisher m_markerPub, m_binaryMapPub, m_fullMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub, m_fmapPub, m_fmarkerPub; 210 | message_filters::Subscriber* m_pointCloudSub; 211 | tf::MessageFilter* m_tfPointCloudSub; 212 | ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService; 213 | tf::TransformListener m_tfListener; 214 | boost::recursive_mutex m_config_mutex; 215 | dynamic_reconfigure::Server m_reconfigureServer; 216 | 217 | OcTreeT* m_octree; 218 | octomap::KeyRay m_keyRay; // temp storage for ray casting 219 | octomap::OcTreeKey m_updateBBXMin; 220 | octomap::OcTreeKey m_updateBBXMax; 221 | 222 | double m_minRange; 223 | double m_maxRange; 224 | std::string m_worldFrameId; // the map frame 225 | std::string m_baseFrameId; // base of the robot for ground plane filtering 226 | bool m_useHeightMap; 227 | std_msgs::ColorRGBA m_color; 228 | std_msgs::ColorRGBA m_colorFree; 229 | double m_colorFactor; 230 | 231 | bool m_latchedTopics; 232 | bool m_publishFreeSpace; 233 | 234 | double m_res; 235 | unsigned m_treeDepth; 236 | unsigned m_maxTreeDepth; 237 | 238 | double m_pointcloudMinX; 239 | double m_pointcloudMaxX; 240 | double m_pointcloudMinY; 241 | double m_pointcloudMaxY; 242 | double m_pointcloudMinZ; 243 | double m_pointcloudMaxZ; 244 | double m_occupancyMinZ; 245 | double m_occupancyMaxZ; 246 | double m_minSizeX; 247 | double m_minSizeY; 248 | bool m_filterSpeckles; 249 | 250 | bool m_filterGroundPlane; 251 | double m_groundFilterDistance; 252 | double m_groundFilterAngle; 253 | double m_groundFilterPlaneDistance; 254 | 255 | bool m_compressMap; 256 | 257 | bool m_initConfig; 258 | 259 | // downprojected 2D map: 260 | bool m_incrementalUpdate; 261 | nav_msgs::OccupancyGrid m_gridmap; 262 | bool m_publish2DMap; 263 | bool m_mapOriginChanged; 264 | octomap::OcTreeKey m_paddedMinKey; 265 | unsigned m_multires2DScale; 266 | bool m_projectCompleteMap; 267 | bool m_useColoredMap; 268 | }; 269 | } 270 | 271 | #endif 272 | -------------------------------------------------------------------------------- /octomap_server/include/octomap_server/OctomapServerMultilayer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010-2013, A. Hornung, M. Philips 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef OCTOMAP_SERVER_OCTOMAPSERVERMULTILAYER_H 30 | #define OCTOMAP_SERVER_OCTOMAPSERVERMULTILAYER_H 31 | 32 | #include 33 | 34 | namespace octomap_server { 35 | class OctomapServerMultilayer : public OctomapServer{ 36 | 37 | public: 38 | OctomapServerMultilayer(ros::NodeHandle private_nh_ = ros::NodeHandle("~")); 39 | virtual ~OctomapServerMultilayer(); 40 | 41 | protected: 42 | struct ProjectedMap{ 43 | double minZ; 44 | double maxZ; 45 | double z; // for visualization 46 | std::string name; 47 | nav_msgs::OccupancyGrid map; 48 | }; 49 | typedef std::vector MultilevelGrid; 50 | 51 | /// hook that is called after traversing all nodes 52 | virtual void handlePreNodeTraversal(const ros::Time& rostime); 53 | 54 | /// updates the downprojected 2D map as either occupied or free 55 | virtual void update2DMap(const OcTreeT::iterator& it, bool occupied); 56 | 57 | /// hook that is called after traversing all nodes 58 | virtual void handlePostNodeTraversal(const ros::Time& rostime); 59 | 60 | std::vector m_multiMapPub; 61 | ros::Subscriber m_attachedObjectsSub; 62 | 63 | std::vector m_armLinks; 64 | std::vector m_armLinkOffsets; 65 | 66 | MultilevelGrid m_multiGridmap; 67 | 68 | 69 | }; 70 | } 71 | 72 | #endif 73 | 74 | -------------------------------------------------------------------------------- /octomap_server/include/octomap_server/TrackingOctomapServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef OCTOMAP_SERVER_TRACKINGOCTOMAPSERVER_H_ 31 | #define OCTOMAP_SERVER_TRACKINGOCTOMAPSERVER_H_ 32 | 33 | #include "octomap_server/OctomapServer.h" 34 | 35 | namespace octomap_server { 36 | 37 | class TrackingOctomapServer: public OctomapServer { 38 | public: 39 | TrackingOctomapServer(const std::string& filename = ""); 40 | virtual ~TrackingOctomapServer(); 41 | 42 | void trackCallback(sensor_msgs::PointCloud2Ptr cloud); 43 | void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground); 44 | 45 | protected: 46 | void trackChanges(); 47 | 48 | bool listen_changes; 49 | bool track_changes; 50 | int min_change_pub; 51 | std::string change_id_frame; 52 | ros::Publisher pubFreeChangeSet; 53 | ros::Publisher pubChangeSet; 54 | ros::Subscriber subChangeSet; 55 | ros::Subscriber subFreeChanges; 56 | }; 57 | 58 | } /* namespace octomap */ 59 | #endif /* TRACKINGOCTOMAPSERVER_H_ */ 60 | -------------------------------------------------------------------------------- /octomap_server/launch/octomap_mapping.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /octomap_server/launch/octomap_mapping_nodelet.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /octomap_server/launch/octomap_tracking_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /octomap_server/launch/octomap_tracking_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /octomap_server/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b octomap_server is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /octomap_server/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Nodelet for running the Octomap server 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /octomap_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap_server 3 | 0.6.8 4 | 5 | octomap_server loads a 3D map (as Octree-based OctoMap) and distributes it to other nodes in a compact binary format. It also allows to incrementally build 3D OctoMaps, and provides map saving in the node octomap_saver. 6 | 7 | Armin Hornung 8 | Wolfgang Merkt 9 | BSD 10 | http://www.ros.org/wiki/octomap_server 11 | https://github.com/OctoMap/octomap_mapping/issues 12 | https://github.com/OctoMap/octomap_mapping 13 | 14 | 15 | 16 | 17 | 18 | 19 | catkin 20 | 21 | roscpp 22 | visualization_msgs 23 | sensor_msgs 24 | pcl_ros 25 | pcl_conversions 26 | nav_msgs 27 | std_msgs 28 | std_srvs 29 | octomap 30 | octomap_msgs 31 | octomap_ros 32 | dynamic_reconfigure 33 | nodelet 34 | 35 | roscpp 36 | visualization_msgs 37 | sensor_msgs 38 | pcl_ros 39 | pcl_conversions 40 | nav_msgs 41 | std_msgs 42 | std_srvs 43 | octomap 44 | octomap_msgs 45 | octomap_ros 46 | dynamic_reconfigure 47 | nodelet 48 | 49 | -------------------------------------------------------------------------------- /octomap_server/params/default.yaml: -------------------------------------------------------------------------------- 1 | # Empty file to load default parameters. 2 | -------------------------------------------------------------------------------- /octomap_server/scripts/octomap_eraser_cli.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """Clear a region specified by a global axis-aligned bounding box in stored 4 | OctoMap. 5 | 6 | """ 7 | 8 | import sys 9 | from time import sleep 10 | 11 | import roslib 12 | roslib.load_manifest('octomap_server') 13 | from geometry_msgs.msg import Point 14 | import octomap_msgs.srv 15 | import rospy 16 | 17 | 18 | SRV_NAME = '/octomap_server/clear_bbx' 19 | SRV_INTERFACE = octomap_msgs.srv.BoundingBoxQuery 20 | 21 | 22 | if __name__ == '__main__': 23 | min = Point(*[float(x) for x in sys.argv[1:4]]) 24 | max = Point(*[float(x) for x in sys.argv[4:7]]) 25 | 26 | rospy.init_node('octomap_eraser_cli', anonymous=True) 27 | sleep(1) 28 | service = rospy.ServiceProxy(SRV_NAME, SRV_INTERFACE) 29 | rospy.loginfo("Connected to %s service." % SRV_NAME) 30 | 31 | service(min, max) 32 | -------------------------------------------------------------------------------- /octomap_server/src/OctomapServer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010-2013, A. Hornung, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | using namespace octomap; 33 | using octomap_msgs::Octomap; 34 | 35 | bool is_equal (double a, double b, double epsilon = 1.0e-7) 36 | { 37 | return std::abs(a - b) < epsilon; 38 | } 39 | 40 | namespace octomap_server{ 41 | 42 | OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeHandle &nh_) 43 | : m_nh(nh_), 44 | m_nh_private(private_nh_), 45 | m_pointCloudSub(NULL), 46 | m_tfPointCloudSub(NULL), 47 | m_reconfigureServer(m_config_mutex, private_nh_), 48 | m_octree(NULL), 49 | m_maxRange(-1.0), 50 | m_minRange(-1.0), 51 | m_worldFrameId("map"), m_baseFrameId("base_footprint"), 52 | m_useHeightMap(true), 53 | m_useColoredMap(false), 54 | m_colorFactor(0.8), 55 | m_latchedTopics(true), 56 | m_publishFreeSpace(false), 57 | m_res(0.05), 58 | m_treeDepth(0), 59 | m_maxTreeDepth(0), 60 | m_pointcloudMinX(-std::numeric_limits::max()), 61 | m_pointcloudMaxX(std::numeric_limits::max()), 62 | m_pointcloudMinY(-std::numeric_limits::max()), 63 | m_pointcloudMaxY(std::numeric_limits::max()), 64 | m_pointcloudMinZ(-std::numeric_limits::max()), 65 | m_pointcloudMaxZ(std::numeric_limits::max()), 66 | m_occupancyMinZ(-std::numeric_limits::max()), 67 | m_occupancyMaxZ(std::numeric_limits::max()), 68 | m_minSizeX(0.0), m_minSizeY(0.0), 69 | m_filterSpeckles(false), m_filterGroundPlane(false), 70 | m_groundFilterDistance(0.04), m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07), 71 | m_compressMap(true), 72 | m_incrementalUpdate(false), 73 | m_initConfig(true) 74 | { 75 | double probHit, probMiss, thresMin, thresMax; 76 | 77 | m_nh_private.param("frame_id", m_worldFrameId, m_worldFrameId); 78 | m_nh_private.param("base_frame_id", m_baseFrameId, m_baseFrameId); 79 | m_nh_private.param("height_map", m_useHeightMap, m_useHeightMap); 80 | m_nh_private.param("colored_map", m_useColoredMap, m_useColoredMap); 81 | m_nh_private.param("color_factor", m_colorFactor, m_colorFactor); 82 | 83 | m_nh_private.param("pointcloud_min_x", m_pointcloudMinX,m_pointcloudMinX); 84 | m_nh_private.param("pointcloud_max_x", m_pointcloudMaxX,m_pointcloudMaxX); 85 | m_nh_private.param("pointcloud_min_y", m_pointcloudMinY,m_pointcloudMinY); 86 | m_nh_private.param("pointcloud_max_y", m_pointcloudMaxY,m_pointcloudMaxY); 87 | m_nh_private.param("pointcloud_min_z", m_pointcloudMinZ,m_pointcloudMinZ); 88 | m_nh_private.param("pointcloud_max_z", m_pointcloudMaxZ,m_pointcloudMaxZ); 89 | m_nh_private.param("occupancy_min_z", m_occupancyMinZ,m_occupancyMinZ); 90 | m_nh_private.param("occupancy_max_z", m_occupancyMaxZ,m_occupancyMaxZ); 91 | m_nh_private.param("min_x_size", m_minSizeX,m_minSizeX); 92 | m_nh_private.param("min_y_size", m_minSizeY,m_minSizeY); 93 | 94 | m_nh_private.param("filter_speckles", m_filterSpeckles, m_filterSpeckles); 95 | m_nh_private.param("filter_ground", m_filterGroundPlane, m_filterGroundPlane); 96 | // distance of points from plane for RANSAC 97 | m_nh_private.param("ground_filter/distance", m_groundFilterDistance, m_groundFilterDistance); 98 | // angular derivation of found plane: 99 | m_nh_private.param("ground_filter/angle", m_groundFilterAngle, m_groundFilterAngle); 100 | // distance of found plane from z=0 to be detected as ground (e.g. to exclude tables) 101 | m_nh_private.param("ground_filter/plane_distance", m_groundFilterPlaneDistance, m_groundFilterPlaneDistance); 102 | 103 | m_nh_private.param("sensor_model/max_range", m_maxRange, m_maxRange); 104 | m_nh_private.param("sensor_model/min_range", m_minRange, m_minRange); 105 | 106 | m_nh_private.param("resolution", m_res, m_res); 107 | m_nh_private.param("sensor_model/hit", probHit, 0.7); 108 | m_nh_private.param("sensor_model/miss", probMiss, 0.4); 109 | m_nh_private.param("sensor_model/min", thresMin, 0.12); 110 | m_nh_private.param("sensor_model/max", thresMax, 0.97); 111 | m_nh_private.param("compress_map", m_compressMap, m_compressMap); 112 | m_nh_private.param("incremental_2D_projection", m_incrementalUpdate, m_incrementalUpdate); 113 | 114 | if (m_filterGroundPlane && (m_pointcloudMinZ > 0.0 || m_pointcloudMaxZ < 0.0)){ 115 | ROS_WARN_STREAM("You enabled ground filtering but incoming pointclouds will be pre-filtered in [" 116 | <setProbHit(probHit); 136 | m_octree->setProbMiss(probMiss); 137 | m_octree->setClampingThresMin(thresMin); 138 | m_octree->setClampingThresMax(thresMax); 139 | m_treeDepth = m_octree->getTreeDepth(); 140 | m_maxTreeDepth = m_treeDepth; 141 | m_gridmap.info.resolution = m_res; 142 | 143 | double r, g, b, a; 144 | m_nh_private.param("color/r", r, 0.0); 145 | m_nh_private.param("color/g", g, 0.0); 146 | m_nh_private.param("color/b", b, 1.0); 147 | m_nh_private.param("color/a", a, 1.0); 148 | m_color.r = r; 149 | m_color.g = g; 150 | m_color.b = b; 151 | m_color.a = a; 152 | 153 | m_nh_private.param("color_free/r", r, 0.0); 154 | m_nh_private.param("color_free/g", g, 1.0); 155 | m_nh_private.param("color_free/b", b, 0.0); 156 | m_nh_private.param("color_free/a", a, 1.0); 157 | m_colorFree.r = r; 158 | m_colorFree.g = g; 159 | m_colorFree.b = b; 160 | m_colorFree.a = a; 161 | 162 | m_nh_private.param("publish_free_space", m_publishFreeSpace, m_publishFreeSpace); 163 | 164 | m_nh_private.param("latch", m_latchedTopics, m_latchedTopics); 165 | if (m_latchedTopics){ 166 | ROS_INFO("Publishing latched (single publish will take longer, all topics are prepared)"); 167 | } else 168 | ROS_INFO("Publishing non-latched (topics are only prepared as needed, will only be re-published on map change"); 169 | 170 | m_markerPub = m_nh.advertise("occupied_cells_vis_array", 1, m_latchedTopics); 171 | m_binaryMapPub = m_nh.advertise("octomap_binary", 1, m_latchedTopics); 172 | m_fullMapPub = m_nh.advertise("octomap_full", 1, m_latchedTopics); 173 | m_pointCloudPub = m_nh.advertise("octomap_point_cloud_centers", 1, m_latchedTopics); 174 | m_mapPub = m_nh.advertise("projected_map", 5, m_latchedTopics); 175 | m_fmarkerPub = m_nh.advertise("free_cells_vis_array", 1, m_latchedTopics); 176 | 177 | m_pointCloudSub = new message_filters::Subscriber (m_nh, "cloud_in", 5); 178 | m_tfPointCloudSub = new tf::MessageFilter (*m_pointCloudSub, m_tfListener, m_worldFrameId, 5); 179 | m_tfPointCloudSub->registerCallback(boost::bind(&OctomapServer::insertCloudCallback, this, boost::placeholders::_1)); 180 | 181 | m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServer::octomapBinarySrv, this); 182 | m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServer::octomapFullSrv, this); 183 | m_clearBBXService = m_nh_private.advertiseService("clear_bbx", &OctomapServer::clearBBXSrv, this); 184 | m_resetService = m_nh_private.advertiseService("reset", &OctomapServer::resetSrv, this); 185 | 186 | dynamic_reconfigure::Server::CallbackType f; 187 | f = boost::bind(&OctomapServer::reconfigureCallback, this, boost::placeholders::_1, boost::placeholders::_2); 188 | m_reconfigureServer.setCallback(f); 189 | } 190 | 191 | OctomapServer::~OctomapServer(){ 192 | if (m_tfPointCloudSub){ 193 | delete m_tfPointCloudSub; 194 | m_tfPointCloudSub = NULL; 195 | } 196 | 197 | if (m_pointCloudSub){ 198 | delete m_pointCloudSub; 199 | m_pointCloudSub = NULL; 200 | } 201 | 202 | 203 | if (m_octree){ 204 | delete m_octree; 205 | m_octree = NULL; 206 | } 207 | 208 | } 209 | 210 | bool OctomapServer::openFile(const std::string& filename){ 211 | if (filename.length() <= 3) 212 | return false; 213 | 214 | std::string suffix = filename.substr(filename.length()-3, 3); 215 | if (suffix== ".bt"){ 216 | if (!m_octree->readBinary(filename)){ 217 | return false; 218 | } 219 | } else if (suffix == ".ot"){ 220 | AbstractOcTree* tree = AbstractOcTree::read(filename); 221 | if (!tree){ 222 | return false; 223 | } 224 | if (m_octree){ 225 | delete m_octree; 226 | m_octree = NULL; 227 | } 228 | m_octree = dynamic_cast(tree); 229 | if (!m_octree){ 230 | ROS_ERROR("Could not read OcTree in file, currently there are no other types supported in .ot"); 231 | return false; 232 | } 233 | 234 | } else{ 235 | return false; 236 | } 237 | 238 | ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(),m_octree->size()); 239 | 240 | m_treeDepth = m_octree->getTreeDepth(); 241 | m_maxTreeDepth = m_treeDepth; 242 | m_res = m_octree->getResolution(); 243 | m_gridmap.info.resolution = m_res; 244 | double minX, minY, minZ; 245 | double maxX, maxY, maxZ; 246 | m_octree->getMetricMin(minX, minY, minZ); 247 | m_octree->getMetricMax(maxX, maxY, maxZ); 248 | 249 | m_updateBBXMin[0] = m_octree->coordToKey(minX); 250 | m_updateBBXMin[1] = m_octree->coordToKey(minY); 251 | m_updateBBXMin[2] = m_octree->coordToKey(minZ); 252 | 253 | m_updateBBXMax[0] = m_octree->coordToKey(maxX); 254 | m_updateBBXMax[1] = m_octree->coordToKey(maxY); 255 | m_updateBBXMax[2] = m_octree->coordToKey(maxZ); 256 | 257 | publishAll(); 258 | 259 | return true; 260 | 261 | } 262 | 263 | void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){ 264 | ros::WallTime startTime = ros::WallTime::now(); 265 | 266 | 267 | // 268 | // ground filtering in base frame 269 | // 270 | PCLPointCloud pc; // input cloud for filtering and ground-detection 271 | pcl::fromROSMsg(*cloud, pc); 272 | 273 | tf::StampedTransform sensorToWorldTf; 274 | try { 275 | m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); 276 | } catch(tf::TransformException& ex){ 277 | ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback"); 278 | return; 279 | } 280 | 281 | Eigen::Matrix4f sensorToWorld; 282 | pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); 283 | 284 | 285 | // set up filter for height range, also removes NANs: 286 | pcl::PassThrough pass_x; 287 | pass_x.setFilterFieldName("x"); 288 | pass_x.setFilterLimits(m_pointcloudMinX, m_pointcloudMaxX); 289 | pcl::PassThrough pass_y; 290 | pass_y.setFilterFieldName("y"); 291 | pass_y.setFilterLimits(m_pointcloudMinY, m_pointcloudMaxY); 292 | pcl::PassThrough pass_z; 293 | pass_z.setFilterFieldName("z"); 294 | pass_z.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); 295 | 296 | PCLPointCloud pc_ground; // segmented ground plane 297 | PCLPointCloud pc_nonground; // everything else 298 | 299 | if (m_filterGroundPlane){ 300 | tf::StampedTransform sensorToBaseTf, baseToWorldTf; 301 | try{ 302 | m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2)); 303 | m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf); 304 | m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf); 305 | 306 | 307 | }catch(tf::TransformException& ex){ 308 | ROS_ERROR_STREAM( "Transform error for ground plane filter: " << ex.what() << ", quitting callback.\n" 309 | "You need to set the base_frame_id or disable filter_ground."); 310 | } 311 | 312 | 313 | Eigen::Matrix4f sensorToBase, baseToWorld; 314 | pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase); 315 | pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld); 316 | 317 | // transform pointcloud from sensor frame to fixed robot frame 318 | pcl::transformPointCloud(pc, pc, sensorToBase); 319 | pass_x.setInputCloud(pc.makeShared()); 320 | pass_x.filter(pc); 321 | pass_y.setInputCloud(pc.makeShared()); 322 | pass_y.filter(pc); 323 | pass_z.setInputCloud(pc.makeShared()); 324 | pass_z.filter(pc); 325 | filterGroundPlane(pc, pc_ground, pc_nonground); 326 | 327 | // transform clouds to world frame for insertion 328 | pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld); 329 | pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld); 330 | } else { 331 | // directly transform to map frame: 332 | pcl::transformPointCloud(pc, pc, sensorToWorld); 333 | 334 | // just filter height range: 335 | pass_x.setInputCloud(pc.makeShared()); 336 | pass_x.filter(pc); 337 | pass_y.setInputCloud(pc.makeShared()); 338 | pass_y.filter(pc); 339 | pass_z.setInputCloud(pc.makeShared()); 340 | pass_z.filter(pc); 341 | 342 | pc_nonground = pc; 343 | // pc_nonground is empty without ground segmentation 344 | pc_ground.header = pc.header; 345 | pc_nonground.header = pc.header; 346 | } 347 | 348 | 349 | insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground); 350 | 351 | double total_elapsed = (ros::WallTime::now() - startTime).toSec(); 352 | ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground.size(), pc_nonground.size(), total_elapsed); 353 | 354 | publishAll(cloud->header.stamp); 355 | } 356 | 357 | void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){ 358 | point3d sensorOrigin = pointTfToOctomap(sensorOriginTf); 359 | 360 | if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin) 361 | || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax)) 362 | { 363 | ROS_ERROR_STREAM("Could not generate Key for origin "<x, it->y, it->z); 375 | 376 | if ((m_minRange > 0) && (point - sensorOrigin).norm() < m_minRange) continue; 377 | 378 | // maxrange check 379 | if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) { 380 | point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange; 381 | } 382 | 383 | // only clear space (ground points) 384 | if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){ 385 | free_cells.insert(m_keyRay.begin(), m_keyRay.end()); 386 | } 387 | 388 | octomap::OcTreeKey endKey; 389 | if (m_octree->coordToKeyChecked(point, endKey)){ 390 | updateMinKey(endKey, m_updateBBXMin); 391 | updateMaxKey(endKey, m_updateBBXMax); 392 | } else{ 393 | ROS_ERROR_STREAM("Could not generate Key for endpoint "<x, it->y, it->z); 400 | 401 | if ((m_minRange > 0) && (point - sensorOrigin).norm() < m_minRange) continue; 402 | 403 | // maxrange check 404 | if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) { 405 | 406 | // free cells 407 | if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){ 408 | free_cells.insert(m_keyRay.begin(), m_keyRay.end()); 409 | } 410 | // occupied endpoint 411 | OcTreeKey key; 412 | if (m_octree->coordToKeyChecked(point, key)){ 413 | occupied_cells.insert(key); 414 | 415 | updateMinKey(key, m_updateBBXMin); 416 | updateMaxKey(key, m_updateBBXMax); 417 | 418 | #ifdef COLOR_OCTOMAP_SERVER // NB: Only read and interpret color if it's an occupied node 419 | m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/it->r, /*g=*/it->g, /*b=*/it->b); 420 | #endif 421 | } 422 | } else {// ray longer than maxrange:; 423 | point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange; 424 | if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){ 425 | free_cells.insert(m_keyRay.begin(), m_keyRay.end()); 426 | 427 | octomap::OcTreeKey endKey; 428 | if (m_octree->coordToKeyChecked(new_end, endKey)){ 429 | free_cells.insert(endKey); 430 | updateMinKey(endKey, m_updateBBXMin); 431 | updateMaxKey(endKey, m_updateBBXMax); 432 | } else{ 433 | ROS_ERROR_STREAM("Could not generate Key for endpoint "<updateNode(*it, false); 445 | } 446 | } 447 | 448 | // now mark all occupied cells: 449 | for (KeySet::iterator it = occupied_cells.begin(), end=occupied_cells.end(); it!= end; it++) { 450 | m_octree->updateNode(*it, true); 451 | } 452 | 453 | // TODO: eval lazy+updateInner vs. proper insertion 454 | // non-lazy by default (updateInnerOccupancy() too slow for large maps) 455 | //m_octree->updateInnerOccupancy(); 456 | octomap::point3d minPt, maxPt; 457 | ROS_DEBUG_STREAM("Bounding box keys (before): " << m_updateBBXMin[0] << " " <getNodeSize( m_maxTreeDepth ) - 1; 465 | // tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; 466 | // tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; 467 | // m_updateBBXMin = tmpMin; 468 | // m_updateBBXMax = tmpMax; 469 | // } 470 | 471 | // TODO: we could also limit the bbx to be within the map bounds here (see publishing check) 472 | minPt = m_octree->keyToCoord(m_updateBBXMin); 473 | maxPt = m_octree->keyToCoord(m_updateBBXMax); 474 | ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<prune(); 479 | 480 | #ifdef COLOR_OCTOMAP_SERVER 481 | if (colors) 482 | { 483 | delete[] colors; 484 | colors = NULL; 485 | } 486 | #endif 487 | } 488 | 489 | void OctomapServer::publishProjected2DMap(const ros::Time& rostime) { 490 | m_publish2DMap = (m_latchedTopics || m_mapPub.getNumSubscribers() > 0); 491 | if (m_publish2DMap) { 492 | m_gridmap.header.stamp = rostime; 493 | m_mapPub.publish(m_gridmap); 494 | } 495 | } 496 | 497 | void OctomapServer::publishAll(const ros::Time& rostime){ 498 | ros::WallTime startTime = ros::WallTime::now(); 499 | size_t octomapSize = m_octree->size(); 500 | // TODO: estimate num occ. voxels for size of arrays (reserve) 501 | if (octomapSize <= 1){ 502 | ROS_WARN("Nothing to publish, octree is empty"); 503 | return; 504 | } 505 | 506 | bool publishFreeMarkerArray = m_publishFreeSpace && (m_latchedTopics || m_fmarkerPub.getNumSubscribers() > 0); 507 | bool publishMarkerArray = (m_latchedTopics || m_markerPub.getNumSubscribers() > 0); 508 | bool publishPointCloud = (m_latchedTopics || m_pointCloudPub.getNumSubscribers() > 0); 509 | bool publishBinaryMap = (m_latchedTopics || m_binaryMapPub.getNumSubscribers() > 0); 510 | bool publishFullMap = (m_latchedTopics || m_fullMapPub.getNumSubscribers() > 0); 511 | 512 | // init markers for free space: 513 | visualization_msgs::MarkerArray freeNodesVis; 514 | // each array stores all cubes of a different size, one for each depth level: 515 | freeNodesVis.markers.resize(m_treeDepth+1); 516 | 517 | geometry_msgs::Pose pose; 518 | pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 519 | 520 | // init markers: 521 | visualization_msgs::MarkerArray occupiedNodesVis; 522 | // each array stores all cubes of a different size, one for each depth level: 523 | occupiedNodesVis.markers.resize(m_treeDepth+1); 524 | 525 | // init pointcloud: 526 | pcl::PointCloud pclCloud; 527 | 528 | // call pre-traversal hook: 529 | handlePreNodeTraversal(rostime); 530 | 531 | // now, traverse all leafs in the tree: 532 | for (OcTreeT::iterator it = m_octree->begin(m_maxTreeDepth), 533 | end = m_octree->end(); it != end; ++it) 534 | { 535 | bool inUpdateBBX = isInUpdateBBX(it); 536 | 537 | // call general hook: 538 | handleNode(it); 539 | if (inUpdateBBX) 540 | handleNodeInBBX(it); 541 | 542 | if (m_octree->isNodeOccupied(*it)){ 543 | double z = it.getZ(); 544 | double half_size = it.getSize() / 2.0; 545 | if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) 546 | { 547 | double size = it.getSize(); 548 | double x = it.getX(); 549 | double y = it.getY(); 550 | #ifdef COLOR_OCTOMAP_SERVER 551 | int r = it->getColor().r; 552 | int g = it->getColor().g; 553 | int b = it->getColor().b; 554 | #endif 555 | 556 | // Ignore speckles in the map: 557 | if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && isSpeckleNode(it.getKey())){ 558 | ROS_DEBUG("Ignoring single speckle at (%f,%f,%f)", x, y, z); 559 | continue; 560 | } // else: current octree node is no speckle, send it out 561 | 562 | handleOccupiedNode(it); 563 | if (inUpdateBBX) 564 | handleOccupiedNodeInBBX(it); 565 | 566 | 567 | //create marker: 568 | if (publishMarkerArray){ 569 | unsigned idx = it.getDepth(); 570 | assert(idx < occupiedNodesVis.markers.size()); 571 | 572 | geometry_msgs::Point cubeCenter; 573 | cubeCenter.x = x; 574 | cubeCenter.y = y; 575 | cubeCenter.z = z; 576 | 577 | occupiedNodesVis.markers[idx].points.push_back(cubeCenter); 578 | if (m_useHeightMap){ 579 | double minX, minY, minZ, maxX, maxY, maxZ; 580 | m_octree->getMetricMin(minX, minY, minZ); 581 | m_octree->getMetricMax(maxX, maxY, maxZ); 582 | 583 | double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor; 584 | occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h)); 585 | } 586 | 587 | #ifdef COLOR_OCTOMAP_SERVER 588 | if (m_useColoredMap) { 589 | std_msgs::ColorRGBA _color; _color.r = (r / 255.); _color.g = (g / 255.); _color.b = (b / 255.); _color.a = 1.0; // TODO/EVALUATE: potentially use occupancy as measure for alpha channel? 590 | occupiedNodesVis.markers[idx].colors.push_back(_color); 591 | } 592 | #endif 593 | } 594 | 595 | // insert into pointcloud: 596 | if (publishPointCloud) { 597 | #ifdef COLOR_OCTOMAP_SERVER 598 | PCLPoint _point = PCLPoint(); 599 | _point.x = x; _point.y = y; _point.z = z; 600 | _point.r = r; _point.g = g; _point.b = b; 601 | pclCloud.push_back(_point); 602 | #else 603 | pclCloud.push_back(PCLPoint(x, y, z)); 604 | #endif 605 | } 606 | 607 | } 608 | } else{ // node not occupied => mark as free in 2D map if unknown so far 609 | double z = it.getZ(); 610 | double half_size = it.getSize() / 2.0; 611 | if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) 612 | { 613 | handleFreeNode(it); 614 | if (inUpdateBBX) 615 | handleFreeNodeInBBX(it); 616 | 617 | if (m_publishFreeSpace){ 618 | double x = it.getX(); 619 | double y = it.getY(); 620 | 621 | //create marker for free space: 622 | if (publishFreeMarkerArray){ 623 | unsigned idx = it.getDepth(); 624 | assert(idx < freeNodesVis.markers.size()); 625 | 626 | geometry_msgs::Point cubeCenter; 627 | cubeCenter.x = x; 628 | cubeCenter.y = y; 629 | cubeCenter.z = z; 630 | 631 | freeNodesVis.markers[idx].points.push_back(cubeCenter); 632 | } 633 | } 634 | 635 | } 636 | } 637 | } 638 | 639 | // call post-traversal hook: 640 | handlePostNodeTraversal(rostime); 641 | 642 | // finish MarkerArray: 643 | if (publishMarkerArray){ 644 | for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){ 645 | double size = m_octree->getNodeSize(i); 646 | 647 | occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; 648 | occupiedNodesVis.markers[i].header.stamp = rostime; 649 | occupiedNodesVis.markers[i].ns = "map"; 650 | occupiedNodesVis.markers[i].id = i; 651 | occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; 652 | occupiedNodesVis.markers[i].scale.x = size; 653 | occupiedNodesVis.markers[i].scale.y = size; 654 | occupiedNodesVis.markers[i].scale.z = size; 655 | occupiedNodesVis.markers[i].pose.orientation.x=0; 656 | occupiedNodesVis.markers[i].pose.orientation.y=0; 657 | occupiedNodesVis.markers[i].pose.orientation.z=0; 658 | occupiedNodesVis.markers[i].pose.orientation.w=1; 659 | if (!m_useColoredMap) 660 | occupiedNodesVis.markers[i].color = m_color; 661 | 662 | 663 | if (occupiedNodesVis.markers[i].points.size() > 0) 664 | occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD; 665 | else 666 | occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; 667 | } 668 | 669 | m_markerPub.publish(occupiedNodesVis); 670 | } 671 | 672 | 673 | // finish FreeMarkerArray: 674 | if (publishFreeMarkerArray){ 675 | for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){ 676 | double size = m_octree->getNodeSize(i); 677 | 678 | freeNodesVis.markers[i].header.frame_id = m_worldFrameId; 679 | freeNodesVis.markers[i].header.stamp = rostime; 680 | freeNodesVis.markers[i].ns = "map"; 681 | freeNodesVis.markers[i].id = i; 682 | freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; 683 | freeNodesVis.markers[i].scale.x = size; 684 | freeNodesVis.markers[i].scale.y = size; 685 | freeNodesVis.markers[i].scale.z = size; 686 | freeNodesVis.markers[i].color = m_colorFree; 687 | 688 | 689 | if (freeNodesVis.markers[i].points.size() > 0) 690 | freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD; 691 | else 692 | freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; 693 | } 694 | 695 | m_fmarkerPub.publish(freeNodesVis); 696 | } 697 | 698 | 699 | // finish pointcloud: 700 | if (publishPointCloud){ 701 | sensor_msgs::PointCloud2 cloud; 702 | pcl::toROSMsg (pclCloud, cloud); 703 | cloud.header.frame_id = m_worldFrameId; 704 | cloud.header.stamp = rostime; 705 | m_pointCloudPub.publish(cloud); 706 | } 707 | 708 | if (publishBinaryMap) 709 | publishBinaryOctoMap(rostime); 710 | 711 | if (publishFullMap) 712 | publishFullOctoMap(rostime); 713 | 714 | 715 | double total_elapsed = (ros::WallTime::now() - startTime).toSec(); 716 | ROS_DEBUG("Map publishing in OctomapServer took %f sec", total_elapsed); 717 | } 718 | 719 | bool OctomapServer::octomapBinarySrv(OctomapSrv::Request &req, 720 | OctomapSrv::Response &res) 721 | { 722 | ros::WallTime startTime = ros::WallTime::now(); 723 | ROS_INFO("Sending binary map data on service request"); 724 | res.map.header.frame_id = m_worldFrameId; 725 | res.map.header.stamp = ros::Time::now(); 726 | if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map)) 727 | return false; 728 | 729 | double total_elapsed = (ros::WallTime::now() - startTime).toSec(); 730 | ROS_INFO("Binary octomap sent in %f sec", total_elapsed); 731 | return true; 732 | } 733 | 734 | bool OctomapServer::octomapFullSrv(OctomapSrv::Request &req, 735 | OctomapSrv::Response &res) 736 | { 737 | ROS_INFO("Sending full map data on service request"); 738 | res.map.header.frame_id = m_worldFrameId; 739 | res.map.header.stamp = ros::Time::now(); 740 | 741 | 742 | if (!octomap_msgs::fullMapToMsg(*m_octree, res.map)) 743 | return false; 744 | 745 | return true; 746 | } 747 | 748 | bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){ 749 | point3d min = pointMsgToOctomap(req.min); 750 | point3d max = pointMsgToOctomap(req.max); 751 | 752 | double thresMin = m_octree->getClampingThresMin(); 753 | for(OcTreeT::leaf_bbx_iterator it = m_octree->begin_leafs_bbx(min,max), 754 | end=m_octree->end_leafs_bbx(); it!= end; ++it){ 755 | 756 | it->setLogOdds(octomap::logodds(thresMin)); 757 | // m_octree->updateNode(it.getKey(), -6.0f); 758 | } 759 | // TODO: eval which is faster (setLogOdds+updateInner or updateNode) 760 | m_octree->updateInnerOccupancy(); 761 | 762 | publishAll(ros::Time::now()); 763 | 764 | return true; 765 | } 766 | 767 | bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp) { 768 | visualization_msgs::MarkerArray occupiedNodesVis; 769 | occupiedNodesVis.markers.resize(m_treeDepth +1); 770 | ros::Time rostime = ros::Time::now(); 771 | m_octree->clear(); 772 | // clear 2D map: 773 | m_gridmap.data.clear(); 774 | m_gridmap.info.height = 0.0; 775 | m_gridmap.info.width = 0.0; 776 | m_gridmap.info.resolution = 0.0; 777 | m_gridmap.info.origin.position.x = 0.0; 778 | m_gridmap.info.origin.position.y = 0.0; 779 | 780 | ROS_INFO("Cleared octomap"); 781 | publishAll(rostime); // Note: This will return as the octree is empty 782 | 783 | publishProjected2DMap(rostime); 784 | 785 | publishBinaryOctoMap(rostime); 786 | 787 | for (std::size_t i = 0; i < occupiedNodesVis.markers.size(); ++i){ 788 | occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; 789 | occupiedNodesVis.markers[i].header.stamp = rostime; 790 | occupiedNodesVis.markers[i].ns = "map"; 791 | occupiedNodesVis.markers[i].id = i; 792 | occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; 793 | occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; 794 | } 795 | m_markerPub.publish(occupiedNodesVis); 796 | 797 | visualization_msgs::MarkerArray freeNodesVis; 798 | freeNodesVis.markers.resize(m_treeDepth +1); 799 | for (std::size_t i = 0; i < freeNodesVis.markers.size(); ++i){ 800 | freeNodesVis.markers[i].header.frame_id = m_worldFrameId; 801 | freeNodesVis.markers[i].header.stamp = rostime; 802 | freeNodesVis.markers[i].ns = "map"; 803 | freeNodesVis.markers[i].id = i; 804 | freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; 805 | freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; 806 | } 807 | m_fmarkerPub.publish(freeNodesVis); 808 | 809 | return true; 810 | } 811 | 812 | void OctomapServer::publishBinaryOctoMap(const ros::Time& rostime) const{ 813 | 814 | Octomap map; 815 | map.header.frame_id = m_worldFrameId; 816 | map.header.stamp = rostime; 817 | 818 | if (octomap_msgs::binaryMapToMsg(*m_octree, map)) 819 | m_binaryMapPub.publish(map); 820 | else 821 | ROS_ERROR("Error serializing OctoMap"); 822 | } 823 | 824 | void OctomapServer::publishFullOctoMap(const ros::Time& rostime) const{ 825 | 826 | Octomap map; 827 | map.header.frame_id = m_worldFrameId; 828 | map.header.stamp = rostime; 829 | 830 | if (octomap_msgs::fullMapToMsg(*m_octree, map)) 831 | m_fullMapPub.publish(map); 832 | else 833 | ROS_ERROR("Error serializing OctoMap"); 834 | 835 | } 836 | 837 | 838 | void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const{ 839 | ground.header = pc.header; 840 | nonground.header = pc.header; 841 | 842 | if (pc.size() < 50){ 843 | ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction"); 844 | nonground = pc; 845 | } else { 846 | // plane detection for ground plane removal: 847 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 848 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 849 | 850 | // Create the segmentation object and set up: 851 | pcl::SACSegmentation seg; 852 | seg.setOptimizeCoefficients (true); 853 | // TODO: maybe a filtering based on the surface normals might be more robust / accurate? 854 | seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); 855 | seg.setMethodType(pcl::SAC_RANSAC); 856 | seg.setMaxIterations(200); 857 | seg.setDistanceThreshold (m_groundFilterDistance); 858 | seg.setAxis(Eigen::Vector3f(0,0,1)); 859 | seg.setEpsAngle(m_groundFilterAngle); 860 | 861 | 862 | PCLPointCloud cloud_filtered(pc); 863 | // Create the filtering object 864 | pcl::ExtractIndices extract; 865 | bool groundPlaneFound = false; 866 | 867 | while(cloud_filtered.size() > 10 && !groundPlaneFound){ 868 | seg.setInputCloud(cloud_filtered.makeShared()); 869 | seg.segment (*inliers, *coefficients); 870 | if (inliers->indices.size () == 0){ 871 | ROS_INFO("PCL segmentation did not find any plane."); 872 | 873 | break; 874 | } 875 | 876 | extract.setInputCloud(cloud_filtered.makeShared()); 877 | extract.setIndices(inliers); 878 | 879 | if (std::abs(coefficients->values.at(3)) < m_groundFilterPlaneDistance){ 880 | ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(), 881 | coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3)); 882 | extract.setNegative (false); 883 | extract.filter (ground); 884 | 885 | // remove ground points from full pointcloud: 886 | // workaround for PCL bug: 887 | if(inliers->indices.size() != cloud_filtered.size()){ 888 | extract.setNegative(true); 889 | PCLPointCloud cloud_out; 890 | extract.filter(cloud_out); 891 | nonground += cloud_out; 892 | cloud_filtered = cloud_out; 893 | } 894 | 895 | groundPlaneFound = true; 896 | } else{ 897 | ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(), 898 | coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3)); 899 | pcl::PointCloud cloud_out; 900 | extract.setNegative (false); 901 | extract.filter(cloud_out); 902 | nonground +=cloud_out; 903 | // debug 904 | // pcl::PCDWriter writer; 905 | // writer.write("nonground_plane.pcd",cloud_out, false); 906 | 907 | // remove current plane from scan for next iteration: 908 | // workaround for PCL bug: 909 | if(inliers->indices.size() != cloud_filtered.size()){ 910 | extract.setNegative(true); 911 | cloud_out.points.clear(); 912 | extract.filter(cloud_out); 913 | cloud_filtered = cloud_out; 914 | } else{ 915 | cloud_filtered.points.clear(); 916 | } 917 | } 918 | 919 | } 920 | // TODO: also do this if overall starting pointcloud too small? 921 | if (!groundPlaneFound){ // no plane found or remaining points too small 922 | ROS_WARN("No ground plane found in scan"); 923 | 924 | // do a rough fitlering on height to prevent spurious obstacles 925 | pcl::PassThrough second_pass; 926 | second_pass.setFilterFieldName("z"); 927 | second_pass.setFilterLimits(-m_groundFilterPlaneDistance, m_groundFilterPlaneDistance); 928 | second_pass.setInputCloud(pc.makeShared()); 929 | second_pass.filter(ground); 930 | 931 | second_pass.setNegative (true); 932 | second_pass.filter(nonground); 933 | } 934 | 935 | // debug: 936 | // pcl::PCDWriter writer; 937 | // if (pc_ground.size() > 0) 938 | // writer.write("ground.pcd",pc_ground, false); 939 | // if (pc_nonground.size() > 0) 940 | // writer.write("nonground.pcd",pc_nonground, false); 941 | 942 | } 943 | 944 | 945 | } 946 | 947 | void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ 948 | if (m_publish2DMap){ 949 | // init projected 2D map: 950 | m_gridmap.header.frame_id = m_worldFrameId; 951 | m_gridmap.header.stamp = rostime; 952 | nav_msgs::MapMetaData oldMapInfo = m_gridmap.info; 953 | 954 | // TODO: move most of this stuff into c'tor and init map only once (adjust if size changes) 955 | double minX, minY, minZ, maxX, maxY, maxZ; 956 | m_octree->getMetricMin(minX, minY, minZ); 957 | m_octree->getMetricMax(maxX, maxY, maxZ); 958 | 959 | octomap::point3d minPt(minX, minY, minZ); 960 | octomap::point3d maxPt(maxX, maxY, maxZ); 961 | octomap::OcTreeKey minKey = m_octree->coordToKey(minPt, m_maxTreeDepth); 962 | octomap::OcTreeKey maxKey = m_octree->coordToKey(maxPt, m_maxTreeDepth); 963 | 964 | ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]); 965 | 966 | // add padding if requested (= new min/maxPts in x&y): 967 | double halfPaddedX = 0.5*m_minSizeX; 968 | double halfPaddedY = 0.5*m_minSizeY; 969 | minX = std::min(minX, -halfPaddedX); 970 | maxX = std::max(maxX, halfPaddedX); 971 | minY = std::min(minY, -halfPaddedY); 972 | maxY = std::max(maxY, halfPaddedY); 973 | minPt = octomap::point3d(minX, minY, minZ); 974 | maxPt = octomap::point3d(maxX, maxY, maxZ); 975 | 976 | OcTreeKey paddedMaxKey; 977 | if (!m_octree->coordToKeyChecked(minPt, m_maxTreeDepth, m_paddedMinKey)){ 978 | ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z()); 979 | return; 980 | } 981 | if (!m_octree->coordToKeyChecked(maxPt, m_maxTreeDepth, paddedMaxKey)){ 982 | ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z()); 983 | return; 984 | } 985 | 986 | ROS_DEBUG("Padded MinKey: %d %d %d / padded MaxKey: %d %d %d", m_paddedMinKey[0], m_paddedMinKey[1], m_paddedMinKey[2], paddedMaxKey[0], paddedMaxKey[1], paddedMaxKey[2]); 987 | assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]); 988 | 989 | m_multires2DScale = 1 << (m_treeDepth - m_maxTreeDepth); 990 | m_gridmap.info.width = (paddedMaxKey[0] - m_paddedMinKey[0])/m_multires2DScale +1; 991 | m_gridmap.info.height = (paddedMaxKey[1] - m_paddedMinKey[1])/m_multires2DScale +1; 992 | 993 | int mapOriginX = minKey[0] - m_paddedMinKey[0]; 994 | int mapOriginY = minKey[1] - m_paddedMinKey[1]; 995 | assert(mapOriginX >= 0 && mapOriginY >= 0); 996 | 997 | // might not exactly be min / max of octree: 998 | octomap::point3d origin = m_octree->keyToCoord(m_paddedMinKey, m_treeDepth); 999 | double gridRes = m_octree->getNodeSize(m_maxTreeDepth); 1000 | m_projectCompleteMap = (!m_incrementalUpdate || (std::abs(gridRes-m_gridmap.info.resolution) > 1e-6)); 1001 | m_gridmap.info.resolution = gridRes; 1002 | m_gridmap.info.origin.position.x = origin.x() - gridRes*0.5; 1003 | m_gridmap.info.origin.position.y = origin.y() - gridRes*0.5; 1004 | if (m_maxTreeDepth != m_treeDepth){ 1005 | m_gridmap.info.origin.position.x -= m_res/2.0; 1006 | m_gridmap.info.origin.position.y -= m_res/2.0; 1007 | } 1008 | 1009 | // workaround for multires. projection not working properly for inner nodes: 1010 | // force re-building complete map 1011 | if (m_maxTreeDepth < m_treeDepth) 1012 | m_projectCompleteMap = true; 1013 | 1014 | 1015 | if(m_projectCompleteMap){ 1016 | ROS_DEBUG("Rebuilding complete 2D map"); 1017 | m_gridmap.data.clear(); 1018 | // init to unknown: 1019 | m_gridmap.data.resize(m_gridmap.info.width * m_gridmap.info.height, -1); 1020 | 1021 | } else { 1022 | 1023 | if (mapChanged(oldMapInfo, m_gridmap.info)){ 1024 | ROS_DEBUG("2D grid map size changed to %dx%d", m_gridmap.info.width, m_gridmap.info.height); 1025 | adjustMapData(m_gridmap, oldMapInfo); 1026 | } 1027 | nav_msgs::OccupancyGrid::_data_type::iterator startIt; 1028 | size_t mapUpdateBBXMinX = std::max(0, (int(m_updateBBXMin[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale)); 1029 | size_t mapUpdateBBXMinY = std::max(0, (int(m_updateBBXMin[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale)); 1030 | size_t mapUpdateBBXMaxX = std::min(int(m_gridmap.info.width-1), (int(m_updateBBXMax[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale)); 1031 | size_t mapUpdateBBXMaxY = std::min(int(m_gridmap.info.height-1), (int(m_updateBBXMax[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale)); 1032 | 1033 | assert(mapUpdateBBXMaxX > mapUpdateBBXMinX); 1034 | assert(mapUpdateBBXMaxY > mapUpdateBBXMinY); 1035 | 1036 | size_t numCols = mapUpdateBBXMaxX-mapUpdateBBXMinX +1; 1037 | 1038 | // test for max idx: 1039 | uint max_idx = m_gridmap.info.width*mapUpdateBBXMaxY + mapUpdateBBXMaxX; 1040 | if (max_idx >= m_gridmap.data.size()) 1041 | ROS_ERROR("BBX index not valid: %d (max index %zu for size %d x %d) update-BBX is: [%zu %zu]-[%zu %zu]", max_idx, m_gridmap.data.size(), m_gridmap.info.width, m_gridmap.info.height, mapUpdateBBXMinX, mapUpdateBBXMinY, mapUpdateBBXMaxX, mapUpdateBBXMaxY); 1042 | 1043 | // reset proj. 2D map in bounding box: 1044 | for (unsigned int j = mapUpdateBBXMinY; j <= mapUpdateBBXMaxY; ++j){ 1045 | std::fill_n(m_gridmap.data.begin() + m_gridmap.info.width*j+mapUpdateBBXMinX, 1046 | numCols, -1); 1047 | } 1048 | 1049 | } 1050 | 1051 | 1052 | 1053 | } 1054 | 1055 | } 1056 | 1057 | void OctomapServer::handlePostNodeTraversal(const ros::Time& rostime){ 1058 | publishProjected2DMap(rostime); 1059 | } 1060 | 1061 | void OctomapServer::handleOccupiedNode(const OcTreeT::iterator& it){ 1062 | if (m_publish2DMap && m_projectCompleteMap){ 1063 | update2DMap(it, true); 1064 | } 1065 | } 1066 | 1067 | void OctomapServer::handleFreeNode(const OcTreeT::iterator& it){ 1068 | if (m_publish2DMap && m_projectCompleteMap){ 1069 | update2DMap(it, false); 1070 | } 1071 | } 1072 | 1073 | void OctomapServer::handleOccupiedNodeInBBX(const OcTreeT::iterator& it){ 1074 | if (m_publish2DMap && !m_projectCompleteMap){ 1075 | update2DMap(it, true); 1076 | } 1077 | } 1078 | 1079 | void OctomapServer::handleFreeNodeInBBX(const OcTreeT::iterator& it){ 1080 | if (m_publish2DMap && !m_projectCompleteMap){ 1081 | update2DMap(it, false); 1082 | } 1083 | } 1084 | 1085 | void OctomapServer::update2DMap(const OcTreeT::iterator& it, bool occupied){ 1086 | // update 2D map (occupied always overrides): 1087 | 1088 | if (it.getDepth() == m_maxTreeDepth){ 1089 | unsigned idx = mapIdx(it.getKey()); 1090 | if (occupied) 1091 | m_gridmap.data[mapIdx(it.getKey())] = 100; 1092 | else if (m_gridmap.data[idx] == -1){ 1093 | m_gridmap.data[idx] = 0; 1094 | } 1095 | 1096 | } else{ 1097 | int intSize = 1 << (m_maxTreeDepth - it.getDepth()); 1098 | octomap::OcTreeKey minKey=it.getIndexKey(); 1099 | for(int dx=0; dx < intSize; dx++){ 1100 | int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale; 1101 | for(int dy=0; dy < intSize; dy++){ 1102 | unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale); 1103 | if (occupied) 1104 | m_gridmap.data[idx] = 100; 1105 | else if (m_gridmap.data[idx] == -1){ 1106 | m_gridmap.data[idx] = 0; 1107 | } 1108 | } 1109 | } 1110 | } 1111 | 1112 | 1113 | } 1114 | 1115 | 1116 | 1117 | bool OctomapServer::isSpeckleNode(const OcTreeKey&nKey) const { 1118 | OcTreeKey key; 1119 | bool neighborFound = false; 1120 | for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] + 1; ++key[2]){ 1121 | for (key[1] = nKey[1] - 1; !neighborFound && key[1] <= nKey[1] + 1; ++key[1]){ 1122 | for (key[0] = nKey[0] - 1; !neighborFound && key[0] <= nKey[0] + 1; ++key[0]){ 1123 | if (key != nKey){ 1124 | OcTreeNode* node = m_octree->search(key); 1125 | if (node && m_octree->isNodeOccupied(node)){ 1126 | // we have a neighbor => break! 1127 | neighborFound = true; 1128 | } 1129 | } 1130 | } 1131 | } 1132 | } 1133 | 1134 | return neighborFound; 1135 | } 1136 | 1137 | void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level){ 1138 | if (m_maxTreeDepth != unsigned(config.max_depth)) 1139 | m_maxTreeDepth = unsigned(config.max_depth); 1140 | else{ 1141 | m_pointcloudMinZ = config.pointcloud_min_z; 1142 | m_pointcloudMaxZ = config.pointcloud_max_z; 1143 | m_occupancyMinZ = config.occupancy_min_z; 1144 | m_occupancyMaxZ = config.occupancy_max_z; 1145 | m_filterSpeckles = config.filter_speckles; 1146 | m_filterGroundPlane = config.filter_ground; 1147 | m_compressMap = config.compress_map; 1148 | m_incrementalUpdate = config.incremental_2D_projection; 1149 | 1150 | // Parameters with a namespace require an special treatment at the beginning, as dynamic reconfigure 1151 | // will overwrite them because the server is not able to match parameters' names. 1152 | if (m_initConfig){ 1153 | // If parameters do not have the default value, dynamic reconfigure server should be updated. 1154 | if(!is_equal(m_groundFilterDistance, 0.04)) 1155 | config.ground_filter_distance = m_groundFilterDistance; 1156 | if(!is_equal(m_groundFilterAngle, 0.15)) 1157 | config.ground_filter_angle = m_groundFilterAngle; 1158 | if(!is_equal( m_groundFilterPlaneDistance, 0.07)) 1159 | config.ground_filter_plane_distance = m_groundFilterPlaneDistance; 1160 | if(!is_equal(m_maxRange, -1.0)) 1161 | config.sensor_model_max_range = m_maxRange; 1162 | if(!is_equal(m_minRange, -1.0)) 1163 | config.sensor_model_min_range = m_minRange; 1164 | if(!is_equal(m_octree->getProbHit(), 0.7)) 1165 | config.sensor_model_hit = m_octree->getProbHit(); 1166 | if(!is_equal(m_octree->getProbMiss(), 0.4)) 1167 | config.sensor_model_miss = m_octree->getProbMiss(); 1168 | if(!is_equal(m_octree->getClampingThresMin(), 0.12)) 1169 | config.sensor_model_min = m_octree->getClampingThresMin(); 1170 | if(!is_equal(m_octree->getClampingThresMax(), 0.97)) 1171 | config.sensor_model_max = m_octree->getClampingThresMax(); 1172 | m_initConfig = false; 1173 | 1174 | boost::recursive_mutex::scoped_lock reconf_lock(m_config_mutex); 1175 | m_reconfigureServer.updateConfig(config); 1176 | } 1177 | else{ 1178 | m_groundFilterDistance = config.ground_filter_distance; 1179 | m_groundFilterAngle = config.ground_filter_angle; 1180 | m_groundFilterPlaneDistance = config.ground_filter_plane_distance; 1181 | m_maxRange = config.sensor_model_max_range; 1182 | m_octree->setClampingThresMin(config.sensor_model_min); 1183 | m_octree->setClampingThresMax(config.sensor_model_max); 1184 | 1185 | // Checking values that might create unexpected behaviors. 1186 | if (is_equal(config.sensor_model_hit, 1.0)) 1187 | config.sensor_model_hit -= 1.0e-6; 1188 | m_octree->setProbHit(config.sensor_model_hit); 1189 | if (is_equal(config.sensor_model_miss, 0.0)) 1190 | config.sensor_model_miss += 1.0e-6; 1191 | m_octree->setProbMiss(config.sensor_model_miss); 1192 | } 1193 | } 1194 | publishAll(); 1195 | } 1196 | 1197 | void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const{ 1198 | if (map.info.resolution != oldMapInfo.resolution){ 1199 | ROS_ERROR("Resolution of map changed, cannot be adjusted"); 1200 | return; 1201 | } 1202 | 1203 | int i_off = int((oldMapInfo.origin.position.x - map.info.origin.position.x)/map.info.resolution +0.5); 1204 | int j_off = int((oldMapInfo.origin.position.y - map.info.origin.position.y)/map.info.resolution +0.5); 1205 | 1206 | if (i_off < 0 || j_off < 0 1207 | || oldMapInfo.width + i_off > map.info.width 1208 | || oldMapInfo.height + j_off > map.info.height) 1209 | { 1210 | ROS_ERROR("New 2D map does not contain old map area, this case is not implemented"); 1211 | return; 1212 | } 1213 | 1214 | nav_msgs::OccupancyGrid::_data_type oldMapData = map.data; 1215 | 1216 | map.data.clear(); 1217 | // init to unknown: 1218 | map.data.resize(map.info.width * map.info.height, -1); 1219 | 1220 | nav_msgs::OccupancyGrid::_data_type::iterator fromStart, fromEnd, toStart; 1221 | 1222 | for (int j =0; j < int(oldMapInfo.height); ++j ){ 1223 | // copy chunks, row by row: 1224 | fromStart = oldMapData.begin() + j*oldMapInfo.width; 1225 | fromEnd = fromStart + oldMapInfo.width; 1226 | toStart = map.data.begin() + ((j+j_off)*m_gridmap.info.width + i_off); 1227 | copy(fromStart, fromEnd, toStart); 1228 | 1229 | // for (int i =0; i < int(oldMapInfo.width); ++i){ 1230 | // map.data[m_gridmap.info.width*(j+j_off) +i+i_off] = oldMapData[oldMapInfo.width*j +i]; 1231 | // } 1232 | 1233 | } 1234 | 1235 | } 1236 | 1237 | 1238 | std_msgs::ColorRGBA OctomapServer::heightMapColor(double h) { 1239 | 1240 | std_msgs::ColorRGBA color; 1241 | color.a = 1.0; 1242 | // blend over HSV-values (more colors) 1243 | 1244 | double s = 1.0; 1245 | double v = 1.0; 1246 | 1247 | h -= floor(h); 1248 | h *= 6; 1249 | int i; 1250 | double m, n, f; 1251 | 1252 | i = floor(h); 1253 | f = h - i; 1254 | if (!(i & 1)) 1255 | f = 1 - f; // if i is even 1256 | m = v * (1 - s); 1257 | n = v * (1 - s * f); 1258 | 1259 | switch (i) { 1260 | case 6: 1261 | case 0: 1262 | color.r = v; color.g = n; color.b = m; 1263 | break; 1264 | case 1: 1265 | color.r = n; color.g = v; color.b = m; 1266 | break; 1267 | case 2: 1268 | color.r = m; color.g = v; color.b = n; 1269 | break; 1270 | case 3: 1271 | color.r = m; color.g = n; color.b = v; 1272 | break; 1273 | case 4: 1274 | color.r = n; color.g = m; color.b = v; 1275 | break; 1276 | case 5: 1277 | color.r = v; color.g = m; color.b = n; 1278 | break; 1279 | default: 1280 | color.r = 1; color.g = 0.5; color.b = 0.5; 1281 | break; 1282 | } 1283 | 1284 | return color; 1285 | } 1286 | } 1287 | 1288 | 1289 | 1290 | -------------------------------------------------------------------------------- /octomap_server/src/OctomapServerMultilayer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011-2013, A. Hornung, M. Philips 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | using namespace octomap; 33 | 34 | namespace octomap_server{ 35 | 36 | 37 | 38 | OctomapServerMultilayer::OctomapServerMultilayer(ros::NodeHandle private_nh_) 39 | : OctomapServer(private_nh_) 40 | { 41 | 42 | // TODO: callback for arm_navigation attached objects was removed, is 43 | // there a replacement functionality? 44 | 45 | // TODO: param maps, limits 46 | // right now 0: base, 1: spine, 2: arms 47 | ProjectedMap m; 48 | m.name = "projected_base_map"; 49 | m.minZ = 0.0; 50 | m.maxZ = 0.3; 51 | m.z = 0.0; 52 | m_multiGridmap.push_back(m); 53 | 54 | m.name = "projected_spine_map"; 55 | m.minZ = 0.25; 56 | m.maxZ = 1.4; 57 | m.z = 0.6; 58 | m_multiGridmap.push_back(m); 59 | 60 | m.name = "projected_arm_map"; 61 | m.minZ = 0.7; 62 | m.maxZ = 0.9; 63 | m.z = 0.8; 64 | m_multiGridmap.push_back(m); 65 | 66 | 67 | for (unsigned i = 0; i < m_multiGridmap.size(); ++i){ 68 | ros::Publisher* pub = new ros::Publisher(m_nh.advertise(m_multiGridmap.at(i).name, 5, m_latchedTopics)); 69 | m_multiMapPub.push_back(pub); 70 | } 71 | 72 | // init arm links (could be params as well) 73 | m_armLinks.push_back("l_elbow_flex_link"); 74 | m_armLinkOffsets.push_back(0.10); 75 | m_armLinks.push_back("l_gripper_l_finger_tip_link"); 76 | m_armLinkOffsets.push_back(0.03); 77 | m_armLinks.push_back("l_gripper_r_finger_tip_link"); 78 | m_armLinkOffsets.push_back(0.03); 79 | m_armLinks.push_back("l_upper_arm_roll_link"); 80 | m_armLinkOffsets.push_back(0.16); 81 | m_armLinks.push_back("l_wrist_flex_link"); 82 | m_armLinkOffsets.push_back(0.05); 83 | m_armLinks.push_back("r_elbow_flex_link"); 84 | m_armLinkOffsets.push_back(0.10); 85 | m_armLinks.push_back("r_gripper_l_finger_tip_link"); 86 | m_armLinkOffsets.push_back(0.03); 87 | m_armLinks.push_back("r_gripper_r_finger_tip_link"); 88 | m_armLinkOffsets.push_back(0.03); 89 | m_armLinks.push_back("r_upper_arm_roll_link"); 90 | m_armLinkOffsets.push_back(0.16); 91 | m_armLinks.push_back("r_wrist_flex_link"); 92 | m_armLinkOffsets.push_back(0.05); 93 | 94 | 95 | } 96 | 97 | OctomapServerMultilayer::~OctomapServerMultilayer(){ 98 | for (unsigned i = 0; i < m_multiMapPub.size(); ++i){ 99 | delete m_multiMapPub[i]; 100 | } 101 | 102 | } 103 | 104 | void OctomapServerMultilayer::handlePreNodeTraversal(const ros::Time& rostime){ 105 | // multilayer server always publishes 2D maps: 106 | m_publish2DMap = true; 107 | nav_msgs::MapMetaData gridmapInfo = m_gridmap.info; 108 | 109 | OctomapServer::handlePreNodeTraversal(rostime); 110 | 111 | 112 | // recalculate height of arm layer (stub, TODO) 113 | geometry_msgs::PointStamped vin; 114 | vin.point.x = 0; 115 | vin.point.y = 0; 116 | vin.point.z = 0; 117 | vin.header.stamp = rostime; 118 | double link_padding = 0.03; 119 | 120 | double minArmHeight = 2.0; 121 | double maxArmHeight = 0.0; 122 | 123 | for (unsigned i = 0; i < m_armLinks.size(); ++i){ 124 | vin.header.frame_id = m_armLinks[i]; 125 | geometry_msgs::PointStamped vout; 126 | const bool found_trans = 127 | m_tfListener.waitForTransform("base_footprint", m_armLinks.at(i), 128 | ros::Time(0), ros::Duration(1.0)); 129 | ROS_ASSERT_MSG(found_trans, "Timed out waiting for transform to %s", 130 | m_armLinks[i].c_str()); 131 | m_tfListener.transformPoint("base_footprint",vin,vout); 132 | maxArmHeight = std::max(maxArmHeight, vout.point.z + (m_armLinkOffsets.at(i) + link_padding)); 133 | minArmHeight = std::min(minArmHeight, vout.point.z - (m_armLinkOffsets.at(i) + link_padding)); 134 | } 135 | ROS_INFO("Arm layer interval adjusted to (%f,%f)", minArmHeight, maxArmHeight); 136 | m_multiGridmap.at(2).minZ = minArmHeight; 137 | m_multiGridmap.at(2).maxZ = maxArmHeight; 138 | m_multiGridmap.at(2).z = (maxArmHeight+minArmHeight)/2.0; 139 | 140 | 141 | 142 | 143 | 144 | // TODO: also clear multilevel maps in BBX region (see OctomapServer.cpp)? 145 | 146 | bool mapInfoChanged = mapChanged(gridmapInfo, m_gridmap.info); 147 | 148 | for (MultilevelGrid::iterator it = m_multiGridmap.begin(); it != m_multiGridmap.end(); ++it){ 149 | it->map.header = m_gridmap.header; 150 | it->map.info = m_gridmap.info; 151 | it->map.info.origin.position.z = it->z; 152 | if (m_projectCompleteMap){ 153 | ROS_INFO("Map resolution changed, rebuilding complete 2D maps"); 154 | it->map.data.clear(); 155 | // init to unknown: 156 | it->map.data.resize(it->map.info.width * it->map.info.height, -1); 157 | } else if (mapInfoChanged){ 158 | adjustMapData(it->map, gridmapInfo); 159 | } 160 | } 161 | } 162 | 163 | void OctomapServerMultilayer::handlePostNodeTraversal(const ros::Time& rostime){ 164 | 165 | // TODO: calc tall / short obs. cells for arm layer, => temp arm layer 166 | // std::vector shortObsCells; 167 | // for(unsigned int i=0; i 0.8) 175 | // arm_map.data[i] = 101; 176 | // else{ 177 | // arm_map.data[i] = 100; 178 | // shortObsCells.push_back(i); 179 | // } 180 | // } 181 | // 182 | // std::vector tallObsCells; 183 | // tallObsCells.reserve(shortObsCells.size()); 184 | // int dxy[8] = {-arm_map.info.width-1, -arm_map.info.width, -arm_map.info.width+1, -1, 1, arm_map.info.width-1, arm_map.info.width, arm_map.info.width+1}; 185 | // for(unsigned int i=0; i=arm_map.data.size()) 189 | // continue; 190 | // if(arm_map.data[temp]==101){ 191 | // tallObsCells.push_back(shortObsCells[i]); 192 | // break; 193 | // } 194 | // } 195 | // } 196 | // for(unsigned int i=0; ipublish(m_multiGridmap.at(i).map); 205 | } 206 | 207 | } 208 | void OctomapServerMultilayer::update2DMap(const OcTreeT::iterator& it, bool occupied){ 209 | double z = it.getZ(); 210 | double s2 = it.getSize()/2.0; 211 | 212 | // create a mask on which maps to update: 213 | std::vector inMapLevel(m_multiGridmap.size(), false); 214 | for (unsigned i = 0; i < m_multiGridmap.size(); ++i){ 215 | if (z+s2 >= m_multiGridmap[i].minZ && z-s2 <= m_multiGridmap[i].maxZ){ 216 | inMapLevel[i] = true; 217 | } 218 | } 219 | 220 | if (it.getDepth() == m_maxTreeDepth){ 221 | unsigned idx = mapIdx(it.getKey()); 222 | if (occupied) 223 | m_gridmap.data[idx] = 100; 224 | else if (m_gridmap.data[idx] == -1){ 225 | m_gridmap.data[idx] = 0; 226 | } 227 | 228 | for (unsigned i = 0; i < inMapLevel.size(); ++i){ 229 | if (inMapLevel[i]){ 230 | if (occupied) 231 | m_multiGridmap[i].map.data[idx] = 100; 232 | else if (m_multiGridmap[i].map.data[idx] == -1) 233 | m_multiGridmap[i].map.data[idx] = 0; 234 | } 235 | } 236 | 237 | } else { 238 | int intSize = 1 << (m_treeDepth - it.getDepth()); 239 | octomap::OcTreeKey minKey=it.getIndexKey(); 240 | for(int dx=0; dx < intSize; dx++){ 241 | int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale; 242 | for(int dy=0; dy < intSize; dy++){ 243 | unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale); 244 | if (occupied) 245 | m_gridmap.data[idx] = 100; 246 | else if (m_gridmap.data[idx] == -1){ 247 | m_gridmap.data[idx] = 0; 248 | } 249 | 250 | for (unsigned i = 0; i < inMapLevel.size(); ++i){ 251 | if (inMapLevel[i]){ 252 | if (occupied) 253 | m_multiGridmap[i].map.data[idx] = 100; 254 | else if (m_multiGridmap[i].map.data[idx] == -1) 255 | m_multiGridmap[i].map.data[idx] = 0; 256 | } 257 | } 258 | } 259 | } 260 | } 261 | 262 | 263 | } 264 | 265 | } 266 | 267 | 268 | 269 | -------------------------------------------------------------------------------- /octomap_server/src/TrackingOctomapServer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | using namespace octomap; 34 | 35 | namespace octomap_server { 36 | 37 | TrackingOctomapServer::TrackingOctomapServer(const std::string& filename) : 38 | OctomapServer() 39 | { 40 | //read tree if necessary 41 | if (filename != "") { 42 | if (m_octree->readBinary(filename)) { 43 | ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(), m_octree->size()); 44 | m_treeDepth = m_octree->getTreeDepth(); 45 | m_res = m_octree->getResolution(); 46 | m_gridmap.info.resolution = m_res; 47 | 48 | publishAll(); 49 | } else { 50 | ROS_ERROR("Could not open requested file %s, exiting.", filename.c_str()); 51 | exit(-1); 52 | } 53 | } 54 | 55 | ros::NodeHandle private_nh("~"); 56 | 57 | std::string changeSetTopic = "changes"; 58 | std::string changeIdFrame = "/talker/changes"; 59 | 60 | private_nh.param("topic_changes", changeSetTopic, changeSetTopic); 61 | private_nh.param("change_id_frame", change_id_frame, changeIdFrame); 62 | private_nh.param("track_changes", track_changes, false); 63 | private_nh.param("listen_changes", listen_changes, false); 64 | private_nh.param("min_change_pub", min_change_pub, 0); 65 | 66 | if (track_changes && listen_changes) { 67 | ROS_WARN("OctoMapServer: It might not be useful to publish changes and at the same time listen to them." 68 | "Setting 'track_changes' to false!"); 69 | track_changes = false; 70 | } 71 | 72 | if (track_changes) { 73 | ROS_INFO("starting server"); 74 | pubChangeSet = private_nh.advertise( 75 | changeSetTopic, 1); 76 | m_octree->enableChangeDetection(true); 77 | } 78 | 79 | if (listen_changes) { 80 | ROS_INFO("starting client"); 81 | subChangeSet = private_nh.subscribe(changeSetTopic, 1, 82 | &TrackingOctomapServer::trackCallback, this); 83 | } 84 | } 85 | 86 | TrackingOctomapServer::~TrackingOctomapServer() { 87 | } 88 | 89 | void TrackingOctomapServer::insertScan(const tf::Point & sensorOrigin, const PCLPointCloud & ground, const PCLPointCloud & nonground) { 90 | OctomapServer::insertScan(sensorOrigin, ground, nonground); 91 | 92 | if (track_changes) { 93 | trackChanges(); 94 | } 95 | } 96 | 97 | void TrackingOctomapServer::trackChanges() { 98 | KeyBoolMap::const_iterator startPnt = m_octree->changedKeysBegin(); 99 | KeyBoolMap::const_iterator endPnt = m_octree->changedKeysEnd(); 100 | 101 | pcl::PointCloud changedCells = pcl::PointCloud(); 102 | 103 | int c = 0; 104 | for (KeyBoolMap::const_iterator iter = startPnt; iter != endPnt; ++iter) { 105 | ++c; 106 | OcTreeNode* node = m_octree->search(iter->first); 107 | 108 | bool occupied = m_octree->isNodeOccupied(node); 109 | 110 | point3d center = m_octree->keyToCoord(iter->first); 111 | 112 | pcl::PointXYZI pnt; 113 | pnt.x = center(0); 114 | pnt.y = center(1); 115 | pnt.z = center(2); 116 | 117 | if (occupied) { 118 | pnt.intensity = 1000; 119 | } 120 | else { 121 | pnt.intensity = -1000; 122 | } 123 | 124 | changedCells.push_back(pnt); 125 | } 126 | 127 | if (c > min_change_pub) 128 | { 129 | sensor_msgs::PointCloud2 changed; 130 | pcl::toROSMsg(changedCells, changed); 131 | changed.header.frame_id = change_id_frame; 132 | changed.header.stamp = ros::Time().now(); 133 | pubChangeSet.publish(changed); 134 | ROS_DEBUG("[server] sending %d changed entries", (int)changedCells.size()); 135 | 136 | m_octree->resetChangeDetection(); 137 | ROS_DEBUG("[server] octomap size after updating: %d", (int)m_octree->calcNumNodes()); 138 | } 139 | } 140 | 141 | void TrackingOctomapServer::trackCallback(sensor_msgs::PointCloud2Ptr cloud) { 142 | pcl::PointCloud cells; 143 | pcl::fromROSMsg(*cloud, cells); 144 | ROS_DEBUG("[client] size of newly occupied cloud: %i", (int)cells.points.size()); 145 | 146 | for (size_t i = 0; i < cells.points.size(); i++) { 147 | pcl::PointXYZI& pnt = cells.points[i]; 148 | m_octree->updateNode(m_octree->coordToKey(pnt.x, pnt.y, pnt.z), pnt.intensity, false); 149 | } 150 | 151 | m_octree->updateInnerOccupancy(); 152 | ROS_DEBUG("[client] octomap size after updating: %d", (int)m_octree->calcNumNodes()); 153 | } 154 | 155 | } /* namespace octomap */ 156 | -------------------------------------------------------------------------------- /octomap_server/src/octomap_saver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010-2013, A. Hornung, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | using octomap_msgs::GetOctomap; 37 | 38 | #define USAGE "\nUSAGE: octomap_saver [-f] \n" \ 39 | " -f: Query for the full occupancy octree, instead of just the compact binary one\n" \ 40 | " mapfile.bt: filename of map to be saved (.bt: binary tree, .ot: general octree)\n" 41 | 42 | using namespace std; 43 | using namespace octomap; 44 | 45 | class MapSaver{ 46 | public: 47 | MapSaver(const std::string& mapname, bool full){ 48 | ros::NodeHandle n; 49 | std::string servname = "octomap_binary"; 50 | if (full) 51 | servname = "octomap_full"; 52 | ROS_INFO("Requesting the map from %s...", n.resolveName(servname).c_str()); 53 | GetOctomap::Request req; 54 | GetOctomap::Response resp; 55 | while(n.ok() && !ros::service::call(servname, req, resp)) 56 | { 57 | ROS_WARN("Request to %s failed; trying again...", n.resolveName(servname).c_str()); 58 | usleep(1000000); 59 | } 60 | 61 | if (n.ok()){ // skip when CTRL-C 62 | 63 | AbstractOcTree* tree = octomap_msgs::msgToMap(resp.map); 64 | AbstractOccupancyOcTree* octree = NULL; 65 | if (tree){ 66 | octree = dynamic_cast(tree); 67 | } else { 68 | ROS_ERROR("Error creating octree from received message"); 69 | if (resp.map.id == "ColorOcTree") 70 | ROS_WARN("You requested a binary map for a ColorOcTree - this is currently not supported. Please add -f to request a full map"); 71 | } 72 | 73 | if (octree){ 74 | ROS_INFO("Map received (%zu nodes, %f m res), saving to %s", octree->size(), octree->getResolution(), mapname.c_str()); 75 | 76 | std::string suffix = mapname.substr(mapname.length()-3, 3); 77 | if (suffix== ".bt"){ // write to binary file: 78 | if (!octree->writeBinary(mapname)){ 79 | ROS_ERROR("Error writing to file %s", mapname.c_str()); 80 | } 81 | } else if (suffix == ".ot"){ // write to full .ot file: 82 | if (!octree->write(mapname)){ 83 | ROS_ERROR("Error writing to file %s", mapname.c_str()); 84 | } 85 | } else{ 86 | ROS_ERROR("Unknown file extension, must be either .bt or .ot"); 87 | } 88 | 89 | 90 | } else{ 91 | ROS_ERROR("Error reading OcTree from stream"); 92 | } 93 | 94 | delete octree; 95 | 96 | } 97 | } 98 | }; 99 | 100 | int main(int argc, char** argv){ 101 | ros::init(argc, argv, "octomap_saver"); 102 | std::string mapFilename(""); 103 | bool fullmap = false; 104 | if (argc == 3 && strcmp(argv[1], "-f")==0){ 105 | fullmap = true; 106 | mapFilename = std::string(argv[2]); 107 | } else if (argc == 2) 108 | mapFilename = std::string(argv[1]); 109 | else{ 110 | ROS_ERROR("%s", USAGE); 111 | exit(1); 112 | } 113 | 114 | try{ 115 | MapSaver ms(mapFilename, fullmap); 116 | }catch(std::runtime_error& e){ 117 | ROS_ERROR("octomap_saver exception: %s", e.what()); 118 | exit(2); 119 | } 120 | 121 | exit(0); 122 | } 123 | 124 | 125 | -------------------------------------------------------------------------------- /octomap_server/src/octomap_server_multilayer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009-2013, A. Hornung, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | #include 32 | #include 33 | 34 | #define USAGE "\nUSAGE: octomap_server_multilayer \n" \ 35 | " map.bt: inital octomap 3D map file to read\n" 36 | 37 | using namespace octomap_server; 38 | 39 | int main(int argc, char** argv){ 40 | ros::init(argc, argv, "octomap_server_multilayer"); 41 | std::string mapFilename(""); 42 | 43 | if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ 44 | ROS_ERROR("%s", USAGE); 45 | exit(-1); 46 | } 47 | 48 | 49 | OctomapServerMultilayer server; 50 | ros::spinOnce(); 51 | 52 | if (argc == 2){ 53 | mapFilename = std::string(argv[1]); 54 | if (!server.openFile(mapFilename)){ 55 | ROS_ERROR("Could not open file %s", mapFilename.c_str()); 56 | exit(1); 57 | } 58 | } 59 | 60 | 61 | 62 | 63 | try{ 64 | ros::spin(); 65 | }catch(std::runtime_error& e){ 66 | ROS_ERROR("octomap_server_multilayer exception: %s", e.what()); 67 | return -1; 68 | } 69 | 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /octomap_server/src/octomap_server_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * octomap_server: A Tool to serve 3D OctoMaps in ROS (binary and as visualization) 3 | * (inspired by the ROS map_saver) 4 | * @author A. Hornung, University of Freiburg, Copyright (C) 2009 - 2012. 5 | * @see http://octomap.sourceforge.net/ 6 | * License: BSD 7 | */ 8 | 9 | /* 10 | * Copyright (c) 2009-2012, A. Hornung, University of Freiburg 11 | * All rights reserved. 12 | * 13 | * Redistribution and use in source and binary forms, with or without 14 | * modification, are permitted provided that the following conditions are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above copyright 19 | * notice, this list of conditions and the following disclaimer in the 20 | * documentation and/or other materials provided with the distribution. 21 | * * Neither the name of the University of Freiburg nor the names of its 22 | * contributors may be used to endorse or promote products derived from 23 | * this software without specific prior written permission. 24 | * 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 29 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | */ 37 | 38 | 39 | #include 40 | #include 41 | 42 | #define USAGE "\nUSAGE: octomap_server \n" \ 43 | " map.bt: inital octomap 3D map file to read\n" 44 | 45 | using namespace octomap_server; 46 | 47 | int main(int argc, char** argv){ 48 | ros::init(argc, argv, "octomap_server"); 49 | const ros::NodeHandle nh; 50 | const ros::NodeHandle private_nh("~"); 51 | std::string mapFilename(""), mapFilenameParam(""); 52 | 53 | if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ 54 | ROS_ERROR("%s", USAGE); 55 | exit(-1); 56 | } 57 | 58 | OctomapServer server(private_nh, nh); 59 | ros::spinOnce(); 60 | 61 | if (argc == 2){ 62 | mapFilename = std::string(argv[1]); 63 | } 64 | 65 | if (private_nh.getParam("map_file", mapFilenameParam)) { 66 | if (mapFilename != "") { 67 | ROS_WARN("map_file is specified by the argument '%s' and rosparam '%s'. now loads '%s'", mapFilename.c_str(), mapFilenameParam.c_str(), mapFilename.c_str()); 68 | } else { 69 | mapFilename = mapFilenameParam; 70 | } 71 | } 72 | 73 | if (mapFilename != "") { 74 | if (!server.openFile(mapFilename)){ 75 | ROS_ERROR("Could not open file %s", mapFilename.c_str()); 76 | exit(1); 77 | } 78 | } 79 | 80 | try{ 81 | ros::spin(); 82 | }catch(std::runtime_error& e){ 83 | ROS_ERROR("octomap_server exception: %s", e.what()); 84 | return -1; 85 | } 86 | 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /octomap_server/src/octomap_server_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * octomap_server_nodelet: A nodelet version of A. Hornung's octomap server 3 | * @author Marcus Liebhardt 4 | * License: BSD 5 | */ 6 | 7 | /* 8 | * Copyright (c) 2012, Marcus Liebhardt, Yujin Robot 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of Yujin Robot nor the names of its 20 | * contributors may be used to endorse or promote products derived from 21 | * this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | 43 | namespace octomap_server 44 | { 45 | 46 | class OctomapServerNodelet : public nodelet::Nodelet 47 | { 48 | public: 49 | virtual void onInit() 50 | { 51 | NODELET_DEBUG("Initializing octomap server nodelet ..."); 52 | ros::NodeHandle& nh = this->getNodeHandle(); 53 | ros::NodeHandle& private_nh = this->getPrivateNodeHandle(); 54 | server_.reset(new OctomapServer(private_nh, nh)); 55 | 56 | std::string mapFilename(""); 57 | if (private_nh.getParam("map_file", mapFilename)) { 58 | if (!server_->openFile(mapFilename)){ 59 | NODELET_WARN("Could not open file %s", mapFilename.c_str()); 60 | } 61 | } 62 | } 63 | private: 64 | boost::shared_ptr server_; 65 | }; 66 | 67 | } // namespace 68 | 69 | PLUGINLIB_EXPORT_CLASS(octomap_server::OctomapServerNodelet, nodelet::Nodelet) 70 | -------------------------------------------------------------------------------- /octomap_server/src/octomap_server_static.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, A. Hornung, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | using octomap_msgs::GetOctomap; 37 | 38 | #define USAGE "\nUSAGE: octomap_server_static \n" \ 39 | " mapfile.bt: OctoMap filename to be loaded (.bt: binary tree, .ot: general octree)\n" 40 | 41 | using namespace std; 42 | using namespace octomap; 43 | 44 | class OctomapServerStatic{ 45 | public: 46 | OctomapServerStatic(const std::string& filename) 47 | : m_octree(NULL), m_worldFrameId("map") 48 | { 49 | 50 | ros::NodeHandle private_nh("~"); 51 | private_nh.param("frame_id", m_worldFrameId, m_worldFrameId); 52 | 53 | 54 | // open file: 55 | if (filename.length() <= 3){ 56 | ROS_ERROR("Octree file does not have .ot extension"); 57 | exit(1); 58 | } 59 | 60 | std::string suffix = filename.substr(filename.length()-3, 3); 61 | 62 | // .bt files only as OcTree, all other classes need to be in .ot files: 63 | if (suffix == ".bt"){ 64 | OcTree* octree = new OcTree(filename); 65 | 66 | m_octree = octree; 67 | } else if (suffix == ".ot"){ 68 | AbstractOcTree* tree = AbstractOcTree::read(filename); 69 | if (!tree){ 70 | ROS_ERROR("Could not read octree from file"); 71 | exit(1); 72 | } 73 | 74 | m_octree = dynamic_cast(tree); 75 | 76 | } else{ 77 | ROS_ERROR("Octree file does not have .bt or .ot extension"); 78 | exit(1); 79 | } 80 | 81 | if (!m_octree ){ 82 | ROS_ERROR("Could not read right octree class in file"); 83 | exit(1); 84 | } 85 | 86 | ROS_INFO("Read octree type \"%s\" from file %s", m_octree->getTreeType().c_str(), filename.c_str()); 87 | ROS_INFO("Octree resultion: %f, size: %zu", m_octree->getResolution(), m_octree->size()); 88 | 89 | 90 | m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServerStatic::octomapBinarySrv, this); 91 | m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServerStatic::octomapFullSrv, this); 92 | 93 | } 94 | 95 | ~OctomapServerStatic(){ 96 | 97 | 98 | } 99 | 100 | bool octomapBinarySrv(GetOctomap::Request &req, 101 | GetOctomap::Response &res) 102 | { 103 | ROS_INFO("Sending binary map data on service request"); 104 | res.map.header.frame_id = m_worldFrameId; 105 | res.map.header.stamp = ros::Time::now(); 106 | if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map)) 107 | return false; 108 | 109 | return true; 110 | } 111 | 112 | bool octomapFullSrv(GetOctomap::Request &req, 113 | GetOctomap::Response &res) 114 | { 115 | ROS_INFO("Sending full map data on service request"); 116 | res.map.header.frame_id = m_worldFrameId; 117 | res.map.header.stamp = ros::Time::now(); 118 | 119 | 120 | if (!octomap_msgs::fullMapToMsg(*m_octree, res.map)) 121 | return false; 122 | 123 | return true; 124 | } 125 | 126 | private: 127 | ros::ServiceServer m_octomapBinaryService, m_octomapFullService; 128 | ros::NodeHandle m_nh; 129 | std::string m_worldFrameId; 130 | AbstractOccupancyOcTree* m_octree; 131 | 132 | }; 133 | 134 | int main(int argc, char** argv){ 135 | ros::init(argc, argv, "octomap_server_static"); 136 | std::string mapFilename(""); 137 | 138 | if (argc == 2) 139 | mapFilename = std::string(argv[1]); 140 | else{ 141 | ROS_ERROR("%s", USAGE); 142 | exit(1); 143 | } 144 | 145 | try{ 146 | OctomapServerStatic ms(mapFilename); 147 | ros::spin(); 148 | }catch(std::runtime_error& e){ 149 | ROS_ERROR("octomap_server_static exception: %s", e.what()); 150 | exit(2); 151 | } 152 | 153 | exit(0); 154 | } 155 | 156 | 157 | -------------------------------------------------------------------------------- /octomap_server/src/octomap_tracking_server_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #define USAGE "\nUSAGE: octomap_tracking_server \n" \ 34 | " map.bt: octomap 3D map file to read\n" 35 | 36 | using namespace octomap_server; 37 | 38 | int main(int argc, char** argv){ 39 | ros::init(argc, argv, "octomap_tracking_server"); 40 | std::string mapFilename(""); 41 | 42 | if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ 43 | ROS_ERROR("%s", USAGE); 44 | exit(-1); 45 | } 46 | 47 | if (argc == 2) 48 | mapFilename = std::string(argv[1]); 49 | 50 | try{ 51 | TrackingOctomapServer ms(mapFilename); 52 | ros::spin(); 53 | }catch(std::runtime_error& e){ 54 | ROS_ERROR("octomap_server exception: %s", e.what()); 55 | return -1; 56 | } 57 | 58 | return 0; 59 | } 60 | --------------------------------------------------------------------------------